使用python编写ros节点

1.服务通信理论模型

2. C++实现：

2.1 服务端实现：

#include "ros/ros.h"
#include "plumbing_server_client/AddInts.h"
/*
    服务端实现：解析客户提交的数据，并运算再产生响应
        1.包含头文件
        2.初始化ROS节点
        3.创建节点句柄
        4.创建一个服务对象
        5.处理请求并产生响应
        6.spin()
*/

bool doNums(plumbing_server_client::AddInts::Request &request, 
            plumbing_server_client::AddInts::Response &response){
            //1.处理请求
            int num1 = request.num1;
            int num2 = request.num2;
            ROS_INFO("收到的请求数据: num1 = %d,num2 = %d",num1, num2);
            //2.组织响应
            int sum = num1 + num2;
            response.sum = sum;
            ROS_INFO("求和结果: sum =%d ",sum);
    return true;
}

int main(int argc, char *argv[])
{
        setlocale(LC_ALL,"");
        // 2.初始化ROS节点
        ros::init(argc,argv,"heiShui"); //节点名称需要保证唯一
        // 3.创建节点句柄
        ros::NodeHandle nh;
        // 4.创建一个服务对象
        ros::ServiceServer server = nh.advertiseService("addInts",doNums);
        ROS_INFO("服务器端启动");
        // 5.处理请求并产生响应
        // 6.spin()
        ros::spin();

    return 0;
}

添加相关配置

这里是引用服务通信自定义srv调用A(C++)

2.1.1 输出

2.2 客户端实现

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

/*
    客户端：提交两个整数，并处理响应的结果
        1.包含头文件
        2.初始化ROS节点
        3.创建节点句柄
        4.创建一个客户端对象
        5.提交请求并处理响应
*/

int main(int argc, char *argv[])
{
        setlocale(LC_ALL,"");
        // 2.初始化ROS节点
        ros::init(argc,argv,"daBao");
        // 3.创建节点句柄
        ros::NodeHandle nh;
        // 4.创建一个客户端对象
        ros::ServiceClient client = nh.serviceClient<plumbing_server_client::AddInts>("addInts");
        // 5.提交请求并处理响应
        plumbing_server_client::AddInts ai;
        //5-1.组织请求
        ai.request.num1 = 100;
        ai.request.num2 = 200;
        //5-2.处理响应
        bool flag = client.call(ai);
        if (flag)
        {
            ROS_INFO("响应成功!");
            //获取结果
            ROS_INFO("响应结果 = %d",ai.response.sum);
        }else
        {
            ROS_INFO("处理失败......");
        }
    return 0;
}

2.2.1 输出：

2.3 优化

在优化实现与组织请求中做修改：

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

/*
    客户端：提交两个整数，并处理响应的结果
        1.包含头文件
        2.初始化ROS节点
        3.创建节点句柄
        4.创建一个客户端对象
        5.提交请求并处理响应

    实现参数的动态提交：
        1.格式：rosrun xxxxx xxxxx 12 34
        2.节点执行时，需要获取命令中的参数，并组织进 request
*/

int main(int argc, char *argv[])
{
        setlocale(LC_ALL,"");
        //优化实现，获取命令中参数
        if(argc !=3){
            ROS_INFO("提交的参数个数不对。");
            return 1;
        }
        // 2.初始化ROS节点
        ros::init(argc,argv,"daBao");
        // 3.创建节点句柄
        ros::NodeHandle nh;
        // 4.创建一个客户端对象
        ros::ServiceClient client = nh.serviceClient<plumbing_server_client::AddInts>("addInts");
        // 5.提交请求并处理响应
        plumbing_server_client::AddInts ai;
        //5-1.组织请求
        ai.request.num1 = atoi(argv[1]);
        ai.request.num2 = atoi(argv[2]);
        //5-2.处理响应
        bool flag = client.call(ai);
        if (flag)
        {
            ROS_INFO("响应成功!");
            //获取结果
            ROS_INFO("响应结果 = %d",ai.response.sum);
        }else
        {
            ROS_INFO("处理失败......");
        }
    return 0;
}

输出：

2.4 注意事项：

要使先启动客户端再启动服务端不抛异常：
问题：
如果先启动客户点，那么会请求异常
需求：
如果先启动客户端，不直接抛异常，而是直接挂起，等服务器启动后，再正常请求
解决：
在ROS中内置了相关函数，这些函数可以让客户端启动后挂起，等待服务器启动
client.waitForExistence(); //二选一，都是挂起功能
ros::service::waitForService(“服务话题”);

（客户端例子5-2处）加入：

client.waitForExistence(); //二选一，都是挂起功能
ros::service::waitForService("addInts");

2.4.1 输出：

3. Python实现

3.1 服务端实现：

#! /usr/bin/env python

from urllib import response
import rospy
#from plumbing_server_client.srv import AddInts, AddIntsRequest, AddIntsResponse
from plumbing_server_client.srv import *    #*:包括上面所有的

"""
    服务端:解析客户端请求,产生响应
        1.导包
        2.初始化ROS节点
        3.创建服务端对象
        4.处理逻辑(回调函数)
        5.spin()
"""
#参数：封装了请求数据
#返回值：响应数据
def doNum(request):
    #1.解析提交的两个整数
    num1 = request.num1
    num2 = request.num2
    #2.求和
    sum = num1 + num2
    #3.将结果封装进响应
    response = AddIntsResponse()
    response.sum = sum
    rospy.loginfo("服务器解析的数据num1 = %d, num2 = %d, 响应的结果:sum=%d", num1, num2, sum)
    return response

if __name__ == "__main__":
        # 2.初始化ROS节点
        rospy.init_node("heiShui")
        # 3.创建服务端对象
        server = rospy.Service("addInts",AddInts,doNum)
        rospy.loginfo("服务器已经启动了")
        # 4.处理逻辑(回调函数)
        # 5.spin()
        rospy.spin()

3.2 输出：

4.1 客户端实现

#! /usr/bin/env python
import rospy
from plumbing_server_client.srv import *
import sys

"""
    客户端:组织并提交请求,处理客户端响应
        1.导包
        2.初始化ROS节点
        3.创建客户端对象
        4.组织请求数据,并发送请求
        5.处理响应

    优化实现:
        可以在执行节点时,动态传入参数
        引入sys
"""

if __name__ == "__main__":
    #判断参数长度
    if len(sys.argv) != 3:
        rospy.logerr("传入的参数个数不对。")
        sys.exit(1)

    # 2.初始化ROS节点
    rospy.init_node("erHei")
    # 3.创建客户端对象
    client = rospy.ServiceProxy("addInts",AddInts)
    # 4.组织请求数据,并发送请求
    num1 = int(sys.argv[1])
    num2 = int(sys.argv[2])
    response = client .call(12,34)
    # 5.处理响应
    rospy.loginfo("响应的数据:%d",response.sum)

4.2 输出：

2.4 注意事项：

要使先启动客户端再启动服务端不抛异常：
问题：
如果先启动客户点，那么会请求异常
需求：
如果先启动客户端，不直接抛异常，而是直接挂起，等服务器启动后，再正常请求
解决：
在ROS中内置了相关函数，这些函数可以让客户端启动后挂起，等待服务器启动，方案二选一

client.wait_for_service();  #二选一
rospy.wait_for_servic("话题名称")

2.4.1 修改

3与4做修改

2.4.2 输出：