需求描述:编码实现乌龟运动控制,让小乌龟做圆周运动。
结果演示:
实现分析:
-
乌龟运动控制实现,关键节点有两个,一个是乌龟运动显示节点 turtlesim_node,另一个是控制节点,二者是订阅发布模式实现通信的,乌龟运动显示节点直接调用即可,运动控制节点之前是使用的turtle_teleop_key通过键盘控制,现在需要自定义控制节点。
-
控制节点自实现时,首先需要了解控制节点与显示节点通信使用的话题与消息,可以使用ros命令结合计算图来获取。
-
了解了话题与消息之后,通过 C++ 或 Python 编写运动控制节点,通过指定的话题,按照一定的逻辑发布消息即可。
实现流程:
-
通过计算图结合ros命令获取话题与消息信息。
-
编码实现运动控制节点。
-
启动 roscore、turtlesim_node 以及自定义的控制节点,查看运行结果。
1.话题与消息获取
准备: 先启动键盘控制乌龟运动案例。
1.1话题获取
查看键盘控制和乌龟GUI之间的话题和消息。
获取话题:/turtle1/cmd_vel
通过计算图查看话题,启动计算图:
rqt_graph
或者通过 rostopic 列出话题:
rostopic list
1.2消息获取
获取消息类型:geometry_msgs/Twist
rostopic type /turtle1/cmd_vel
或
rostopic info /turtle1/cmd_vel
获取消息格式:
rosmsg info geometry_msgs/Twist
响应结果:
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
linear(线速度) 下的xyz分别对应在x、y和z方向上的速度(单位是 m/s);
angular(角速度)下的xyz分别对应x轴上的翻滚、y轴上俯仰和z轴上偏航的速度(单位是rad/s)。
补充资料:
弧度: 单位弧度定义为圆弧长度等于半径时的圆心角。
偏航、翻滚与俯仰
坐标系图解:
偏航:【绕z轴转】
俯仰:【绕y轴转】
翻滚:【绕x轴转】
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
对于本案例的乌龟转圈只有线速度的x方向和角速度的z反向有变化
验证,打印消息
rostopic echo /turtle1/cmd_vel
控制乌龟运动
rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist "linear:
x: 2.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 2.0"
2.实现发布节点
创建功能包需要依赖的功能包: roscpp rospy std_msgs geometry_msgs
实现方案A: C++
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
/*
需求:发布速度消息
话题:/turtle1/cmd_vel
消息:geometry_msgs/Twist
1 包含头文件
2 初始化ros节点
3 创建节点句柄
4 创建发布对象
5 发布逻辑实现
6 回旋函数【可选】 spinOnce()
*/
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2 初始化ros节点
ros::init(argc,argv,"my_control");
// 3 创建节点句柄
ros::NodeHandle nh;
// 4 创建发布对象
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
// 5 发布逻辑实现
ros::Rate rate(10); //设置发布频率
//组织被发布的消息
geometry_msgs::Twist twist;
//线速度
twist.linear.x = 1.0; //浮点类型
twist.linear.y = 0.0 ;
twist.linear.z = 0.0;
//角速度
twist.angular.x = 0.0;
twist.angular. y =0.0;
twist.angular.z = 1.0;
//循环发布
while(ros::ok())
{
pub.publish(twist);
//休眠
rate.sleep();
// 6 回旋函数【可选】 spinOnce()
ros::spinOnce();
}
return 0;
}
配置文件
编译
运行
roscore
rosrun turtlesim turtlesim_node
source ./devel/setup.bash
rosrun plumbing_test test01_pub_twist
总结:
本案例采用的是发布订阅方式。发布方是test01_pub_twist节点。其目的是让海龟实现圆周运动;订阅方是海龟GUI。
首先需要知道海龟GUI和键盘控制节点之间的话题,让test01_pub_twist“替代”键盘控制节点;【利用rqt_graph或者rostopic list查看】
然后在利用话题获取到消息类型【利用rostopic type 话题名 或者rostopic info 话题名】,利用消息类型获取到消息格式【rosmsg info 消息类型】;
最后在消息格式上修改数据,完成相应的功能。