ROS自主导航学习———ROS通信机制

前言

前面ROS很多都忘记了 ，现在来重新回顾一下内容

ROS 中的基本通信机制主要有如下三种实现策略:

• 话题通信(发布订阅模式)

• 服务通信(请求响应模式)

• 参数服务器(参数共享模式)

• Action通信

ROS常用API

第 3 章 ROS通信机制进阶 · GitBookhttp://www.autolabor.com.cn/book/ROSTutorials/di-3-zhang-ros-tong-xin-ji-zhi-jin-jie.html

话题通信(发布订阅模式)

以发布订阅的方式实现不同节点之间数据交互的通信模式。

话题通信实现模型是比较复杂的，该模型如下图所示,该模型中涉及到三个角色:

• ROS Master (管理者)
• Talker (发布者)
• Listener (订阅者)

ROS Master 负责保管 Talker 和 Listener 注册的信息，并匹配话题相同的 Talker 与 Listener，帮助 Talker 与 Listener 建立连接，连接建立后，Talker 可以发布消息，且发布的消息会被 Listener 订阅

ROS中的通信方式中，topic是常用的一种。对于实时性、周期性的消息，使用topic来传输是最佳的选择。topic是一种点对点的单向通信方式，这里的“点”指的是node，也就是说node之间可以通过topic方式来传递信息。topic要经历下面几步的初始化过程：首先，publisher节点和subscriber节点都要到节点管理器进行注册，然后publisher会发布topic，subscriber在master的指挥下会订阅该topic，从而建立起sub-pub之间的通信。注意整个过程是单向的。

其中在话题通信中值得注意的是

• Subscriber接收消息会进行处理，一般这个过程叫做回调(Callback)。所谓回调就是提前定义好了一个处理函数（写在代码中），当有消息来就会触发这个处理函数，函数会对消息进行处理
• ROS是一种分布式的架构，一个topic可以被多个节点同时发布，也可以同时被多个节点接收。

话题通信的基本操作

代码

//topic_pub1.cpp
#include"ros/ros.h"
#include
#include

int main(int args ,char *argv[])
{
    //设置编码 会在终端输出信息 防止乱码
    setlocale(LC_ALL,"");

     //初始化 ROS 节点:命名(唯一)  节点命名不能重复
    // 参数1和参数2 后期为节点传值会使用
    // 参数3 是节点名称，是一个标识符，需要保证运行后，在 ROS 网络拓扑中唯一
    ros::init(args,argv,"talker");

    //实例化句柄该类封装了 ROS 中的一些常用功能
    ros::NodeHandle nh;

     //实例化 发布者 对象
    //泛型: 发布的消息类型
    //参数1: 要发布到的话题
    //参数2: 队列中最大保存的消息数，超出此阀值时，先进的先销毁(时间早的先销毁)

    //n.advertise通过NodeHandle的对象n告诉ROS系统我要创建一个可以发布信息的对象了。这个信息是什么类型的呢？
    //告诉ROS我要发布的是标准信息中的String类型。
    //那些信息叫啥名字呢？名字叫chatter。这个chatter就是我们之前提到的topic！

    ros::Publisher pub=nh.advertise("chatter",10);

    std_msgs::String msg;

    std::string msg_fromt="day day up";
    int count =0; //计数器
    //逻辑(一秒10次) 可以制定循环的频率
    //程序如果在不断地发布信息，那么有时候我会想控制发布的信息的快慢，这行表示你希望你发布信息的速度为10Hz。
    //这个函数要和ros::Rate r.sleep()配合使用才能达到控制速度目的
    ros::Rate r(10);

    while(ros::ok())
    {
        std::stringstream ss;

        ss<

//topic_sub1.cpp
// 1.包含头文件 
#include "ros/ros.h"
#include "std_msgs/String.h"

void doMsg(const std_msgs::String::ConstPtr& msg_p){
    ROS_INFO("我听见:%s",msg_p->data.c_str());

}
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化 ROS 节点:命名(唯一)
    ros::init(argc,argv,"listener");
    //3.实例化 ROS 句柄
    ros::NodeHandle nh;

    //4.实例化 订阅者 对象
    ros::Subscriber sub = nh.subscribe("chatter",10,doMsg);
    //5.处理订阅的消息(回调函数)

    //     6.设置循环调用回调函数
    ros::spin();//循环读取接收的数据，并调用回调函数处理

    return 0;
}

话题通信自定义msg  

ROS 中通过 std_msgs 封装了一些原生的数据类型,比如:String、Int32、Int64、Char、Bool、Empty.... 但是，这些数据一般只包含一个 data 字段，结构的单一意味着功能上的局限性，当传输一些复杂的数据，比如: 激光雷达的信息... std_msgs 由于描述性较差而显得力不从心，这种场景下可以使用自定义的消息类型

其中  ros::spin() 是进入了循环执行回调函数，而 ros::spinOnce() 只会执行一次回调函数(没有循环)，在 ros::spin() 后的语句不会执行到，而 ros::spinOnce() 后的语句可以执行

功能包下新建 msg 目录，添加文件 Person.msg

string name
uint16 age
float64 height

#include"ros/ros.h"
#include
#include "Topic/Person.h"
#include

int main(int args ,char *argv[])
{
    //设置编码 会在终端输出信息 防止乱码
    setlocale(LC_ALL,"");

     //初始化 ROS 节点:命名(唯一)  节点命名不能重复
    // 参数1和参数2 后期为节点传值会使用
    // 参数3 是节点名称，是一个标识符，需要保证运行后，在 ROS 网络拓扑中唯一
    ros::init(args,argv,"person_talker");

    //实例化句柄该类封装了 ROS 中的一些常用功能
    ros::NodeHandle nh;

     //实例化 发布者 对象
    //泛型: 发布的消息类型
    //参数1: 要发布到的话题
    //参数2: 队列中最大保存的消息数，超出此阀值时，先进的先销毁(时间早的先销毁)

    //n.advertise通过NodeHandle的对象n告诉ROS系统我要创建一个可以发布信息的对象了。这个信息是什么类型的呢？
    //告诉ROS我要发布的是标准信息中的String类型。
    //那些信息叫啥名字呢？名字叫chatter。这个chatter就是我们之前提到的topic！

    ros::Publisher pub=nh.advertise("person_chatter",10);

    Topic::Person p;
    p.name="xxd";
    p.age=10;
    p.height=1.78;


    int count =0; //计数器
    //逻辑(一秒10次) 可以制定循环的频率
    //程序如果在不断地发布信息，那么有时候我会想控制发布的信息的快慢，这行表示你希望你发布信息的速度为10Hz。
    //这个函数要和ros::Rate r.sleep()配合使用才能达到控制速度目的
    ros::Rate r(10);

    while(ros::ok())
    {
        //发布消息
        pub.publish(p);
        p.age += 1;
        ROS_INFO("我叫:%s,今年%d岁,高%.2f米", p.name.c_str(), p.age, p.height);
        
        r.sleep();
        //回调函数
        ros::spinOnce();
    }
    return 0;
}

// 1.包含头文件 
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "Topic/Person.h"

void doMsg(const Topic::Person::ConstPtr& person_p){
    ROS_INFO("我听见:%s",person_p->name.c_str());

}
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化 ROS 节点:命名(唯一)
    ros::init(argc,argv,"person_listener");
    //3.实例化 ROS 句柄
    ros::NodeHandle nh;
    ROS_INFO("hello");
    //4.实例化 订阅者 对象
    ros::Subscriber sub = nh.subscribe("person_chatter",10,doMsg);

    ROS_INFO("hello2");
    // 5.设置循环调用回调函数
    ros::spin();
    ROS_INFO("hello3");
    return 0;
}

服务通信

服务通信也是ROS中一种极其常用的通信模式，服务通信是基于请求响应模式的，是一种应答机制。也即: 一个节点A向另一个节点B发送请求，B接收处理请求并产生响应结果返回给A。

Service通信是双向的，它不仅可以发送消息，同时还会有反馈。所以service包括两部分，一部分是请求方（Clinet），另一部分是应答方/服务提供方（Server）。这时请求方（Client）就会发送一个request，要等待server处理，反馈回一个reply，这样通过类似“请求-应答”的机制完成整个服务通信

代码

服务通信中，客户端提交两个整数至服务端，服务端求和并响应结果到客户端，请创建服务器与客户端通信的数据载体。

#include "ros/ros.h"
#include "Topic/AddInts.h"

// bool 返回值由于标志是否处理成功
bool doReq(Topic::AddInts::Request& req,
          Topic::AddInts::Response& resp){
    int num1 = req.num1;
    int num2 = req.num2;

    ROS_INFO("服务器接收到的请求数据为:num1 = %d, num2 = %d",num1, num2);

    //逻辑处理
    if (num1 < 0 || num2 < 0)
    {
        ROS_ERROR("提交的数据异常:数据不可以为负数");
        return false;
    }

    //如果没有异常，那么相加并将结果赋值给 resp
    resp.sum = num1 + num2;
    return true;


}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"AddInts_Server");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建 服务 对象
    ros::ServiceServer server = nh.advertiseService("AddInts",doReq);
    ROS_INFO("服务已经启动....");
    //     5.回调函数处理请求并产生响应
    //     6.由于请求有多个，需要调用 ros::spin()
    ros::spin();
    return 0;
}

#include "ros/ros.h"
#include "Topic/AddInts.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");

    // 调用时动态传值,如果通过 launch 的 args 传参，需要传递的参数个数 +3
    if (argc != 3)
    // if (argc != 5)//launch 传参(0-文件路径 1传入的参数 2传入的参数 3节点名称 4日志路径)
    {
        ROS_ERROR("请提交两个整数");
        return 1;
    }
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"AddInts_Client");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建 客户端 对象
    ros::ServiceClient client = nh.serviceClient("AddInts");
    //等待服务启动成功

    ros::service::waitForService("AddInts");

    Topic::AddInts add;
    add.request.num1=atoi(argv[1]);
    add.request.num2=atoi(argv[2]);

    bool flag=client.call(add);

        if (flag)
    {
        ROS_INFO("请求正常处理,响应结果:%d",add.response.sum);
    }
    else
    {
        ROS_ERROR("请求处理失败....");
        return 1;
    }

    return 0;
}

流程:

• 需要先启动服务:rosrun 包名 服务

• 然后再调用客户端 :rosrun 包名 客户端 参数1 参数2

参数服务器

参数服务器也可以说是特殊的“通信方式”。特殊点在于参数服务器是节点存储参数的地方、用于配置参数，全局共享参数。参数服务器使用互联网传输，在节点管理器中运行，实现整个通信过程。

在 C++ 中实现参数服务器数据的增删改查，可以通过两套 API 实现:

• ros::NodeHandle

• ros::param

代码 

//param_add.cpp
#include

int main(int argc,char *argv[])
{
    ros::init(argc,argv,"set_update_param");
    std::vector stus;
    stus.push_back("xxd");
    stus.push_back("李四");
    stus.push_back("王五");
    stus.push_back("123");

    std::map friends;
    friends["guo"] = "huang";
    friends["yuang"] = "xiao";

    ros::NodeHandle nh;
    nh.setParam("nh_int",10);
    nh.setParam("nh_double",3.14);
    nh.setParam("nh_bool",true);
    nh.setParam("nh_string","hello NodeHandle"); //字符串
    nh.setParam("nh_vector",stus); // vector
    nh.setParam("nh_map",friends); // map

    //修改
    nh.setParam("nh_int",10000);

    //param--------------------------------------------------------
    ros::param::set("param_int",20);
    ros::param::set("param_double",3.14);
    ros::param::set("param_string","Hello Param");
    ros::param::set("param_bool",false);
    ros::param::set("param_vector",stus);
    ros::param::set("param_map",friends);


    ros::param::set("param_int",20000);
    return 0;
}

//param_get.cpp
#include "ros/ros.h"
/*
        param(键,默认值) 
            存在，返回对应结果，否则返回默认值

        getParam(键,存储结果的变量)
            存在,返回 true,且将值赋值给参数2
            若果键不存在，那么返回值为 false，且不为参数2赋值

        getParamCached键,存储结果的变量)--提高变量获取效率
            存在,返回 true,且将值赋值给参数2
            若果键不存在，那么返回值为 false，且不为参数2赋值

        getParamNames(std::vector)
            获取所有的键,并存储在参数 vector 中 

        hasParam(键)
            是否包含某个键，存在返回 true，否则返回 false

        searchParam(参数1，参数2)
            搜索键，参数1是被搜索的键，参数2存储搜索结果的变量
*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"get_param");
    ROS_INFO("++++++++++++++++++++++++++++++++++++++++");
    int res3 = ros::param::param("param_int",20); //存在
    int res4 = ros::param::param("param_int2",20); // 不存在返回默认
    ROS_INFO("param获取结果:%d,%d",res3,res4);

    int param_int_value;
    double param_double_value;
    bool param_bool_value;

    std::string param_string_value;
    std::vector param_stus;
    std::map param_friends;

    ros::param::get("param_int",param_int_value);
    ros::param::get("param_double",param_double_value);
    ros::param::get("param_bool",param_bool_value);
    ros::param::get("param_string",param_string_value);
    ros::param::get("param_vector",param_stus);
    ros::param::get("param_map",param_friends);

    ROS_INFO("getParam获取的结果:%d,%.2f,%s,%d",
        param_int_value,
        param_double_value,
        param_string_value.c_str(),
        param_bool_value
        );

    for(auto &&stu:param_stus){
        ROS_INFO("stus 元素:%s",stu.c_str());   
    }
    for (auto &&f : param_friends)
    {
        ROS_INFO("map 元素:%s = %s",f.first.c_str(), f.second.c_str());
    }
    ros::param::getCached("param_int",param_int_value);
    ROS_INFO("通过缓存获取数据:%d",param_int_value);

    //getParamNames()
    std::vector param_names2;
    ros::param::getParamNames(param_names2);
    for (auto &&name : param_names2)
    {
        ROS_INFO("名称解析name = %s",name.c_str());        
    }
    ROS_INFO("----------------------------");

    ROS_INFO("存在 param_int 吗? %d",ros::param::has("param_int"));
    ROS_INFO("存在 param_intttt 吗? %d",ros::param::has("param_intttt"));

    std::string key;
    ros::param::search("param_int",key);
    ROS_INFO("搜索键:%s",key.c_str());

    return 0;
}

//param_del.cpp
#include "ros/ros.h"


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

    ros::NodeHandle nh;
    bool r1 = nh.deleteParam("nh_int");
    ROS_INFO("nh 删除结果:%d",r1);

    bool r2 = ros::param::del("param_int");
    ROS_INFO("param 删除结果:%d",r2);

    return 0;
}

action通信

ROS中有一个名为actionlib的功能包，实现了action的通信机制。

action是一种类似于Service的问答通信机制，不同之处在于action带有连续反馈，可以不断反馈任务进度，也可以在任务过程中中止运行。

我们使用action来发布一个机器人的运动目标，机器人接到这个action后，就开始运动，在运动过程中不断反馈当前的运动状态，在运动过程中我们可以取消运动，让机器人停止，如果机器人完成了运动目标，那么action会返回任务完成的标志

想要创建一个action，首先需要在action定义文件中定义三个消息格式，分别是：目标，结果，反馈

1. goal：任务目标
2. cancel：请求取消任务
3. status：通知client当前的状态
4. feedback：周期反馈任务运行的监控数据
5. result：向client发送任务的执行结果，这个topic只会发布一次。

#include 
#include

typedef actionlib::SimpleActionClient Client;

void doneCb(const actionlib::SimpleClientGoalState& state,const ActionTest::AddIntsResultConstPtr &result)
{
    ROS_INFO("Yay! The dishes are now clean");
    ros::shutdown();
}
void activeCb()
{
   ROS_INFO("Goal just went active");
}
void feedbackCb(const ActionTest::AddIntsFeedbackConstPtr& feedback)
{
      ROS_INFO(" percent_complete : %f ", feedback->percent_complete);
}
int main(int argc, char** argv)
{
    ros::init(argc,argv,"action_test");
    Client client("do_action",true);
    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");

    ActionTest::AddIntsGoal goal;
    goal.dishwasher_id=1;

       // 发送action的goal给服务器端，并且设置回调函数
   client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);

   ros::spin();

   return 0;
    
}

#include 
#include 
#include "ActionTest/AddIntsAction.h"

typedef actionlib::SimpleActionServer Server;

void execute(const ActionTest::AddIntsGoalConstPtr &goal ,Server *as)
{
    ros::Rate r(1);
    ActionTest::AddIntsFeedback feedback;

    ROS_INFO("Dishwasher %d is working.", goal->dishwasher_id);

   // 按照1hz的频率发布进度feedback
   for(int i=1; i<=10; i++)
   {
       feedback.percent_complete = i * 10;
       as->publishFeedback(feedback);
       r.sleep();
   }
    // 当action完成后，向客户端返回结果
   ROS_INFO("Dishwasher %d finish working.", goal->dishwasher_id);
   as->setSucceeded();
}
int main (int argc,char *argv[])
{
    ros::init(argc,argv,"action2");
    ros::NodeHandle nh;

    Server server(nh,"do_action", boost::bind(&execute, _1, &server), false);
    server.start();
   ros::spin();

}

通信机制实操

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

话题发布

编码实现乌龟运动控制，让小乌龟做圆周运动

/*
    编写 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("/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;
}

 话题订阅

#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("/turtle1/pose",1000,doPose);
    // 5.回调函数处理订阅的数据
    // 6.spin
    ros::spin();
    return 0;
}

服务调用

#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("/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;
}

 

通信对比

Reference

机器人操作系统ROS:从入门到放弃(一)　发布接收消息 - 简书

ROS技术点滴 —— action通信 - 知乎

代码      第 2 章 ROS通信机制 · GitBook

赵老师的视频 【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程_哔哩哔哩_bilibili