文章目录
ROS内置的turtlesim案例
发布:rostopic info /turtle1/cmd_vel(/turtle1/cmd_vel是话题,类似于前面的liaotian)
- /turtle1/cmd_vel:控制小乌龟运动的节点
Type: geometry_msgs/Twist
Publishers: /teleop_turtle (http://rosnoetic-VirtualBox:33275/)
Subscribers: /turtlesim (http://rosnoetic-VirtualBox:40025/)
订阅:rostopic info /turtle1/pose
Type: turtlesim/Pose
Publishers: /turtle1 (http://rosnoetic-VirtualBox:46159/)
Subscribers: None
本节主要是通过ROS内置的turtlesim案例,结合已经介绍ROS命令获取节点、话题、话题消息、服务、服务消息与参数的信息,最终再以编码的方式实现乌龟运动的控制、乌龟位姿的订阅、乌龟生成与乌龟窗体背景颜色的修改。
00 添加功能包依赖 package.xml + CMakeLists.txt (重要)
- 创建 catkin 文件时,添加 roscpp rospy std_msgs geometry_msgs turtlesim 共五个包,如果需要添加新的包,在package.xml + CMakeLists.txt里面添加。
- package.xml 文件内容如下
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>turtlesim</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>turtlesim</exec_depend>
- CMakeLists.txt 文件
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
std_msgs
turtlesim
)
01 话题发布 plumbing_test
1.0 需求 - 分析 - 流程
需求描述:编码实现乌龟运动控制,让小乌龟做圆周运动。
实现分析:
- 乌龟运动控制实现,关键节点有两个,一个是乌龟运动显示节点 turtlesim_node,另一个是控制节点,二者是订阅发布模式实现通信的,乌龟运动显示节点直接调用即可,运动控制节点是使用的 turtle_teleop_key 通过键盘控制,现在需要自定义控制节点。
- 控制节点自实现时,首先需要了解控制节点与显示节点通信使用的话题与消息,可以使用ros命令结合计算图来获取。
- 了解了话题与消息之后,通过 C++ 或 Python 编写运动控制节点,通过指定的话题,按照一定的逻辑发布消息即可。
实现流程
- 通过计算图结合ros命令获取话题与消息信息
- 编码实现运动控制节点
- 启动 roscore、turtlesim_node 以及自定义的控制节点,查看运行结果
1.1 话题与消息获取
- 准备: 先启动键盘控制乌龟运动案例
1.1.0 启动键盘控制乌龟运动(获得话题和消息,直接启动)
- 窗口1
roscore
- 窗口2
rosrun turtlesim turtlesim_node
- 窗口3
rosrun turtlesim turtle_teleop_key
1.1.1 获取话题 /turtle1/cmd_vel
- 获取话题:/turtle1/cmd_vel(两个节点turtlesim_node与turtle_teleop_key之间的话题)
- 方式1:rqt_graph —— 启动计算图获取话题,左右两端是节点信息
- 方式2:rostopic list,下面是结果
/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose
1.1.2 获取消息类型 geometry_msgs/Twist(参考功能包plumbing_pub_sub 中Person.msg)
- 获取消息类型:geometry_msgs/Twist(类似于自定义的Person.msg)
- 方式1:rostopic info /turtle1/cmd_vel(/turtle1/cmd_vel是话题),下面是结果
Type: geometry_msgs/Twist
Publishers: /teleop_turtle (http://rosnoetic-VirtualBox:33275/)
Subscribers: /turtlesim (http://rosnoetic-VirtualBox:40025/)
- 方式2:rostopic type /turtle1/cmd_vel,下面是结果
geometry_msgs/Twist
1.1.3 获取消息格式
- 获取消息格式
- rosmsg show geometry_msgs/Twist(消息格式类似于功能包plumbing_pub_sub中Person.msg 内部的name height age)得到内部参数
- 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
- rostopic echo /turtle1/cmd_vel:控制键盘指挥乌龟运动,打印数据
- 以10Hz的方式进行订阅:rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist
1.1.4 线速度 - 角速度 - 弧度(重要)
- linear:(线速度,前进后退速度) 下的xyz分别对应在x、y和z方向上的速度(单位是 m/s),数据类型是浮点double。
- 正常的移动机器人只有x方向速度,麦克纳姆轮有x与y方向的速度,无人机有x,y,z方向线速度取值。
- angular:(角速度,转弯速度)下的xyz分别对应x轴上的翻滚、y轴上俯仰和z轴上偏航的速度(单位是rad/s)
- 弧度: 单位弧度定义为圆弧长度等于半径时的圆心角。即图中的radian 周长为2pir,圆周上转1个半径长度是1弧度,转1圈是相当于6.28个半径,就是6.28个弧度,1秒转1圈是6.28rad/s。
1.1.5 偏航 - 翻滚 - 俯仰(重要)
- angular:(角速度,转弯速度)下的xyz分别对应x轴上的翻滚、y轴上俯仰和z轴上偏航的速度(单位是rad/s)
- 偏航:绕z轴旋转
- 俯仰:绕y轴旋转
- 翻滚:绕 x 轴旋转
1.2 实现发布节点 —— plumbing_test
- 运动分析:线速度只有 x 和 y的取值,角速度只有 z 的取值
1.2.1 C++实现 —— test01_pub_twist.cpp
#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::init(argc,argv,"my_control");
// 3.创建节点句柄
ros::NodeHandle nh;
// 4.创建发布对象
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);// 话题名称/tur...
// 5.发布逻辑实现 10Hz 1秒10次
// 5.1 设置发布频率
ros::Rate rate(10);
// 5.2 组织被发布的消息
geometry_msgs::Twist twist;
twist.linear.x = 1.0;//数据类型是浮点double
twist.linear.y = 0.0;
twist.linear.z = 0.0;
twist.angular.z = 1.0;//只有偏航角
twist.angular.x = 0.0;
twist.angular.y = 0.0;
// 5.3 循环发布
while(ros::ok())
{
pub.publish(twist);
// 休眠
rate.sleep();
// 6.spinOnce() 循环发布
ros::spinOnce();
}
return 0;
}
1.2.2 配置 CMakeLists.txt
add_executable(test01_pub_twist src/test01_pub_twist.cpp)
add_dependencies(test01_pub_twist ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) —— 可以不添加,因为没有自定义消息
target_link_libraries(test01_pub_twist
${catkin_LIBRARIES}
)
1.2.3 编译运行
步骤:
- roscore
- rosrun turtlesim turtlesim_node
- cd demo01_ws
- source ./devel/setup.bash
- rosrun plumbing_test test01_pub_twist
- 编译:Ctrl + Shift + B
- 启动 roscore(窗口1)
roscore
- 启动 乌龟节点(窗口2)
rosrun turtlesim turtlesim_node
- 启动发布节点(窗口3)
cd demo01_ws/
source ./devel/setup.bash
rosrun plumbing_test test01_pub_twist
- 小乌龟做圆周运动
1.2.4 Python实现 —— test01_pub_twist_p.py
#! /usr/bin/eny python
"""
发布方:发布速度消息
话题: /turtle1/cmd_vel
消息: geometry_msgs/Twist
1. 导包
2. 初始化ROS节点
3. 创建发布者对象
4. 组织数据,发布数据
"""
import rospy
from geometry_msgs.msg import Twist
if __name__ == "__main__":
# 2. 初始化ROS节点
rospy.init_node("my_control_p")
# 3. 创建发布者对象
pub = rospy.Publisher("/turtle/cmd_vel",Twist,queue_size = 10)
# 4. 组织数据,发布数据
# 4.1 设置发布频率
rate = rospy.Rate(10)
# 4.2 创建速度消息
twist = Twist()
twist.linear.x = 0.5
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
# 4.3 循环发布
while not rospy.is_shutdown():
pub.publish(twist)
rate.sleep()
1.2.5 添加可执行权限
scripts文件右击,在终端中打开
终端下进入 scripts 执行
chmod +x *.py
ll
1.2.6 配置 CMakeLists.txt
catkin_install_python(PROGRAMS
scripts/test01_pub_twist_p.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
1.2.7 编译运行
步骤:
- roscore
- rosrun turtlesim turtlesim_node
- cd demo01_ws
- source ./devel/setup.bash
- rosrun plumbing_test test01_pub_twist_p.py
- 编译:Ctrl + Shift + B
- 启动 roscore(窗口1)
roscore
- 启动 乌龟节点(窗口2)
rosrun turtlesim turtlesim_node
- 启动发布节点(窗口3)
cd demo01_ws/
source ./devel/setup.bash
rosrun plumbing_test test01_pub_twist_p.py
02 话题订阅 plumbing_test
2.0 需求 - 实现 - 分析
需求描述
- :已知 turtlesim 中的乌龟显示节点,会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向),要求控制乌龟运动,并时时打印当前乌龟的位姿。
实现分析:
- 首先,需要启动乌龟显示以及运动控制节点并控制乌龟运动。
- 要通过 ROS 命令,来获取乌龟位姿发布的话题以及消息。
- 编写订阅节点,订阅并打印乌龟的位姿。
实现流程:
- 通过 ros 命令获取话题与消息信息。
- 编码实现位姿获取节点。
- 启动 roscore、turtlesim_node 、控制节点以及位姿订阅节点,控制乌龟运动并输出乌龟的位姿
2.1 话题与消息获取
2.1.0 启动键盘控制乌龟运动节点(获得话题和消息,使用launch方式) start_turtle.launch
<!-- 启动乌龟GUI与键盘控制节点-->
<launch>
<!-- 乌龟GUI-->
<!-- pkg——包,type——被执行的节点,name——节点取的名字,output = "screen"——日志输出到屏幕上-->
<!-- rosrun turtlesim(pkg)功能包 turtlrsim_node(type)-->
<node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
<!-- 键盘控制-->
<node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />
</launch>
2.1.0.0 vscode终端启动 launch 文件
cd demo01_ws
source ./devel/setup.bash
roslaunch plumbing_test start_turtle.launch
2.1.1 获取话题 /turtle1/pose(类似于 liaoTian)
- rostopic list
/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose
2.1.2 获取消息类型 turtlesim/Pose(类似于Person.msg)
- rostopic info /turtle1/pose
Type: turtlesim/Pose
Publishers: /turtle1 (http://rosnoetic-VirtualBox:46159/)
Subscribers: None
2.1.3 获取消息格式
rosmsg info turtlesim/Pose
float32 x(坐标)
float32 y
float32 theta(朝向)
float32 linear_velocity(线速度)
float32 angular_velocity(角速度)
- rostopic echo /turtle1/pose:打印信息
2.2 实现订阅节点 —— plumbing_test
2.2.1 C++实现 —— 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); //doPose是回调函数
// 5. 处理订阅到的数据(回调函数)
// 6. spin()回头
ros::spin();
return 0;
}
2.2.2 配置 CMakeLists.txt
add_executable(test02_sub_pose src/test02_sub_pose.cpp)
add_dependencies(test02_sub_pose ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) —— 可以不添加,因为没有自定义消息
target_link_libraries(test02_sub_pose
${catkin_LIBRARIES}
)
2.2.3 编译运行
- 编译:Ctrl + Shift + B
- 启动 launch vscode终端窗口 —— 要把光标放在vscode终端启动launch文件的位置!!!
source ./devel/setup.bash
roslaunch plumbing_test start_turtle.launch
-
ctrl+alt+t:开新的窗口
-
启动 roscore(窗口1)
roscore
- 启动订阅节点(窗口2)
cd demo01_ws
source ./devel/setup.bash
rosrun plumbing_test test02_sub_pose
- 就可以根据键盘控制乌龟运动的同时,实时打印出乌龟的运动信息
2.2.4 Python实现 —— test02_sub_pose_p.py
#! /usr/bin/env python
"""
需求:订阅并输出乌龟的位资信息
1. 导包
2. 初始化ROS节点
3. 创建订阅对象
4. 使用回调函数处理订阅到的信息
5. spin()
"""
import rospy
from turtlesim.msg import Pose
def doPose(pose):
rospy.loginfo("p->乌龟位姿信息:坐标(%.2f,%.2f),朝向(%.2f),线速度:%.2f,角速度:%.2f",
pose.x,pose.y,pose.theta,pose.linear_velocity,pose.angular_velocity)
if __name__ == "__main__":
# 2. 初始化ROS节点
rospy.init_node("sub_pose_p")
# 3. 创建订阅对象
sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100)# 消息类型是Pose
# 4. 使用回调函数处理订阅到的信息
# 5. spin()
rospy.spin()
2.2.5 添加可执行权限
scripts文件右击,在终端中打开
终端下进入 scripts 执行
chmod +x *.py
ll
2.2.6 配置 CMakeLists.txt
catkin_install_python(PROGRAMS
scripts/test02_sub_pose_p.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
2.2.7 编译运行
- 编译:Ctrl + Shift + B
- 启动 launch vscode终端窗口 —— 要把光标放在vscode终端启动launch文件的位置!!!
source ./devel/setup.bash
roslaunch plumbing_test start_turtle.launch
-
ctrl+alt+t:开新的窗口
-
启动 roscore(窗口1)
roscore
- ctrl+alt+t:开新窗口
- 启动订阅节点(窗口2)
cd demo01_ws
source ./devel/setup.bash
rosrun plumbing_test test02_sub_pose_p.py