diff --git a/src/rm_nav/rm_simple_move/README.md b/src/rm_nav/rm_simple_move/README.md index 3645225..88d0fa6 100644 --- a/src/rm_nav/rm_simple_move/README.md +++ b/src/rm_nav/rm_simple_move/README.md @@ -46,8 +46,8 @@ git clone https://github.com/goldenfishs/rm_msgs.git ### 输出话题 -#### `/cmd_vel` (geometry_msgs/Twist) -标准速度控制指令 +#### `/cmd_vel_move` (geometry_msgs/Twist) +统一的速度控制指令输出(底盘订阅此话题) - `linear.x`: x 方向线速度 - `linear.y`: y 方向线速度 - `angular.z`: z 轴角速度 @@ -147,8 +147,8 @@ ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{ **查看输出:** ```bash -# 查看速度控制指令 -ros2 topic echo /cmd_vel +# 查看速度控制指令(底盘订阅此话题) +ros2 topic echo /cmd_vel_move # 查看导航状态 ros2 topic echo /nav_status @@ -166,13 +166,19 @@ ros2 topic echo /current_pose 2. 通过 TF 获取当前位置 (map -> base_link) 3. 计算目标点在 base_link 坐标系下的相对位置 4. 使用 PID 控制器计算速度指令 -5. 发布标准 `/cmd_vel` 控制指令 +5. 发布到 `/cmd_vel_move` ### NAV 模式 1. 接收目标点 `/nav_goal` (mode=1) 2. 转换为 `geometry_msgs/PoseStamped` 格式 3. 发布到 `/goal_pose` 给导航系统 -4. 导航系统负责路径规划并发布 `/cmd_vel` +4. 导航系统输出 `/cmd_vel` +5. `rm_simple_move` 订阅 `/cmd_vel` 并转发到 `/cmd_vel_move` + +**注意**: 导航系统内部流程为: +``` +/goal_pose → Nav2 → /cmd_vel_nav → velocity_smoother → /cmd_vel +``` ### 状态发布 - 实时发布当前位置到 `/current_pose` @@ -186,13 +192,24 @@ ros2 topic echo /current_pose ``` 决策层 (Decision Layer) ↓ /nav_goal (mode + target) + rm_simple_move (Bridge) - ↓ PID 模式: /cmd_vel (直接控制) - ↓ NAV 模式: /goal_pose → Navigation → /cmd_vel -控制层 (Control Layer) - ← /cmd_vel (标准速度指令) + ├─ PID 模式: 直接计算 → /cmd_vel_move + └─ NAV 模式: /goal_pose → Navigation Stack + ↓ + /cmd_vel_nav → velocity_smoother → /cmd_vel + ↓ + rm_simple_move 转发 → /cmd_vel_move + +底盘驱动 (Chassis Driver) + ← /cmd_vel_move (统一的速度指令) ``` +**话题流向**: +- **PID 模式**: `rm_simple_move` 直接计算并发布到 `/cmd_vel_move` +- **NAV 模式**: `rm_simple_move` 发布 `/goal_pose` → Nav2 处理后输出 `/cmd_vel` → `rm_simple_move` 转发到 `/cmd_vel_move` +- **底盘驱动**: 只订阅 `/cmd_vel_move`,避免冲突 + --- ## Python 示例 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 65d905a..fdc6608 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 @@ -55,6 +55,7 @@ public: private: void timer_callback(); void nav_goal_callback(const rm_msgs::msg::NavGoal::SharedPtr msg); + void cmd_vel_nav_callback(const geometry_msgs::msg::Twist::SharedPtr msg); int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const geometry_msgs::msg::Quaternion &quat); bool is_goal_reached(float x, float y, float tolerance); @@ -73,10 +74,11 @@ private: // Subscribers rclcpp::Subscription::SharedPtr nav_goal_sub_; + rclcpp::Subscription::SharedPtr cmd_vel_nav_sub_; // Publishers rclcpp::Publisher::SharedPtr nav_goal_pub_; - rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_move_pub_; rclcpp::Publisher::SharedPtr nav_status_pub_; rclcpp::Publisher::SharedPtr current_pose_pub_; 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 8348d28..c4f9993 100644 --- a/src/rm_nav/rm_simple_move/src/simple_move.cpp +++ b/src/rm_nav/rm_simple_move/src/simple_move.cpp @@ -96,12 +96,24 @@ namespace rm_simpal_move std::bind(&RMSimpleMove::nav_goal_callback, this, std::placeholders::_1) ); + cmd_vel_nav_sub_ = this->create_subscription( + "/cmd_vel", 10, + std::bind(&RMSimpleMove::cmd_vel_nav_callback, this, std::placeholders::_1) + ); + // 发布者 nav_goal_pub_ = this->create_publisher("/goal_pose", 10); - cmd_vel_pub_ = this->create_publisher("/cmd_vel", 10); + cmd_vel_move_pub_ = this->create_publisher("/cmd_vel_move", 10); nav_status_pub_ = this->create_publisher("/nav_status", 10); current_pose_pub_ = this->create_publisher("/current_pose", 10); + 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模式: /goal_pose → 导航 → /cmd_vel → 转发到 /cmd_vel_move"); + RCLCPP_INFO(this->get_logger(), " 状态: /nav_status, /current_pose"); + last_time_ = this->get_clock()->now(); RCLCPP_INFO(this->get_logger(), "RMSimpleMove 启动"); @@ -231,7 +243,7 @@ namespace rm_simpal_move vy *= scale; } - // 发布标准 cmd_vel + // 发布标准 cmd_vel_move auto cmd_vel_msg = geometry_msgs::msg::Twist(); cmd_vel_msg.linear.x = vx; cmd_vel_msg.linear.y = vy; @@ -239,7 +251,7 @@ namespace rm_simpal_move cmd_vel_msg.angular.x = 0.0; cmd_vel_msg.angular.y = 0.0; cmd_vel_msg.angular.z = wz; - cmd_vel_pub_->publish(cmd_vel_msg); + cmd_vel_move_pub_->publish(cmd_vel_msg); } catch (const tf2::TransformException &ex) { RCLCPP_WARN(this->get_logger(), "PID 控制 TF 失败: %s", ex.what()); @@ -341,6 +353,17 @@ namespace rm_simpal_move msg->target_x, msg->target_y, msg->target_angle); } + /** + * @brief 导航 cmd_vel 回调 (NAV 模式下转发) + */ + void RMSimpleMove::cmd_vel_nav_callback(const geometry_msgs::msg::Twist::SharedPtr msg) + { + // 只在 NAV 模式下转发 + if (control_mode_ == 1) { + cmd_vel_move_pub_->publish(*msg); + } + } + /** * @brief 判断是否到达目标 */