ROS入门 Client Library与roscpp

一、Client Library

ROS为机器人开发者们提供了不同语言的编程接口，比如C++接口叫做roscpp，Python接口叫做rospy，Java接口叫做rosjava。即使语言不通，利用这些接口，ROS的编写都可以实现topic、service的编写。目前最常用的只有roscpp和rospy，而其余的语言版本基本都还是测试版。

二、roscpp

roscpp实际上就是利用c++文件编写ROS的通信文件，在ROS中c++用的是catkin这个编译系统进行编译的，在c++文件中调用ros时，就需要先引入ros/ros.h，之后就可以利用roscpp预留的接口，快速开发一些功能，常用的接口如下：

ros::init() : 解析传入的ROS参数 用于初始化节点 是创建node第一步需要用到的函数
ros::NodeHandle : 创建节点的句柄 和topic、service、param等交互的公共接口
ros::master : 包含从master查询信息的函数
ros::this_node：包含查询这个进程(node)的函数
ros::service：包含查询服务的函数
ros::param：包含查询参数服务器的函数，而不需要用到NodeHandle
ros::names：包含处理ROS图资源名称的函数

三、roscpp编写Topic代码

#include<sstream>
#include"ros/ros.h"
#include"std_msgs/String.h"int main(int argc, char **argv)
{ros::init(argc,argv,"talker");ros::NodeHandle n;ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter",1000);ros::Rate loop_rate(10);int count = 0;while (ros::ok()){std_msgs::String msg; std::stringstream ss;ss<<"hello world"<<count;msg.data = ss.str();ROS_INFO("%s",msg.data.c_str());chatter_pub.publish(msg);ros::spinOnce();loop_rate.sleep();count++;}return 0;
}

作为最简单的发布订阅机制的演示代码，这段代码仅仅传递了一句简单的字符串helloworld，作为话题的发布者，做的主要的事有三个：初始化节点、初始化话题、持续不断地向话题发送自己的信息。

对于这段代码，初始化节点用了两句代码：

 ros::init(argc,argv,"talker");// 初始化ROS节点 第三个参数表示创建的节点的名称ros::NodeHandle n;// 创建一个节点句柄 方便管理节点 后面就可以用这一个句柄来代替创建的节点

其中创建ROS节点用的是init函数，这个函数有三个参数，前两个参数是命令行或者launch文件输入的参数，可以用来完成命名重映射等功能，第三个参数定义了这个节点的名称，也就是说这个publisher节点的名字叫做talker，这个名字在运行的ROS中必须是独一无二的，不允许同时存在两个名字一样的节点。创建节点句柄是为了后面操作方便，就不需要再输入很长的名字了。

注册话题则用了两句代码：

 ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter",1000);// 在Master端注册一个Publisher 告知系统该结点会发送chatter话题 话题的内容为Stringros::Rate loop_rate(10);// 设置循环的频率

这里利用Publisher函数注册了一个话题，叫做chatter，发布者是句柄n代表的节点，并且告诉系统发送的消息是String类型的消息。第二个参数表示消息队列的长度。之后设置循环的频率，主要是在调用sleep的时候，根据这里的频率设置休眠时间。

最后是循环发送消息的部分：

 while (ros::ok()){// 在节点未出现错误的情况下一直进行循环std_msgs::String msg; std::stringstream ss;ss<<"hello world"<<count;msg.data = ss.str();// 初始化即将发布的信息ROS_INFO("%s",msg.data.c_str());// 打印日志信息chatter_pub.publish(msg);// 发布封装完毕的消息ros::spinOnce();//  处理节点订阅话题的所有回调函数 虽然在这里没有回调函数loop_rate.sleep();// 让节点进入休眠状态count++;}

循环的条件是在ros节点没有出现错误的情况一直循环发送消息，之后初始化即将发布的消息，消息的类型是String，名字叫做msg（味精），之后利用流输入给msg中唯一的成员data赋值，最后利用publish函数将这个msg发送出去。

#include"ros/ros.h"
#include"std_msgs/String.h"void chatterCallback(const std_msgs::String::ConstPtr& msg)
{ROS_INFO("I heard %s",msg->data.c_str());
}
int main(int argc, char **argv)
{ros::init(argc,argv,"listener");// 初始化ROS节点 第三个参数表示创建的节点的名称ros::NodeHandle n;// 创建一个节点句柄 方便管理节点 后面就可以用这一个句柄来代替创建的节点ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);// 创建subscriber 并且订阅前面程序中publisher发布的chatter话题 ros::spin();return 0;
}

subscriber的代码和publisher的代码本身是很像的，都是初始化节点，区别在于由于订阅者是利用回调函数去响应信息的，在这里就需要改一改订阅的写法，主函数第三行代码就是，这个函数有三个参数，第一个参数是订阅的话题的名称，第二个参数是消息队列的长度，第三个参数是回调函数的名称，也就是提前写好的回调函数，回调函数里只有简单的一句代码，用来打印收到的消息。

代码按照下面的组织形式存放后，对CMakeList.txt文件进行修改，增加可执行文件等内容，由于使用的RoboWare可以自动添加，所以就省去了配置的时间。

之后使用catkin_make指令编译项目，完成后source一下工作空间，就可以运行程序了。运行程序首先开一个roscore，之后用rosrun的指令打开两个节点，最后效果如下：

可以看见，发布时的信息和订阅者收到的信息刚好是对应的，说明代码运行成功。

最后在关闭程序时稍作补充，关闭publisher后subscriber也没有新的信息显示，重新打开publisher后新的信息会继续显示，关闭roscore并不影响现有的两个节点之间信息的传递，信息依然正常显示，正好证明了Master节点负责的是连接的建立，而消息传递则不从Master那里经手。

最后总结一下，一个最基本的topic演示要写两个代码，publisher代码中初始化节点-初始化话题-循环发送消息，subscriber代码中初始化节点-订阅话题-利用回调函数处理消息。代码写好后修改CMakeList.txt文件，使用catkin_make指令编译。运行是打开Master和两个编写的节点，就可以看见收发的消息。

四、自定义消息类型

自定义消息主要是使用msg文件进行操作，在package下面新建msg文件夹用于存放专门的msg文件，新建文件后编写msg文件的内容，这个文件的内容很像一个结构体，具体如下：

float32[] data
float32 vel
geometry_msgs/Pose pose
string name

在这个消息中，依赖了其它包中的消息类型，需要在后面的package.xml文件中进行修改。写好消息类型之后需要修改两个配置文件，如果是在RoboWare里面写的话，一般是不需要自己再修改的，IDE会自动补全配置文件的内容，具体的修改可以参考：https://blog.csdn.net/u013453604/article/details/72903398

修改好之后一定要保存再编译，RoboWare不带自动保存功能！

编译好后可以从控制台查看消息的类型：

再对程序做一下修改，修改的地方并不多，主要还是修改一下消息类型相关的部分，因为换了类型，所以一些写法上需要做改变：

#include<sstream>
#include"ros/ros.h"
#include"std_msgs/String.h"
#include<test2/test.h>
// 由于使用了自定义的消息 需要引入头文件
int main(int argc, char **argv)
{ros::init(argc,argv,"talker");ros::NodeHandle n;ros::Publisher chatter_pub = n.advertise<test2::test>("chatter",1000);// 在Master端注册一个Publisher 告知系统该结点会发送chatter话题 话题的内容换成了自己定义的msg 定义的类型用尖括号包裹ros::Rate loop_rate(10);int count = 0;while (ros::ok()){std::stringstream ss;ss<<"hello world"<<count;test2::test msg;// 创建消息的写法也要改变msg.data={1,2,3,4};msg.vel=count;msg.name=ss.str();// 初始化即将发布的信息ROS_INFO("%f",msg.vel);chatter_pub.publish(msg);ros::spinOnce();loop_rate.sleep();count++;}return 0;
}
#include"ros/ros.h"
#include"std_msgs/String.h"
#include<test2/test.h>void chatterCallback(const test2::test::ConstPtr& msg)
// 换用了自定义的消息格式 形参的类型也要改变
{ROS_INFO("I heard %f",msg->vel);
}
int main(int argc, char **argv)
{ros::init(argc,argv,"listener");ros::NodeHandle n;ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);ros::spin();return 0;
}

最后运行效果如下：

如果要引用别的包下面的自定义消息，操作也一样，只不过在编写publisher和subscriber时，要修改消息的类型和引用的头文件，基本的格式都是包名+msg文件名。

五、roscpp编写Service代码

编写Service从本质上来看和Topic的代码思路是一样的，新建两个节点，之后建立两个节点之间的关系，编译之后运行。但是区别于Topic，编写Servie时必须要初始化自己的srv，因为srv没有ros已经写好的，是需要自己去配置的，所以第一步就是先写自己的srv文件。

int64 a
int64 b
---
int64 sum

srv文件可以看做是两个结构体，用—划分为两部分，上面的是请求应该满足的格式，下面的则是响应应该满足的格式，只有满足格式才能够正确通信。

写好的srv文件放在package下面的srv文件夹里，之后就可以写Server和client文件了：

#include"ros/ros.h"
#include"srv_test/AddTwoInts.h"bool add(srv_test::AddTwoInts::Request &req,srv_test::AddTwoInts::Response &res)
{res.sum=req.a+req.b;ROS_INFO("request: x=%ld,y=%ld",(long int)req.a,(long int)req.b);ROS_INFO("sending back response:[%ld]",(long int)res.sum);return true;
} int main(int argc,char **argv)
{ros::init(argc,argv,"add_two_ints_server");ros::NodeHandle n;ros::ServiceServer service = n.advertiseService("add_two_ints",add);ROS_INFO("Ready to add two ints");ros::spin();return 0;
}

这段代码也是先初始化节点和句柄，和topic不同的是这里建立service的语句换了，使用的是ros::ServiceServer service = n.advertiseService(“add_two_ints”,add)，这句代码相当于初始化了一个句柄叫做service的service，提供这个service的节点叫add_two_ints_server，提供的service名字叫做add_two_ints，收到消息之后使用名字叫做add的回调函数处理收到的请求。回调函数的写法要注意，形参一共有两个，正好对应请求和响应，尤其是类型一定要和自己写的rsv文件对应。

#include"ros/ros.h"
#include"srv_test/AddTwoInts.h"
#include<cstdlib>int main(int argc,char **argv)
{ros::init(argc,argv,"add_two_ints_client");ros::NodeHandle n;ros::ServiceClient client = n.serviceClient<srv_test::AddTwoInts>("add_two_ints");srv_test::AddTwoInts srv;srv.request.a=atoll(argv[1]);srv.request.b=atoll(argv[2]);if(client.call(srv))ROS_INFO("Sum:%ld",(long int)srv.response.sum);return 0;}

client的代码相对要简单一些，因为不需要写回调函数，关键的一部分是对client的初始化和操作：

 ros::ServiceClient client = n.serviceClient<srv_test::AddTwoInts>("add_two_ints");// 初始化一个句柄叫做client的client 采用的srv为AddTwoInts文件 要使用的service是add_two_intssrv_test::AddTwoInts srv;// 准备一个请求srv.request.a=atoll(argv[1]);srv.request.b=atoll(argv[2]);// 给请求的两个参数赋值if(client.call(srv))// 如果收到了响应就打印结果信息ROS_INFO("Sum:%ld",(long int)srv.response.sum);

最后运行结果如图：