ROS通信机制

ROS通信机制

为了解耦合，在ROS中每一个功能点都是一个单独的进程，每一个进程都是独立运行的。更确切的讲，ROS是进程（也称为Nodes**）的分布式框架。** 因为这些进程甚至还可分布于不同主机，不同主机协同工作，从而分散计算压力。不过随之也有一个问题: 不同的进程是如何通信的？也

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

• 话题通信(发布订阅模式)
• 服务通信(请求响应模式)
• 参数服务器(参数共享模式)

参考和完整代码：第 2 章 ROS通信机制 · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程

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

发布方

1. 包含头文件 、导包
2. 初始化 ROS 节点:命名(唯一)
3. 实例化 ROS 句柄（python中不需初始化句柄）
4. 实例化 发布者 对象
5. 组织被发布的数据，并编写逻辑发布数据

#include "ros/ros.h"
#include "std_msgs/String.h" //普通文本类型的消息
#include <sstream>

int main(int argc, char  *argv[])
{   
    //设置编码
    setlocale(LC_ALL,"");

    //2.初始化 ROS 节点:命名(唯一)
    // 参数1和参数2 后期为节点传值会使用
    // 参数3 是节点名称，是一个标识符，需要保证运行后，在 ROS 网络拓扑中唯一
    ros::init(argc,argv,"talker");
    //3.实例化 ROS 句柄
    ros::NodeHandle nh;//该类封装了 ROS 中的一些常用功能

    //4.实例化 发布者 对象
    //泛型: 发布的消息类型
    //参数1: 要发布到的话题
    //参数2: 队列中最大保存的消息数，超出此阀值时，先进的先销毁(时间早的先销毁)
    ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",10);

    //5.组织被发布的数据，并编写逻辑发布数据
    //数据(动态组织)
    std_msgs::String msg;
    // msg.data = "你好啊！！！";
    std::string msg_front = "Hello 你好！"; //消息前缀
    int count = 0; //消息计数器

    //逻辑(一秒10次)
    ros::Rate r(1);

    //节点不死
    while (ros::ok())
    {
        //使用 stringstream 拼接字符串与编号
        std::stringstream ss;
        ss << msg_front << count;
        msg.data = ss.str();
        //发布消息
        pub.publish(msg);
        //加入调试，打印发送的消息
        ROS_INFO("发送的消息:%s",msg.data.c_str());

        //根据前面制定的发送贫频率自动休眠 休眠时间 = 1/频率；
        r.sleep();
        count++;//循环结束前，让 count 自增
        //暂无应用
        ros::spinOnce();
    }
    return 0;
}

import rospy
from std_msgs.msg import String

if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("talker_p")
    #3.实例化 发布者 对象
    pub = rospy.Publisher("chatter",String,queue_size=10)
    #4.组织被发布的数据，并编写逻辑发布数据
    msg = String()  #创建 msg 对象
    msg_front = "hello 你好"
    count = 0  #计数器 
    # 设置循环频率
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():

        #拼接字符串
        msg.data = msg_front + str(count)

        pub.publish(msg)
        rate.sleep()
        rospy.loginfo("写出的数据:%s",msg.data)
        count += 1

订阅方

1. 包含头文件
2. 定义接收到数据的回调函数
3. 初始化
4. 设置循环调用回调函数

#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());
    // 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<std_msgs::String>("chatter",10,doMsg);
    //5.处理订阅的消息(回调函数)

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

    return 0;
}

import rospy
from std_msgs.msg import String

def doMsg(msg):
    rospy.loginfo("I heard:%s",msg.data)

if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("listener_p")
    #3.实例化 订阅者 对象
    sub = rospy.Subscriber("chatter",String,doMsg,queue_size=10)
    #4.处理订阅的消息(回调函数)
    #5.设置循环调用回调函数
    rospy.spin()

配置 CMakeLists.txt

add_executable(Hello_pub
  src/Hello_pub.cpp
)
add_executable(Hello_sub
  src/Hello_sub.cpp
)

target_link_libraries(Hello_pub
  ${catkin_LIBRARIES}
)
target_link_libraries(Hello_sub
  ${catkin_LIBRARIES}
)

catkin_install_python(PROGRAMS
  scripts/talker_p.py
  scripts/listener_p.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

自定义msg

需求:创建自定义消息，该消息包含人的信息:姓名、身高、年龄等。

流程

1. 按照固定格式创建 msg 文件
2. 编辑配置文件
3. 编译生成可以被 Python 或 C++ 调用的中间文件

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

string name
uint16 age
float64 height

package.xml中添加编译依赖与执行依赖

      <build_depend>message_generation</build_depend>
      <exec_depend>message_runtime</exec_depend>
      <!-- 
      exce_depend 以前对应的是 run_depend 现在非法
      -->

CMakeLists.txt编辑 msg 相关配置

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)
# 需要加入 message_generation,必须有 std_msgs


## 配置 msg 源文件
add_message_files(
  FILES
  Person.msg
)


# 生成消息时依赖于 std_msgs
generate_messages(
  DEPENDENCIES
  std_msgs
)


#执行时依赖
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES demo02_talker_listener
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

服务通信理论模型

服务通信自定义srv

需求:

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

流程:

srv 文件内的可用数据类型与 msg 文件一致，且定义 srv 实现流程与自定义 msg 实现流程类似:

1. 按照固定格式创建srv文件

2. 编辑配置文件

3. 编译生成中间文件

功能包下新建 srv 目录，添加 xxx.srv 文件，内容:

    # 客户端请求时发送的两个数字
    int32 num1
    int32 num2
    ---
    # 服务器响应发送的数据
    int32 sum

package.xml中添加编译依赖与执行依赖

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>
  <!-- 
  exce_depend 以前对应的是 run_depend 现在非法
  -->

CMakeLists.txt编辑 srv 相关配置

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)
# 需要加入 message_generation,必须有 std_msgs
add_service_files(
  FILES
  AddInts.srv
)


generate_messages(
  DEPENDENCIES
  std_msgs
)

服务端

1. 包含头文件
2. 初始化 ROS 节点
3. 创建 ROS 句柄
4. 创建 服务 对象
5. 回调函数处理请求并产生响应
6. 由于请求有多个，需要调用 ros::spin()

bool doReq(demo03_server_client::AddInts::Request& req,
          demo03_server_client::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;
}

import rospy
from demo03_server_client.srv import AddInts,AddIntsRequest,AddIntsResponse
# 回调函数的参数是请求对象，返回值是响应对象
def doReq(req):
    # 解析提交的数据
    sum = req.num1 + req.num2
    rospy.loginfo("提交的数据:num1 = %d, num2 = %d, sum = %d",req.num1, req.num2, sum)

    # 创建响应对象，赋值并返回
    # resp = AddIntsResponse()
    # resp.sum = sum
    resp = AddIntsResponse(sum)
    return resp


if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("addints_server_p")
    # 3.创建服务对象
    server = rospy.Service("AddInts",AddInts,doReq)
    # 4.回调函数处理请求并产生响应
    # 5.spin 函数
    rospy.spin()

客户端

1. 包含头文件
2. 初始化 ROS 节点
3. 创建 ROS 句柄
4. 创建 客户端 对象
5. 请求服务，接收响应

#include "ros/ros.h"
#include "demo03_server_client/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<demo03_server_client::AddInts>("AddInts");
    //等待服务启动成功
    //方式1
    ros::service::waitForService("AddInts");
    //方式2
    // client.waitForExistence();
    // 5.组织请求数据
    demo03_server_client::AddInts ai;
    ai.request.num1 = atoi(argv[1]);
    ai.request.num2 = atoi(argv[2]);
    // 6.发送请求,返回 bool 值，标记是否成功
    bool flag = client.call(ai);
    // 7.处理响应
    if (flag)
    {
        ROS_INFO("请求正常处理,响应结果:%d",ai.response.sum);
    }
    else
    {
        ROS_ERROR("请求处理失败....");
        return 1;
    }
    return 0;
}

    import rospy
    from demo03_server_client.srv import *
    import sys
    
    if __name__ == "__main__":
    
        #优化实现
        if len(sys.argv) != 3:
            rospy.logerr("请正确提交参数")
            sys.exit(1)
    
        # 2.初始化 ROS 节点
        rospy.init_node("AddInts_Client_p")
        # 3.创建请求对象
        client = rospy.ServiceProxy("AddInts",AddInts)
        # 请求前，等待服务已经就绪
        # 方式1:
        # rospy.wait_for_service("AddInts")
        # 方式2
        client.wait_for_service()
        # 4.发送请求,接收并处理响应
        # 方式1
        # resp = client(3,4)
        # 方式2
        # resp = client(AddIntsRequest(1,5))
        # 方式3
        req = AddIntsRequest()
        # req.num1 = 100
        # req.num2 = 200 
    
        #优化
        req.num1 = int(sys.argv[1])
        req.num2 = int(sys.argv[2]) 
    
        resp = client.call(req)
        rospy.loginfo("响应结果:%d",resp.sum)

参数服务器

参数服务器在ROS中主要用于实现不同节点之间的数据共享。参数服务器相当于是独立于所有节点的一个公共容器，可以将数据存储在该容器中，被不同的节点调用。

新增(修改)参数

参数服务器操作之新增与修改(二者API一样)C++实现:

ros::NodeHandle nh;
nh.setParam("键",值)
    
ros::param::set("键","值")

    rospy.set_param()

获取参数

ros::NodeHandle

    param(键,默认值) 
        存在，返回对应结果，否则返回默认值

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

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

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

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

    searchParam(参数1，参数2)
        搜索键，参数1是被搜索的键，参数2存储搜索结果的变量

ros::param ----- 与 NodeHandle 类似

    rospy
        get_param(键,默认值)
            当键存在时，返回对应的值，如果不存在返回默认值
        get_param_cached(键)

删除参数

ros::NodeHandle
    deleteParam("键")
    根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false

ros::param
    del("键")
    根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false

try:
    rospy.delete_param("p_int")
except Exception as e:
    rospy.loginfo("删除失败")