From ac92e49cee2eb45a13dcc4d27a89034eb30e0ff9 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Thu, 12 Mar 2026 00:41:14 +0800 Subject: [PATCH] simple move add config --- .vscode/settings.json | 3 + nav.sh | 5 +- src/rm_nav/rm_simple_move/CMakeLists.txt | 4 +- src/rm_nav/rm_simple_move/README.md | 10 +- .../rm_simple_move/config/simple_move.yaml | 35 ++++++ .../include/rm_simpal_move/simple_move.hpp | 2 + .../launch/simple_move.launch.py | 13 +-- src/rm_nav/rm_simple_move/src/simple_move.cpp | 108 ++++++++++++------ 8 files changed, 128 insertions(+), 52 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 src/rm_nav/rm_simple_move/config/simple_move.yaml diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..5b89cd2 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "cmake.sourceDirectory": "/home/robofish/MOVE_AI/src/rm_decision" +} \ No newline at end of file diff --git a/nav.sh b/nav.sh index 7884934..1fae5ee 100644 --- a/nav.sh +++ b/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 " diff --git a/src/rm_nav/rm_simple_move/CMakeLists.txt b/src/rm_nav/rm_simple_move/CMakeLists.txt index f4fa575..7ca00ed 100644 --- a/src/rm_nav/rm_simple_move/CMakeLists.txt +++ b/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}/ ) diff --git a/src/rm_nav/rm_simple_move/README.md b/src/rm_nav/rm_simple_move/README.md index 785ea0e..dec1a1f 100644 --- a/src/rm_nav/rm_simple_move/README.md +++ b/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 ``` diff --git a/src/rm_nav/rm_simple_move/config/simple_move.yaml b/src/rm_nav/rm_simple_move/config/simple_move.yaml new file mode 100644 index 0000000..abff449 --- /dev/null +++ b/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.0005 + ki: 0.0 + kd: 0.0001 + max_output: 0.05 + + # 最大角速度限幅 (rad/s) + max_angular_speed: 0.05 diff --git a/src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp b/src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp index 894cd1c..e3a0d0e 100644 --- a/src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp +++ b/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_; }; diff --git a/src/rm_nav/rm_simple_move/launch/simple_move.launch.py b/src/rm_nav/rm_simple_move/launch/simple_move.launch.py index c64e3bf..1591051 100644 --- a/src/rm_nav/rm_simple_move/launch/simple_move.launch.py +++ b/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', diff --git a/src/rm_nav/rm_simple_move/src/simple_move.cpp b/src/rm_nav/rm_simple_move/src/simple_move.cpp index caf12e3..db8f08b 100644 --- a/src/rm_nav/rm_simple_move/src/simple_move.cpp +++ b/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(this); - // 定时器 10Hz + // 定时器 + int period_ms = static_cast(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( - "/nav_goal", 10, + nav_goal_topic, 10, std::bind(&RMSimpleMove::nav_goal_callback, this, std::placeholders::_1) ); cmd_vel_nav_sub_ = this->create_subscription( - "/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("/cmd_vel_move", 10); - nav_status_pub_ = this->create_publisher("/nav_status", 10); + cmd_vel_move_pub_ = this->create_publisher(cmd_vel_output_topic, 10); + nav_status_pub_ = this->create_publisher(nav_status_topic, 10); // Action Client nav_action_client_ = rclcpp_action::create_client(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();