add test

sim.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_sim.launch.py \
+    world:=RMUC \
+    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_decision/CMakeLists.txt

@@ -44,10 +44,16 @@ install(DIRECTORY include/
   DESTINATION include
 )
 
-install(DIRECTORY launch
+install(DIRECTORY launch test
   DESTINATION share/${PROJECT_NAME}
 )
 
+# 安装 Python 测试脚本
+install(PROGRAMS
+  test/simple_nav_test.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

src/rm_decision/test/README.md

@@ -0,0 +1,79 @@
+# 简单导航测试脚本
+
+## 快速使用
+
+```bash
+# 直接运行
+python3 src/rm_decision/test/simple_nav_test.py
+
+# 或者通过 ros2 run
+ros2 run rm_decision simple_nav_test.py
+```
+
+## 修改目标点
+
+直接编辑 `simple_nav_test.py` 文件中的 `waypoints` 列表：
+
+```python
+# 格式: [x, y, angle, max_speed, tolerance, name]
+self.waypoints = [
+    [0.0, 0.0, 0.0, 1.0, 0.15, "起点"],
+    [1.0, 0.0, 0.0, 1.5, 0.15, "右"],
+    [1.0, 1.0, math.pi/2, 1.5, 0.15, "右上"],
+    [0.0, 1.0, math.pi, 1.5, 0.15, "左上"],
+    [0.0, 0.0, 0.0, 1.0, 0.15, "回到起点"],
+]
+```
+
+### 参数说明
+
+- `x, y`: 目标坐标（米）
+- `angle`: 目标角度（弧度）
+  - 0° = 0
+  - 90° = math.pi/2 = 1.57
+  - 180° = math.pi = 3.14
+  - -90° = -math.pi/2 = -1.57
+- `max_speed`: 最大速度（米/秒）
+- `tolerance`: 到达容差（米）
+- `name`: 目标点名称（用于日志显示）
+
+## 示例输出
+
+```
+[simple_nav_test]: ========================================
+[simple_nav_test]: 简单导航测试脚本启动
+[simple_nav_test]: 目标点数量: 5
+[simple_nav_test]: ========================================
+[simple_nav_test]:   [0] 起点: (0.00, 0.00, 0°) speed=1.0 tol=0.15
+[simple_nav_test]:   [1] 右: (1.00, 0.00, 0°) speed=1.5 tol=0.15
+[simple_nav_test]:   [2] 右上: (1.00, 1.00, 90°) speed=1.5 tol=0.15
+[simple_nav_test]:   [3] 左上: (0.00, 1.00, 180°) speed=1.5 tol=0.15
+[simple_nav_test]:   [4] 回到起点: (0.00, 0.00, 0°) speed=1.0 tol=0.15
+[simple_nav_test]: ========================================
+[simple_nav_test]: 📍 发送目标点 [1/5] 起点: (0.00, 0.00, 0°) speed=1.0
+[simple_nav_test]: 🚗 导航中 [1/5] 起点: 距离=0.50m 状态=1 (5s)
+[simple_nav_test]: ✓ 到达目标点 [1/5] 起点 (用时 8.2秒)
+[simple_nav_test]: 📍 发送目标点 [2/5] 右: (1.00, 0.00, 0°) speed=1.5
+...
+[simple_nav_test]: ✅ 所有目标点完成！
+```
+
+## 优点
+
+- ✅ 无需编译，直接运行
+- ✅ 修改目标点只需编辑 Python 代码
+- ✅ 代码简单易懂
+- ✅ 实时显示导航进度
+- ✅ 自动处理超时和失败
+
+## 与 C++ 版本对比
+
+| 特性 | Python 脚本 | C++ test_nav_node |
+|------|------------|-------------------|
+| 修改目标点 | 编辑 Python 代码 | 编辑 YAML 配置 |
+| 运行方式 | 直接运行 | 需要编译 |
+| 灵活性 | 高（可随时修改） | 中（需重新编译） |
+| 性能 | 足够 | 更高 |
+| 适用场景 | 快速测试 | 生产环境 |
+
+推荐使用 Python 脚本进行快速测试！

src/rm_decision/test/simple_nav_test.py

@@ -0,0 +1,156 @@
+#!/usr/bin/env python3
+"""
+简单的导航测试脚本
+直接在代码中定义目标点，通过 /nav_goal 话题发送给 simple_move
+"""
+
+import rclpy
+from rclpy.node import Node
+from rm_msgs.msg import NavGoal, NavStatus
+import time
+import math
+
+
+class SimpleNavTest(Node):
+    def __init__(self):
+        super().__init__('simple_nav_test')
+
+        # 发布者和订阅者
+        self.nav_goal_pub = self.create_publisher(NavGoal, '/nav_goal', 10)
+        self.nav_status_sub = self.create_subscription(
+            NavStatus, '/nav_status', self.nav_status_callback, 10)
+
+        # 状态
+        self.current_status = None
+        self.current_distance = 0.0
+
+        # 定义目标点列表 [x, y, angle, max_speed, tolerance, name]
+        self.waypoints = [
+            [0.0, 0.0, 0.0, 1.0, 0.15, "起点"],
+            [1.0, 0.0, 0.0, 1.5, 0.15, "右"],
+            [1.0, 1.0, math.pi/2, 1.5, 0.15, "右上"],
+            [0.0, 1.0, math.pi, 1.5, 0.15, "左上"],
+            [0.0, 0.0, 0.0, 1.0, 0.15, "回到起点"],
+        ]
+
+        self.current_index = 0
+        self.goal_sent = False
+        self.goal_timeout = 30.0  # 超时时间（秒）
+        self.goal_start_time = None
+
+        self.get_logger().info('========================================')
+        self.get_logger().info('简单导航测试脚本启动')
+        self.get_logger().info(f'目标点数量: {len(self.waypoints)}')
+        self.get_logger().info('========================================')
+
+        # 显示所有目标点
+        for i, wp in enumerate(self.waypoints):
+            self.get_logger().info(
+                f'  [{i}] {wp[5]}: ({wp[0]:.2f}, {wp[1]:.2f}, {wp[2]*180/math.pi:.0f}°) '
+                f'speed={wp[3]:.1f} tol={wp[4]:.2f}')
+
+        self.get_logger().info('========================================')
+
+        # 等待2秒后开始
+        time.sleep(2.0)
+        self.send_next_goal()
+
+        # 创建定时器检查状态
+        self.timer = self.create_timer(0.5, self.check_status)
+
+    def nav_status_callback(self, msg):
+        """接收导航状态"""
+        self.current_status = msg.status
+        self.current_distance = msg.distance
+
+    def send_next_goal(self):
+        """发送下一个目标点"""
+        if self.current_index >= len(self.waypoints):
+            self.get_logger().info('✅ 所有目标点完成！')
+            rclpy.shutdown()
+            return
+
+        wp = self.waypoints[self.current_index]
+
+        # 创建导航目标消息
+        goal_msg = NavGoal()
+        goal_msg.control_mode = 0  # 0: PID模式
+        goal_msg.x = float(wp[0])
+        goal_msg.y = float(wp[1])
+        goal_msg.angle = float(wp[2])
+        goal_msg.max_speed = float(wp[3])
+        goal_msg.tolerance = float(wp[4])
+
+        # 发布目标
+        self.nav_goal_pub.publish(goal_msg)
+        self.goal_sent = True
+        self.goal_start_time = time.time()
+
+        self.get_logger().info(
+            f'📍 发送目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]}: '
+            f'({wp[0]:.2f}, {wp[1]:.2f}, {wp[2]*180/math.pi:.0f}°) speed={wp[3]:.1f}')
+
+    def check_status(self):
+        """检查导航状态"""
+        if not self.goal_sent:
+            return
+
+        if self.current_status is None:
+            return
+
+        wp = self.waypoints[self.current_index]
+        elapsed = time.time() - self.goal_start_time
+
+        # 检查是否到达 (status == 2)
+        if self.current_status == 2:
+            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秒
+            self.send_next_goal()
+            return
+
+        # 检查是否失败 (status == 3)
+        if self.current_status == 3:
+            self.get_logger().warn(
+                f'✗ 目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} 导航失败')
+            self.current_index += 1
+            self.goal_sent = False
+            time.sleep(1.0)
+            self.send_next_goal()
+            return
+
+        # 检查超时
+        if elapsed > self.goal_timeout:
+            self.get_logger().warn(
+                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)
+            self.send_next_goal()
+            return
+
+        # 每5秒显示一次进度
+        if int(elapsed) % 5 == 0 and int(elapsed * 2) % 2 == 0:
+            self.get_logger().info(
+                f'🚗 导航中 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]}: '
+                f'距离={self.current_distance:.2f}m 状态={self.current_status} ({elapsed:.0f}s)')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    try:
+        node = SimpleNavTest()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        print('\n用户中断')
+    finally:
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rm_nav/rm_nav_bringup/config/simulation/nav2_params_sim.yaml

@@ -141,9 +141,9 @@ controller_server:
       #*******************************************************************************
       max_vel_x: 3.0                        #最大平移速度
       max_vel_x_backwards: 3.0              #最大后退速度
-      max_vel_theta: 6.0                    #最大转向角速度
+      max_vel_theta: 0.0                    #最大转向角速度
       acc_lim_x: 4.0                        #最大平移加速度
-      acc_lim_theta: 6.0                    #最大角加速度
+      acc_lim_theta: 0.0                    #最大角加速度
       is_footprint_dynamic: False           #如果为真，在检查轨迹可行性之前更新足迹
       use_proportional_saturation: False    #如果为true，则按比例减少所有扭曲分量（线性x和y以及角度z）（如果任何扭曲分量超过其相应的界限），而不是单独饱和每个扭曲分量
       transform_tolerance: 0.5              #在TF树中查询转换时的容差（秒）

src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h

@@ -135,13 +135,21 @@ public:
    
 
     _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
-    
+
     // ANGULAR ACCELERATION
     const double omega1 = angle_diff1 / dt1->dt();
     const double omega2 = angle_diff2 / dt2->dt();
     const double acc_rot  = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );
-      
-    _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    // 当角加速度限制为0或很小时，几乎完全忽略这个约束
+    if (cfg_->robot.acc_lim_theta < 0.01) {
+      _error[1] = 0.0;  // 完全忽略
+    } else if (cfg_->robot.acc_lim_theta < 1.0) {
+      _error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
+      _error[1] *= 0.1;  // 大幅降低影响
+    } else {
+      _error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
+    }
 
     TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
     TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() rotational: _error[1]=%f\n",_error[1]);
@@ -330,13 +338,21 @@ public:
     const double acc_lin  = (vel2 - vel1) / dt->dt();
     
     _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
-    
+
     // ANGULAR ACCELERATION
     const double omega1 = _measurement->angular.z;
     const double omega2 = angle_diff / dt->dt();
     const double acc_rot  = (omega2 - omega1) / dt->dt();
-      
-    _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    // 当角加速度限制为0或很小时，几乎完全忽略这个约束
+    if (cfg_->robot.acc_lim_theta < 0.01) {
+      _error[1] = 0.0;
+    } else if (cfg_->robot.acc_lim_theta < 1.0) {
+      _error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
+      _error[1] *= 0.1;
+    } else {
+      _error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
+    }
 
     TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
     TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() rotational: _error[1]=%f\n",_error[1]);
@@ -422,13 +438,21 @@ public:
     const double acc_lin  = (vel2 - vel1) / dt->dt();
 
     _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
-    
+
     // ANGULAR ACCELERATION
     const double omega1 = angle_diff / dt->dt();
     const double omega2 = _measurement->angular.z;
     const double acc_rot  = (omega2 - omega1) / dt->dt();
-      
-    _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    // 当角加速度限制为0或很小时，几乎完全忽略这个约束
+    if (cfg_->robot.acc_lim_theta < 0.01) {
+      _error[1] = 0.0;
+    } else if (cfg_->robot.acc_lim_theta < 1.0) {
+      _error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
+      _error[1] *= 0.1;
+    } else {
+      _error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
+    }
 
     TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
     TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() rotational: _error[1]=%f\n",_error[1]);
@@ -521,13 +545,21 @@ public:
    
     _error[0] = penaltyBoundToInterval(acc_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
     _error[1] = penaltyBoundToInterval(acc_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
-    
+
     // ANGULAR ACCELERATION
     double omega1 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt1->dt();
     double omega2 = g2o::normalize_theta(pose3->theta() - pose2->theta()) / dt2->dt();
     double acc_rot  = (omega2 - omega1)*2 / dt12;
-      
-    _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    // 当角加速度限制为0或很小时，几乎完全忽略这个约束
+    if (cfg_->robot.acc_lim_theta < 0.01) {
+      _error[2] = 0.0;
+    } else if (cfg_->robot.acc_lim_theta < 1.0) {
+      _error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
+      _error[2] *= 0.1;
+    } else {
+      _error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
+    }
 
     
     TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
@@ -604,13 +636,21 @@ public:
     
     _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
     _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
-    
+
     // ANGULAR ACCELERATION
     double omega1 = _measurement->angular.z;
     double omega2 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt->dt();
     double acc_rot  = (omega2 - omega1) / dt->dt();
-      
-    _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    // 当角加速度限制为0或很小时，几乎完全忽略这个约束
+    if (cfg_->robot.acc_lim_theta < 0.01) {
+      _error[2] = 0.0;
+    } else if (cfg_->robot.acc_lim_theta < 1.0) {
+      _error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
+      _error[2] *= 0.1;
+    } else {
+      _error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
+    }
 
     TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
     TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() strafing: _error[1]=%f\n",_error[1]);
@@ -696,13 +736,21 @@ public:
 
     _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
     _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
-    
+
     // ANGULAR ACCELERATION
     double omega1 = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()) / dt->dt();
     double omega2 = _measurement->angular.z;
     double acc_rot  = (omega2 - omega1) / dt->dt();
-      
-    _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    // 当角加速度限制为0或很小时，几乎完全忽略这个约束
+    if (cfg_->robot.acc_lim_theta < 0.01) {
+      _error[2] = 0.0;
+    } else if (cfg_->robot.acc_lim_theta < 1.0) {
+      _error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
+      _error[2] *= 0.1;
+    } else {
+      _error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
+    }
 
     TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
     TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() strafing: _error[1]=%f\n",_error[1]);

src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h

@@ -88,16 +88,16 @@ public:
   
   /**
    * @brief Actual cost function
-   */  
+   */
   void computeError()
   {
     TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
     const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
     const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
     const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
-    
+
     const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
-    
+
     double dist = deltaS.norm();
     const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());
     if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
@@ -106,14 +106,23 @@ public:
         dist = fabs( angle_diff * radius ); // actual arg length!
     }
     double vel = dist / deltaT->estimate();
-    
+
 //     vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
     vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
-    
+
     const double omega = angle_diff / deltaT->estimate();
-  
+
     _error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
-    _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
+
+    // 当旋转速度限制为0或很小时，几乎完全忽略这个约束
+    if (cfg_->robot.max_vel_theta < 0.01) {
+      _error[1] = 0.0;  // 完全忽略
+    } else if (cfg_->robot.max_vel_theta < 0.5) {
+      _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon * 5.0);
+      _error[1] *= 0.1;
+    } else {
+      _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon);
+    }
 
     TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
   }
@@ -234,7 +243,7 @@ public:
   
   /**
    * @brief Actual cost function
-   */  
+   */
   void computeError()
   {
     TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocityHolonomic()");
@@ -242,21 +251,30 @@ public:
     const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
     const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
     Eigen::Vector2d deltaS = conf2->position() - conf1->position();
-    
+
     double cos_theta1 = std::cos(conf1->theta());
-    double sin_theta1 = std::sin(conf1->theta()); 
-    
+    double sin_theta1 = std::sin(conf1->theta());
+
     // transform conf2 into current robot frame conf1 (inverse 2d rotation matrix)
     double r_dx =  cos_theta1*deltaS.x() + sin_theta1*deltaS.y();
     double r_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();
-    
+
     double vx = r_dx / deltaT->estimate();
     double vy = r_dy / deltaT->estimate();
     double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate();
-    
+
     _error[0] = penaltyBoundToInterval(vx, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon);
     _error[1] = penaltyBoundToInterval(vy, cfg_->robot.max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero
-    _error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
+
+    // 当旋转速度限制为0或很小时，几乎完全忽略这个约束
+    if (cfg_->robot.max_vel_theta < 0.01) {
+      _error[2] = 0.0;
+    } else if (cfg_->robot.max_vel_theta < 0.5) {
+      _error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon * 5.0);
+      _error[2] *= 0.1;
+    } else {
+      _error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon);
+    }
 
     TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]) && std::isfinite(_error[2]),
                    "EdgeVelocityHolonomic::computeError() _error[0]=%f _error[1]=%f _error[2]=%f\n",_error[0],_error[1],_error[2]);

src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/src/timed_elastic_band.cpp

@@ -55,10 +55,15 @@ namespace
       double trans_dist = (end.position() - start.position()).norm();
       dt_constant_motion = trans_dist / max_vel_x;
     }
-    if (max_vel_theta > 0) {
+
+    // 如果旋转速度限制很小（< 0.1 rad/s），不让旋转时间主导整体时间
+    // 这样可以避免在旋转速度受限时导致平移速度也变得很慢
+    if (max_vel_theta > 0.1) {
       double rot_dist = std::abs(g2o::normalize_theta(end.theta() - start.theta()));
       dt_constant_motion = std::max(dt_constant_motion, rot_dist / max_vel_theta);
     }
+    // 如果旋转速度很小或为0，只考虑平移时间，让优化器自己处理旋转约束
+
     return dt_constant_motion;
   }
 } // namespace