sim.sh

@@ -1,20 +0,0 @@
-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,16 +44,10 @@ install(DIRECTORY include/
   DESTINATION include
 )
 
-install(DIRECTORY launch test
+install(DIRECTORY launch
   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

@@ -1,79 +0,0 @@
-# 简单导航测试脚本
-
-## 快速使用
-
-```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

@@ -1,156 +0,0 @@
-#!/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: 0.0                    #最大转向角速度
+      max_vel_theta: 6.0                    #最大转向角速度
       acc_lim_x: 4.0                        #最大平移加速度
-      acc_lim_theta: 0.0                    #最大角加速度
+      acc_lim_theta: 6.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

@@ -141,15 +141,7 @@ public:
     const double omega2 = angle_diff2 / dt2->dt();
     const double acc_rot  = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );
       
-    // 当角加速度限制为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]);
@@ -344,15 +336,7 @@ public:
     const double omega2 = angle_diff / dt->dt();
     const double acc_rot  = (omega2 - omega1) / dt->dt();
       
-    // 当角加速度限制为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]);
@@ -444,15 +428,7 @@ public:
     const double omega2 = _measurement->angular.z;
     const double acc_rot  = (omega2 - omega1) / dt->dt();
       
-    // 当角加速度限制为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]);
@@ -551,15 +527,7 @@ public:
     double omega2 = g2o::normalize_theta(pose3->theta() - pose2->theta()) / dt2->dt();
     double acc_rot  = (omega2 - omega1)*2 / dt12;
       
-    // 当角加速度限制为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]);
@@ -642,15 +610,7 @@ public:
     double omega2 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt->dt();
     double acc_rot  = (omega2 - omega1) / dt->dt();
       
-    // 当角加速度限制为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]);
@@ -742,15 +702,7 @@ public:
     double omega2 = _measurement->angular.z;
     double acc_rot  = (omega2 - omega1) / dt->dt();
       
-    // 当角加速度限制为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

@@ -113,16 +113,7 @@ public:
     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);
-
-    // 当旋转速度限制为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]);
   }
@@ -265,16 +256,7 @@ public:
     
     _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
-
-    // 当旋转速度限制为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,15 +55,10 @@ namespace
       double trans_dist = (end.position() - start.position()).norm();
       dt_constant_motion = trans_dist / max_vel_x;
     }
-
-    // 如果旋转速度限制很小（< 0.1 rad/s），不让旋转时间主导整体时间
-    // 这样可以避免在旋转速度受限时导致平移速度也变得很慢
-    if (max_vel_theta > 0.1) {
+    if (max_vel_theta > 0) {
       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