ROS服务与客户端编写详解

float64 init_x
float64 init_y
float64 init_z
float64 delta_x
float64 delta_y
float64 delta_z
---
float64 x
float64 y
float64 z

// 包括了ROS常用的头文件
#include "ros/ros.h"

// 创建srv文件时生成的头文件，其中kinematics_demo为软件包名称，trans为srv文件名称，并在其后加上.h扩展名
#include "kinematics_demo/trans.h"

// 软件包名与srv文件名共同组成一个类名（kinematics_demo::trans），此类包括两个成员变量request、response和两个类定义Request、Response
// 服务中执行主要功能的回调函数的原型
bool trans_pose(kinematics_demo::trans::Request &req,
                kinematics_demo::trans::Response &res);

int main(int argc, char **argv)
{
    // 初始化ROS，并指定节点名称为kindmatics_sever
    ros::init(argc, argv, "kinematics_server");

    // 为节点创建句柄n
    ros::NodeHandle n;

    // 创建名称是trans_pose的服务，并注册回调函数trans_pose
    // 第一个trans_pose为服务名称，第二个trans_pose为回调函数名
    ros::ServiceServer service = n.advertiseService("trans_pose", trans_pose);
    ROS_INFO("Kinematics server");

    // 循环等待回调函数
    ros::spin();

    return 0;
}

bool trans_pose(kinematics_demo::trans::Request &req,
                kinematics_demo::trans::Response &res)
{
    res.x = req.init_x + req.delta_x;
    res.y = req.init_y + req.delta_y;
    res.z = req.init_z + req.delta_z;
    
    ROS_INFO("Pose before trans: (%f, %f, %f)", (double)req.init_x, (double)req.init_y, (double)req.init_z);
    ROS_INFO("Pose trans:        (%f, %f, %f)", (double)req.delta_x, (double)req.delta_y, (double)req.delta_z);
    ROS_INFO("Pose after trans:  (%f, %f, %f)", (double)res.x, (double)res.y, (double)res.z);

    return true;
}

// 包括了ROS常用的头文件
#include "ros/ros.h"
// 创建srv文件时生成的头文件，其中kinematics_demo为软件包名称，trans为srv文件名称，并在其后加上.h扩展名
#include "kinematics_demo/trans.h"
#include <cstdlib>

// 第一个形参argc(argument counter)表示main函数的参数个数
// 第二个形参argv(argument value)表示main函数的参数值
// 也可写为int main(int argc, char *argv[])，即由指针组成的数组，数组中每个元素都是指向char的指针
// 当不带参数运行主函数时，操作系统向主函数传递的参数argc为1，而argv[0]（是一个指针）指向程序的路径及名称
// 当带参数运行主函数时，操作系统向主函数传递的参数argc为1加上参数个数，argv[0]意义不变，从arg[1]开始依次指向参数字符串

int main(int argc, char **argv)
{
    // 初始化ROS，并指定节点名称为kindmatics_client
    ros::init(argc, argv, "kinematics_client");

    // 当参数个数不为7（即未传入6个参数，与默认的1个参数共7个）
    if (argc != 7)
    {
        ROS_INFO("usage: kinematics_client init_x, init_y, init_z, delta_x, delta_y, delta_z");
        return 1;
    }

    // 为节点创建句柄n
    ros::NodeHandle n;

    // 为名称是trans_pose的服务创建客户端，并赋给名称为client的ros::ServiceClient的对象
    // 尖括号中为<软件包名::srv文件名>
    // 此处或为函数模板语法，sericeClient为函数名，尖括号中内容显式表明函数参数类型，圆括号中为参数值
    ros::ServiceClient client = n.serviceClient<kinematics_demo::trans>("trans_pose");
    
    // 实例化一个自动生成的服务类，即声明一个kinematics_demo::trans对象srv
    // *注：此处服务的意义与服务器的服务并不相同，此服务强调服务内容而非动作，与srv文件所表示的服务相同
    // 即软件包名与srv文件名二者共同组成一个类名，用此类名声明一个对象srv 
    kinematics_demo::trans srv;

    // 上面实例化的名为srv的服务类，包括两个成员变量request、response和两个类定义Request、Response
    // 为request成员赋值
    srv.request.init_x = atof(argv[1]);
    srv.request.init_y = atof(argv[2]);
    srv.request.init_z = atof(argv[3]);
    srv.request.delta_x = atof(argv[4]);
    srv.request.delta_y = atof(argv[5]);
    srv.request.delta_z = atof(argv[6]);

    // 客户端调用服务
    if(client.call(srv))
    {
        ROS_INFO("Pose after trans: (%f, %f, %f)", (double)srv.response.x, (double)srv.response.y, (double)srv.response.z);
    }
    else
    {
        ROS_ERROR("Failed to call service");
        return 1;
    }
    
    return 0;
}