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

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_nav_bringup/config/reality/nav2_params_real.yaml

@@ -106,7 +106,7 @@ controller_server:
     general_goal_checker:
       stateful: True
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.25
+      xy_goal_tolerance: 0.4
       yaw_goal_tolerance: 6.28
     FollowPath:
       plugin: teb_local_planner::TebLocalPlannerROS
@@ -116,7 +116,7 @@ controller_server:
       # Trajectory
       #*******************************************************************************
       teb_autosize: 1.0                             #是否自动调整轨迹大小。根据时间分辨率自动调整轨迹大小
-      dt_ref: 0.6                                   #轨迹的期望时间分辨率（应该与底层控制速率的数量级相符）
+      dt_ref: 0.3                                   #轨迹的期望时间分辨率（应该与底层控制速率的数量级相符）
       dt_hysteresis: 0.06                           #自动调整大小的滞后（根据当前时间分辨率 dt）：通常为 dt_ref 的 10%
       max_samples: 500                              #最大样本数量；警告：如果太小，离散化/分辨率可能不足以用于给定的机器人模型，或者避障不再有效。
       global_plan_overwrite_orientation: False      #覆盖全局规划器提供的局部子目标的方向
@@ -139,10 +139,9 @@ controller_server:
       #*******************************************************************************
       # Robot
       #*******************************************************************************
-      max_vel_x: 2.0                        #最大平移速度
+      max_vel_x: 1.2                        #最大平移速度
-      max_vel_x_backwards: 2.0              #最大后退速度
+      max_vel_x_backwards: 1.2              #最大后退      max_vel_theta: 0.0                    #最大转向角速度（暂时关闭wz）
-      max_vel_theta: 0.0                    #最大转向角速度（暂时关闭wz）
+      acc_lim_x: 1.2                        #最大平移加速度
-      acc_lim_x: 2.0                        #最大平移加速度
       acc_lim_theta: 0.0                    #最大角加速度（暂时关闭wz）
       is_footprint_dynamic: False           #如果为真，在检查轨迹可行性之前更新足迹
       use_proportional_saturation: False    #如果为true，则按比例减少所有扭曲分量（线性x和y以及角度z）（如果任何扭曲分量超过其相应的界限），而不是单独饱和每个扭曲分量
@@ -158,8 +157,8 @@ controller_server:
       #*******************************************************************************
       # Robot/Omni 此部分参数仅适用于全方位移动机器人
       #*******************************************************************************
-      max_vel_y: 2.0
+      max_vel_y: 1.2
-      acc_lim_y: 2.0
+      acc_lim_y: 1.2
 
       #*******************************************************************************
       # GoalTolerance 目标容差
@@ -401,11 +400,11 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [2.0, 2.0, 0.0]
+    max_velocity: [1.2, 1.2, 0.0]
-    min_velocity: [-2.0, -2.0, 0.0]
+    min_velocity: [-1.2, -1.2, 0.0]
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0
-    max_accel: [2.0, 2.0, 0.0]
+    max_accel: [1.2, 1.2, 0.0]
-    max_decel: [-2.0, -2.0, 0.0]
+    max_decel: [-1.2, -1.2, 0.0]
     odom_topic: "Odometry"
     odom_duration: 0.1

src/rm_nav/rm_simple_move/README.md

@@ -124,7 +124,7 @@ python3 move_test_new.py
 **PID 模式：**
 ```bash
 ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
-  control_mode: 0,
+  control_mode: 1,
   x: 1.0,
   y: -5.0,
   angle: 0.0,

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