准备完整形态

nav.sh

@@ -0,0 +1,20 @@
+source install/setup.bash
+
+commands=(
+    "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
+    "ros2 launch rm_decision decision.launch.py"
+    "ros2 launch rm_simpal_move simple_move.launch.py"
+    "ros2 launch rm_nav_bringup bringup_real.launch.py \
+      world:=C_ROOM \
+      mode:=nav \
+      lio:=fastlio \
+      localization:=gicp \
+      lio_rviz:=False \
+      nav_rviz:=True
+    "
+)
+
+for cmd in "${commands[@]}"; do
+  gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
+  sleep 0.5
+done

src/rm_msgs/.gitignore

@@ -11,6 +11,7 @@ msg/*ActionGoal.msg
 msg/*ActionResult.msg
 msg/*Feedback.msg
 msg/*Goal.msg
+!msg/NavGoal.msg
 msg/*Result.msg
 msg/_*.py
 build_isolated/

src/rm_msgs/msg/NavGoal.msg

@@ -0,0 +1,11 @@
+# 导航目标
+
+uint8 MODE_PID=0
+uint8 MODE_NAV=1
+
+uint8 control_mode      # 控制模式: 0=PID, 1=NAV
+float32 x               # 目标 X 坐标 (map)
+float32 y               # 目标 Y 坐标 (map)
+float32 angle           # 目标角度 (-pi ~ pi)
+float32 max_speed       # 最大线速度 (PID 模式使用)
+float32 tolerance       # 到达容差

src/rm_nav/rm_nav_bringup/config/reality/fastlio_mid360_real.yaml

@@ -7,7 +7,7 @@
         filter_size_map: 0.5
         cube_side_length: 1000.0
         runtime_pos_log_enable: false
-        map_file_path: "src/rm_nav_bringup/PCD/YOUR_MAP_NAME.pcd"
+        map_file_path: "src/rm_nav/rm_nav_bringup/PCD/C_ROOM.pcd"
 
         common:
             lid_topic:  "/livox/lidar"

src/rm_nav/rm_nav_bringup/map/C_ROOM.yaml

@@ -0,0 +1,7 @@
+image: C_ROOM.pgm
+mode: trinary
+resolution: 0.05
+origin: [-6.3, -8.32, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

src/rm_nav/rm_simple_move/README.md

@@ -124,11 +124,11 @@ python3 move_test_new.py
 **PID 模式：**
 ```bash
 ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
-  mode: 0,
-  target_x: 1.0,
-  target_y: 2.0,
-  target_angle: 0.0,
-  max_speed: 2.0,
+  control_mode: 1,
+  x: -2.0,
+  y: -1.0,
+  angle: 180.0,
+  max_speed: 1.0,
   tolerance: 0.1
 }" --once
 ```
@@ -137,8 +137,8 @@ ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
 ```bash
 ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
   mode: 1,
-  target_x: 1.0,
-  target_y: 2.0,
+  target_x: -2.0,
+  target_y: -1.0,
   target_angle: 0.0,
   max_speed: 0.0,
   tolerance: 0.0

src/rm_nav/rm_simple_move/src/simple_move.cpp

@@ -68,7 +68,7 @@ namespace rm_simpal_move
           pid_angular_(1.5f, 0.0f, 0.2f, 2.0f)
     {
         // 声明参数
-        this->declare_parameter("pid_linear_kp", 0.8);
+        this->declare_parameter("pid_linear_kp", 0.3);
         this->declare_parameter("pid_linear_ki", 0.0);
         this->declare_parameter("pid_linear_kd", 0.1);
         this->declare_parameter("pid_angular_kp", 1.5);
@@ -242,7 +242,13 @@ namespace rm_simpal_move
                 vx *= scale;
                 vy *= scale;
             }
-
+            // if (wz >goal_pose_.max_speed/2.0f) {
+            //     wz = goal_pose_.max_speed;
+            // }
+            // if (wz < -goal_pose_.max_speed/2.0f) {
+            //     wz = -goal_pose_.max_speed;
+            // }
+            wz = 0.0f;
             // 发布标准 cmd_vel_move
             auto cmd_vel_msg = geometry_msgs::msg::Twist();
             cmd_vel_msg.linear.x = vx;