第 2 章 ROS 通信机制 5 —— 通信机制实操 1 plumbing_test

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 需求 - 分析 - 流程

需求描述:编码实现乌龟运动控制,让小乌龟做圆周运动。

实现分析

  1. 乌龟运动控制实现,关键节点有两个,一个是乌龟运动显示节点 turtlesim_node,另一个是控制节点,二者是订阅发布模式实现通信的,乌龟运动显示节点直接调用即可,运动控制节点是使用的 turtle_teleop_key 通过键盘控制,现在需要自定义控制节点。
  2. 控制节点自实现时,首先需要了解控制节点与显示节点通信使用的话题与消息,可以使用ros命令结合计算图来获取。
  3. 了解了话题与消息之后,通过 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 编译运行

步骤:

  1. roscore
  2. rosrun turtlesim turtlesim_node
  3. cd demo01_ws
  4. source ./devel/setup.bash
  5. 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 编译运行

步骤:

  1. roscore
  2. rosrun turtlesim turtlesim_node
  3. cd demo01_ws
  4. source ./devel/setup.bash
  5. 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
  • 1
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

2021 Nqq

你的鼓励是我学习的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值