关闭nav的yaw

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "cmake.sourceDirectory": "/home/robofish/MOVE_AI/src/rm_decision"
+}

nav.sh

@@ -4,12 +4,11 @@ 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:=RMUL \
+    "ros2 launch rm_nav_bringup bringup_real.launch.py \
+      world:=wanzheng \
       mode:=nav \
       lio:=fastlio \
       localization:=gicp \
-      controller:=mppi \
       lio_rviz:=False \
       nav_rviz:=True
     "

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

@@ -141,9 +141,9 @@ controller_server:
       #*******************************************************************************
       max_vel_x: 2.0                        #最大平移速度
       max_vel_x_backwards: 2.0              #最大后退速度
-      max_vel_theta: 6.0                    #最大转向角速度
+      max_vel_theta: 0.0                    #最大转向角速度（暂时关闭wz）
       acc_lim_x: 2.0                        #最大平移加速度
-      acc_lim_theta: 6.0                    #最大角加速度
+      acc_lim_theta: 0.0                    #最大角加速度（暂时关闭wz）
       is_footprint_dynamic: False           #如果为真，在检查轨迹可行性之前更新足迹
       use_proportional_saturation: False    #如果为true，则按比例减少所有扭曲分量（线性x和y以及角度z）（如果任何扭曲分量超过其相应的界限），而不是单独饱和每个扭曲分量
       transform_tolerance: 0.5              #在TF树中查询转换时的容差（秒）
@@ -401,11 +401,11 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [2.0, 2.0, 3.0]
-    min_velocity: [-2.0, -2.0, -3.0]
+    max_velocity: [2.0, 2.0, 0.0]
+    min_velocity: [-2.0, -2.0, 0.0]
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0
-    max_accel: [2.0, 2.0, 6.0]
-    max_decel: [-2.0, -2.0, -6.0]
+    max_accel: [2.0, 2.0, 0.0]
+    max_decel: [-2.0, -2.0, 0.0]
     odom_topic: "Odometry"
     odom_duration: 0.1

src/rm_nav/rm_simple_move/CMakeLists.txt

@@ -36,8 +36,8 @@ install(TARGETS rm_simpal_move
   RUNTIME DESTINATION bin
 )
 
-# install launch files
-install(DIRECTORY launch
+# install launch and config files
+install(DIRECTORY launch config
   DESTINATION share/${PROJECT_NAME}/
 )
 

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 "{
-  control_mode: 1,
-  x: -2.0,
-  y: -1.0,
-  angle: 180.0,
-  max_speed: 1.0,
+  control_mode: 0,
+  x: 1.0,
+  y: -5.0,
+  angle: 0.0,
+  max_speed: 2.0,
   tolerance: 0.1
 }" --once
 ```

src/rm_nav/rm_simple_move/config/simple_move.yaml

@@ -0,0 +1,35 @@
+rm_simple_move:
+  ros__parameters:
+    # 通用参数
+    use_sim_time: false
+    control_frequency: 10.0        # 控制循环频率 (Hz)
+
+    # 话题配置
+    nav_goal_topic: "/nav_goal"
+    cmd_vel_input_topic: "/cmd_vel"
+    cmd_vel_output_topic: "/cmd_vel_move"
+    nav_status_topic: "/nav_status"
+
+    # PID 线速度 X
+    pid_linear_x:
+      kp: 0.8
+      ki: 0.0
+      kd: 0.1
+      max_output: 3.0
+
+    # PID 线速度 Y
+    pid_linear_y:
+      kp: 0.8
+      ki: 0.0
+      kd: 0.1
+      max_output: 3.0
+
+    # PID 角速度
+    pid_angular:
+      kp: 0.5
+      ki: 0.0
+      kd: 0.1
+      max_output: 1.0
+
+    # 最大角速度限幅 (rad/s)
+    max_angular_speed: 1.0

src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp

@@ -102,6 +102,8 @@ private:
     PIDController pid_linear_y_;
     PIDController pid_angular_;
 
+    double max_angular_speed_;  // 角速度独立限幅 (rad/s)
+
     rclcpp::Time last_time_;
 };
 

src/rm_nav/rm_simple_move/launch/simple_move.launch.py

@@ -6,6 +6,9 @@ from launch_ros.actions import ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
 
 def generate_launch_description():
+    pkg_dir = get_package_share_directory('rm_simpal_move')
+    config_file = os.path.join(pkg_dir, 'config', 'simple_move.yaml')
+
     return LaunchDescription([
         ComposableNodeContainer(
             name='simple_move_container',
@@ -17,15 +20,7 @@ def generate_launch_description():
                     package='rm_simpal_move',
                     plugin='rm_simpal_move::RMSimpleMove',
                     name='rm_simple_move',
-                    parameters=[
-                        {'use_sim_time': False},
-                        {'pid_linear_kp': 0.8},
-                        {'pid_linear_ki': 0.0},
-                        {'pid_linear_kd': 0.1},
-                        {'pid_angular_kp': 1.5},
-                        {'pid_angular_ki': 0.0},
-                        {'pid_angular_kd': 0.2}
-                    ]
+                    parameters=[config_file]
                 ),
             ],
             output='screen',

src/rm_nav/rm_simple_move/src/simple_move.cpp

@@ -64,63 +64,106 @@ namespace rm_simpal_move
         : Node("rm_simple_move", options),
           tf_buffer_(this->get_clock()),
           tf_listener_(tf_buffer_),
-          control_mode_(0),  // 默认 PID 模式
+          control_mode_(0),
           running_(true),
           goal_reached_(false),
           has_goal_(false),
-          nav_goal_sent_(false),
-          pid_linear_x_(0.8f, 0.0f, 0.1f, 3.0f),
-          pid_linear_y_(0.8f, 0.0f, 0.1f, 3.0f),
-          pid_angular_(0.8f, 0.0f, 0.1f, 2.0f)
+          nav_goal_sent_(false)
     {
-        // 声明参数
-        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);
-        this->declare_parameter("pid_angular_ki", 0.0);
-        this->declare_parameter("pid_angular_kd", 0.2);
+        // ========== 声明所有参数（默认值与 yaml 一致） ==========
+        // 通用
+        this->declare_parameter("control_frequency", 10.0);
 
-        // 读取参数
-        pid_linear_x_.kp = pid_linear_y_.kp = this->get_parameter("pid_linear_kp").as_double();
-        pid_linear_x_.ki = pid_linear_y_.ki = this->get_parameter("pid_linear_ki").as_double();
-        pid_linear_x_.kd = pid_linear_y_.kd = this->get_parameter("pid_linear_kd").as_double();
-        pid_angular_.kp = this->get_parameter("pid_angular_kp").as_double();
-        pid_angular_.ki = this->get_parameter("pid_angular_ki").as_double();
-        pid_angular_.kd = this->get_parameter("pid_angular_kd").as_double();
+        // 话题
+        this->declare_parameter("nav_goal_topic", std::string("/nav_goal"));
+        this->declare_parameter("cmd_vel_input_topic", std::string("/cmd_vel"));
+        this->declare_parameter("cmd_vel_output_topic", std::string("/cmd_vel_move"));
+        this->declare_parameter("nav_status_topic", std::string("/nav_status"));
+
+        // PID 线速度 X
+        this->declare_parameter("pid_linear_x.kp", 0.8);
+        this->declare_parameter("pid_linear_x.ki", 0.0);
+        this->declare_parameter("pid_linear_x.kd", 0.1);
+        this->declare_parameter("pid_linear_x.max_output", 3.0);
+
+        // PID 线速度 Y
+        this->declare_parameter("pid_linear_y.kp", 0.8);
+        this->declare_parameter("pid_linear_y.ki", 0.0);
+        this->declare_parameter("pid_linear_y.kd", 0.1);
+        this->declare_parameter("pid_linear_y.max_output", 3.0);
+
+        // PID 角速度
+        this->declare_parameter("pid_angular.kp", 0.0005);
+        this->declare_parameter("pid_angular.ki", 0.0);
+        this->declare_parameter("pid_angular.kd", 0.0001);
+        this->declare_parameter("pid_angular.max_output", 0.05);
+
+        // 最大角速度
+        this->declare_parameter("max_angular_speed", 0.05);
+
+        // ========== 读取参数 ==========
+        double control_freq = this->get_parameter("control_frequency").as_double();
+
+        std::string nav_goal_topic = this->get_parameter("nav_goal_topic").as_string();
+        std::string cmd_vel_input_topic = this->get_parameter("cmd_vel_input_topic").as_string();
+        std::string cmd_vel_output_topic = this->get_parameter("cmd_vel_output_topic").as_string();
+        std::string nav_status_topic = this->get_parameter("nav_status_topic").as_string();
+
+        pid_linear_x_ = PIDController(
+            this->get_parameter("pid_linear_x.kp").as_double(),
+            this->get_parameter("pid_linear_x.ki").as_double(),
+            this->get_parameter("pid_linear_x.kd").as_double(),
+            this->get_parameter("pid_linear_x.max_output").as_double()
+        );
+        pid_linear_y_ = PIDController(
+            this->get_parameter("pid_linear_y.kp").as_double(),
+            this->get_parameter("pid_linear_y.ki").as_double(),
+            this->get_parameter("pid_linear_y.kd").as_double(),
+            this->get_parameter("pid_linear_y.max_output").as_double()
+        );
+        pid_angular_ = PIDController(
+            this->get_parameter("pid_angular.kp").as_double(),
+            this->get_parameter("pid_angular.ki").as_double(),
+            this->get_parameter("pid_angular.kd").as_double(),
+            this->get_parameter("pid_angular.max_output").as_double()
+        );
+        max_angular_speed_ = this->get_parameter("max_angular_speed").as_double();
 
         tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
 
-        // 定时器 10Hz
+        // 定时器
+        int period_ms = static_cast<int>(1000.0 / control_freq);
         timer_ = this->create_wall_timer(
-            std::chrono::milliseconds(100),
+            std::chrono::milliseconds(period_ms),
             std::bind(&RMSimpleMove::timer_callback, this)
         );
 
         // 订阅者
         nav_goal_sub_ = this->create_subscription<rm_msgs::msg::NavGoal>(
-            "/nav_goal", 10,
+            nav_goal_topic, 10,
             std::bind(&RMSimpleMove::nav_goal_callback, this, std::placeholders::_1)
         );
 
         cmd_vel_nav_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
-            "/cmd_vel", 10,
+            cmd_vel_input_topic, 10,
             std::bind(&RMSimpleMove::cmd_vel_nav_callback, this, std::placeholders::_1)
         );
 
         // 发布者
-        cmd_vel_move_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_move", 10);
-        nav_status_pub_ = this->create_publisher<rm_msgs::msg::NavStatus>("/nav_status", 10);
+        cmd_vel_move_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(cmd_vel_output_topic, 10);
+        nav_status_pub_ = this->create_publisher<rm_msgs::msg::NavStatus>(nav_status_topic, 10);
 
         // Action Client
         nav_action_client_ = rclcpp_action::create_client<NavigateToPose>(this, "/navigate_to_pose");
 
         RCLCPP_INFO(this->get_logger(), "话题配置:");
-        RCLCPP_INFO(this->get_logger(), "  输入: /nav_goal");
-        RCLCPP_INFO(this->get_logger(), "  输出: /cmd_vel_move (统一输出)");
-        RCLCPP_INFO(this->get_logger(), "  PID模式: 直接计算发布到 /cmd_vel_move");
-        RCLCPP_INFO(this->get_logger(), "  NAV模式: Action /navigate_to_pose → 导航 → /cmd_vel → 转发到 /cmd_vel_move");
-        RCLCPP_INFO(this->get_logger(), "  状态: /nav_status");
+        RCLCPP_INFO(this->get_logger(), "  输入: %s", nav_goal_topic.c_str());
+        RCLCPP_INFO(this->get_logger(), "  输出: %s (统一输出)", cmd_vel_output_topic.c_str());
+        RCLCPP_INFO(this->get_logger(), "  PID模式: 直接计算发布到 %s", cmd_vel_output_topic.c_str());
+        RCLCPP_INFO(this->get_logger(), "  NAV模式: Action /navigate_to_pose → 导航 → %s → 转发到 %s",
+                    cmd_vel_input_topic.c_str(), cmd_vel_output_topic.c_str());
+        RCLCPP_INFO(this->get_logger(), "  状态: %s", nav_status_topic.c_str());
+        RCLCPP_INFO(this->get_logger(), "  控制频率: %.1f Hz", control_freq);
 
         last_time_ = this->get_clock()->now();
 
@@ -252,9 +295,8 @@ namespace rm_simpal_move
             }
 
             // 角速度限幅（独立于线速度限幅）
-            float max_angular_speed = goal_pose_.max_speed;
-            if (wz > max_angular_speed) wz = max_angular_speed;
-            if (wz < -max_angular_speed) wz = -max_angular_speed;
+            if (wz > max_angular_speed_) wz = max_angular_speed_;
+            if (wz < -max_angular_speed_) wz = -max_angular_speed_;
 
             // 发布标准 cmd_vel_move
             auto cmd_vel_msg = geometry_msgs::msg::Twist();