robofish/MOVE_AI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: robofish:f82cbf86b2b1333bcfcf7ddc44a1f240ddc566ec
Branches Tags
robofish:main
robofish:sentry
robofish:vision
robofish:Navigation
robofish:radar
..
compare: robofish:298d700d81c5d90caf6250538048ce091a813cf6
Branches Tags
robofish:sentry
robofish:vision
robofish:Navigation
robofish:main
robofish:radar

No commits in common. "f82cbf86b2b1333bcfcf7ddc44a1f240ddc566ec" and "298d700d81c5d90caf6250538048ce091a813cf6" have entirely different histories.
f82cbf86b2 ... 298d700d81

5 changed files with 39 additions and 146 deletions
Show Stats Expand all files Collapse all files

4
src/rm_nav/rm_simple_move/CMakeLists.txt
View File

@ -9,12 +9,10 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(rm_msgs REQUIRED)
# include directories
@ -24,7 +22,7 @@ include_directories(include)
add_library(rm_simpal_move SHARED src/simple_move.cpp)
# link libraries
ament_target_dependencies(rm_simpal_move rclcpp rclcpp_components rclcpp_action tf2_ros geometry_msgs tf2_geometry_msgs nav_msgs nav2_msgs rm_msgs)
ament_target_dependencies(rm_simpal_move rclcpp rclcpp_components tf2_ros geometry_msgs tf2_geometry_msgs nav_msgs rm_msgs)
# register node
rclcpp_components_register_nodes(rm_simpal_move "rm_simpal_move::RMSimpleMove")

37
src/rm_nav/rm_simple_move/README.md
View File

@ -46,8 +46,8 @@ git clone https://github.com/goldenfishs/rm_msgs.git
### 输出话题
#### `/cmd_vel_move` (geometry_msgs/Twist)
统一的速度控制指令输出（底盘订阅此话题）
#### `/cmd_vel` (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_move
# 查看速度控制指令
ros2 topic echo /cmd_vel
# 查看导航状态
ros2 topic echo /nav_status
@ -166,19 +166,13 @@ ros2 topic echo /current_pose
2. 通过 TF 获取当前位置 (map -> base_link)
3. 计算目标点在 base_link 坐标系下的相对位置
4. 使用 PID 控制器计算速度指令
5. 发布到 `/cmd_vel_move`
5. 发布标准 `/cmd_vel` 控制指令
### NAV 模式
1. 接收目标点 `/nav_goal` (mode=1)
2. 转换为 `geometry_msgs/PoseStamped` 格式
3. 发布到 `/goal_pose` 给导航系统
4. 导航系统输出 `/cmd_vel`
5. `rm_simple_move` 订阅 `/cmd_vel` 并转发到 `/cmd_vel_move`
**注意**: 导航系统内部流程为：
```
/goal_pose → Nav2 → /cmd_vel_nav → velocity_smoother → /cmd_vel
```
4. 导航系统负责路径规划并发布 `/cmd_vel`
### 状态发布
- 实时发布当前位置到 `/current_pose`
@ -192,24 +186,13 @@ ros2 topic echo /current_pose
```
决策层 (Decision Layer)
↓ /nav_goal (mode + target)
rm_simple_move (Bridge)
├─ 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 模式: /cmd_vel (直接控制)
↓ NAV 模式: /goal_pose → Navigation → /cmd_vel
控制层 (Control Layer)
← /cmd_vel (标准速度指令)
```
**话题流向**:
- **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 示例

14
src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp
View File

@ -1,7 +1,6 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
@ -10,7 +9,6 @@
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <rm_msgs/msg/nav_goal.hpp>
#include <rm_msgs/msg/nav_status.hpp>
#include <atomic>
@ -51,16 +49,12 @@ struct PIDController {
class RMSimpleMove : public rclcpp::Node {
public:
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;
RMSimpleMove(const rclcpp::NodeOptions &options);
~RMSimpleMove() override;
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);
@ -79,22 +73,18 @@ private:
// Subscribers
rclcpp::Subscription<rm_msgs::msg::NavGoal>::SharedPtr nav_goal_sub_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_nav_sub_;
// Publishers
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_move_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr nav_goal_pub_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
rclcpp::Publisher<rm_msgs::msg::NavStatus>::SharedPtr nav_status_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr current_pose_pub_;
// Action Client
rclcpp_action::Client<NavigateToPose>::SharedPtr nav_action_client_;
// State
std::atomic<uint8_t> control_mode_; // 0: PID, 1: NAV
std::atomic<bool> running_;
std::atomic<bool> goal_reached_;
std::atomic<bool> has_goal_;
std::atomic<bool> nav_goal_sent_; // NAV 模式下目标是否已发送
AHRS_Eulr_t current_eulr_;
Goal_t goal_pose_;

2
src/rm_nav/rm_simple_move/package.xml
View File

@ -10,12 +10,10 @@
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>rclcpp_action</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>nav2_msgs</depend>
<depend>launch</depend>
<depend>launch_ros</depend>
<depend>rm_msgs</depend>

128
src/rm_nav/rm_simple_move/src/simple_move.cpp
View File

@ -62,7 +62,6 @@ namespace rm_simpal_move
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_(1.5f, 0.0f, 0.2f, 2.0f)
@ -97,26 +96,12 @@ namespace rm_simpal_move
std::bind(&RMSimpleMove::nav_goal_callback, this, std::placeholders::_1)
);
cmd_vel_nav_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", 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_goal_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/goal_pose", 10);
cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
nav_status_pub_ = this->create_publisher<rm_msgs::msg::NavStatus>("/nav_status", 10);
current_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/current_pose", 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, /current_pose");
last_time_ = this->get_clock()->now();
RCLCPP_INFO(this->get_logger(), "RMSimpleMove 启动");
@ -135,10 +120,14 @@ namespace rm_simpal_move
*/
void RMSimpleMove::timer_callback()
{
if (!has_goal_) {
return;
}
try
{
// 获取当前位置（始终发布，不管有没有目标）
auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero, tf2::durationFromSec(0.5));
// 获取当前位置
auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero);
AHRS_GetEulr(&current_eulr_, trans.transform.rotation);
// 发布当前位置
@ -151,11 +140,6 @@ namespace rm_simpal_move
current_pose_msg.pose.orientation = trans.transform.rotation;
current_pose_pub_->publish(current_pose_msg);
// 如果没有目标，只发布当前位置，不执行控制
if (!has_goal_) {
return;
}
// 发布目标点 TF
geometry_msgs::msg::TransformStamped goal_transform;
goal_transform.header.stamp = this->get_clock()->now();
@ -176,7 +160,7 @@ namespace rm_simpal_move
// 计算目标点在 base_link 坐标系下的位置
geometry_msgs::msg::TransformStamped goal_in_base_link;
goal_in_base_link = tf_buffer_.lookupTransform("base_link", "goal_pose", tf2::TimePointZero, tf2::durationFromSec(0.5));
goal_in_base_link = tf_buffer_.lookupTransform("base_link", "goal_pose", tf2::TimePointZero);
float goal_x_in_base_link = goal_in_base_link.transform.translation.x;
float goal_y_in_base_link = goal_in_base_link.transform.translation.y;
@ -196,11 +180,8 @@ namespace rm_simpal_move
// PID 模式
publish_pid_control();
} else {
// NAV 模式：只在第一次发送目标
if (!nav_goal_sent_) {
publish_nav_goal();
nav_goal_sent_ = true;
}
// NAV 模式
publish_nav_goal();
}
// 发布状态
@ -208,7 +189,7 @@ namespace rm_simpal_move
}
catch (const tf2::TransformException &ex)
{
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "TF 变换失败: %s", ex.what());
RCLCPP_WARN(this->get_logger(), "TF 变换失败: %s", ex.what());
}
}
@ -250,7 +231,7 @@ namespace rm_simpal_move
vy *= scale;
}
// 发布标准 cmd_vel_move
// 发布标准 cmd_vel
auto cmd_vel_msg = geometry_msgs::msg::Twist();
cmd_vel_msg.linear.x = vx;
cmd_vel_msg.linear.y = vy;
@ -258,7 +239,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_move_pub_->publish(cmd_vel_msg);
cmd_vel_pub_->publish(cmd_vel_msg);
} catch (const tf2::TransformException &ex) {
RCLCPP_WARN(this->get_logger(), "PID 控制 TF 失败: %s", ex.what());
@ -266,64 +247,25 @@ namespace rm_simpal_move
}
/**
* @brief 发布导航目标 (使用 Action)
* @brief 发布导航目标
*/
void RMSimpleMove::publish_nav_goal()
{
// 检查 Action Server 是否可用
if (!nav_action_client_->wait_for_action_server(std::chrono::seconds(1))) {
RCLCPP_WARN(this->get_logger(), "导航 Action Server 不可用");
return;
}
// 创建 Action Goal
auto goal_msg = NavigateToPose::Goal();
goal_msg.pose.header.stamp = this->get_clock()->now();
goal_msg.pose.header.frame_id = "map";
goal_msg.pose.pose.position.x = goal_pose_.x;
goal_msg.pose.pose.position.y = goal_pose_.y;
goal_msg.pose.pose.position.z = 0.0;
auto goal_msg = geometry_msgs::msg::PoseStamped();
goal_msg.header.stamp = this->get_clock()->now();
goal_msg.header.frame_id = "map";
goal_msg.pose.position.x = goal_pose_.x;
goal_msg.pose.position.y = goal_pose_.y;
goal_msg.pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, goal_pose_.angle);
goal_msg.pose.pose.orientation.x = q.x();
goal_msg.pose.pose.orientation.y = q.y();
goal_msg.pose.pose.orientation.z = q.z();
goal_msg.pose.pose.orientation.w = q.w();
goal_msg.pose.orientation.x = q.x();
goal_msg.pose.orientation.y = q.y();
goal_msg.pose.orientation.z = q.z();
goal_msg.pose.orientation.w = q.w();
// 发送 Action Goal
auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
send_goal_options.goal_response_callback =
[this](GoalHandleNavigateToPose::SharedPtr goal_handle) {
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "导航目标被拒绝");
} else {
RCLCPP_INFO(this->get_logger(), "导航目标已接受");
}
};
send_goal_options.result_callback =
[this](const GoalHandleNavigateToPose::WrappedResult & result) {
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_logger(), "导航成功完成");
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "导航被中止");
break;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_WARN(this->get_logger(), "导航被取消");
break;
default:
RCLCPP_ERROR(this->get_logger(), "导航未知结果");
break;
}
};
nav_action_client_->async_send_goal(goal_msg, send_goal_options);
RCLCPP_INFO(this->get_logger(), "发送导航目标: x=%.2f, y=%.2f, angle=%.2f",
goal_pose_.x, goal_pose_.y, goal_pose_.angle);
nav_goal_pub_->publish(goal_msg);
}
/**
@ -388,7 +330,6 @@ namespace rm_simpal_move
has_goal_ = true;
goal_reached_ = false;
nav_goal_sent_ = false; // 重置 NAV 目标发送标志
// 重置 PID
pid_linear_x_.reset();
@ -398,23 +339,6 @@ namespace rm_simpal_move
RCLCPP_INFO(this->get_logger(), "收到新目标: mode=%s, x=%.2f, y=%.2f, angle=%.2f",
control_mode_ == 0 ? "PID" : "NAV",
msg->target_x, msg->target_y, msg->target_angle);
// 如果是 NAV 模式，立即发送导航目标
if (control_mode_ == 1) {
publish_nav_goal();
nav_goal_sent_ = true;
}
}
/**
* @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);
}
}
/**