python ros 多个publisher ros python接口

一、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话题 话题的内容为String
    ros::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会自动补全配置文件的内容，具体的修改可以参考：

修改好之后一定要保存再编译，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_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);

最后运行结果如图：