C++ 编写 ROS node

引言

用 C++ 编写的 ROS node 程序本质上依然是 C++ 程序，只不过额外添加了 ROS 相关的内容，尤其是与 ROS 的接口，包括初始化 node，关闭 node，pub topic，sub topic 等。

尽管 ROS node 功能千差万别，但基本框架是通用的。本文以 ROS 官网程序为例，简单分析 C++ 编写 ROS node 的基本框架，方便以后写程序时参考。

程序示例

创建 ROS node，名为 "listener"，接收 "chatter" topic 上的数据，然后通过 ROS_INFO 的形式记录下来，默认也会在屏幕显示。

#include "ros/ros.h"
#include "std_msgs/String.h"

// chatterCallback 是回调函数（callback function），每当 node 接收到 topic 上的数据，就会调用该函数。
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
// 初始化 node，并指定名字。必须先执行这一步，生成 node，再进行后续 ros 相关的操作。
// argc, argv 是必须写的，以便接受额外的参数，例如对 node 重命名。
  ros::init(argc, argv, "listener");

// 生成 node 句柄，也就是与当前 node 交互的接口。后续对 node 的操作都要通过 node 句柄实施。
  ros::NodeHandle n;

// 生成 Subscriber 对象 sub，用来监听 "chatter" topic，收到数据之后，等待 spin() 执行时，调用回调函数 chatterCallback。
// 1000 为缓冲区长度，表明回调函数被执行之前，可以存储多少 msg
// 默认将 msg 作为第一个参数送入回调函数。
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

// spin() 函数可以看作是不断循环的函数，每次循环时回调函数处理队列中的数据。
// spin() 函数可以保证程序不退出，一直等待 topic 上的数据。
// ctrl+c 或者来自 ROS master 的命令可以退出 spin() 循环
  ros::spin();

// 当结束 spin() 循环时，程序结束，返回 0.
  return 0;
}

关于 NodeHandle

对于句柄（Handle）这个概念，可以把它理解为一个“把手”，只要握住了门把手,就可以很容易把整扇门拉开，而不必关心门是什么样子。类似地，NodeHandle就是对节点资源的描述，有了它就可以操作这个节点了，比如为程序提供服务、监听某个topic上的消息、访问和修改 param 等。

在 ROS 中，NodeHandle 是 ROS 自带的类，通过 include<ros/ros.h> 调用。

NodeHandle常用 methods 包括:

• advertise(const string &topic, uint32_t queue_size, bool latch=false)
  向 topic 发送数据
  第一个参数为 topic 的名称。
  第二个是数据队列的最大长度，如果缓存中 topic msg 超过这个长度，那么旧的消息就会被删除。
  第三个参数表明是否在 topic 上启用锁存功能。某些 topic 中数据更新频率很低，甚至是静态的，比如在机器人定位中用到的 map 数据。如果启用了 topic 的锁存功能，则该 topic 上最后一次发布的数据将被保留，每次有 node sub 该 topic 时，就发送该数据。

• subscribe(const string &topic, uint32_t queue_size, void(*)(M));
  接受 topic 中的数据
  第一个参数是 topic 的名称。
  第二个参数是缓冲区 msg 队列的最大长度，如果接收的数据超出了缓存长度，则丢掉旧的数据。
  第三个参数是回调函数指针，指向回调函数来处理接收到的数据

• advertiseService(const string &service, bool(*srv_func)(Mreq &, Mres &));
  创建 server，作为 service 通讯中的服务端
  第一个参数是该 server 提供的 service 名称
  第二个参数是服务函数的指针，指向服务函数。服务函数接受两个参数，分别为请求和响应。

• serviceClient(const string &service_name, bool persistent=false)
  创建服务的 client
  第一个参数为 service 名称
  第二个参数用于设置服务的连接是否持续，如果为 true，client 将会保持与远程主机的连接,这样后续的请求会快一些。

Ros Param 相关的 methods

• getParam(const std::string &key, std::string &s) const
• getParam(const std::string &key, double &d) const
• getParam(const std::string &key, int &i) const

从参数服务器上获取 key 对应的值，调用时要区分不同类型的数据

• setParam(const std::string &key, const std::string &s) const
• setParam(const std::string &key, const char *s) const
• setParam(const std::string &key, int i) const

给 key 赋值，调用时要区分不同类型的数据

Written by SH
Revised by QP