目录
1.ROS节点的基本结构
2.核心概念详解
3. ROS通信机制详解(重点)
4. 代码文件详解(供参考)
5. 常见修改场景详解(如果我想添加一个功能，我该怎么做)
示例代码比较多，重点在于代码结构和整个流程的理解
一、ROS节点的基本结构
一个完整的ROS节点通常包含以下文件（参考）：
your_package/
├── include/your_package/
│ ├── planner.hpp # 算法核心类头文件
│ └── planner_node.hpp # ROS节点类头文件
├── src/
│ ├── planner.cpp # 算法核心类实现
│ └── planner_node.cpp # ROS节点实现和main函数
├── launch/
│ └── launch.py # 启动文件
├── CMakeLists.txt # 编译配置
└── package.xml # 包依赖配置
二、核心概念详解
1. 回调函数（Callback）
回调函数是ROS中处理消息订阅的核心机制。当有新消息到达时，ROS会自动调用你注册的回调函数。
工作原理：
// 订阅器会"监听"某个话题
// 当话题有新消息时，自动调用callback_function
subscription_ = this->create_subscription<MsgType>(
"topic_name", // 话题名称
10, // 队列大小
std::bind(&Class::callback_function, this, std::placeholders::_1)
);
回调函数示例：
void PlannerNode::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) {
// msg是智能指针，指向接收到的消息
// 使用 -> 访问消息内容
RCLCPP_INFO(this->get_logger(), "收到路径，包含 %zu 个点", msg->poses.size());
// 处理消息
for (const auto& pose : msg->poses) {
// 访问每个位姿点
double x = pose.pose.position.x;
double y = pose.pose.position.y;
}
}
关键点：
回调函数在后台线程中自动执行参数通常是
SharedPtr
2. 依赖管理
<depend>
<depend>
在
package.xml
<?xml version="1.0"?>
<package format="3">
<name>your_planner</name>
<version>1.0.0</version>
<description>路径规划节点</description>
<maintainer email="you@example.com">Your Name</maintainer>
<license>Apache-2.0</license>
<!-- 构建工具依赖 -->
<buildtool_depend>ament_cmake</buildtool_depend>
<!-- 运行时和编译时都需要的依赖 -->
<depend>rclcpp</depend> <!-- ROS2 C++核心库 -->
<depend>std_msgs</depend> <!-- 标准消息类型 -->
<depend>geometry_msgs</depend> <!-- 几何消息（点、姿态等）-->
<depend>nav_msgs</depend> <!-- 导航消息（路径、地图等）-->
<depend>sensor_msgs</depend> <!-- 传感器消息 -->
<depend>tf2_ros</depend> <!-- 坐标变换 -->
<!-- 自定义消息包 -->
<depend>your_custom_msgs</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
依赖类型说明：
<depend>
<build_depend>
<exec_depend>
<test_depend>
3. 智能指针
shared_ptr
shared_ptr
智能指针自动管理内存，避免内存泄漏。
指针符号对比：
#include <memory>
class Planner {
public:
void plan(const Point& start, const Point& goal);
double getCost() const;
};
// 1. 普通指针（不推荐，需手动delete）
Planner* planner_ptr = new Planner();
planner_ptr->plan(start, goal); // 使用 -> 访问成员
delete planner_ptr; // 必须手动释放
// 2. 智能指针（推荐）
std::shared_ptr<Planner> planner = std::make_shared<Planner>();
planner->plan(start, goal); // 使用 -> 访问成员（自动管理内存）
// 3. 解引用获取对象本身
Planner& planner_ref = *planner; // * 解引用，得到对象引用
planner_ref.plan(start, goal); // 使用 . 访问成员
// 4. 获取原始指针地址
Planner* raw_ptr = planner.get(); // 获取原始指针
Planner* addr = &(*planner); // & 取地址
在ROS中的应用：
class PlannerNode : public rclcpp::Node {
private:
// 智能指针成员变量
std::shared_ptr<Planner> planner_;
rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
public:
PlannerNode() : Node("planner_node") {
// 创建智能指针
planner_ = std::make_shared<Planner>();
// 订阅话题（回调函数中接收SharedPtr）
path_sub_ = this->create_subscription<nav_msgs::msg::Path>(
"input_path", 10,
std::bind(&PlannerNode::pathCallback, this, std::placeholders::_1)
);
}
void pathCallback(const nav_msgs::msg::Path::SharedPtr msg) {
// msg 是智能指针，使用 -> 访问
RCLCPP_INFO(this->get_logger(), "路径点数: %zu", msg->poses.size());
// 调用planner_的方法
planner_->updatePath(*msg); // 解引用传递对象
// 或者传递指针
planner_->updatePath(msg); // 直接传递智能指针
}
};
关键点：
->
.
*
&
SharedPtr
三、ROS通信机制详解
1. 发布话题（Publisher）
发布器用于向ROS网络发送消息。
创建发布器的步骤：
// ===== planner_node.hpp =====
class PlannerNode : public rclcpp::Node {
private:
// 1. 声明发布器成员变量（智能指针）
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr current_pose_pub_;
};
// ===== planner_node.cpp 构造函数 =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 2. 创建发布器
path_pub_ = this->create_publisher<nav_msgs::msg::Path>(
"planned_path", // 话题名称
10 // 队列大小（QoS）
);
current_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
"current_pose", 10
);
}
// ===== 发布消息 =====
void PlannerNode::publishPath() {
// 3. 创建消息对象
nav_msgs::msg::Path path_msg;
// 4. 填充消息内容
path_msg.header.stamp = this->now(); // 时间戳
path_msg.header.frame_id = "map"; // 坐标系
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 1.0;
pose.pose.position.y = 2.0;
path_msg.poses.push_back(pose);
// 5. 发布消息
path_pub_->publish(path_msg);
RCLCPP_INFO(this->get_logger(), "已发布路径");
}
QoS（服务质量）设置：
// 默认QoS（队列大小10）
auto pub = this->create_publisher<std_msgs::msg::String>("topic", 10);
// 自定义QoS
#include <rclcpp/qos.hpp>
rclcpp::QoS qos_profile(10); // 队列深度
qos_profile.reliability(rclcpp::ReliabilityPolicy::BestEffort); // 尽力而为
// 或 .reliability(rclcpp::ReliabilityPolicy::Reliable); // 可靠传输
qos_profile.durability(rclcpp::DurabilityPolicy::TransientLocal); // 保留最后一条
// 或 .durability(rclcpp::DurabilityPolicy::Volatile); // 不保留
auto pub = this->create_publisher<std_msgs::msg::String>("topic", qos_profile);
2. 订阅话题（Subscriber）
订阅器用于接收来自其他节点的消息。
订阅器的完整流程：
// ===== planner_node.hpp =====
class PlannerNode : public rclcpp::Node {
private:
// 1. 声明订阅器
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
// 2. 声明回调函数
void goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
// 3. 存储接收到的数据
geometry_msgs::msg::PoseStamped latest_goal_;
nav_msgs::msg::Odometry latest_odom_;
bool has_odom_;
};
// ===== planner_node.cpp =====
PlannerNode::PlannerNode() : Node("planner_node"), has_odom_(false) {
// 4. 创建订阅器
goal_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"goal_pose", // 话题名
10, // 队列大小
std::bind(&PlannerNode::goalCallback, this, std::placeholders::_1)
// std::bind: 绑定成员函数
// this: 当前对象指针
// std::placeholders::_1: 第一个参数（接收到的消息）
);
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
"odom", 10,
std::bind(&PlannerNode::odomCallback, this, std::placeholders::_1)
);
}
// 5. 实现回调函数
void PlannerNode::goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
// msg 是智能指针，使用 -> 访问成员
RCLCPP_INFO(this->get_logger(),
"收到目标: [%.2f, %.2f]",
msg->pose.position.x,
msg->pose.position.y);
// 存储消息（需要解引用）
latest_goal_ = *msg;
// 使用消息数据
double x = msg->pose.position.x;
double y = msg->pose.position.y;
}
void PlannerNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
latest_odom_ = *msg;
has_odom_ = true;
// 访问嵌套结构
double linear_vel = msg->twist.twist.linear.x;
double angular_vel = msg->twist.twist.angular.z;
}
回调函数的注意事项：
// ❌ 错误：回调函数中做耗时操作
void callback(const std_msgs::msg::String::SharedPtr msg) {
std::this_thread::sleep_for(std::chrono::seconds(5)); // 阻塞5秒！
// 这会导致其他消息无法及时处理
}
// ✅ 正确：快速处理或启动异步任务
void callback(const std_msgs::msg::String::SharedPtr msg) {
data_ = *msg; // 快速存储
// 或者：
std::thread([this, msg]() {
// 耗时操作在单独线程中执行
processData(*msg);
}).detach();
}
3. 服务（Service）
服务提供请求-响应式通信。
创建服务端：
// ===== planner_node.hpp =====
#include <std_srvs/srv/trigger.hpp>
#include <example_interfaces/srv/add_two_ints.hpp>
class PlannerNode : public rclcpp::Node {
private:
// 声明服务
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_service_;
rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr add_service_;
// 服务回调函数
void resetCallback(
const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
std::shared_ptr<std_srvs::srv::Trigger::Response> response);
void addCallback(
const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response);
};
// ===== planner_node.cpp =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 创建服务
reset_service_ = this->create_service<std_srvs::srv::Trigger>(
"reset_planner",
std::bind(&PlannerNode::resetCallback, this,
std::placeholders::_1, std::placeholders::_2)
);
add_service_ = this->create_service<example_interfaces::srv::AddTwoInts>(
"add_two_ints",
std::bind(&PlannerNode::addCallback, this,
std::placeholders::_1, std::placeholders::_2)
);
}
// 实现服务回调
void PlannerNode::resetCallback(
const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
std::shared_ptr<std_srvs::srv::Trigger::Response> response)
{
// request 可能为空或不使用
(void)request; // 避免未使用警告
// 执行重置操作
has_goal_ = false;
planner_->reset();
// 填充响应
response->success = true;
response->message = "规划器已成功重置";
RCLCPP_INFO(this->get_logger(), "服务调用：重置规划器");
}
void PlannerNode::addCallback(
const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response)
{
// 使用请求数据
response->sum = request->a + request->b;
RCLCPP_INFO(this->get_logger(), "计算: %ld + %ld = %ld",
request->a, request->b, response->sum);
}
创建服务客户端：
// ===== planner_node.hpp =====
class PlannerNode : public rclcpp::Node {
private:
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr reset_client_;
void callResetService();
};
// ===== planner_node.cpp =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 创建客户端
reset_client_ = this->create_client<std_srvs::srv::Trigger>("reset_planner");
}
void PlannerNode::callResetService() {
// 1. 等待服务可用
while (!reset_client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(this->get_logger(), "客户端中断");
return;
}
RCLCPP_INFO(this->get_logger(), "等待服务可用...");
}
// 2. 创建请求
auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
// 3. 发送异步请求
auto future = reset_client_->async_send_request(request);
// 4. 等待响应
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) ==
rclcpp::FutureReturnCode::SUCCESS)
{
auto response = future.get();
RCLCPP_INFO(this->get_logger(), "服务响应: %s", response->message.c_str());
} else {
RCLCPP_ERROR(this->get_logger(), "服务调用失败");
}
}
4. 定时器（Timer）
定时器用于周期性执行任务。
// ===== planner_node.hpp =====
class PlannerNode : public rclcpp::Node {
private:
rclcpp::TimerBase::SharedPtr timer_;
void timerCallback();
};
// ===== planner_node.cpp =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 创建定时器（每500毫秒执行一次）
timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&PlannerNode::timerCallback, this)
);
}
void PlannerNode::timerCallback() {
RCLCPP_INFO(this->get_logger(), "定时器触发");
// 执行周期性任务
publishStatus();
checkGoalReached();
}
// 动态修改定时器周期
void PlannerNode::changeTimerPeriod(int ms) {
timer_->cancel(); // 取消旧定时器
timer_ = this->create_wall_timer(
std::chrono::milliseconds(ms),
std::bind(&PlannerNode::timerCallback, this)
);
}
5. 参数（Parameters）
参数用于动态配置节点行为。
声明和使用参数：
// ===== planner_node.cpp 构造函数 =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 1. 声明参数（带默认值）
this->declare_parameter("step_size", 0.1);
this->declare_parameter("max_speed", 1.0);
this->declare_parameter("frame_id", "map");
this->declare_parameter("enable_debug", false);
// 2. 获取参数
double step_size = this->get_parameter("step_size").as_double();
double max_speed = this->get_parameter("max_speed").as_double();
std::string frame_id = this->get_parameter("frame_id").as_string();
bool enable_debug = this->get_parameter("enable_debug").as_bool();
// 3. 使用参数
planner_->setStepSize(step_size);
RCLCPP_INFO(this->get_logger(), "参数: step_size=%.2f, max_speed=%.2f",
step_size, max_speed);
}
// 运行时获取参数
void PlannerNode::updateConfig() {
double step_size = this->get_parameter("step_size").as_double();
planner_->setStepSize(step_size);
}
参数回调（动态更新）：
// ===== planner_node.hpp =====
class PlannerNode : public rclcpp::Node {
private:
OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
rcl_interfaces::msg::SetParametersResult
parametersCallback(const std::vector<rclcpp::Parameter>& parameters);
};
// ===== planner_node.cpp =====
PlannerNode::PlannerNode() : Node("planner_node") {
this->declare_parameter("step_size", 0.1);
// 注册参数更改回调
param_callback_handle_ = this->add_on_set_parameters_callback(
std::bind(&PlannerNode::parametersCallback, this, std::placeholders::_1)
);
}
rcl_interfaces::msg::SetParametersResult
PlannerNode::parametersCallback(const std::vector<rclcpp::Parameter>& parameters) {
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
for (const auto& param : parameters) {
if (param.get_name() == "step_size") {
double value = param.as_double();
if (value > 0.0 && value < 1.0) {
planner_->setStepSize(value);
RCLCPP_INFO(this->get_logger(), "更新step_size: %.2f", value);
} else {
result.successful = false;
result.reason = "step_size必须在0-1之间";
}
}
}
return result;
}
6. TF坐标变换
TF用于管理不同坐标系之间的变换。
// ===== package.xml 添加依赖 =====
// <depend>tf2</depend>
// <depend>tf2_ros</depend>
// <depend>tf2_geometry_msgs</depend>
// ===== planner_node.hpp =====
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
class PlannerNode : public rclcpp::Node {
private:
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
bool transformPose(const geometry_msgs::msg::PoseStamped& in,
geometry_msgs::msg::PoseStamped& out,
const std::string& target_frame);
};
// ===== planner_node.cpp =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 创建TF监听器
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
}
bool PlannerNode::transformPose(
const geometry_msgs::msg::PoseStamped& in,
geometry_msgs::msg::PoseStamped& out,
const std::string& target_frame)
{
try {
// 查询变换（等待最多1秒）
out = tf_buffer_->transform(in, target_frame,
std::chrono::seconds(1));
return true;
} catch (tf2::TransformException& ex) {
RCLCPP_ERROR(this->get_logger(), "TF变换失败: %s", ex.what());
return false;
}
}
// 使用示例
void PlannerNode::processGoal(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
geometry_msgs::msg::PoseStamped goal_in_map;
// 将目标从 base_link 坐标系转换到 map 坐标系
if (transformPose(*msg, goal_in_map, "map")) {
RCLCPP_INFO(this->get_logger(), "目标位置(map): [%.2f, %.2f]",
goal_in_map.pose.position.x,
goal_in_map.pose.position.y);
}
}
四、代码文件详解
planner.hpp（算法核心类）
#ifndef YOUR_PACKAGE_PLANNER_HPP
#define YOUR_PACKAGE_PLANNER_HPP
#include <vector>
#include <geometry_msgs/msg/pose_stamped.hpp>
namespace your_package {
class Planner {
public:
Planner();
~Planner();
// 核心规划接口
bool plan(const geometry_msgs::msg::PoseStamped& start,
const geometry_msgs::msg::PoseStamped& goal,
std::vector<geometry_msgs::msg::PoseStamped>& path);
// 参数设置
void setStepSize(double step_size);
void reset();
private:
double step_size_;
// 内部算法函数（简化示例）
double calculateDistance(const geometry_msgs::msg::PoseStamped& p1,
const geometry_msgs::msg::PoseStamped& p2);
};
} // namespace your_package
#endif
planner.cpp（算法实现）
这里你不需要仔细看，这是MPC的内容，具体编写代码的时候，这里换成你具体的代码实现，不用参考这个
#include "your_package/planner.hpp"
#include <cmath>
namespace your_package {
Planner::Planner() : step_size_(0.1) {}
Planner::~Planner() {}
bool Planner::plan(
const geometry_msgs::msg::PoseStamped& start,
const geometry_msgs::msg::PoseStamped& goal,
std::vector<geometry_msgs::msg::PoseStamped>& path)
{
path.clear();
// 这里放置你的规划算法
// 示例：简单直线插值
double dist = calculateDistance(start, goal);
int num_points = static_cast<int>(dist / step_size_);
for (int i = 0; i <= num_points; ++i) {
double t = static_cast<double>(i) / num_points;
geometry_msgs::msg::PoseStamped point;
point.header = start.header;
point.pose.position.x = start.pose.position.x +
t * (goal.pose.position.x - start.pose.position.x);
point.pose.position.y = start.pose.position.y +
t * (goal.pose.position.y - start.pose.position.y);
path.push_back(point);
}
return !path.empty();
}
void Planner::setStepSize(double step_size) {
step_size_ = step_size;
}
void Planner::reset() {
// 重置内部状态
}
double Planner::calculateDistance(
const geometry_msgs::msg::PoseStamped& p1,
const geometry_msgs::msg::PoseStamped& p2)
{
double dx = p2.pose.position.x - p1.pose.position.x;
double dy = p2.pose.position.y - p1.pose.position.y;
return std::sqrt(dx*dx + dy*dy);
}
} // namespace your_package
planner_node.hpp（ROS节点类）
#ifndef YOUR_PACKAGE_PLANNER_NODE_HPP
#define YOUR_PACKAGE_PLANNER_NODE_HPP
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav_msgs/msg/path.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <std_srvs/srv/trigger.hpp>
#include "your_package/planner.hpp"
namespace your_package {
class PlannerNode : public rclcpp::Node {
public:
PlannerNode();
private:
// 回调函数
void goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
void timerCallback();
void resetServiceCallback(
const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
std::shared_ptr<std_srvs::srv::Trigger::Response> response);
// 算法对象
std::shared_ptr<Planner> planner_;
// 发布器
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
// 订阅器
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
// 服务
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_service_;
// 定时器
rclcpp::TimerBase::SharedPtr timer_;
// 数据成员
geometry_msgs::msg::PoseStamped current_goal_;
nav_msgs::msg::Odometry current_odom_;
bool has_goal_;
bool has_odom_;
};
} // namespace your_package
#endif
planner_node.cpp（ROS节点实现）
#include "your_package/planner_node.hpp"
namespace your_package {
PlannerNode::PlannerNode()
: Node("planner_node"),
has_goal_(false),
has_odom_(false)
{
// ===== 1. 声明参数 =====
this->declare_parameter("step_size", 0.1);
this->declare_parameter("planning_frequency", 2.0); // Hz
// ===== 2. 获取参数 =====
double step_size = this->get_parameter("step_size").as_double();
double freq = this->get_parameter("planning_frequency").as_double();
// ===== 3. 创建算法对象 =====
planner_ = std::make_shared<Planner>();
planner_->setStepSize(step_size);
// ===== 4. 创建发布器 =====
path_pub_ = this->create_publisher<nav_msgs::msg::Path>(
"planned_path", 10
);
// ===== 5. 创建订阅器 =====
goal_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"goal_pose", 10,
std::bind(&PlannerNode::goalCallback, this, std::placeholders::_1)
);
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
"odom", 10,
std::bind(&PlannerNode::odomCallback, this, std::placeholders::_1)
);
// ===== 6. 创建服务 =====
reset_service_ = this->create_service<std_srvs::srv::Trigger>(
"reset_planner",
std::bind(&PlannerNode::resetServiceCallback, this,
std::placeholders::_1, std::placeholders::_2)
);
// ===== 7. 创建定时器 =====
int period_ms = static_cast<int>(1000.0 / freq);
timer_ = this->create_wall_timer(
std::chrono::milliseconds(period_ms),
std::bind(&PlannerNode::timerCallback, this)
);
RCLCPP_INFO(this->get_logger(), "Planner节点启动成功");
RCLCPP_INFO(this->get_logger(), "参数: step_size=%.2f, freq=%.1fHz",
step_size, freq);
}
// ===== 目标点回调 =====
void PlannerNode::goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
RCLCPP_INFO(this->get_logger(),
"收到目标点: [%.2f, %.2f] in frame '%s'",
msg->pose.position.x,
msg->pose.position.y,
msg->header.frame_id.c_str());
current_goal_ = *msg; // 解引用存储
has_goal_ = true;
}
// ===== 里程计回调 =====
void PlannerNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
current_odom_ = *msg;
has_odom_ = true;
// 可以在这里提取机器人当前位置和速度
// double x = msg->pose.pose.position.x;
// double y = msg->pose.pose.position.y;
// double vx = msg->twist.twist.linear.x;
}
// ===== 定时器回调（周期性规划）=====
void PlannerNode::timerCallback() {
if (!has_goal_) {
RCLCPP_DEBUG(this->get_logger(), "等待目标点...");
return;
}
if (!has_odom_) {
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
"等待里程计数据...");
return;
}
// 从里程计获取起点
geometry_msgs::msg::PoseStamped start;
start.header = current_odom_.header;
start.pose = current_odom_.pose.pose;
// 调用规划算法
std::vector<geometry_msgs::msg::PoseStamped> path_points;
bool success = planner_->plan(start, current_goal_, path_points);
if (success && !path_points.empty()) {
// 构造Path消息
nav_msgs::msg::Path path_msg;
path_msg.header.stamp = this->now();
path_msg.header.frame_id = start.header.frame_id;
path_msg.poses = path_points;
// 发布路径
path_pub_->publish(path_msg);
RCLCPP_INFO(this->get_logger(),
"发布路径: %zu个点, 从[%.2f,%.2f]到[%.2f,%.2f]",
path_points.size(),
start.pose.position.x, start.pose.position.y,
current_goal_.pose.position.x, current_goal_.pose.position.y);
} else {
RCLCPP_ERROR(this->get_logger(), "路径规划失败");
}
}
// ===== 重置服务回调 =====
void PlannerNode::resetServiceCallback(
const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
std::shared_ptr<std_srvs::srv::Trigger::Response> response)
{
(void)request; // 未使用
has_goal_ = false;
planner_->reset();
response->success = true;
response->message = "规划器已重置";
RCLCPP_INFO(this->get_logger(), "规划器已通过服务重置");
}
} // namespace your_package
// ===== main函数 =====
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<your_package::PlannerNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
CMakeLists.txt（编译配置）
cmake_minimum_required(VERSION 3.8)
project(your_package)
# ===== 编译选项 =====
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# C++17标准
set(CMAKE_CXX_STANDARD 17)
# ===== 查找依赖包 =====
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
# ===== 依赖列表（方便复用）=====
set(dependencies
rclcpp
std_msgs
geometry_msgs
nav_msgs
sensor_msgs
std_srvs
tf2_ros
tf2_geometry_msgs
)
# ===== 包含头文件目录 =====
include_directories(
include
)
# ===== 创建库（算法核心）=====
add_library(${PROJECT_NAME}_lib SHARED
src/planner.cpp
)
ament_target_dependencies(${PROJECT_NAME}_lib ${dependencies})
# ===== 创建可执行文件（节点）=====
add_executable(planner_node
src/planner_node.cpp
)
target_link_libraries(planner_node ${PROJECT_NAME}_lib)
ament_target_dependencies(planner_node ${dependencies})
# ===== 安装目标 =====
# 安装库
install(TARGETS ${PROJECT_NAME}_lib
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
# 安装可执行文件
install(TARGETS planner_node
DESTINATION lib/${PROJECT_NAME}
)
# 安装头文件
install(DIRECTORY include/
DESTINATION include/
)
# 安装launch文件
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}
)
# 安装配置文件（可选）
install(DIRECTORY config
DESTINATION share/${PROJECT_NAME}
)
# ===== 导出信息 =====
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME}_lib)
ament_export_dependencies(${dependencies})
ament_package()
package.xml（包依赖配置）
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>your_package</name>
<version>1.0.0</version>
<description>路径规划节点包</description>
<maintainer email="your_email@example.com">Your Name</maintainer>
<license>Apache-2.0</license>
<!-- 构建工具 -->
<buildtool_depend>ament_cmake</buildtool_depend>
<!-- 核心依赖 -->
<depend>rclcpp</depend>
<!-- 消息类型 -->
<depend>std_msgs</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>sensor_msgs</depend>
<depend>std_srvs</depend>
<!-- 坐标变换 -->
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<!-- 测试依赖（可选）-->
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
launch.py（启动文件）
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, LogInfo
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch.conditions import IfCondition
def generate_launch_description():
# ===== 声明启动参数 =====
step_size_arg = DeclareLaunchArgument(
'step_size',
default_value='0.1',
description='路径规划的步长'
)
planning_frequency_arg = DeclareLaunchArgument(
'planning_frequency',
default_value='2.0',
description='规划频率（Hz）'
)
use_sim_time_arg = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='是否使用仿真时间'
)
debug_arg = DeclareLaunchArgument(
'debug',
default_value='false',
description='是否启用调试模式'
)
# ===== 定义节点 =====
planner_node = Node(
package='your_package',
executable='planner_node',
name='planner',
output='screen',
parameters=[{
'step_size': LaunchConfiguration('step_size'),
'planning_frequency': LaunchConfiguration('planning_frequency'),
'use_sim_time': LaunchConfiguration('use_sim_time'),
}],
remappings=[
# 话题重映射：(节点内部名称, 外部名称)
('goal_pose', '/move_base_simple/goal'),
('odom', '/odom'),
('planned_path', '/planned_path'),
],
arguments=[
'--ros-args',
'--log-level',
PythonExpression([
"'debug' if '", LaunchConfiguration('debug'), "' == 'true' else 'info'"
])
]
)
# ===== 启动信息 =====
log_info = LogInfo(
msg=['启动planner节点，参数: step_size=',
LaunchConfiguration('step_size')]
)
# ===== 返回LaunchDescription =====
return LaunchDescription([
step_size_arg,
planning_frequency_arg,
use_sim_time_arg,
debug_arg,
log_info,
planner_node
])
五、常见修改场景详解
场景1: 添加新的订阅器
需求： 订阅激光雷达数据进行避障
// ===== 1. package.xml 添加依赖 =====
<depend>sensor_msgs</depend>
// ===== 2. CMakeLists.txt 添加依赖 =====
find_package(sensor_msgs REQUIRED)
# 在dependencies列表中添加
set(dependencies
...
sensor_msgs
)
// ===== 3. planner_node.hpp 添加 =====
#include <sensor_msgs/msg/laser_scan.hpp>
class PlannerNode : public rclcpp::Node {
private:
// 添加订阅器成员
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_;
// 添加回调函数声明
void laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
// 存储最新数据
sensor_msgs::msg::LaserScan latest_scan_;
bool has_scan_;
};
// ===== 4. planner_node.cpp 实现 =====
PlannerNode::PlannerNode() : Node("planner_node"), has_scan_(false) {
// 在构造函数中创建订阅器
laser_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"scan", 10,
std::bind(&PlannerNode::laserCallback, this, std::placeholders::_1)
);
}
void PlannerNode::laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
latest_scan_ = *msg;
has_scan_ = true;
// 处理激光数据
if (!msg->ranges.empty()) {
float min_range = *std::min_element(msg->ranges.begin(), msg->ranges.end());
if (min_range < 0.5) { // 障碍物太近
RCLCPP_WARN(this->get_logger(), "检测到近距离障碍物: %.2fm", min_range);
}
}
}
// ===== 5. launch.py 添加话题重映射（可选）=====
remappings=[
...
('scan', '/scan'),
]
场景2: 添加新的发布器
需求： 发布规划状态信息
// ===== 1. planner_node.hpp 添加 =====
#include <std_msgs/msg/string.hpp>
class PlannerNode : public rclcpp::Node {
private:
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr status_pub_;
void publishStatus(const std::string& status);
};
// ===== 2. planner_node.cpp 实现 =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 创建发布器
status_pub_ = this->create_publisher<std_msgs::msg::String>(
"planner_status", 10
);
}
void PlannerNode::publishStatus(const std::string& status) {
std_msgs::msg::String msg;
msg.data = status;
status_pub_->publish(msg);
RCLCPP_DEBUG(this->get_logger(), "状态: %s", status.c_str());
}
// 在其他函数中调用
void PlannerNode::timerCallback() {
if (!has_goal_) {
publishStatus("IDLE");
return;
}
publishStatus("PLANNING");
if (success) {
publishStatus("SUCCESS");
} else {
publishStatus("FAILED");
}
}
场景3: 添加服务客户端
需求： 调用地图服务获取障碍物信息
// ===== 1. package.xml 添加依赖 =====
<depend>nav_msgs</depend>
// ===== 2. planner_node.hpp 添加 =====
#include <nav_msgs/srv/get_map.hpp>
class PlannerNode : public rclcpp::Node {
private:
rclcpp::Client<nav_msgs::srv::GetMap>::SharedPtr map_client_;
void requestMap();
};
// ===== 3. planner_node.cpp 实现 =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 创建客户端
map_client_ = this->create_client<nav_msgs::srv::GetMap>("static_map");
}
void PlannerNode::requestMap() {
// 等待服务可用
while (!map_client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(this->get_logger(), "等待地图服务被中断");
return;
}
RCLCPP_INFO(this->get_logger(), "等待地图服务...");
}
// 创建请求
auto request = std::make_shared<nav_msgs::srv::GetMap::Request>();
// 发送异步请求
auto future = map_client_->async_send_request(request);
// 等待响应
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) ==
rclcpp::FutureReturnCode::SUCCESS)
{
auto response = future.get();
RCLCPP_INFO(this->get_logger(),
"收到地图: %dx%d, 分辨率%.2f",
response->map.info.width,
response->map.info.height,
response->map.info.resolution);
// 将地图传递给planner
// planner_->setMap(response->map);
} else {
RCLCPP_ERROR(this->get_logger(), "获取地图失败");
}
}
场景4: 添加动作服务器（Action）
需求： 提供导航动作接口，支持取消和反馈
// ===== 1. package.xml 添加 =====
<depend>rclcpp_action</depend>
<depend>nav2_msgs</depend> <!-- 或自定义action消息包 -->
// ===== 2. CMakeLists.txt 添加 =====
find_package(rclcpp_action REQUIRED)
find_package(nav2_msgs REQUIRED)
set(dependencies
...
rclcpp_action
nav2_msgs
)
// ===== 3. planner_node.hpp 添加 =====
#include <rclcpp_action/rclcpp_action.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
class PlannerNode : public rclcpp::Node {
private:
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNavigate = rclcpp_action::ServerGoalHandle<NavigateToPose>;
rclcpp_action::Server<NavigateToPose>::SharedPtr action_server_;
// Action回调函数
rclcpp_action::GoalResponse handleGoal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const NavigateToPose::Goal> goal);
rclcpp_action::CancelResponse handleCancel(
const std::shared_ptr<GoalHandleNavigate> goal_handle);
void handleAccepted(const std::shared_ptr<GoalHandleNavigate> goal_handle);
void execute(const std::shared_ptr<GoalHandleNavigate> goal_handle);
};
// ===== 4. planner_node.cpp 实现 =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 创建Action服务器
action_server_ = rclcpp_action::create_server<NavigateToPose>(
this,
"navigate_to_pose",
std::bind(&PlannerNode::handleGoal, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&PlannerNode::handleCancel, this, std::placeholders::_1),
std::bind(&PlannerNode::handleAccepted, this, std::placeholders::_1)
);
}
rclcpp_action::GoalResponse PlannerNode::handleGoal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const NavigateToPose::Goal> goal)
{
(void)uuid;
RCLCPP_INFO(this->get_logger(), "收到导航请求到 [%.2f, %.2f]",
goal->pose.pose.position.x,
goal->pose.pose.position.y);
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse PlannerNode::handleCancel(
const std::shared_ptr<GoalHandleNavigate> goal_handle)
{
(void)goal_handle;
RCLCPP_INFO(this->get_logger(), "收到取消请求");
return rclcpp_action::CancelResponse::ACCEPT;
}
void PlannerNode::handleAccepted(const std::shared_ptr<GoalHandleNavigate> goal_handle) {
// 在新线程中执行，避免阻塞
std::thread{std::bind(&PlannerNode::execute, this, std::placeholders::_1), goal_handle}.detach();
}
void PlannerNode::execute(const std::shared_ptr<GoalHandleNavigate> goal_handle) {
RCLCPP_INFO(this->get_logger(), "开始执行导航");
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<NavigateToPose::Feedback>();
auto result = std::make_shared<NavigateToPose::Result>();
rclcpp::Rate loop_rate(10); // 10Hz
while (rclcpp::ok()) {
// 检查是否被取消
if (goal_handle->is_canceling()) {
result->error_code = 0;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "导航被取消");
return;
}
// 执行规划和控制逻辑
// ...
// 发布反馈
feedback->distance_remaining = 1.5; // 示例：剩余距离
goal_handle->publish_feedback(feedback);
// 检查是否到达目标
bool reached = false; // 你的到达判断逻辑
if (reached) {
result->error_code = 0;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "导航成功完成");
return;
}
loop_rate.sleep();
}
}
场景5: 添加参数动态重配置
需求： 运行时修改参数并实时生效
// ===== planner_node.hpp 添加 =====
class PlannerNode : public rclcpp::Node {
private:
OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
rcl_interfaces::msg::SetParametersResult
parametersCallback(const std::vector<rclcpp::Parameter>& parameters);
// 参数变量
double step_size_;
double max_speed_;
};
// ===== planner_node.cpp 实现 =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 声明参数
this->declare_parameter("step_size", 0.1);
this->declare_parameter("max_speed", 1.0);
// 获取初始值
step_size_ = this->get_parameter("step_size").as_double();
max_speed_ = this->get_parameter("max_speed").as_double();
// 注册参数更改回调
param_callback_handle_ = this->add_on_set_parameters_callback(
std::bind(&PlannerNode::parametersCallback, this, std::placeholders::_1)
);
}
rcl_interfaces::msg::SetParametersResult
PlannerNode::parametersCallback(const std::vector<rclcpp::Parameter>& parameters) {
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
result.reason = "成功";
for (const auto& param : parameters) {
if (param.get_name() == "step_size") {
double value = param.as_double();
if (value > 0.0 && value <= 1.0) {
step_size_ = value;
planner_->setStepSize(value);
RCLCPP_INFO(this->get_logger(), "更新step_size: %.2f", value);
} else {
result.successful = false;
result.reason = "step_size必须在(0, 1]范围内";
}
}
else if (param.get_name() == "max_speed") {
double value = param.as_double();
if (value > 0.0) {
max_speed_ = value;
RCLCPP_INFO(this->get_logger(), "更新max_speed: %.2f", value);
} else {
result.successful = false;
result.reason = "max_speed必须大于0";
}
}
}
return result;
}
运行时修改参数：
# 查看所有参数
ros2 param list
# 获取参数值
ros2 param get /planner step_size
# 设置参数值
ros2 param set /planner step_size 0.2
# 保存参数到文件
ros2 param dump /planner --output-dir ~/
场景6: 添加TF发布
需求： 发布机器人当前位置的TF变换
// ===== 1. package.xml 添加 =====
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
// ===== 2. planner_node.hpp 添加 =====
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
class PlannerNode : public rclcpp::Node {
private:
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
void publishTransform();
};
// ===== 3. planner_node.cpp 实现 =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 创建TF广播器
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
}
void PlannerNode::publishTransform() {
geometry_msgs::msg::TransformStamped transform;
// 填充变换信息
transform.header.stamp = this->now();
transform.header.frame_id = "map";
transform.child_frame_id = "robot_base";
// 设置平移
transform.transform.translation.x = 1.0;
transform.transform.translation.y = 2.0;
transform.transform.translation.z = 0.0;
// 设置旋转（四元数）
transform.transform.rotation.x = 0.0;
transform.transform.rotation.y = 0.0;
transform.transform.rotation.z = 0.0;
transform.transform.rotation.w = 1.0;
// 发布变换
tf_broadcaster_->sendTransform(transform);
}
// 在定时器或回调中调用
void PlannerNode::timerCallback() {
publishTransform();
// ... 其他逻辑
}
场景7: 添加多线程执行器
需求： 使用多线程处理回调，提高性能
// ===== main函数中使用多线程执行器 =====
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<your_package::PlannerNode>();
// 创建多线程执行器
rclcpp::executors::MultiThreadedExecutor executor(
rclcpp::ExecutorOptions(),
4 // 线程数量
);
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}
使用回调组分离执行：
// ===== planner_node.hpp 添加 =====
class PlannerNode : public rclcpp::Node {
private:
rclcpp::CallbackGroup::SharedPtr callback_group_1_;
rclcpp::CallbackGroup::SharedPtr callback_group_2_;
};
// ===== planner_node.cpp 实现 =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 创建互斥回调组
callback_group_1_ = this->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive);
callback_group_2_ = this->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive);
// 将订阅器分配到不同的回调组
auto sub_options_1 = rclcpp::SubscriptionOptions();
sub_options_1.callback_group = callback_group_1_;
goal_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"goal_pose", 10,
std::bind(&PlannerNode::goalCallback, this, std::placeholders::_1),
sub_options_1
);
auto sub_options_2 = rclcpp::SubscriptionOptions();
sub_options_2.callback_group = callback_group_2_;
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
"odom", 10,
std::bind(&PlannerNode::odomCallback, this, std::placeholders::_1),
sub_options_2
);
}
场景8: 添加日志文件记录
需求： 将规划结果保存到文件
// ===== planner_node.hpp 添加 =====
#include <fstream>
class PlannerNode : public rclcpp::Node {
private:
std::ofstream log_file_;
void logPlanningResult(const nav_msgs::msg::Path& path);
};
// ===== planner_node.cpp 实现 =====
PlannerNode::PlannerNode() : Node("planner_node") {
// 打开日志文件
std::string log_path = "/tmp/planner_log.txt";
log_file_.open(log_path, std::ios::out | std::ios::app);
if (!log_file_.is_open()) {
RCLCPP_ERROR(this->get_logger(), "无法打开日志文件: %s", log_path.c_str());
} else {
RCLCPP_INFO(this->get_logger(), "日志文件: %s", log_path.c_str());
}
}
void PlannerNode::logPlanningResult(const nav_msgs::msg::Path& path) {
if (!log_file_.is_open()) return;
auto now = this->now();
log_file_ << "========== " << now.seconds() << " ==========
";
log_file_ << "路径点数: " << path.poses.size() << "
";
if (!path.poses.empty()) {
const auto& start = path.poses.front();
const auto& end = path.poses.back();
log_file_ << "起点: [" << start.pose.position.x << ", "
<< start.pose.position.y << "]
";
log_file_ << "终点: [" << end.pose.position.x << ", "
<< end.pose.position.y << "]
";
}
log_file_ << "
";
log_file_.flush();
}
// 析构函数中关闭文件
PlannerNode::~PlannerNode() {
if (log_file_.is_open()) {
log_file_.close();
}
}
场景9: 添加可视化标记（RViz）
需求： 在RViz中显示规划的关键点
// ===== 1. package.xml 添加 =====
<depend>visualization_msgs</depend>
// ===== 2. planner_node.hpp 添加 =====
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
class PlannerNode : public rclcpp::Node {
private:
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
void publishMarkers(const std::vector<geometry_msgs::msg::PoseStamped>& waypoints);
};
// ===== 3. planner_node.cpp 实现 =====
PlannerNode::PlannerNode() : Node("planner_node") {
marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
"planning_markers", 10
);
}
void PlannerNode::publishMarkers(
const std::vector<geometry_msgs::msg::PoseStamped>& waypoints)
{
visualization_msgs::msg::MarkerArray marker_array;
// 清除旧标记
visualization_msgs::msg::Marker delete_marker;
delete_marker.action = visualization_msgs::msg::Marker::DELETEALL;
marker_array.markers.push_back(delete_marker);
// 创建路径点标记
for (size_t i = 0; i < waypoints.size(); ++i) {
visualization_msgs::msg::Marker marker;
marker.header.frame_id = "map";
marker.header.stamp = this->now();
marker.ns = "waypoints";
marker.id = i;
marker.type = visualization_msgs::msg::Marker::SPHERE;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.pose = waypoints[i].pose;
marker.scale.x = 0.2;
marker.scale.y = 0.2;
marker.scale.z = 0.2;
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
marker_array.markers.push_back(marker);
}
// 创建路径线条
visualization_msgs::msg::Marker line_marker;
line_marker.header.frame_id = "map";
line_marker.header.stamp = this->now();
line_marker.ns = "path_line";
line_marker.id = 0;
line_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
line_marker.action = visualization_msgs::msg::Marker::ADD;
line_marker.scale.x = 0.05; // 线宽
line_marker.color.r = 1.0f;
line_marker.color.g = 0.0f;
line_marker.color.b = 0.0f;
line_marker.color.a = 1.0;
for (const auto& wp : waypoints) {
line_marker.points.push_back(wp.pose.position);
}
marker_array.markers.push_back(line_marker);
marker_pub_->publish(marker_array);
}
六、编译和运行
编译流程
# 1. 进入工作空间（对应的文件夹）
cd ~/ros2_ws
# 2. 编译单个包
colcon build --packages-select your_package
# 或者 使用符号连接模式（对整个工作空间)
colcon build --symlink-install
# 单独编译单个包
colcon build --symlink-install --packages-select my_package
# 3. 编译并显示详细输出
colcon build --packages-select your_package --event-handlers console_direct+
# 4. 编译并启用编译器优化
colcon build --packages-select your_package --cmake-args -DCMAKE_BUILD_TYPE=Release
# 5. 清理并重新编译
colcon build --packages-select your_package --cmake-clean-cache
注意：
colcon build 和 colcon build –symlink-install是不同的，如果为了保险的化，选择colcon build基本不会出问题，而添加symlink-install适合自己调试的时候使用
具体差别见我的另一篇文章
超通俗的ROS2 colcon build和colcon build –symlink-install区别讲解
运行节点
# 1. 刷新环境变量
source ~/ros2_ws/install/setup.bash
# 2. 直接运行节点
ros2 run your_package planner_node
# 3. 使用launch文件
ros2 launch your_package launch.py
# 4. 传递参数
ros2 launch your_package launch.py step_size:=0.2 planning_frequency:=5.0
# 5. 启用调试模式
ros2 launch your_package launch.py debug:=true
测试和调试
# 查看节点信息
ros2 node info /planner
# 查看话题列表
ros2 topic list
# 查看话题内容
ros2 topic echo /planned_path
# 发布测试目标点
ros2 topic pub /goal_pose geometry_msgs/msg/PoseStamped "{
header: {frame_id: 'map'},
pose: {
position: {x: 5.0, y: 3.0, z: 0.0},
orientation: {w: 1.0}
}
}"
# 调用服务
ros2 service call /reset_planner std_srvs/srv/Trigger
# 查看参数
ros2 param list /planner
ros2 param get /planner step_size
# 设置参数
ros2 param set /planner step_size 0.2
# 查看TF树
ros2 run tf2_tools view_frames
# 录制数据包
ros2 bag record -a # 录制所有话题
ros2 bag record /planned_path /odom # 录制指定话题
# 回放数据包
ros2 bag play rosbag2_xxx
七、调试技巧
1. 日志级别
// 不同级别的日志
RCLCPP_DEBUG(this->get_logger(), "详细调试信息");
RCLCPP_INFO(this->get_logger(), "一般信息");
RCLCPP_WARN(this->get_logger(), "警告信息");
RCLCPP_ERROR(this->get_logger(), "错误信息");
RCLCPP_FATAL(this->get_logger(), "致命错误");
// 限流日志（避免刷屏）
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
"每秒最多打印一次");
// 条件日志
RCLCPP_INFO_EXPRESSION(this->get_logger(), has_goal_, "有目标时才打印");
// 一次性日志
RCLCPP_INFO_ONCE(this->get_logger(), "只打印一次");
设置日志级别：
# 运行时设置
ros2 run your_package planner_node --ros-args --log-level debug
# launch文件中设置
arguments=['--ros-args', '--log-level', 'debug']
2. 断言和异常处理
// 断言检查
#include <cassert>
assert(planner_ != nullptr && "planner未初始化");
// 异常处理
try {
auto result = planner_->plan(start, goal, path);
if (!result) {
throw std::runtime_error("规划失败");
}
} catch (const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "异常: %s", e.what());
}
3. 性能分析
#include <chrono>
void PlannerNode::timerCallback() {
auto start_time = std::chrono::high_resolution_clock::now();
// 执行规划
planner_->plan(start, goal, path);
auto end_time = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
end_time - start_time);
RCLCPP_INFO(this->get_logger(), "规划耗时: %ld ms", duration.count());
}
八、常见问题和解决方案
问题1: 找不到头文件
# 错误信息：
# fatal error: your_package/planner.hpp: No such file or directory
# 解决方案：
# 1. 检查CMakeLists.txt中的include_directories
include_directories(include)
# 2. 检查头文件路径是否正确
# 应该是: include/your_package/planner.hpp
问题2: 链接错误
# 错误信息：
# undefined reference to `your_package::Planner::plan(...)'
# 解决方案：
# 检查CMakeLists.txt中是否链接了库
target_link_libraries(planner_node ${PROJECT_NAME}_lib)
问题3: 话题没有数据
# 检查步骤：
# 1. 确认节点正在运行
ros2 node list
# 2. 确认话题存在
ros2 topic list
# 3. 查看话题信息
ros2 topic info /planned_path
# 4. 检查话题类型匹配
ros2 interface show nav_msgs/msg/Path
# 5. 检查QoS设置是否兼容
问题4: 回调函数不执行
// 可能原因：
// 1. spin没有调用
rclcpp::spin(node); // 必须调用
// 2. 回调函数中有阻塞操作
// ❌ 错误
void callback(const MsgType::SharedPtr msg) {
std::this_thread::sleep_for(std::chrono::seconds(10)); // 阻塞！
}
// ✅ 正确
void callback(const MsgType::SharedPtr msg) {
data_ = *msg; // 快速处理
}
九、总结
1. 代码组织
算法类和ROS节点类分离使用命名空间避免冲突头文件使用include guard
2. 内存管理
优先使用智能指针（
std::shared_ptr
new
delete
3. 通信机制
回调函数保持简短快速耗时操作在单独线程执行合理设置QoS参数
4. 参数管理
所有可配置项使用参数提供合理的默认值实现参数验证逻辑
5. 错误处理
检查指针有效性使用try-catch处理异常记录详细的错误日志
6. 调试和测试
使用合适的日志级别添加性能监控提供测试launch文件
十、参考资源
ROS 2官方文档: https://docs.ros.org/ROS 2设计文档: https://design.ros2.org/rclcpp API文档: https://docs.ros2.org/latest/api/rclcpp/ROS 2示例代码: https://github.com/ros2/examples
© 版权声明
文章版权归作者所有，未经允许请勿转载。如内容涉嫌侵权，请在本页底部进入<联系我们>进行举报投诉!
THE END
暂无评论内容