【ROS实操2话题订阅输出乌龟位姿】

需求描述

已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。

实现分析

1.首先，需要启动乌龟显示以及运动控制节点并控制乌龟运动。
2.要通过ROS命令，来获取乌龟位姿发布的话题以及消息。
3.编写订阅节点，订阅并打印乌龟的位姿。

实现流程

1.通过ros命令获取话题与消息信息。
2.编码实现位姿获取节点。
3.启动roscore、turtlesim_node、控制节点以及位姿订阅节点，控制乌龟运动并输出乌龟的位姿。

具体实现

首先
在plumbing_test下面新建launch文件，然后新建文件start_turtle.launch
在这里插入图片描述
粘贴以下代码：

<!--启动乌龟GUI与键盘控制节点-->
<launch>
<!--乌龟GUI -->
<node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen"    />
<!--键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen"   />
</launch>

按住ctrl+shift+b进行编译运行。
没有报错可以进行下一步
点击加号新建终端
在这里插入图片描述
输入：

source ./devel/setup.bash
roslaunch plumbing_test start_turtle.launch

在这里插入图片描述
就会出现小乌龟：

按住键盘上下左右即可控制乌龟运动。
注：如果运行结果报错

ERROR: cannot launch node of type [turtlesim/turtlesim _node]: Cannot locate node of type [turtlesim _node] in package [turtlesim]. Make sure file exists in package path and permission is set to executable (chmod +x)
ERROR: cannot launch node of type [turtlesim/turtlg_teleop_key]: Cannot locate node of type [turtlg_teleop_key] in package [turtlesim]. Make sure file exists in package path and permission is set to executable (chmod +x)
则很大程度上是因为节点部分出现错误，重新敲击以下代码即可，记住/>与前面的“中间有空格。

<node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen"    />
<node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen"   />

获取话题，获取话题消息格式

新建终端：

rostopic list

在这里插入图片描述
获取话题:/turtle1/pose
rostopic list
获取消息类型:turtlesim/Pose
rostopic type /turtle1/pose
获取消息格式:
rosmsg info turtlelim/Pose
响应结果:
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

先在终端中输出乌龟位姿：

rostopic echo turtle1/pose

在这里插入图片描述

实现订阅节点

创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim
如果原背包里有turtlesim，则不需要添加，如果没有，则需要添加配置文件。
在package.xml添加
<build_depend>turtlesim</build_depend>
<exec_depend>turtlesim</exec_depend>
在这里插入图片描述
然后在cmakelist.txt里面find_package加入turtlesim.

在src下面新建文件test02_sub_pose.cpp
粘贴下面代码：

#include "ros/ros.h"
#include"turtlesim/Pose.h"
/*需求:订阅乌龟的位姿信息，并输出到控制台
1.包含头文件
2.初始化ROS节点
3.创建节点句柄
4.创建订阅对象
5.处理订阅到的数据
6.spin().
*/
void doPose(const turtlesim::Pose::ConstPtr &pose)
{
    
    
    ROS_INFO("乌龟的位姿信息:坐标(%.2f,%.2f),朝向(%.2f),线速度:%.2f ,角速度:%.2f",
pose->x, pose->y,pose->theta,pose->linear_velocity , pose->angular_velocity);

}
int main(int argc,char *argv[ ])
{
    
    
setlocale(LC_ALL,"" );
//2.初始化ROS节点;
ros::init(argc ,argv , "sub_pose" );
//3.创建节点句柄;
ros::NodeHandle nh;
///4.创建订阅对象;
ros::Subscriber sub = nh. subscribe( "/turtle1/Pose" ,100, doPose);
//5.处理订阅到的数据(回调函数);
// 6.spin() .
ros::spin();
return 0;
}

配置cmakelist文件
在这里插入图片描述

编译无错误，启动一个bash文件：

source ./devel/setup.bash
roslaunch plumbing_test start_turtle.launch

然后新建终端，启动

source ./devel/setup.bash
roslaunch plumbing_test test02_sub_pose

在这里插入图片描述
运行结果：