ROS-ROS通信机制-小乌龟

文章目录

1.话题发布
2.话题订阅
3.服务调用
4.参数设置
5.通信机制比较

1.话题发布

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

实现分析:

乌龟运动控制实现，关键节点有两个，一个是乌龟运动显示节点 turtlesim_node，另一个是控制节点，二者是订阅发布模式实现通信的，乌龟运动显示节点直接调用即可，运动控制节点之前是使用的 turtle_teleop_key通过键盘控制，现在需要自定义控制节点。
控制节点自实现时，首先需要了解控制节点与显示节点通信使用的话题与消息，可以使用ros命令结合计算图来获取。
了解了话题与消息之后，通过C++ 或 Python 编写运动控制节点，通过指定的话题，按照一定的逻辑发布消息即可。

实现流程:

通过计算图结合ros命令获取话题与消息信息。
编码实现运动控制节点。启动 roscore、turtlesim_node
以及自定义的控制节点，查看运行结果。

1.话题与消息获取
准备: 先启动键盘控制乌龟运动案例。
在这里插入图片描述
1.1话题获取
获取话题:/turtle1/cmd_vel

通过计算图查看话题，启动计算图:
在这里插入图片描述

rqt_graph

或者通过 rostopic 列出话题:

rostopic list

在这里插入图片描述
1.2消息获取
获取消息类型:geometry_msgs/Twist

rostopic type /turtle1/cmd_vel

在这里插入图片描述
也可使用rostopic info获取消息类型

rostopic info /turtle1/cmd_vel

获取消息格式:

rosmsg info geometry_msgs/Twist

也可用rosmsg show获取消息格式

rosmsg show geometry_msgs/Twist

在这里插入图片描述

linear(线速度) 下的xyz分别对应在x、y和z方向上的速度(单位是 m/s)；
angular(角速度)下的xyz分别对应x轴上的翻滚、y轴上俯仰和z轴上偏航的速度(单位是rad/s)。

对于小乌龟只需要设置x方向的线速度和z方向的角速度。

弧度: 单位弧度定义为圆弧长度等于半径时的圆心角。

偏航、翻滚与俯仰

2.实现发布节点
创建功能包需要依赖的功能包: roscpp rospy std_msgs geometry_msgs
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;
}

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；
然后启动乌龟显示节点；
最后执行运动控制节点；
运行效果为

1.也不知道自己啥时候把小乌龟的包给删了，导致运行出现Error: package ‘turtlesim’ not found。使用下面这行代码安装。

sudo apt-get install ros-melodic-ros-tutorials

2.简易实现
优点：便捷
缺点：有一定的局限性，无法包含复杂的逻辑

rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist "linear: 
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 1.0"

在这里插入图片描述
3.在使用Python实现时，编译一直出现错误Invoking “make cmake_check_build_system” failed。尝试了好几次，也没有发现哪里错了，最后新建了功能包，重新写，并且把原来的功能包删除才能编译通过。疑惑???
！！！！！！！！！！！发现问题了，竟然是scripts少写了一个"r"，啊啊啊啊啊啊啊啊

4.在使用Python实现时，好不容易编译通过了，运行时又出现这个问题
在这里插入图片描述
然后通过百度发现了解决办法
修改前的代码为：

 pub = rospy.Publisher("/turtle1/cmd_vel",Twist,10)

修改后的代码为

 pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=10)

rospy 中的 Publisher 函数与 roscpp 中的不太一样。rospy 中的 Publisher 函数的第三个参数是 subscribe_listener ，queue_size 是最后一个参数。因此需要指定数字1给的是 queue_size。

2.话题订阅

需求描述:
已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。

实现分析:

首先，需要启动乌龟显示以及运动控制节点并控制乌龟运动。
要通过ROS命令，来获取乌龟位姿发布的话题以及消息。
编写订阅节点，订阅并打印乌龟的位姿。

实现流程:

通过ros命令获取话题与消息信息。
编码实现位姿获取节点。
启动 roscore、turtlesim_node、控制节点以及位姿订阅节点，控制乌龟运动并输出乌龟的位姿。

1.话题与消息获取

获取话题:/turtle1/pose （小写p）

rostopic list

在这里插入图片描述

获取消息类型:turtlesim/Pose(大写P)

rostopic type  /turtle1/pose

在这里插入图片描述

获取消息格式:

rosmsg info turtlesim/Pose

响应结果:
在这里插入图片描述
注：Pose中"P"要大写。

2.实现订阅节点
创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim
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;
}

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；
然后启动乌龟显示节点，执行运动控制节点；
最后启动乌龟位姿订阅节点；

1.运行launch文件时，多打了一个空格导致一直运行错误
在这里插入图片描述

解决：一定要认真检查！！！，删除空格就可以了。
2.运行成功launch文件，键盘控制小乌龟不动
如果想要实现按键控制，必须把鼠标移到启动按键控制的终端上选中。而不是选中小乌龟窗口。按照常规操作，可能都会优先去选中小乌龟窗口，此时按下方向键是不会有反应的。
在这里插入图片描述
3.小乌龟的坐标是相对于GUI界面左下角为原点。

3.服务调用

需求描述:
编码实现向 turtlesim 发送请求，在乌龟显示节点的窗体指定位置生成一乌龟，这是一个服务请求操作。

实现分析:

首先，需要启动乌龟显示节点。
要通过ROS命令，来获取乌龟生成服务的服务名称以及服务消息类型。
编写服务请求节点，生成新的乌龟。

实现流程:

通过ros命令获取服务与服务消息信息。
编码实现服务请求节点。
启动 roscore、turtlesim_node、乌龟生成节点，生成新的乌龟。

1.服务名称与服务消息获取
获取话题:/spawn

rosservice list

在这里插入图片描述
获取消息类型:turtlesim/Spawn

rosservice type /spawn

在这里插入图片描述

获取消息格式:

rossrv info turtlesim/Spawn

响应结果:
在这里插入图片描述

2.服务客户端实现
创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim
C++

/*
    生成一只小乌龟
    准备工作:
        1.服务话题 /spawn
        2.服务消息类型 turtlesim/Spawn
        3.运行前先启动 turtlesim_node 节点

    实现流程:
        1.包含头文件
          需要包含 turtlesim 包下资源，注意在 package.xml 配置
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 service 客户端
        5.等待服务启动
        6.发送请求
        7.处理响应

*/

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"set_turtle");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 service 客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    // 5.等待服务启动
    // client.waitForExistence();
    ros::service::waitForService("/spawn");
    // 6.发送请求
    turtlesim::Spawn spawn;
    spawn.request.x = 1.0;
    spawn.request.y = 1.0;
    spawn.request.theta = 1.57;
    spawn.request.name = "my_turtle";
    bool flag = client.call(spawn);
    // 7.处理响应结果
    if (flag)
    {
        ROS_INFO("新的乌龟生成,名字:%s",spawn.response.name.c_str());
    } else {
        ROS_INFO("乌龟生成失败！！！");
    }


    return 0;
}

Python

#! /usr/bin/env python
"""
    生成一只小乌龟
    准备工作:
        1.服务话题 /spawn
        2.服务消息类型 turtlesim/Spawn
        3.运行前先启动 turtlesim_node 节点

    实现流程:
        1.导包
          需要包含 turtlesim 包下资源，注意在 package.xml 配置
        2.初始化 ros 节点
        3.创建 service 客户端
        4.等待服务启动
        5.发送请求
        6.处理响应

"""

import rospy
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse

if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("set_turtle_p")
    # 3.创建 service 客户端
    client = rospy.ServiceProxy("/spawn",Spawn)
    # 4.等待服务启动
    client.wait_for_service()
    # 5.发送请求
    req = SpawnRequest()
    req.x = 2.0
    req.y = 2.0
    req.theta = -1.57
    req.name = "my_turtle_p"
    try:
        response = client.call(req)
        # 6.处理响应
        rospy.loginfo("乌龟创建成功!，叫:%s",response.name)
    except expression as identifier:
        rospy.loginfo("服务调用失败")

3.运行

首先，启动 roscore；
然后启动乌龟显示节点；
最后启动乌龟生成请求节点；

4.参数设置

需求描述:
修改turtlesim乌龟显示节点窗体的背景色，已知背景色是通过参数服务器的方式以 rgb 方式设置的。
实现分析:

首先，需要启动乌龟显示节点。
要通过ROS命令，来获取参数服务器中设置背景色的参数。
编写参数设置节点，修改参数服务器中的参数值。

实现流程:

通过ros命令获取参数。
编码实现服参数设置节点。
启动 roscore、turtlesim_node 与参数设置节点，查看运行结果。

1.参数名获取
获取参数列表:

rosparam list

响应结果:
在这里插入图片描述

2.参数修改
C++

/*
    注意命名空间的使用。

*/
#include "ros/ros.h"


int main(int argc, char *argv[])
{
    ros::init(argc,argv,"haha");

    ros::NodeHandle nh("turtlesim");
    //ros::NodeHandle nh;

    // ros::param::set("/turtlesim/background_r",0);
    // ros::param::set("/turtlesim/background_g",0);
    // ros::param::set("/turtlesim/background_b",0);

    nh.setParam("background_r",0);
    nh.setParam("background_g",0);
    nh.setParam("background_b",0);


    return 0;
}

Python

#! /usr/bin/env python

import rospy

if __name__ == "__main__":
    rospy.init_node("hehe")
    # rospy.set_param("/turtlesim/background_r",255)
    # rospy.set_param("/turtlesim/background_g",255)
    # rospy.set_param("/turtlesim/background_b",255)
    rospy.set_param("background_r",255)
    rospy.set_param("background_g",255)
    rospy.set_param("background_b",255)  # 调用时，需要传入 __ns:=xxx

3.运行

首先，启动 roscore；
然后启动背景色设置节点；
最后启动乌龟显示节点；

PS: 注意节点启动顺序，如果先启动乌龟显示节点，后启动背景色设置节点，那么颜色设置不会生效。

4.其他设置方式
方式1:修改小乌龟节点的背景色(命令行实现)

rosparam set /turtlesim/background_b 自定义数值

rosparam set /turtlesim/background_g 自定义数值

rosparam set /turtlesim/background_r 自定义数值

修改相关参数后，重启 turtlesim_node 节点，背景色就会发生改变了

方式2:启动节点时，直接设置参数

rosrun turtlesim turtlesim_node _background_r:=100 _background_g=0 _background_b=0

方式3:通过launch文件传参

<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="set_bg" output="screen">
        <!-- launch 传参策略1 -->
        <!-- <param name="background_b" value="0" type="int" />
        <param name="background_g" value="0" type="int" />
        <param name="background_r" value="0" type="int" /> -->

        <!-- launch 传参策略2 -->
        <rosparam command="load" file="$(find demo03_test_parameter)/cfg/color.yaml" />
    </node>

</

5.通信机制比较

三种通信机制中，参数服务器是一种数据共享机制，可以在不同的节点之间共享数据，话题通信与服务通信是在不同的节点之间传递数据的，三者是ROS中最基础也是应用最为广泛的通信机制。

这其中，话题通信和服务通信有一定的相似性也有本质上的差异，在此将二者做一下简单比较:

二者的实现流程是比较相似的，都是涉及到四个要素:

要素1: 消息的发布方/客户端(Publisher/Client)
要素2: 消息的订阅方/服务端(Subscriber/Server)
要素3: 话题名称(Topic/Service)
要素4: 数据载体(msg/srv)

可以概括为: 两个节点通过话题关联到一起，并使用某种类型的数据载体实现数据传输。

二者的实现也是有本质差异的，具体比较如下:

	Topic(话题)	Service(服务)
通信模式	发布/订阅	请求/响应
同步性	异步	同步
底层协议	ROSTCP/ROSUDP	ROSTCP/ROSUDP
缓冲区	有	无
实时性	弱	强
节点关系	多对多	一对多(一个 Server)
通信数据	msg	srv
使用场景	连续高频的数据发布与接收:雷达、里程计	偶尔调用或执行某一项特定功能：拍照、语音识别

不同通信机制有一定的互补性，都有各自适应的应用场景。尤其是话题与服务通信，需要结合具体的应用场景与二者的差异，选择合适的通信机制。

参考：
[1]Autolabor-ROS机器人入门课程《ROS理论与实践》季基础教程
[2]【Autolabor初级教程】ROS机器人入门
[3]胡春旭.ROS机器人开发实践[M].机械工业出版社,2018.
[4]https://blog.csdn.net/qq_51303289/article/details/128215248