ROS提高篇

• 只有干货，没有废话
• 书山有路勤为径，学海无涯苦作舟
• 冲就对了

1. 进阶操作

1.1 工作空间及功能包

• 创建工作区间并初始化

mkdir -p ~/work_space/src # 创建文件目录，-p：若无该目录就创建
cd ~/work_space/src # 切换到src目录下
catkin_init_workspace # 初始化工作空间

• 编译空间

cd .. # 返回上一目录
catkin_make # 编译

• 编译单独功能包

catkin_make -DCATKIN_WHITELIST_PACKAGES="package_name1;package_name2"

注意在使用了以下命令后，想要重新编译整个工作空间，并不是使用catkin_make，而是需要使用上述指令且包名留空白。

• 指定编译线程

catkin_make -j4 -l4 # j代表并行运行任务数，l代表最大负载平均值，防止过载。

• 添加环境变量

source devel/setup.bash

• 查看环境变量

echo $ROS_PACKAGE_PATH

• CMakeLists.txt：编译规则文件。
• package.xml：功能包描述文件。

---

1.2 launch文件详解

• 标签总览

<launch> ... </launch>
<node>			# 启动节点     
<include>		# 嵌套
<remap>       	# 话题重命名
<param>       	# 全局参数
<rosparam>		# 加载参数文件
<group>       	# 节点分组
<arg>           # 局部参数
<machine>		# 远程标签
<env>         	# 环境变量
<test>         	# 测试节点

---

1.2.1 node标签

rosrun <package_name> <executable|python>
<node pkg="package_name" type="executable|python" name="node_name" args="$(arg map_file)" output="screen" respawn="true" required="false" ns="">
</node>

结合上述代码：

• 含义：启动包中的某可执行文件，并重命名该节点名为node_name（在对节点进行初始化的时候也存在节点命名，但launch文件会覆盖该命名）。
• 例子：

rosrun turn_on_wheeltec_robot wheeltec_robot_node
<node pkg="turn_on_wheeltec_robot" type="wheeltec_robot_node" name="wheeltec_robot"/>

属性	含义
pkg	功能包
type	可执行文件
name	重命名节点名
args	传递参数
output	日志输出
respawn	“true”，节点失效则重启，默认false
required	“true”，节点失效则关闭整个launch文件，默认false
ns	在命名空间中运行此节点

---

1.2.2 include标签

<include file="$(find turn_on_wheeltec_robot)/launch/include/dwa_local_planner.launch"/>
<include file="$(find turn_on_wheeltec_robot)/launch/include/base_serial.launch" unless="$(arg repeat)">
  <arg name="odom_frame_id"  value="$(arg odom_frame_id)"/>
  <arg name="use_FDI_IMU_GNSS" value="$(arg use_FDI_IMU_GNSS)"/>
</include>

结合上述代码：

• **$(find turn_on_wheeltec_robot)：**索引文件，作用：**打开该目录下的dwa_local_planner.launch文件，**一行结束。
• include可以包含其它内容。

---

1.2.3 arg、param、rosparam

• arg：跟node中的args不同，在launch文件中声明一个参数。
• **param：**向参数服务器中传递一个参数。
• rosparam：从.yaml文件中加载大量参数。

---

1.2.4 参数服务器与param

在launch文件中加载参数到参数服务器中：

<!--打开节点wheeltec_robot，初始化串口等操作-->
<node pkg="wheeltec_arm_pick" type="wheeltec_$(arg moveit_num)" name="wheeltec_$(arg moveit_num)" output="screen" respawn="true">
	<param name="joint_num"          type="int"    value="$(arg joint_num)"/>
	<param name="usart_port_name"    type="string" value="/dev/wheeltec_controller"/>  
	<param name="serial_baud_rate"   type="int"    value="115200"/>
	<param name="robot_frame_id"     type="string" value="base_footprint"/>
	<param name="smoother_cmd_vel"   type="string" value="smoother_cmd_vel"/> 
	<param name="product_number"     type="int"    value="0"/>
</node>

• 上述代码：**将参数名为“name”以及数据类型为“type”传递参数值“value”，**加载五个参数到参数服务器中。

那么该怎样在C++使用呢：

// 将类成员参数信息注册到参数服务器上
ros::NodeHandle private_nh("~"); // 创建私有节点句柄
// 入口参数：参数服务器名称 对应变量 初始值
private_nh.param<std::string>("usart_port_name", usart_port_name, "/dev/wheeltec_controller"); // 固定串口号
private_nh.param<int>("serial_baud_rate", serial_baud_rate, 115200); // 波特率
private_nh.param<std::string>("odom_frame_id", odom_frame_id, "odom_combined"); // 里程计话题对应父TF坐标
private_nh.param<std::string>("robot_frame_id", robot_frame_id, "base_footprint"); // 里程计话题对应子TF坐标
private_nh.param<std::string>("gyro_frame_id", gyro_frame_id, "gyro_link"); // IMU对应TF坐标

其中，初始值是当在参数服务器中找不到对应参数时，将初始值赋给变量。

---

1.2.5 arg与param

这种方法类似与局部变量赋值给全局变量：

<launch>
    <include file="$(find turn_on_wheeltec_robot)/launch/wheeltec_camera.launch" />
    <arg name="markerId"        default="582"/>
    <arg name="markerSize"      default="0.034"/>
    <arg name="car_mode"  default="mini_mec"  doc="opt: mini_akm,senior_akm,top_akm_bs,top_akm_dl"/>
    <node pkg="aruco_ros" type="single" name="aruco_single">
        <param name="marker_size"        value="$(arg markerSize)"/>
        <param name="marker_id"          value="$(arg markerId)"/>
    </node>

</launch>

• 定义arg属性，arg只在该launch文件中生效。
• **$(arg markerSize)：**索引arg名称为markerSize参数，将其value值传给它。
• **doc：**字典，供用户查看可选值。

---

1.2.6 group标签

group标签的作用有两种：

• 若干个节点划分进同一命名空间：
  • 运行：rosrun turn_on_wheeltec_robot base_serial.launch，可以看见话题和节点加上了别名。

<group ns="robot1">
    <node pkg="" type="" name="" output="screen" respawn="true">
        <param name="joint_num"          type="int"    value="$(arg joint_num)"/>
        <param name="usart_port_name"    type="string" value="/dev/wheeltec_controller"/>  
        <param name="serial_baud_rate"   type="int"    value="115200"/>
        <param name="robot_frame_id"     type="string" value="base_footprint"/>
        <param name="smoother_cmd_vel"   type="string" value="smoother_cmd_vel"/> 
        <param name="product_number"   type="int"    value="0"/>
    </node>
</group>

注意，ns不与group结合使用也行，可单独使用，功能一样将节点划分到同一命名空间中，适合跨launch文件。

• 条件判断执行：
  • 1：条件成立执行，关键词“eval”，代表等式判别式。
  • 2：条件成立不执行，关键词“eval”，代表等式判别式。
  • 3：arg参数对应值为假，不执行。

<group if="$(eval (a=='hello' or b=='no heelo') and c==true)" />
<group unless="$(eval (a=='hello' or b=='no heelo') and c==true)"/>
<group if="$(arg lidar_is_cx)">

注意，可直接 if = "1"。

注意，group可嵌套node标签，但node标签不可以嵌套group。

---

1.2.7 remap标签

<remap from = "/source_topic"  to = "/rename_topic" />

• remap分话题输入跟输出：
  • **输出：**节点发布话题，更改话题名称再发布。
  • **输入：**节点订阅话题，先重定向话题名称再订阅。

注意，无论是输入还是输出，本身话题都会消失。

---

1.3 节点句柄

• **节点：**一个单独的可执行文件，ROS系统就是由多个节点组成的。
• **节点句柄：**由ROS系统提供的对象，主要用于发布和订阅主题、调用服务、获取参数等。

1.3.1 创建节点句柄

创建节点句柄最简单的方式：

ros::NodeHandle nh;

值得注意的是，在创建节点句柄之后有：

• 创造节点句柄的时候，若内部节点尚未启动，则节点句柄会先启动节点，同时同一节点下的句柄全被销毁，则节点关闭。

1.3.2 命名空间

同时，在创建句柄的时候，还允许自定义命名空间：

ros::NodeHandle nh("namespace");

值得注意的是，若在launch文件中指定了node标签的ns属性，则nh属于该命名空间，若未指定，则属于namespace。

还可以创建多个节点句柄，同时为某个句柄指定父节点句柄：

ros::NodeHandle nh1("ns1");
ros::NodeHandle nh2(nh1, "ns2");

1.3.3 全局命名

ros::NodeHandle nh("/global_namespace"); // 不受外部命名空间的影响

注意：在ROS的命名中，加或不加/是存在区别的。 若加上/，说明这是一个全局命名，则无法再被放入一个命名空间当中；若不加/，则可以通过如launch中设置ns，使得该命名被放入到该命名空间当中。比如，我定义了两个发布器，发布话题分别为/cmd_vel与cmd_vel，此外若外部还存在一个命名空间为dwa_planner，则它们的命名分别变为：

• /cmd_vel：/cmd_vel
• cmd_vel： /dwa_planner/cmd_vel

1.3.4 私有命名

// 1.～+name
ros::NodeHandle nh("~my_private_ns");
ros::Subscriber sub = nh.subscribe("my_private_topic", ...);  // <node_namespace>/<node_name>/my_private_ns/my_private_topic

// 2.~
ros::NodeHandle nh("~");
ros::Subscriber sub = nh.subscribe("my_private_topic", ...);  // <node_namespace>/<node_name>/my_private_topic

• 私有命名与上述提到的命名都不同，它会把原本命名放入到<node_name>下。因此，上面订阅器的话题的命名如下：
  • <node_namespace>/<node_name>/my_private_ns/my_private_topic

---

1.4 常用命令

1.4.1 rosnode

rosnode ping    	测试到节点的连接状态
rosnode list    	列出活动节点
rosnode info   	 	打印节点信息
rosnode machine    	列出指定设备上节点
rosnode kill    	杀死某个节点
rosnode cleanup    	清除不可连接的节点

1.4.2 rostopic

rostopic bw     	显示主题使用的带宽
rostopic delay  	显示带有 header 的主题延迟
rostopic echo   	打印消息到屏幕
rostopic find   	根据类型查找主题
rostopic hz     	显示主题的发布频率
rostopic info   	显示主题相关信息
rostopic list   	显示所有活动状态下的主题
rostopic pub    	将数据发布到主题
rostopic type   	打印主题类型

1.4.3 rosmsg

rosmsg show    		显示消息描述
rosmsg info    		显示消息信息
rosmsg list    		列出所有消息
rosmsg md5    		显示 md5 加密后的消息
rosmsg package    	显示某个功能包下的所有消息
rosmsg packages    	列出包含消息的功能包

1.4.4 rosservice

rosservice args 	打印服务参数
rosservice call    	使用提供的参数调用服务
rosservice find    	按照服务类型查找服务
rosservice info    	打印有关服务的信息
rosservice list    	列出所有活动的服务
rosservice type    	打印服务类型
rosservice uri    	打印服务的 ROSRPC uri

1.4.5 rossrv

rossrv show    		显示服务消息详情
rossrv info    		显示服务消息相关信息
rossrv list    		列出所有服务信息
rossrv md5    		显示 md5 加密后的服务消息
rossrv package    	显示某个包下所有服务消息
rossrv packages    	显示包含服务消息的所有包

1.4.6 rosparam

rosparam set    	设置参数
rosparam get    	获取参数
rosparam load   	从外部文件加载参数
rosparam dump   	将参数写出到外部文件
rosparam delete    	删除参数
rosparam list   	列出所有参数

1.4.7 rosbag

rosbag record topic1 topic2 	记录话题信息
rosbag play -r 1.5 xxx.bag		回放数据包信息

---

2. 坐标系变换

2.1 geometry_msgs

该功能包定义了一系列消息类型，涵盖了位置、方向、速度等几何概念。

2.1.1 基本几何类型

• Point: 表示三维空间中的一个点，包含 x, y, z 坐标。
• Vector3: 类似于 Point，通常用来表示方向和速度。
• Quaternion: 表示空间中的旋转，包含 x, y, z, w 参数（四元数）。
• Pose: 结合了 Point 和 Quaternion，用来表示空间中的一个位置和方向。
• Twist: 描述线性和角速度，包含 Vector3 类型的 linear 和 angular 成分。

2.1.2 带时间戳的类型

带时间戳的消息类型，一般包含：基本几何类型、Header、时间戳以及坐标帧消息，用以表示所处的坐标信息。

我们可以在命令行键入：

rosmsg info geometry_msgs/消息类型

例如：
rosmsg info geometry_msgs/PointStamped
结果：
std_msgs/Header header                    # 头
  uint32 seq                              #|-- 序号
  time stamp                              #|-- 时间戳
  string frame_id                         #|-- 所属坐标系的 id
  
geometry_msgs/Point point                 # 点坐标
  float64 x                               #|-- x y z 坐标
  float64 y
  float64 z

同时，我们还可以查看空间变换对应的消息类型：rosmsg info geometry_msgs/TransformStamped。

结果：

std_msgs/Header header                    # 头信息
  uint32 seq                              #|-- 序列号
  time stamp                              #|-- 时间戳
  string frame_id                         #|-- 坐标 ID
string child_frame_id                     # 子坐标系的 id
geometry_msgs/Transform transform         # 坐标信息
  geometry_msgs/Vector3 translation       # 偏移量
    float64 x                             #|-- X 方向的偏移量
    float64 y                             #|-- Y 方向的偏移量
    float64 z                             #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation       # 四元数
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w

2.2 坐标转换

2.2.1 静态坐标系变换

在机器人控制中，经常会进行不同坐标系之间的变换，而静态指的是：坐标系之间的相对位置保持不变。

例如我们现在有一辆小车，上面搭载一个多线雷达，雷达检测到某处存在障碍物，则障碍物在主体坐标中的位置信息为？

实现逻辑：

• 设计发布方，将雷达检测到的障碍物信息及坐标系信息进行发布。
• 设计订阅者，收到上述信息，将其转换为主体坐标系下的位置信息。
• 使用依赖：tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs。

发布方：

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_brocast");
    tf2_ros::StaticTransformBroadcaster broadcaster; // 创建静态坐标转换广播
    geometry_msgs::TransformStamped ts; // 创建坐标信息
    ts.header.seq = 100; // 设置消息头
    ts.header.stamp = ros::Time::now(); // 获取时间戳
    ts.header.frame_id = "base_link"; // 设置坐标系，主体
    ts.child_frame_id = "laser"; // 设置子坐标系，雷达
   
    ts.transform.translation.x = 0.2; // 设置子坐标系相对父坐标系的偏移量
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.5;
    
    tf2::Quaternion qtn; // 设置四元数，并将欧拉角数据转为四元数
    qtn.setRPY(0,0,0);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();

    broadcaster.sendTransform(ts); // 发布坐标系广播信息
    ros::spin(); // 回旋函数  
    return 0;
}

订阅方：

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tf_sub");
    ros::NodeHandle nh;
    tf2_ros::Buffer buffer; // 创建TF订阅节点
    tf2_ros::TransformListener listener(buffer);
    ros::Rate r(1); // 监听速率
    while (ros::ok())
    {
        geometry_msgs::PointStamped point_laser; // 生成雷达坐标系点变量
        point_laser.header.frame_id = "laser";
        point_laser.header.stamp = ros::Time::now();
        point_laser.point.x = 1;
        point_laser.point.y = 2;
        point_laser.point.z = 7.3;

        // 进行转换，使用try防止数据接收延迟导致转换失败
        try
        {
            geometry_msgs::PointStamped point_base; // 新建点，用于保存结果
            point_base = buffer.transform(point_laser,"base_link"); // 将获取到的TF信息，转换点point_laser为主体坐标系中
            ROS_INFO("转换OK.\n");
        }
        catch(const std::exception& e)
        {
            ROS_INFO("程序异常.....");
        }
        r.sleep();  
        ros::spinOnce(); // 回旋函数，将回调队列中的所有函数执行完才执行下面的语句。
    }
    return 0;
}

---

2.2.2 动态坐标系的转换

在机器人控制中，不单单只有静态坐标系，还有动态坐标系，例如：世界坐标系与行动的机器人之间的坐标系关系。

假设：ROS乌龟案例中，以网格左下角为世界坐标系，乌龟自身为子坐标系，将乌龟的绝对运动信息转换为相当坐标系下的信息。

实现逻辑：

• 设计发布方，负责发布乌龟在世界坐标系下的坐标、偏移量、线速度以及角速度。
• 设计订阅者，进行转换，输出相对坐标信息。
• 使用依赖：tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim。

发布者：

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose){
    static tf2_ros::TransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped tfs; // 创建坐标消息
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = "turtle1"; // 子坐标系
    
    tfs.transform.translation.x = pose->x; // 偏移量，其实就是自身坐标信息
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0

    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    broadcaster.sendTransform(tfs); // 发布坐标消息
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_tf_pub");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose); // 获取乌龟自身信息
    ros::spin(); // 回旋函数，实现广播
    return 0;
}

订阅者：

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_tf_sub");
    ros::NodeHandle nh;
    tf2_ros::Buffer buffer; // 创建订阅节点
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "turtle1";
        point_laser.header.stamp = ros::Time();
        point_laser.point.x = 1;
        point_laser.point.y = 1;
        point_laser.point.z = 0;

        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"world");
            ROS_INFO("转换OK.\n");

        }
        catch(const std::exception& e)
        {
            ROS_INFO("程序异常:%s",e.what());
        }
        r.sleep();  
        ros::spinOnce();
    }
    return 0;
}

可以看出：静态坐标系与动态坐标系之间的转换大致相同，区别在于：偏移量是否为变值。

---

2.2.3 多坐标系的转换

通过我们的学习，已经掌握了简单的两个坐标系之间的转换了。然鹅，实际中往往是多个坐标系之间的信息转换。

例如：在一世界坐标系下，存在机器人手关节、肘关节两个子坐标系，现在我们想要得到手关节在肘关节下的坐标信息。

假设：世界坐标系为world，肘关节坐标系为sub1，手关节坐标系为sub2。

实现逻辑：

• 设计发布方，发布sub1、sub2与世界坐标系之间的关系，由于实现逻辑一样，可采用launch中的node传参实现。

• 设计订阅方，订阅两个子坐标系的转换信息，再实现子坐标系之间的位置信息转换。

• 实现依赖：tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim。

发布方：

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="sub1" args="0.2 0.8 0.3 0 0 0 /world /sub1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="sub2" args="0.5 0 0 0 0 0 /world /sub2" output="screen" />
</launch>

值得注意的是，传参有两种传参方式：

1、传姿态角：x y z yaw pitch roll frame_id child_frame_id period_in_ms

2、传四元数：x y z qx qy qz qw frame_id child_frame_id period_in_ms

订阅方：

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/PointStamped.h"

int main(int argc, char *argv[])
{   setlocale(LC_ALL,"");
    ros::init(argc,argv,"sub_frames");
    ros::NodeHandle nh;
    tf2_ros::Buffer buffer; 
    tf2_ros::TransformListener listener(buffer);
    ros::Rate r(1);
    while (ros::ok())
    {
        try
        {
            // 将sub1（源坐标系）转换到sub2（目标坐标系）中，返回值为具体的变换信息transform。
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("sub2","sub1",ros::Time(0));
            
            ROS_INFO("sub1 相对于 sub2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());
            ROS_INFO("sub1 相对于 sub2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());
            ROS_INFO("sub1 相对于 sub2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",
                    tfs.transform.translation.x,
                    tfs.transform.translation.y,
                    tfs.transform.translation.z
                    );

            // 坐标点解析
            geometry_msgs::PointStamped ps;
            ps.header.frame_id = "sub1";
            ps.header.stamp = ros::Time::now();
            ps.point.x = 1.0;
            ps.point.y = 2.0;
            ps.point.z = 3.0;

            geometry_msgs::PointStamped psAtSub2;
            psAtSub2 = buffer.transform(ps,"sub2");
            ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f",
                    psAtSub2.point.x,
                    psAtSub2.point.y,
                    psAtSub2.point.z
                    );
        }
        catch(const std::exception& e)
        {
            ROS_INFO("异常信息:%s",e.what());
        }
        r.sleep();
        ros::spinOnce();
    }
    return 0;
}

---

2.2.3 查看TF转换关系

• 安装依赖：sudo apt install ros-noetic-tf2-tools。
• 启动坐标广播。
• 生成pdf关系图：rosrun tf2_tools view_frames.py。
• 查看：evince frames.pdf。

---

2.2.4 TF转换实操

现在进入实操环节，

假设：在乌龟控制案例中，有一乌龟A处于中央，另一乌龟B处于左下角，现在控制A乌龟运动，B乌龟实现跟随。

实现逻辑：

• 订阅A乌龟跟B乌龟相对于世界坐标系的位置信息。
• 将A的位置信息转换到B坐标系中。
• 计算B的速度，并控制运动。

创建第二个小乌龟：

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"create_turtle");
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn"); // 创建客户端

    ros::service::waitForService("/spawn"); // 等待服务端启动
    turtlesim::Spawn spawn; // 话题实例
    spawn.request.name = "turtle2";
    spawn.request.x = 1.0;
    spawn.request.y = 2.0;
    spawn.request.theta = 3.12415926;
    bool flag = client.call(spawn); // 请求
    if (flag)
    {
        ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("乌龟2创建失败!");
    }
    ros::spin(); // 回旋函数
    return 0;
}

发布方：

/*  
    该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息

    注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
          其他的话题名称和实现逻辑都是一样的，
          所以我们可以将所需的命名空间通过 args 动态传入
*/

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"
std::string turtle_name; // 保存乌龟名称


void doPose(const turtlesim::Pose::ConstPtr& pose){
    static tf2_ros::TransformBroadcaster broadcaster; // 创建静态TF广播
    geometry_msgs::TransformStamped tfs; // 将pose消息转为TransformStamped
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = turtle_name;
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0;
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    broadcaster.sendTransform(tfs); // 发布

} 

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"pub_tf");
    
    // 解析传入参数
    if (argc != 2)
    {
        ROS_ERROR("请传入正确的参数");
    } else {
        turtle_name = argv[1];
        ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
    }

    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",1000,doPose); // 订阅乌龟的pose信息，并调用回调函数
    ros::spin();
    return 0;
}

订阅方：

/*  
    订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
    将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布
*/

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"sub_TF");
    ros::NodeHandle nh;
    tf2_ros::Buffer buffer; // 创建订阅节点
    tf2_ros::TransformListener listener(buffer);

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",1000); // 创建乌龟B的发布对象

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            // 先获取 turtle1 相对 turtle2 的坐标信息
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));

            // 根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
            twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);

            pub.publish(twist); // 发布速度信息
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示:%s",e.what());
        }

        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

启动文件：

<launch>
    <!-- 启动乌龟节点与键盘控制节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>
    <!-- 启动创建第二只乌龟的节点 -->
    <node pkg="demo_tf2_test" type="Test01_Create_Turtle2" name="turtle2" output="screen" />
    <!-- 启动两个坐标发布节点 -->
    <node pkg="demo_tf2_test" type="Test02_TF2_Caster" name="caster1" output="screen" args="turtle1" />
    <node pkg="demo_tf2_test" type="Test02_TF2_Caster" name="caster2" output="screen" args="turtle2" />
    <!-- 启动坐标转换节点 -->
    <node pkg="demo_tf2_test" type="Test03_TF2_Listener" name="listener" output="screen" />
</launch>

注意，应该注意多使用tf2，而非tf，tf2更加高效便捷。

---

3. action通讯

action 是一种类似于服务通信的实现，其实现模型也包含请求和响应，但是不同的是，在请求和响应的过程中，服务端还可以连续的反馈当前任务进度，客户端可以接收连续反馈并且还可以取消任务。

action结构图解:

action通信接口图解:

• goal:目标任务;
• cacel:取消任务;
• status:服务端状态;
• result:最终执行结果(只会发布一次);
• feedback:连续反馈(可以发布多次)。

作用

一般适用于耗时的请求响应场景,用以获取连续的状态反馈。

案例

创建两个ROS 节点，服务器和客户端，客户端可以向服务器发送目标数据N(一个整型数据)服务器会计算 1 到 N 之间所有整数的和,这是一个循环累加的过程，返回给客户端，这是基于请求响应模式的，又已知服务器从接收到请求到产生响应是一个耗时操作，每累加一次耗时0.1s，为了良好的用户体验，需要服务器在计算过程中，每累加一次，就给客户端响应一次百分比格式的执行进度，使用 action实现。

另请参考:

• http://wiki.ros.org/actionlib
• http://wiki.ros.org/actionlib_tutorials/Tutorials

---

3.1 自定义action文件

1.定义action文件

首先新建功能包，并导入依赖: roscpp rospy std_msgs actionlib actionlib_msgs；

然后功能包下新建 action 目录，新增 Xxx.action(比如:AddInts.action)。

action 文件内容组成分为三部分：请求目标值、最终响应结果、连续反馈，三者之间使用---分割示例内容如下:

#目标值
int32 num
---
#最终结果
int32 result
---
#连续反馈
float64 progress_bar

2.编辑配置文件

CMakeLists.txt

find_package
(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  actionlib
  actionlib_msgs
)

add_action_files(
  FILES
  AddInts.action
)

generate_messages(
  DEPENDENCIES
  std_msgs
  actionlib_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES demo04_action
 CATKIN_DEPENDS roscpp rospy std_msgs actionlib actionlib_msgs
#  DEPENDS system_lib
)

3.编译

编译后会生成一些中间文件。

msg文件(…/工作空间/devel/share/包名/msg/xxx.msg):

C++ 调用的文件(…/工作空间/devel/include/包名/xxx.h):

---

3.2 action通讯实现（C++）

需求:

创建两个ROS 节点，服务器和客户端，客户端可以向服务器发送目标数据N(一个整型数据)服务器会计算 1 到 N 之间所有整数的和,这是一个循环累加的过程，返回给客户端，这是基于请求响应模式的，又已知服务器从接收到请求到产生响应是一个耗时操作，每累加一次耗时0.1s，为了良好的用户体验，需要服务器在计算过程中，每累加一次，就给客户端响应一次百分比格式的执行进度，使用 action实现。

流程:

1. 编写action服务端实现；
2. 编写action客户端实现；
3. 编辑CMakeLists.txt；
4. 编译并执行。

1. 服务端

#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include "demo01_action/AddIntsAction.h"
/*  
    需求:
        创建两个ROS节点，服务器和客户端，
        客户端可以向服务器发送目标数据N（一个整型数据）
        服务器会计算1到N之间所有整数的和，这是一个循环累加的过程，返回给客户端，
        这是基于请求响应模式的，
        又已知服务器从接收到请求到产生响应是一个耗时操作，每累加一次耗时0.1s，
        为了良好的用户体验，需要服务器在计算过程中，
        每累加一次，就给客户端响应一次百分比格式的执行进度，使用action实现。

    流程:
        1.包含头文件;
        2.初始化ROS节点;
        3.创建NodeHandle;
        4.创建action服务对象;
        5.处理请求,产生反馈与响应;
        6.spin().

*/

typedef actionlib::SimpleActionServer<demo01_action::AddIntsAction> Server;

void cb(const demo01_action::AddIntsGoalConstPtr &goal,Server* server){
    //获取目标值
    int num = goal->num;
    ROS_INFO("目标值:%d",num);
    //累加并响应连续反馈
    int result = 0;
    demo01_action::AddIntsFeedback feedback;//连续反馈
    ros::Rate rate(10);//通过频率设置休眠时间
    for (int i = 1; i <= num; i++)
    {
        result += i;
        //组织连续数据并发布
        feedback.progress_bar = i / (double)num;
        server->publishFeedback(feedback);
        rate.sleep();
    }
    //设置最终结果
    demo01_action::AddIntsResult r;
    r.result = result;
    server->setSucceeded(r);
    ROS_INFO("最终结果:%d",r.result);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ROS_INFO("action服务端实现");
    // 2.初始化ROS节点;
    ros::init(argc,argv,"AddInts_server");
    // 3.创建NodeHandle;
    ros::NodeHandle nh;
    // 4.创建action服务对象;
    /*SimpleActionServer(ros::NodeHandle n, 
                        std::string name, 
                        boost::function<void (const demo01_action::AddIntsGoalConstPtr &)> execute_callback, 
                        bool auto_start)
    */
    // actionlib::SimpleActionServer<demo01_action::AddIntsAction> server(....);
    Server server(nh,"addInts",boost::bind(&cb,_1,&server),false);
    server.start();
    // 5.处理请求,产生反馈与响应;

    // 6.spin().   
    ros::spin();
    return 0;
}

PS:

可以先配置CMakeLists.tx文件并启动上述action服务端，然后通过 rostopic 查看话题，向action相关话题发送消息，或订阅action相关话题的消息。

2. 客户端

#include "ros/ros.h"
#include "actionlib/client/simple_action_client.h"
#include "demo01_action/AddIntsAction.h"

/*  
    需求:
        创建两个ROS节点，服务器和客户端，
        客户端可以向服务器发送目标数据N（一个整型数据）
        服务器会计算1到N之间所有整数的和，这是一个循环累加的过程，返回给客户端，
        这是基于请求响应模式的，
        又已知服务器从接收到请求到产生响应是一个耗时操作，每累加一次耗时0.1s，
        为了良好的用户体验，需要服务器在计算过程中，
        每累加一次，就给客户端响应一次百分比格式的执行进度，使用action实现。

    流程:
        1.包含头文件;
        2.初始化ROS节点;
        3.创建NodeHandle;
        4.创建action客户端对象;
        5.发送目标，处理反馈以及最终结果;
        6.spin().

*/
typedef actionlib::SimpleActionClient<demo01_action::AddIntsAction> Client;


//处理最终结果
void done_cb(const actionlib::SimpleClientGoalState &state, const demo01_action::AddIntsResultConstPtr &result){
    if (state.state_ == state.SUCCEEDED)
    {
        ROS_INFO("最终结果:%d",result->result);
    } else {
        ROS_INFO("任务失败！");
    }

}
//服务已经激活
void active_cb(){
    ROS_INFO("服务已经被激活....");
}
//处理连续反馈
void  feedback_cb(const demo01_action::AddIntsFeedbackConstPtr &feedback){
    ROS_INFO("当前进度:%.2f",feedback->progress_bar);
}


int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化ROS节点;
    ros::init(argc,argv,"AddInts_client");
    // 3.创建NodeHandle;
    ros::NodeHandle nh;
    // 4.创建action客户端对象;
    // SimpleActionClient(ros::NodeHandle & n, const std::string & name, bool spin_thread = true)
    // actionlib::SimpleActionClient<demo01_action::AddIntsAction> client(nh,"addInts");
    Client client(nh,"addInts",true);
    //等待服务启动
    client.waitForServer();
    // 5.发送目标，处理反馈以及最终结果;
    /*  
        void sendGoal(const demo01_action::AddIntsGoal &goal, 
            boost::function<void (const actionlib::SimpleClientGoalState &state, const demo01_action::AddIntsResultConstPtr &result)> done_cb, 
            boost::function<void ()> active_cb, 
            boost::function<void (const demo01_action::AddIntsFeedbackConstPtr &feedback)> feedback_cb)

    */
    demo01_action::AddIntsGoal goal;
    goal.num = 10;

    client.sendGoal(goal,&done_cb,&active_cb,&feedback_cb);
    // 6.spin().
    ros::spin();
    return 0;
}

**PS:**等待服务启动，只可以使用client.waitForServer();，之前服务中等待启动的另一种方式ros::service::waitForService("addInts");不适用

3. 编译配置文件

add_executable(action01_server src/action01_server.cpp)
add_executable(action02_client src/action02_client.cpp)
...

add_dependencies(action01_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(action02_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
...

target_link_libraries(action01_server
  ${catkin_LIBRARIES}
)
target_link_libraries(action02_client
  ${catkin_LIBRARIES}
)
Copy

4. 执行

首先启动 roscore，然后分别启动action服务端与action客户端，最终运行结果与案例类似。