nav特调

mapping.sh

@@ -3,14 +3,14 @@ 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_simpal_move simple_move.launch.py"
-    "ros2 launch rm_nav_bringup bringup_real.launch.py \
+    # "ros2 launch rm_nav_bringup bringup_real.launch.py \
-      world:=wanzheng \
+    #   world:=wanzheng \
-      mode:=mapping  \
+    #   mode:=mapping  \
-      lio:=fastlio \
+    #   lio:=fastlio \
-      lio_rviz:=True \
+    #   lio_rviz:=True \
-      nav_rviz:=True
+    #   nav_rviz:=True
-    "
+    # "
 )
 
 for cmd in "${commands[@]}"; do

nav.sh

@@ -1,7 +1,7 @@
 source install/setup.bash
 
 commands=(
-    # "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
+    "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 \

src/rm_decision/test/simple_nav_test.py

@@ -23,19 +23,26 @@ class SimpleNavTest(Node):
         # 状态
         self.current_status = None
         self.current_distance = 0.0
+        self.navigation_started = False  # 标记导航是否真正开始
 
         # 定义目标点列表 [x, y, angle, max_speed, tolerance, name]
         self.waypoints = [
-            [0.0, 0.0, 0.0, 1.0, 0.15, "起点"],
+            [0.0, 0.0, 0.0, 0.8, 1.0, "起点"],
-            [1.0, 0.0, 0.0, 1.5, 0.15, "右"],
+            [0.12, -6.75, 0.0, 0.8, 1.0, "1"],
-            [1.0, 1.0, math.pi/2, 1.5, 0.15, "右上"],
+            [1.65, -6.76, 0.0, 0.8, 1.0, "2"],
-            [0.0, 1.0, math.pi, 1.5, 0.15, "左上"],
+            [1.15, 0.5, 0.0, 0.8, 1.0, "3"],
-            [0.0, 0.0, 0.0, 1.0, 0.15, "回到起点"],
+            [2.70, 0.7, 0.0, 0.8, 1.0, "4"],
+            [2.10, -6.72, 0.0, 0.8, 1.0, "5"],
+            [3.56, -6.82, 0.0, 0.8, 1.0, "6"],
+            [3.2, 0.9, 0.0, 0.8, 1.0, "7"],
+            [5.0, 0.56, 0.0, 0.8, 1.0, "8"],
+            [4.96, -5.96, 0.0, 0.8, 1.0, "9"],
+            # [0.0, 0.0, 0.0, 1.0, 0.25, "回到起点"],
         ]
 
         self.current_index = 0
         self.goal_sent = False
-        self.goal_timeout = 30.0  # 超时时间（秒）
+        self.goal_timeout = 120.0  # 超时时间（秒）
         self.goal_start_time = None
 
         self.get_logger().info('========================================')
@@ -81,6 +88,9 @@ class SimpleNavTest(Node):
         goal_msg.max_speed = float(wp[3])
         goal_msg.tolerance = float(wp[4])
 
+        # 重置状态标志
+        self.navigation_started = False
+
         # 发布目标
         self.nav_goal_pub.publish(goal_msg)
         self.goal_sent = True
@@ -101,14 +111,22 @@ class SimpleNavTest(Node):
         wp = self.waypoints[self.current_index]
         elapsed = time.time() - self.goal_start_time
 
-        # 检查是否到达 (status == 2)
+        # 标记导航已开始（状态为1表示正在导航）
+        if self.current_status == 1:
+            self.navigation_started = True
+
+        # 检查是否到达 (status == 2)，但必须先经过导航中状态
         if self.current_status == 2:
+            # 如果导航还没开始就到达，说明是误判，等待真正开始
+            if not self.navigation_started and elapsed < 2.0:
+                return
+
             self.get_logger().info(
                 f'✓ 到达目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} '
                 f'(用时 {elapsed:.1f}秒)')
             self.current_index += 1
             self.goal_sent = False
-            time.sleep(1.0)  # 等待1秒
+            # time.sleep(0.1)  # 短暂延迟，避免消息丢失
             self.send_next_goal()
             return
 
@@ -118,7 +136,7 @@ class SimpleNavTest(Node):
                 f'✗ 目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} 导航失败')
             self.current_index += 1
             self.goal_sent = False
-            time.sleep(1.0)
+            # time.sleep(0.1)
             self.send_next_goal()
             return
 
@@ -129,7 +147,7 @@ class SimpleNavTest(Node):
                 f'超时 ({elapsed:.1f}秒)，跳过')
             self.current_index += 1
             self.goal_sent = False
-            time.sleep(1.0)
+            # time.sleep(0.1)
             self.send_next_goal()
             return
 

src/rm_nav/rm_simple_move/config/simple_move.yaml

@@ -12,14 +12,14 @@ rm_simple_move:
 
     # PID 线速度 X
     pid_linear_x:
-      kp: 0.8
+      kp: 0.5
       ki: 0.0
       kd: 0.1
       max_output: 3.0
 
     # PID 线速度 Y
     pid_linear_y:
-      kp: 0.8
+      kp: 0.5
       ki: 0.0
       kd: 0.1
       max_output: 3.0