承接上一部分

2.5 通信机制实操

本节主要是通过ROS内置的turtlesim案例，结合已经介绍ROS命令获取节点、话题、话题消息、服务、服务消息与参数的信息，最终再以编码的方式实现乌龟运动的控制、乌龟位姿的订阅、乌龟生成与乌龟窗体背景颜色的修改。

目的:熟悉、强化通信模式应用

2.5.1 实操01_话题发布

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

替换键盘，自己写小程序控制乌龟运动

Node 替换 话题+消息 编码+ros命令以及计算图

在这里插入图片描述

实现分析:

乌龟运动控制实现，关键节点有两个，一个是乌龟运动显示节点 turtlesim_node，另一个是控制节点，二者是订阅发布模式实现通信的，乌龟运动显示节点直接调用即可，运动控制节点之前是使用的 turtle_teleop_key通过键盘 控制，现在需要自定义控制节点。

控制节点自实现时，首先需要了解控制节点与显示节点通信使用的话题与消息，可以使用ros命令结合计算图来获取。

了解了话题与消息之后，通过 C++ 或 Python 编写运动控制节点，通过指定的话题，按照一定的逻辑发布消息即可。

实现流程:

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

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
话题是中间的
获取消息

在这里插入图片描述
获取话题消息
在这里插入图片描述
获取话题消息格式
在这里插入图片描述

线速度角速度 线速度设置x（直行的速度）,角速度设置z（转弯的速度）
乌龟只有x 螃蟹有y 鸟有z取值

麦克纳姆轮有 x,y线速度
无人机有x,y,z

在这里插入图片描述


贯穿机体x轴（翻滚），贯穿机翼y轴（俯仰），垂直z轴（偏航）

乌龟不能翻滚，俯仰，但是可以偏航

linear(线速度) 下的xyz分别对应在x、y和z方向上的速度(单位是 m/s)；

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

验证：rostopic echo /turtle1/cmd_vel
在这里插入图片描述
只有线速度x轴和角速度z轴的移动,验证完成

使用rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist "linear:
后面双tab补齐，做测试，但是遇到复杂的就不行了，还是得用程序实现

在这里插入图片描述1 c++实现
新建功能包，包依赖多一个geometry_msgs

#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[])
{
    ros::init(argc,argv,"mycontrol");
    ros::NodeHandle nh;
    ros::Publisher pub=nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    ros::Rate rate(10);//设置发布频率
    //组织消息
    geometry_msgs::Twist twist;
    //消息格式和前面的呼应 rosmsg info
    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();
        ros::spinOnce();
    }   
    return 0;
}

编辑cmake后

在这里插入图片描述

实验结果如下：

在这里插入图片描述
2 python 实现

#!  /usr/bin/env python
#coding=utf-8

# 发布方发布消息：
#     话题名字：/turtle1/cmd_vel
#     消息类型：geometry_msgs/Twist
#     1.导包
#     2.初始化ros节点
#     3.创建发布者对象
#     4.组织数据并发布数据
import rospy
from geometry_msgs.msg import Twist

if __name__=="__main__":
    rospy.init_node("mycontrol_P")
    pub=rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=10)
    #设置发布频率
    rate=rospy.Rate(10)
    #创建速度消息
    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=0.5
  
    #循环发布  
    while not rospy.is_shutdown():
        pub.publish(twist)
        rate.sleep()

实验结果如下

在这里插入图片描述

2.5.2 实操02_话题订阅

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

实现分析:

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

实现流程:

通过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_velocity

结果如下所示

在这里插入图片描述

2 rostopic echo 完成基本测试

rostopic echo /turtle1/pose

在这里插入图片描述

运动过后结果如下

在这里插入图片描述

2.C++实现订阅节点

创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim

直接在package.xml, cmake文件中添加所需要的依赖项
如下图所示

在这里插入图片描述

cmake
在这里插入图片描述
代码实现

#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[])
{
//     2.初始化ros节点
//     3.创建节点句柄
//     4.创建订阅对象
//     5.处理订阅到的数据（回调函数）不知道啥时候过来
//     6.spin()
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"sub_pose");
    ros::NodeHandle nh;
    ros::Subscriber sub=nh.subscribe("/turtle1/pose",100,doPose);
    ros::spin();
    return 0;
}

cmake
在这里插入图片描述
启动封装好的launch文件

启动 test02_sub_pose

roslaunch plumbingtest start_turtle.launch

rosrun plumbingtest test02_sub_pose

实验结果如下：

在这里插入图片描述
至此使用C++完成了话题订阅

2.Python实现订阅节点

创建啥的就不说了

#! /usr/bin/env python  
#coding=utf-8
#包+消息格式
from turtlesim.msg import Pose
# 需求：订阅并且输出乌龟位姿信息
#     1. 导包
#     2. 初始化ros节点
#     3. 创建订阅对象
#     4. 使用回调函数处理订阅到的消息
#     5. spin()

import rospy
#使用rosmsg 查看消息格式 rosmsg info turtlesim/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__":
    rospy.init_node("sub_pose_p")
    sub=rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100)
    rospy.spin()

权限+cmake

在这里插入图片描述

roslaunch plumbingtest start_turtle.launch 
rosrun plumbingtest test02_s_pose_p.py

实验结果

在这里插入图片描述

坐标原点在GUI图像的左下方 默认朝向是与x轴平行向右，左转弧度递增，右转弧度递减

最大值：3.141
最小值：-3.141
都是掉头的时候

2.5.3 实操03_服务调用

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

实现分析:

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

实现流程:

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

在这里插入图片描述
spawn 是产卵的意思，用这个服务可以生成一只新的乌龟

在这里插入图片描述type 是消息类型
args 是参数的意思，需要提交的字段：坐标，朝向，名字

rossrv是用于显示有关ROS服务类型的信息的命令行工具，与 rosmsg 使用语法高度雷同。

 rossrv info turtlesim/Spawn

上半部分请求，下半部分响应

在这里插入图片描述
请求有4个字段

响应只有1个字段

总结一下：
1.服务名称与服务消息获取

获取话题:/spawn

rosservice list

从话题可以得到消息类型
获取消息类型:turtlesim/Spawn

rosservice type /spawn

从消息类型得到消息格式
获取消息格式:

rossrv info turtlesim/Spawn

响应结果:

float32 x
float32 y
float32 theta
string name
---
string name

使用rosservice call 测试一下

rosservice call /spawn "x: 10.0

在这里插入图片描述

成功实现

在这里插入图片描述
2. c++实现服务客户端

代码如下

#include "ros/ros.h"
#include "turtlesim/Spawn.h"
//  需求：是向服务器发送请求，生成一只新的乌龟
//     话题：/spawn
//     消息：turtlesim/Spawn

//     1 包含头文件
//     2 初始化ros节点
//     3 创建节点句柄
//     4 创建客户端对象
//     5 组织数据并发送
//     6 处理响应
int main(int argc,char* argv[])
{
    setlocale(LC_ALL,"");
//     1 包含头文件
//     2 初始化ros节点
    ros::init(argc,argv,"service_call");
//     3 创建节点句柄
    ros::NodeHandle nh;
//     4 创建客户端对象
    ros::ServiceClient client=nh.serviceClient<turtlesim::Spawn>("/spawn");
//     5 组织数据并发送
    turtlesim::Spawn spawn;  
    spawn.request.x=1.0;
    spawn.request.y=4.0;
    spawn.request.theta=3.14;
    spawn.request.name="turtle2";
       //发送请求
    //判断服务器状态
    ros::service::waitForService("/spawn");
    bool flag=client.call(spawn);//spawn request会有数据
   //flag 接收响应状态，响应结果也会被设置进入spawn对象
 //   6 处理响应
    if (flag)
    {
        ROS_INFO("乌龟生成成功，新乌龟叫：%s",spawn.response.name.c_str());   
    } else
    {
        ROS_INFO("请求失败");
    }
    return 0;
}

cmake记得修改
在这里插入图片描述

实现：

在这里插入图片描述

注意：小乌龟名字唯一，如果重复执行会报错

3. Python 实现服务客户端

这次多加了一个异常处理：try

#! /usr/bin/env python
#coding=utf-8
import rospy
#服务对应 srv
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse
# 向服务器发送请求生成一直小乌龟
#     话题：/spawn
#     消息：turtlesim/Spawn
    
#     1. 导包
#     2. 初始化ros节点
#     3. 创建服务的客户端对象
#     4. 组织数据并发送请求
#     5. 处理响应结果
if __name__=="__main__":
    rospy.init_node("service_call_p")
    #     3. 创建服务的客户端对象
    client=rospy.ServiceProxy("/spawn",Spawn)
    
    #     4. 组织数据并发送请求
    request=SpawnRequest()
    request.x=4.5
    request.y=2.0
    request.theta=-3
    request.name="turtle3"
    #判断服务器状态
    client.wait_for_service()
try:    
    response=client.call(request)
    
    #5. 处理响应结果
    rospy.loginfo("生成乌龟的名字叫：%s",response.name)
except Exception as e:
    rospy.logerr("请求处理异常")

cmake 权限都改下

实现结果如下

在这里插入图片描述
在这里插入图片描述

2.5.4 实操04_参数设置

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

实现分析:

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

实现流程:

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

获取参数

在这里插入图片描述

参数详情

在这里插入图片描述

简单方式修改

在这里插入图片描述

1 使用c++完成参数修改

如果出现编译错误，一定要去devel中把对应的包名字删掉。

这样再编译才不会报错

#include "ros/ros.h"
#include "turtlesim/Spawn.h"
//  需求：是向服务器发送请求，生成一只新的乌龟
//     话题：/spawn
//     消息：turtlesim/Spawn

//     1 包含头文件
//     2 初始化ros节点
//     3 创建节点句柄
//     4 创建客户端对象
//     5 组织数据并发送
//     6 处理响应
int main(int argc,char* argv[])
{
    setlocale(LC_ALL,"");
//     1 包含头文件
//     2 初始化ros节点
    ros::init(argc,argv,"service_call");
//     3 创建节点句柄
    ros::NodeHandle nh;
//     4 创建客户端对象
    ros::ServiceClient client=nh.serviceClient<turtlesim::Spawn>("/spawn");
//     5 组织数据并发送
    turtlesim::Spawn spawn;  
    spawn.request.x=1.0;
    spawn.request.y=4.0;
    spawn.request.theta=3.14;
    spawn.request.name="turtle2";
       //发送请求
    //判断服务器状态
    ros::service::waitForService("/spawn");
    bool flag=client.call(spawn);//spawn request会有数据
   //flag 接收响应状态，响应结果也会被设置进入spawn对象
 //   6 处理响应
    if (flag)
    {
        ROS_INFO("乌龟生成成功，新乌龟叫：%s",spawn.response.name.c_str());   
    } else
    {
        ROS_INFO("请求失败");
    }
    return 0;
}

完成cmake修改+编译
rosrun实现

在这里插入图片描述

第二种方式：

#include"ros/ros.h"
    // 需求：修改参数服务器中，turtlesim 背景色相关的参数

    // 1.初始化ros节点；
    // 2.不一定需要创建节点句柄（和后续API有关系）
    // 3.修改参数


int main(int argc, char* argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"change_bgColor");
    // 3.修改参数
    ros::NodeHandle nh("turtlesim");//添加命名空间 turtlesim,和以往的不一样，当然了命名空间也可在下面back之前添加，效果相同
    nh.setParam("background_r",255);
    nh.setParam("background_r",255);
    nh.setParam("background_r",255);
    //如果调用ros::param 不需要创建节点句柄
    // ros::param::set("/turtlesim/background_r",0);
    // ros::param::set("/turtlesim/background_g",0);
    // ros::param::set("/turtlesim/background_b",0);
    return 0;
}
  1. 使用python完成参数设置

代码如下：

#! /usr/bin/env python
import rospy


if __name__=="__main__":
    rospy.init_node("change_color_p")
    rospy.set_param("/turtlesim/background_r",100)
    rospy.set_param("/turtlesim/background_g",200)
    rospy.set_param("/turtlesim/background_b",300)

给权限+cmake+编译
实现

在这里插入图片描述
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>

</

2，3在当前阶段不演示，后面会介绍

2.6 通信机制比较

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

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

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

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

可以概括为: 两个节点通过话题关联到一起，并使用某种类型的数据载体实现数据传输。
在这里插入图片描述同步按照一对请求响应顺序依次执行，异步可能订阅方没处理完第二个话题就发布了

队列长度就是缓冲区

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

2.7 本章小结

本章主要介绍了ROS中最基本的也是最核心的通信机制实现: 话题通信、服务通信、参数服务器。每种通信机制，都介绍了如下内容:

介绍了当前通信机制的应用场景；

介绍了当前通信机制的理论模型；

分别介绍了当前通信机制的C++与Python实现。

除此之外，还介绍了:

ROS中的常用命令方便操作、调试节点以及通信信息；
通过实操又将上述知识点加以整合；
最后又着重比较了话题通信与服务通信的相同点以及差异。

掌握本章内容后，基本上就可以从容应对ROS中大部分应用场景了。

至此，完成第二章介绍，最后编辑时间：2022.9.22

咱们下章见