ROS2 入门笔记
在终端中调用ros2启动脚本以加载ros2相关的环境变量和命令
source/opt/ros/humble/setup.bash
其中humble为ros的版本名称，不同版本有着不同的名称。
节点
节点可以认为是一个可执行文件或是一个进程，在ROS中，节点是操作的最小单位，机器人在完成某一个操作时，可能会调用一个或多个节点程序，而且只有在真正需要时才会被调用。
使用功能包组织C++节点
ros2 pkg create pkg_name --build_type ament_cmake --license Apache-2.0
ros2 pkg create [name]使用ros2功能包的打包功能创建一个功能包,并指定功能包的名称；--build_type指定了使用哪种方式，c++功能包使用ament-cmake，表示用cmake的组织方式；python功能包使用ament_python，表示使用python的组织方式；--license指定证书
通过上述命令，实际上是创建了一个项目模板，仍用c++项目举例，cpp文件将会放在src目录下，头文件将会放在include/pkg_name目录下，然后通过CMakeLists.txt管理。
在生成的CMakeLists.txt中，可以使用ament_dependencies(project_name dependency_name)命令——依赖ament_cmake库，为当前的项目添加依赖源，然后使用install命令，将项目安装到lib/${PROJECT_NAME}下，供外部使用；最后，通过ament_package()命令收集信息，生成索引和进行相关配置。
在开发功能时，除了需要修改CMakeLists.txt文件，还需要在package.xml文件中添加相关库的依赖声明，比如添加对rclcpp的依赖声明:
...
<depend>rclcpp</depend>
...
构建功能包
在功能包开发完成后，在终端中输入colcon build命令，就会构建当前路径及子目录下的所有功能包，如果当前路径下不存在build、install和log目录，会自动创建，并将中间文件自动放入对应目录下，构建完成后，在install目录下就可以看到功能包了，可执行文件会生成在对应的功能包路径下。
PS：使用--packages-select pkg_name 可以单独构建指定的项目
最后，在终端中输入ros2 run [pkg_name] [node_name]就会调用相应功能包中的对应节点程序。
话题通信
ROS2使用一套订阅发布机制用于在机器人系统中传递传感器和执行器数据。
话题机制包括四个关键部分：发布者、订阅者、话题名称和话题类型。
以海龟模拟节点为例，启动海龟模拟节点后在新终端中输入ros2 node info /turtlesim，可以看到海龟模拟节点详细信息：
turtlesim
Subscribers:
/parameter_events: rcl_interfaces/msg/ParameterEvent
/turtle1/cmd_vel: geometry_msgs/msg/Twist
Publishers:
/parameter_events: rcl_interfaces/msg/ParameterEvent
/rosout: rcl_interfaces/msg/Log
/turtle1/color_sensor: turtlesim/msg/Color
/turtle1/pose: turtlesim/msg/Pose
Service Servers:
/clear: std_srvs/srv/Empty
/kill: turtlesim/srv/Kill
/reset: std_srvs/srv/Empty
/spawn: turtlesim/srv/Spawn
/turtle1/set_pen: turtlesim/srv/SetPen
/turtle1/teleport_absolute: turtlesim/srv/TeleportAbsolute
/turtle1/teleport_relative: turtlesim/srv/TeleportRelative
/turtlesim/describe_parameters: rcl_interfaces/srv/DescribeParameters
/turtlesim/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
/turtlesim/get_parameters: rcl_interfaces/srv/GetParameters
/turtlesim/list_parameters: rcl_interfaces/srv/ListParameters
/turtlesim/set_parameters: rcl_interfaces/srv/SetParameters
/turtlesim/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
Service Clients:
Action Servers:
/turtle1/rotate_absolute: turtlesim/action/RotateAbsolute
Action Clients:
Subscribers处打印了海龟模拟节点订阅的所有话题，比如名为/turtle1/cmd_vel的话题，该话题用于接收控制指令，它的消息接口是eometry_msgs/msg/Twist。
Publishers下是海龟模拟节点发布的所有话题，包括日志输出，位置信息等。
如果我们尝试订阅海龟模拟节点的位置信息，在新终端中输入ros2 topic echo /turtle1/pose，就会在终端中看到持续打印的海龟位置信息。
使用ros2 topic info /turtle1/cmd_vel -v可以看到相关话题的具体信息。
使用ros2 interface show eometry_msgs/msg/Twist，可以看到消息接口的详细定义。
C++话题订阅和发布
仍以海龟模拟节点为例，程序通过发布相关话题，然后海龟模拟节点订阅相关话题，就可以实现海龟的运动控制。
控制海龟圆周运动
首先按照节点章节的描述，创建一个控制海龟运动的功能包ros2 pkg create demo_cpp_topic --build-type ament_cmake --dependencies rclcpp geometry_msgs turtlesim --license Apache-2.0，为了能够在代码中订阅和发布相关话题，需要在创建功能包时添加geometry_msgs和turtlesim的依赖。
class TurtleCircle : public rclcpp::Node {
public:
explicit TurtleCircle(const std::string& name);
private:
void timer_callback();
private:
rclcpp::TimerBase::SharedPtr m_timer;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr m_publisher;
};
#include "demo_cpp_topic/turtle_circle.hpp"
#include <chrono>
TurtleCircle::TurtleCircle(const std::string& name) : rclcpp::Node(name) {
m_publisher =
create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 10);
m_timer = create_wall_timer(std::chrono::milliseconds(1000),
std::bind(&TurtleCircle::timer_callback, this));
}
void TurtleCircle::timer_callback() {
auto msg = geometry_msgs::msg::Twist();
msg.linear.x = 1.0;
msg.angular.z = 0.5;
m_publisher->publish(msg);
}
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<TurtleCircle>("turtle_circle");
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
ROS2中总是以节点作为单位进行通讯的，控制海龟圆周运动的节点继承rclcpp:Node，在主程序中创建这样一个节点，就可以通过多态的方式实现自定义节点的发布。
自定义的类中有两个成员变量，一个负责发布数据，另一个是计时器，控制发布间隔，在构造函数中通过基类的成员方法创建。
闭环控制
通过发布控制命令到话题/turtle1/cmd_vel可以控制海龟运动，通过订阅/turtle1/pos可以获取海龟的实时位置，结合两者的数据，就可以闭环控制海龟移动到指定位置。所谓闭环控制是指通过测量输出，根据反馈调节系统的输入，使系统的输出更加接近期望值。
模拟海龟的例子中，就可以通过检测海龟当前位置和目标位置的误差，实时调整发布的指令，最终使海龟到达指定目标点。
#include <geometry_msgs/msg/twist.hpp>
#include <rclcpp/rclcpp.hpp>
#include <turtlesim/msg/pose.hpp>
class TurtleControl : public rclcpp::Node {
public:
TurtleControl() : rclcpp::Node("turtle_control") {
m_publish =
create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 10);
m_pos_sub = create_subscription<turtlesim::msg::Pose>(
"/turtle1/pose", 10,
std::bind(&TurtleControl::control, this, std::placeholders::_1));
}
private:
void control(const turtlesim::msg::Pose::SharedPtr pose) {
auto message = geometry_msgs::msg::Twist();
// 1, 记录当前位置
double cur_x = pose->x;
double cur_y = pose->y;
RCLCPP_INFO(this->get_logger(), "当前位置：(x = %f, y = %f)", cur_x,
cur_y);
// 2. 计算差值
double distance = std::sqrt((m_xtarget - cur_x) * (m_xtarget - cur_x) +
(m_ytarget - cur_y) * (m_ytarget - cur_y));
double angle =
std::atan2(m_ytarget - cur_y, m_xtarget - cur_x) - pose->theta;
// 3. 控制策略：距离大于0.1继续运动，角度差大于0.2原地旋转，否则直行
if (distance > 0.1) {
if (fabs(angle) > 0.2) {
message.angular.z = fabs(angle);
} else {
message.linear.x = m_k * distance;
}
}
// 控制最大值并发布
if (message.linear.x > m_maxspeed) {
message.linear.x = m_maxspeed;
}
m_publish->publish(message);
}
private:
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr m_pos_sub;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr m_publish;
double m_xtarget{1};
double m_ytarget{1};
double m_k{1};
double m_maxspeed{3};
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<TurtleControl>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
自定义话题通信接口
自定义话题通信接口也是创建一个功能包，区别在于依赖的库不同。
ros2 pkg create custom_topic --build-type ament_cmake --dependencies rosidl_default_generators builtin_interfaces --license Apache-2.0
- builtin_interfaces 是ROS2提供的一个消息接口功能包，可以使用其时间接口Time,记录信息的时间；
- rosidl_default_generators 用于将自定义的消息文件转换为C++、Python源码的模块。
PS: ROS2的话题消息定义文件需要放到功能包的msg路径下，文件名必须以大写字母开头，只能由字母和数字组成。
例如在功能包的msg路径下创建MyTopic.msg
builtin_interfaces/Time timestamp # 时间戳
string hostname # 系统名称
float32 cpu_ratio # cpu使用率
float32 memory_ratio # 内存使用率
float32 memory_total # 内存总量
float32 memory_avalible # 内存剩余
float64 net_sent # 网络发送数据总量
float64 net_recv # 网络接收数据总量
之后，在CMakeLists.txt文件中添加
rosidl_generate_interfaces(
${PROJECT_NAME}
"msg/MyTopic.msg"
DEPENDENCIES builtin_interfaces
)
除了修改CMakeLists.txt文件，在package.xml文件中最好也添加上声明<member_of_group>rosidl_interface_packages</member_of_group>，声明该功能包是一个消息接口功能包。
构建完成后，如果需要使用到该自定义的消息话题，就在创建功能包时将该功能包作为依赖包含进去。
服务通信
话题通信用于从发布者到订阅者的单向通信，如果还需要使用双向的数据传递就需要使用到服务通信，比如一个场景下A节点需要将图片发送给B节点识别，识别完成后需要将结果返回给A节点。服务就是这种基于响应和请求的双向通信机制。
一些脚本命令
ros2 service list -t：查看当前的所有服务，-t会打印出服务使用的接口类型；ros2 interface show [interface_name]: 查看接口类型的具体定义，使用---隔开了请求和响应的结构；ros2 service call [serv_name] [interface_name] "[data]"：该命令可以调用指定的服务，并将数据作为请求发送;ros2 param list：查看参数列表，ROS2允许参数作为服务动态配置；ros2 param describe [param_name]：查看参数服务的描述；ros2 param get [node] [param_name]：查看节点当前参数；ros2 param set [node] [param_name]：设置节点的参数，另外还可以在节点启动时使用配置文件配置参数，需要在启动节点时在脚本命令后面追加--ros-args --params-file [config.yaml]选项。
使用服务通信
首先自定义服务接口类型
像话题的自定义接口一样，创建一个接口的功能包，在srv路径下添加一个接口文件PoseProcess.srv
float32 x_target # 目标x值
float32 y_target # 目标y值
---
int8 SUCCESS = 0 # 定义状态常量
int8 FAILURE = -1
int8 result # 处理结果
上半部分表示请求结构，下半部分表示响应结构。
创建服务端和客户端
服务器使用话题通信时的海龟控制程序，在构造函数中再创建一个服务器对象的成员参数.
TurtleControl() : rclcpp::Node("turtle_control") {
// ...
// 创建服务
m_serv = create_service<PoseProcess>(
"position",
[this](const std::shared_ptr<PoseProcess::Request> request,
std::shared_ptr<PoseProcess::Response> response) {
// 判断逻辑点是否在模拟器内
if ((0 < request->x_target && request->x_target < 12) &&
(0 < request->y_target && request->y_target < 12)) {
m_xtarget = request->x_target;
m_ytarget = request->y_target;
response->result = PoseProcess::Response::SUCCESS;
} else {
response->result = PoseProcess::Response::FAILURE;
}
});
}
再创建一个客户端节点，在客户端节点的构造函数中创建一个客户端对象的成员变量，然后使用异步接口发送请求，如下：
class PoseClient : public rclcpp::Node {
public:
PoseClient() : Node("pose_client") {
m_client = create_client<PoseProcess>("position");
m_timer = create_wall_timer(std::chrono::seconds(10), [&]() {
// 1.等待服务器
while (!m_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<PoseProcess::Request>();
request->x_target = random_num(eng) % 15;
request->y_target = random_num(eng) % 15;
RCLCPP_INFO(this->get_logger(), "巡逻地点：(%f, %f)",
request->x_target, request->y_target);
// 3.发送异步请求
m_client->async_send_request(
request, [&](rclcpp::Client<PoseProcess>::SharedFuture result) {
auto response = result.get();
if (response->result == PoseProcess::Response::SUCCESS) {
RCLCPP_INFO(this->get_logger(), "目标点处理成功");
} else if (response->result ==
PoseProcess::Response::FAILURE) {
RCLCPP_INFO(this->get_logger(), "目标点处理失败");
}
});
});
}
private:
rclcpp::TimerBase::SharedPtr m_timer;
rclcpp::Client<PoseProcess>::SharedPtr m_client;
};
使用参数通信
可以使用节点的成员函数declare_parameter声明一个参数通信服务，使用成员函数get_parameter根据参数通信服务名称获取参数。这样做可以达到修改参数值的目的，但是无法监控参数设置的时机。
在节点类中，有成员函数add_on_set_parameters_callback负责参数更新时的回调，通过传入回调函数，达到监控参数更改的目的。
#include <demo_serv_interface/srv/pose_process.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <rcl_interfaces/msg/parameter.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <rclcpp/rclcpp.hpp>
#include <turtlesim/msg/pose.hpp>
using PoseProcess = demo_serv_interface::srv::PoseProcess;
class TurtleControl : public rclcpp::Node {
public:
TurtleControl() : rclcpp::Node("turtle_control") {
// ...
declare_parameter("k", 1.0);
declare_parameter("max_speed", 1.0);
get_parameter("k", m_k);
get_parameter("max_speed", m_maxspeed);
m_param_callback_handler = add_on_set_parameters_callback(
[&](const std::vector<rclcpp::Parameter>& params) {
for (auto& param : params) {
RCLCPP_INFO(this->get_logger(), "更新参数%s值为：%f",
param.get_name().c_str(), param.as_double());
if (param.get_name() == "k") {
m_k = param.as_double();
} else if (param.get_name() == "max_speed") {
m_maxspeed = param.as_double();
}
}
auto result = rcl_interfaces::msg::SetParametersResult();
result.successful = true;
return result;
});
}
private:
// ...
OnSetParametersCallbackHandle::SharedPtr m_param_callback_handler;
};
然后我们就可以在客户端中使用参数通信来更新服务端中的参数
#include <chrono>
#include <demo_serv_interface/srv/pose_process.hpp>
#include <random>
#include <rcl_interfaces/msg/parameter.hpp>
#include <rcl_interfaces/msg/parameter_type.hpp>
#include <rcl_interfaces/msg/parameter_value.hpp>
#include <rcl_interfaces/srv/set_parameters.hpp>
#include <rclcpp/rclcpp.hpp>
using demo_serv_interface::srv::PoseProcess;
using rcl_interfaces::srv::SetParameters;
static std::default_random_engine eng(std::random_device{}());
static std::uniform_int_distribution<int> random_num;
class PoseClient : public rclcpp::Node {
public:
void update_server_param(double k) {
auto param = rcl_interfaces::msg::Parameter();
param.name = "k";
auto param_value = rcl_interfaces::msg::ParameterValue();
param_value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
param_value.double_value = k;
param.value = param_value;
auto response = call_set_paramters(param);
if (response == nullptr) {
RCLCPP_WARN(get_logger(), "参数修改失败");
return;
} else {
for (auto result : response->results) {
if (result.successful) {
RCLCPP_INFO(get_logger(), "参数k已修改为: %f", k);
} else {
RCLCPP_WARN(get_logger(), "参数k失败原因：%s",
result.reason.c_str());
}
}
}
}
private:
std::shared_ptr<SetParameters::Response> call_set_paramters(
rcl_interfaces::msg::Parameter& parameter) {
auto param_client = this->create_client<SetParameters>(
"/turtle_control/set_parameters");
while (!param_client->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(get_logger(), "等待服务端流程中断");
return nullptr;
}
RCLCPP_INFO(get_logger(), "等待参数设置服务端上线中...");
}
auto request = std::make_shared<SetParameters::Request>();
request->parameters.push_back(parameter);
auto future = param_client->async_send_request(request);
rclcpp::spin_until_future_complete(shared_from_this(), future);
auto response = future.get();
return response;
}
private:
rclcpp::TimerBase::SharedPtr m_timer;
rclcpp::Client<PoseProcess>::SharedPtr m_client;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<PoseClient>();
node->update_server_param(1.5);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
坐标变换工具tf2
传感器中心相对基座的偏移是(0.1, 0.0, 0.2)，当传感器检测到障碍物位置为(0.3, 0.0, 0.0)时，对于基座来说，障碍物的位置在哪？
使用tf2的命令可以获得。简单起见，演示使用静态坐标。
- 首先建立传感器相对基座的坐标系位置
ros2 run tf2_ros static_transform_publisher --x 0.1 --y 0 --z 0.2 --roll 0 --pitch 0 --yaw 0 --frame-id base_link --child-frame-id base_laser
- 然后在新的终端中，再建立目标点到传感器的坐标系位置
ros2 run tf2_ros static_transform_publisher --x 0.3 --y 0 --z 0 --roll 0 --pitch 0 --yaw 0 --frame-id base_laser --child-frame-id wall_point
- 最后在新的终端中打印目标在基座坐标系中的位置，打印的信息如下
ros2 run tf2_ros tf2_echo base_link wall_point
[INFO] [1746024330.946262785] [tf2_echo]: Waiting for transform base_link -> wall_point: Invalid frame ID "base_link" passed to canTransform argument target_frame - frame does not exist
At time 0.0
- Translation: [0.400, 0.000, 0.200]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
- Rotation: in RPY (radian) [0.000, -0.000, 0.000]
- Rotation: in RPY (degree) [0.000, -0.000, 0.000]
- Matrix:
1.000 0.000 0.000 0.400
0.000 1.000 0.000 0.000
0.000 0.000 1.000 0.200
0.000 0.000 0.000 1.000
...
可以看到，使用tf2成功完成了目标点在传感器坐标系位置到基座坐标系位置的转换，打印信息中通过矩阵的形式展示出来了。因为只是简单的位置偏移，可以口算验证下，x方向累计0.4。，z方向累计0.2，验证无误。
使用ros2 topic info /tf_static命令可以查看到，实际上，tf2也是使用了话题通信的方式，完成了坐标数据信息的收发，所以说订阅-发布机制的通信是ROS2的核心机制。
在程序中使用tf
ros2 pkg create demo_cpp_tf --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs --license Apache-2.0
按照套路，首先创建功能包，包含必要的依赖，其中tf2和tf2_ros是使用tf2功能包的基础依赖，geometry_msgs提供消息接口的依赖，tf2_geometry_msgs提供了信息类型转换函数。
首先定义了静态的坐标转换
#include <tf2_ros/static_transform_broadcaster.h> /* 提供静态坐标广播器类 */
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.hpp> /* 提供tf2::Quaternion类 */
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> /* 提供消息类型转换函数 */
class DemoTFBroadcaster : public rclcpp::Node {
public:
DemoTFBroadcaster() : rclcpp::Node("demo_tf_broadcaster") {
m_broadcaster =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(*this);
publishTF();
}
private:
void publishTF() {
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = get_clock()->now();
transform.header.frame_id = "map";
transform.child_frame_id = "target_point";
transform.transform.translation.x = 5;
transform.transform.translation.y = 3;
transform.transform.translation.z = 0;
tf2::Quaternion quat;
quat.setRPY(0, 0, 60 * M_PI / 180);
transform.transform.rotation = tf2::toMsg(quat);
m_broadcaster->sendTransform(transform);
}
private:
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> m_broadcaster;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<DemoTFBroadcaster>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
再定义一个节点用于动态转换
#include <tf2_ros/transform_broadcaster.h> /*提供坐标广播器类*/
#include <chrono>
#include <geometry_msgs/msg/transform_stamped.hpp> /*提供消息接口*/
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.hpp> /*提供tf2::Quaternion类*/
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> /*提供消息类型转换函数*/
class DynamicBroadcaster : public rclcpp::Node {
public:
DynamicBroadcaster() : rclcpp::Node("dynamic_broadcaster") {
m_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(this);
m_timer = create_wall_timer(std::chrono::milliseconds(10), [this]() {
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = get_clock()->now();
transform.header.frame_id = "map";
transform.child_frame_id = "base_link";
transform.transform.translation.x = 2;
transform.transform.translation.y = 3;
transform.transform.translation.z = 0;
tf2::Quaternion quat;
quat.setRPY(0, 0, 30 * M_PI / 180);
transform.transform.rotation = tf2::toMsg(quat);
m_broadcaster->sendTransform(transform);
});
}
private:
std::shared_ptr<tf2_ros::TransformBroadcaster> m_broadcaster;
rclcpp::TimerBase::SharedPtr m_timer;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<DynamicBroadcaster>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
定义好了目标点相对地图的坐标和传感器相对地图的坐标，就可以实现任意两者坐标位置的转换，这里再定义一个监听坐标变化的客户端,每隔5秒打印一次传感器到目标点的坐标情况。
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <chrono>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
#include <tf2/utils.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
class TFListener : public rclcpp::Node {
public:
TFListener() : rclcpp::Node("tf_listener") {
m_buffer = std::make_shared<tf2_ros::Buffer>(get_clock());
m_listener =
std::make_shared<tf2_ros::TransformListener>(*m_buffer, this);
m_timer = create_wall_timer(std::chrono::seconds(5), [this]() {
const auto transform = m_buffer->lookupTransform(
"base_link", "target_point", this->get_clock()->now(),
rclcpp::Duration::from_seconds(1));
const auto& translation = transform.transform.translation;
const auto& rotation = transform.transform.rotation;
double yaw, pitch, roll;
tf2::getEulerYPR(rotation, yaw, pitch, roll);
RCLCPP_INFO(get_logger(), "平移分量：(%f, %f, %f)", translation.x,
translation.y, translation.z);
RCLCPP_INFO(get_logger(), "旋转分量：(%f, %f, %f)", roll, pitch,
yaw);
});
}
private:
std::shared_ptr<tf2_ros::Buffer> m_buffer;
std::shared_ptr<tf2_ros::TransformListener> m_listener;
rclcpp::TimerBase::SharedPtr m_timer;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<TFListener>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
依次启动三个节点后，就可以在监听节点所在终端中看到打印的坐标信息，如下：
[INFO] [1746080036.849043816] [tf_listener]: 平移分量：(2.598076, -1.500000, 0.000000)
[INFO] [1746080036.849084033] [tf_listener]: 旋转分量：(0.000000, -0.000000, 0.523599)
...
消息服务质量QoS
QoS(Quality of Service)即服务质量，配置了消息在服务中的传递行为和状态。
QoS策略
ROS2基于中间件DDS的QoS提供了多样的QoS策略。
- 历史记录(History)
- 仅保留最新(Keep last)：仅存储最多N个样本，可通过历史队列深度策略进行配置。
- 全部保留(Keep all)：存储所有样本，会受到中间件的资源限制配置的影响。
- 历史队列深度(Depth)
- 队列大小(Queue size)：历史记录策略设置为仅保留最新时生效。
- 可靠性(Reliability)
- 尽力而为(Best effort)：尝试传递样本，但如果网络不稳定可能会丢失。
- 可靠性传递(Reliable)：保证样本被传递，丢失会进行重传。
- 持久性(Durability)
- 瞬态本地(Transient local)：发布者负责为“后续加入”的订阅者保留数据。
- 易失性(Volatile)：不保留任何数据。
- 截止时间(Deadline)
- 持续时间(Duration)：消息需要在截止时间前被接收，否则可能会被丢弃。
- 寿命(Lifespan)
- 持续时间(Duration)：消息寿命用于定义消息可以存在的最长时间，超过寿命的消息会被丢弃。
- 活跃度(Livelines)
- 自动(Automatic)：一个话题是否活跃由ROS的rmw层自动检测和报告，每次报告在下个租约持续时间内都将视其为活跃状态。
- 按主题手动(Manual by topic)：话题每发布一次或手动声明一次，则在下个租约持续时间内都将其视为活跃状态。
- 租约持续时间(Livelines Lease Duration)
- 持续时间(Duration)：超过租约持续时间没有进行活跃度更新的发布者则视为无效。
在使用ros2 topic info [topic_name] -v命令时，在打印的信息中可以看到QoS profile: ... 部分展示了当前话题的服务质量策略。
由于发布者和订阅者都会配置自己的QoS策略，在配置自己服务的QoS策略时，需要保证与订阅服务的QoS策略一致或是兼容。
执行器和回调组
ROS2在创建话题订阅者或者创建定时器时，都需要传入一个回调函数，这个回调函数实际上就是为了执行器调用的。上面的例子的主函数中，都会使用rclcpp::spin()函数调用节点，这个函数中就是封装了执行器的操作，以单线程执行器举例：
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
executor.spin();
在调用了执行器的spin方法后，执行器实例会不断查询中间层传入的消息和事件，然后调用相应的回调函数进行处理，直到节点关闭为止。
ROS2提供了单线程执行器、多线程执行器和静态单线程执行器三种，三者都继承自Executor基类。除了线程数量的区别外，静态单线程执行器只会在添加节点时扫描节点的结构，确定节点的订阅和定时器，而其他执行器都会定时进行扫描。
与此同时，ROS2允许将节点的回调组织成组的形式，配合多线程执行器就可以使得回调函数在单独的线程中执行，设置回调组为互斥，还可以保证回调组中的回调函数同时只会有一个在执行，用于保证一些必要逻辑执行的先后顺序。
#include <example_interfaces/srv/add_two_ints.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sstream>
#include <std_msgs/msg/string.hpp>
class LearnExecutorNode : public rclcpp::Node {
public:
LearnExecutorNode() : rclcpp::Node("learn_executor") {
m_publish = create_publisher<std_msgs::msg::String>("string_topic", 10);
m_timer = create_wall_timer(std::chrono::seconds(5), [this]() {
auto msg = std_msgs::msg::String();
msg.data = "话题发布：" + threadInfo();
RCLCPP_INFO(get_logger(), msg.data.c_str());
m_publish->publish(msg);
});
m_serv_callback_group =
create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
m_service = create_service<example_interfaces::srv::AddTwoInts>(
"add_two_ints",
[this](
const std::shared_ptr<
example_interfaces::srv::AddTwoInts::Request>
request,
std::shared_ptr<example_interfaces::srv::AddTwoInts::Response>
response) {
RCLCPP_INFO(get_logger(), "服务开始处理：%s",
threadInfo().c_str());
std::this_thread::sleep_for(std::chrono::seconds(10));
response->sum = request->a + request->b;
RCLCPP_INFO(get_logger(), "服务处理完成：%s",
threadInfo().c_str());
},
rmw_qos_profile_services_default, m_serv_callback_group);
}
private:
std::string threadInfo() {
std::ostringstream thread_str;
thread_str << "线程ID: " << std::this_thread::get_id();
return thread_str.str();
}
private:
rclcpp::TimerBase::SharedPtr m_timer;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr m_publish;
rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr m_service;
rclcpp::CallbackGroup::SharedPtr m_serv_callback_group;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<LearnExecutorNode>();
auto executor =
rclcpp::executors::MultiThreadedExecutor(rclcpp::ExecutorOptions(), 4);
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}
这段代码演示了多线程执行器和回调组的使用，首先创建了一个回调组，然后在创建服务时将回调函数加入了这个回调组，这样在多线程执行器中就可以正常使用了。
需要说明，在没有创建回调组的情况下，ROS2会将所有的回调函数放到同一个默认回调组中，在上述代码执行过程中的体现就是当服务在执行响应回调时，计时器的回调函数无法正常打印。
生命周期节点
生命周期节点相对普通节点的区别在于，生命周期节点的状态可以被读取和修改。在ROS2的生命周期节点实例中可以感受和学习一下：
ros2 run lifecycle lifecycle_talker
在启动了生命周期节点后，在该节点下会有一些服务，例如change_state用于改变节点的状态，get_state用于获取当前的状态。
生命周期节点提供四种可以和持续保持的状态，包括Unconfigured(待配置)、Inactive(待激活)、Active(已激活)、Finalized(已结束)，以及在切换时会产生的中间状态，包括Configuring(配置中)、CleaningUp(清理中)、ShuttingDown(关闭中)、Activating(激活中)、Deactivating(失活中)和ErrorProcessing(错误处理中)。
如果要自定义生命周期节点，需要继承rclcpp::rclcpp::lifecycle::LifecycleNode。
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
using CallbackReturn =
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
class LifecycleNode : public rclcpp_lifecycle::LifecycleNode {
public:
LifecycleNode() : rclcpp_lifecycle::LifecycleNode("my_lifecycle") {
m_period = 1;
m_timer = nullptr;
RCLCPP_INFO(get_logger(), "%s 已创建", get_name());
}
private:
CallbackReturn on_configure(const rclcpp_lifecycle::State& state) override {
m_period = 1;
RCLCPP_INFO(get_logger(), "on_configure()配置周期:%f, 状态: %d-%s",
m_period, state.id(), state.label().c_str());
return CallbackReturn::SUCCESS;
}
CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override {
m_timer = create_wall_timer(
std::chrono::seconds(static_cast<int>(m_period)),
[this]() { RCLCPP_INFO(get_logger(), "定时器输出中..."); });
return CallbackReturn::SUCCESS;
}
CallbackReturn on_deactivate(
const rclcpp_lifecycle::State& state) override {
m_timer->reset();
RCLCPP_INFO(get_logger(), "on_deactivate(): 处理失活指令，停止计时器");
return CallbackReturn::SUCCESS;
}
CallbackReturn on_shutdown(const rclcpp_lifecycle::State& state) override {
m_timer->reset();
RCLCPP_INFO(get_logger(), "on_shutdown()：处理关闭指令");
return CallbackReturn::SUCCESS;
}
private:
rclcpp::TimerBase::SharedPtr m_timer;
double m_period;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<LifecycleNode>();
rclcpp::spin(node->get_node_base_interface());
rclcpp::shutdown();
return 0;
}
继承了生命周期节点要点就是重写生命周期节点状态对应的响应函数，每次设置节点状态时，会对应地调用相关的虚函数。
消息过滤器同步数据
在实际的机器人系统中，往往有着若干的传感器来获取环境信息，为了正确地将不同的传感器数据关联起来进行进一步的数据融合或处理，需要保证它们对应着同一时间的信息，所以需要时间同步机制。ROS2提供了message_filters功能包来实现这一要求。
消息过滤器由订阅者和同步策略两个部分构成：订阅者主要用于配合时间同步器，在使用时为其注册一个回调函数，当收到订阅消息时会调用该回调函数；ROS2提供了多个同步策略，包括严格时间对齐(ExactTime)、大约时间对齐(ApproximateTime)和最新时间对齐(LastestTime)。顾名思义，严格时间对齐要求数据的时间戳完全对齐时才会调用回调函数；大约时间对齐则是放宽了对时间戳的要求，只要两者大致在一个区间内，就会调用相应的回调函数；而最新时间对齐总会针对最新的两条数据调用回调函数，这样可以重复地将频率较慢的数据对齐到频率较快的数据上。
浙公网安备 33010602011771号