// 包括了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;
}