编程实现ROS话题通信——手撕 turtlesim 节点

一、实验任务

任务要求如下：使用 C++ 编程实现1个ROS节点，订阅 turtlesim 例程中 turtle_teleop_key 节点发出的消息，并将此消息进行一定的转换后（自行定义，如将数值按适当比例缩小、运动方向变换等），再发布给 turtlesim_node 节点，并将接收的和转换后发布的消息内容实时输出打印到终端，运行时的计算图应如下图所示。

二、实验原理

本人通过学习 talker 和 chatter 的代码，进行相应的修改。

在终端输入

catkin_create_pkgs rospy roscpp std_msgs #创建一个功能包

然后在功能包内的src文件夹下创建一个节点tranform_node，编写transform_node的cpp文件。

首先订阅turtle_teleop_key节点发布的cmd_vel的话题，编写回调函数，在回调函数中，对小海龟的坐标进行一个处理，我将小海龟的x轴和y轴进行了一个调换。在回调函数中，需要定义一下发布的消息类型transform，和cmd_vel保持一致，都是两组，分别是linear和anagular中的float x，float y和float z。

然后编写一个发布者，将transform消息发布出去，定义一个叫做cmd_vel_cov的话题，将这个话题中的transform消息发布出去。

通过ROS重映射，将turtlesim_node节点订阅的话题修改为cmd_vel_cov，就能实现任务的要求了

其中ROS重映射的知识可以参考这个链接：wiki.ros.org/Remapping Arguments

三、实验过程

如果不进行重映射，那么transform和turtlesim_node都是订阅的cmd_vel的话题，没有人订阅transform发布的cmd_vel_cov话题。

终端输入指令如下：

roscore
rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teleop_key
rosrun tranform_node transform_node
rqt_graph

rqt_graph如下所示：

根据ros重映射的知识，在终端输入：

rosrun turtlesim turtlesim_node /turtle1/cmd_vel:=/cmd_vel_cov

将turtlesim_node节点的订阅话题从cmd_vel改为cmd_vel_cov，这样就能让turtlesim_node节点订阅我设计的tranform节点发布的话题了，最终实现，turtle_teleop发布cmd_vel话题，tranform订阅cmd_vel话题，修改数据后再发布一个cmd_vel_cov的话题，turtlesim_node订阅cmd_vel_cov话题。

终端输入完整的指令如下：

roscore
rosrun turtlesim turtlesim_node /turtle1/cmd_vel:=/cmd_vel_cov
rosrun turtlesim turtle_teleop_key
rosrun tranform_node transform_node
rqt_graph

rqt_graph如下所示：

按照任务要求的截图如下：

四、代码实现

本人编写的 cpp 代码如下：

#include"ros/ros.h"//引用ros的头文件
#include"geometry_msgs/Twist.h"//引用小海龟的消息类型

ros::Publisher transform_pub;//定义一个话题的发布者transform_pub

void TransformCallback(const geometry_msgs::Twist msg)//定义一个回调函数，用于海龟/turtle1/cmd_vel话题的订阅
{
    geometry_msgs::Twist transform;//定义一个消息类型，用于处理cmd_vel的转换
    //数据处理：将海龟cmd_vel中数据（线速度和角速度）的x,y,z坐标进行旋转，将x轴变成y轴，y轴变成x轴
    transform.linear.x = msg.linear.y;
    transform.linear.y = msg.linear.x;
    transform.linear.z = msg.linear.z;
    transform.angular.x = msg.angular.y;
    transform.angular.y = msg.angular.x;
    transform.angular.z = msg.angular.z;
    //将处理后的数据发布出去
    transform_pub.publish(transform);
    //通过ROS_INFO和ROS_WARN在终端中输出海龟的运动信息
    ROS_INFO("坐标变换后，小海龟x轴线速度为%f,y轴线速度为%f,z轴线速度为%f,",transform.linear.x,transform.linear.y,transform.linear.z);
    ROS_WARN("坐标变换后，小海龟x轴角速度为%f,y轴角速度为%f,z轴角速度为%f,",transform.angular.x,transform.angular.y,transform.angular.z);

}


int main(int argc, char **argv)
{
    setlocale(LC_ALL,"");//中文字符不乱码
    ros::init(argc, argv, "transform_node");//初始化transform_node节点
    
    ros::NodeHandle n;//定义节点管理者

    transform_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel_cov",10);//发布一个叫做/cmd_vel_cov的话题

    ros::Subscriber turtle_sub = n.subscribe("/turtle1/cmd_vel", 10, TransformCallback);//通过回调函数订阅/turtle1/cmd_vel

    ros::spin();//进行循环
    return 0;
}

在之前创建包时系统自动创建的 CmakeLists.txt 文件末尾添加如下代码即可编译：

#添加可执行程序
add_executable(transform_node src/transform_node.cpp)
#链接库文件
target_link_libraries(transform_node ${catkin_LIBRARIES} )

此外，还可以结合 launch文件 进行节点的集成化运行。

<launch>

<node pkg="turtlesim" type="turtlesim_node" name="turtle1">
<remap from="/turtle1/cmd_vel" to="/cmd_vel_cov"/>
</node>

<node pkg="transform_node" type="transform_node" name="tranform" output="screen">
</node>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop_key" output="screen">
</node>

<param name="scale_linear" value="1" type="double"/>
<param name="scale_anagular" value="1" type="double"/>

</launch>

【对于launch文件的说明】

launch文件将之前实验实现的订阅与发布集成的节点transform_node，与ROS系统自带的turtlesim_node和turtlesim_teleop_key节点，合并写在一个launch启动文件中，最终实现在终端只运行roslaunch [包名] [launch文件名]，对于本人编写的代码， roslaunch transform_node turtlesim.launch即可将全部功能实现，启动了所有节点（包括主节点），实现了话题通讯。

下面是运行launch文件的截图：