重要参考：
课程链接:https://www.bilibili.com/video/BV1Ci4y1L7ZZ
讲义链接:Introduction · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程
2.5.1 实操01_话题发布
需求描述：编码实现乌龟运动控制，让小乌龟做圆周运动。
结果演示：
实现分析：
- 乌龟运动控制实现，关键节点有两个，一个是乌龟运动显示节点 turtlesim_node，另一个是控制节点，二者是订阅发布模式实现通信的，乌龟运动显示节点直接调用即可，运动控制节点之前是使用的 turtle_teleop_key通过键盘控制，现在需要自定义控制节点。
- 控制节点自实现时，首先需要了解控制节点与显示节点通信使用的话题与消息，可以使用ros命令结合计算图来获取。
- 了解了话题与消息之后，通过 C++ 或 Python 编写运动控制节点，通过指定的话题，按照一定的逻辑发布消息即可。
实现流程：
- 通过计算图结合ros命令获取话题与消息信息。
- 编码实现运动控制节点。
- 启动 roscore、turtlesim_node 以及自定义的控制节点，查看运行结果。
1.话题与消息获取
准备: 先启动键盘控制乌龟运动案例。
1.1话题获取
获取话题：/turtle1/cmd_vel
通过计算图查看话题，启动计算图：
rqt_graph或者通过 rostopic 列出话题：
rostopic list1.2消息获取
获取消息类型：geometry_msgs/Twist
rostopic type /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 zlinear(线速度) 下的xyz分别对应在x、y和z方向上的速度(单位是 m/s)；
angular(角速度)下的xyz分别对应x轴上的翻滚、y轴上俯仰和z轴上偏航的速度(单位是rad/s)。
详情请查看补充资料。
2.实现发布节点
创建功能包需要依赖的功能包： roscpp rospy std_msgs geometry_msgs
实现方案A： C++
/*
编写 ROS 节点，控制小乌龟画圆
准备工作:
1.获取topic(已知: /turtle1/cmd_vel)
2.获取消息类型(已知: geometry_msgs/Twist)
3.运行前，注意先启动 turtlesim_node 节点
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建发布者对象
4.循环发布运动控制消息
*/
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"control");
ros::NodeHandle nh;
// 3.创建发布者对象
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1000);
// 4.循环发布运动控制消息
//4-1.组织消息
geometry_msgs::Twist msg;
msg.linear.x = 1.0;
msg.linear.y = 0.0;
msg.linear.z = 0.0;
msg.angular.x = 0.0;
msg.angular.y = 0.0;
msg.angular.z = 2.0;
//4-2.设置发送频率
ros::Rate r(10);
//4-3.循环发送
while (ros::ok())
{
pub.publish(msg);
ros::spinOnce();
}
return 0;
}配置文件此处略。
实现方案B：Python
#! /usr/bin/env python
"""
编写 ROS 节点，控制小乌龟画圆
准备工作:
1.获取topic(已知: /turtle1/cmd_vel)
2.获取消息类型(已知: geometry_msgs/Twist)
3.运行前，注意先启动 turtlesim_node 节点
实现流程:
1.导包
2.初始化 ROS 节点
3.创建发布者对象
4.循环发布运动控制消息
"""
import rospy
from geometry_msgs.msg import Twist
if __name__ == "__main__":
# 2.初始化 ROS 节点
rospy.init_node("control_circle_p")
# 3.创建发布者对象
pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=1000)
# 4.循环发布运动控制消息
rate = rospy.Rate(10)
msg = Twist()
msg.linear.x = 1.0
msg.linear.y = 0.0
msg.linear.z = 0.0
msg.angular.x = 0.0
msg.angular.y = 0.0
msg.angular.z = 0.5
while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep()权限设置以及配置文件此处略。
3.运行
首先，启动 roscore；
然后启动乌龟显示节点；
最后执行运动控制节点；
最终执行结果与演示结果类似。
4.补充资料1
弧度：单位弧度定义为圆弧长度等于半径时的圆心角。
补充资料2：偏航、翻滚与俯仰
坐标系图解：
偏航：
俯仰：
翻滚：
2.5.2 实操02_话题订阅
需求描述：已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。
结果演示：
实现分析：
- 首先，需要启动乌龟显示以及运动控制节点并控制乌龟运动。
- 要通过ROS命令，来获取乌龟位姿发布的话题以及消息。
- 编写订阅节点，订阅并打印乌龟的位姿。
实现流程:
- 通过ros命令获取话题与消息信息。
- 编码实现位姿获取节点。
- 启动 roscore、turtlesim_node 、控制节点以及位姿订阅节点，控制乌龟运动并输出乌龟的位姿。
1.话题与消息获取
获取话题：/turtle1/pose
rostopic list获取消息类型：turtlesim/Pose
rostopic type /turtle1/pose获取消息格式：
rosmsg info turtlesim/Pose响应结果：
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity2.实现订阅节点
创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim
实现方案A：C++
/*
订阅小乌龟的位姿: 时时获取小乌龟在窗体中的坐标并打印
准备工作:
1.获取话题名称 /turtle1/pose
2.获取消息类型 turtlesim/Pose
3.运行前启动 turtlesim_node 与 turtle_teleop_key 节点
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建 ROS 句柄
4.创建订阅者对象
5.回调函数处理订阅的数据
6.spin
*/
#include "ros/ros.h"
#include "turtlesim/Pose.h"
void doPose(const turtlesim::Pose::ConstPtr& p){
ROS_INFO("乌龟位姿信息:x=%.2f,y=%.2f,theta=%.2f,lv=%.2f,av=%.2f",
p->x,p->y,p->theta,p->linear_velocity,p->angular_velocity
);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"sub_pose");
// 3.创建 ROS 句柄
ros::NodeHandle nh;
// 4.创建订阅者对象
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
// 5.回调函数处理订阅的数据
// 6.spin
ros::spin();
return 0;
}配置文件此处略。
实现方案B: Python
#! /usr/bin/env python
"""
订阅小乌龟的位姿: 时时获取小乌龟在窗体中的坐标并打印
准备工作:
1.获取话题名称 /turtle1/pose
2.获取消息类型 turtlesim/Pose
3.运行前启动 turtlesim_node 与 turtle_teleop_key 节点
实现流程:
1.导包
2.初始化 ROS 节点
3.创建订阅者对象
4.回调函数处理订阅的数据
5.spin
"""
import rospy
from turtlesim.msg import Pose
def doPose(data):
rospy.loginfo("乌龟坐标:x=%.2f, y=%.2f,theta=%.2f",data.x,data.y,data.theta)
if __name__ == "__main__":
# 2.初始化 ROS 节点
rospy.init_node("sub_pose_p")
# 3.创建订阅者对象
sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=1000)
# 4.回调函数处理订阅的数据
# 5.spin
rospy.spin()权限设置以及配置文件此处略。
3.运行
首先，启动 roscore；
然后启动乌龟显示节点，执行运动控制节点；
最后启动乌龟位姿订阅节点；
最终执行结果与演示结果类似。
