需求描述: 已知turtlesim中的乌龟显示节点,会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向),要求控制乌龟运动,并时时打印当前乌龟的位姿。
结果演示:
实现分析:
-
首先,需要启动乌龟显示以及运动控制节点并控制乌龟运动。
-
要通过ROS命令,来获取乌龟位姿发布的话题以及消息。
-
编写订阅节点,订阅并打印乌龟的位姿。
实现流程:
-
通过ros命令获取话题与消息信息。
-
编码实现位姿获取节点。
-
启动 roscore、turtlesim_node 、控制节点以及位姿订阅节点,控制乌龟运动并输出乌龟的位姿。
扫描二维码关注公众号,回复: 12933888 查看本文章
编写launch文件【启动乌龟GUI和键盘控制节点】
<!-- 启动乌龟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>
source ./devel/setup.bash
roslaunch plumbing_test start_turtle.launch
1.话题与消息获取
获取话题:/turtle1/pose
rostopic list
获取消息类型:turtlesim/Pose
rostopic type /turtle1/pose
或
rostopic info /turtle1/pose
获取消息格式:
rosmsg info turtlesim/Pose
响应结果:
float32 x
float32 y
float32 theta #平行x轴,左转加【最大是pi】,右转减【最小值是-pi】
float32 linear_velocity
float32 angular_velocity
先以命令的方式获取乌龟的位姿
rostopic echo /turtle1/pose
2.实现订阅节点
创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim
实现方案A: C++
配置文件
package.xml
CMakeLists.txt
#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,"");
ros::init(argc,argv,"sub_pose");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
ros::spin();
return 0;
}
编译
启动launch文件中封装的乌龟GUI和键盘控制节点
source ./devel/setup.bash
roslaunch plumbing_test start_turtle.launch
运行节点
source ./devel/setup.bash
rosrun plumbing_test test02_sub_pose
总结:
本案例采用的是发布/订阅方式,发布方乌龟显示节点,发布乌龟的位姿,订阅方是test02_sub_pose。
首先查看话题【rostopic list 】,通过话题查看消息类型【rostopic type 话题名 或rostopic info 话题名】,通过消息类型查看消息格式【rosmsg info 消息类型】