ROS服务中自定义数据类型
在上一篇文章中,描述了一种两层封装的点集传输服务消息类型,比较复杂。上一篇文章
事后我就在想何必包两层,直接在服务中利用提供的数据类型定义数组不就行了。所以动手试了一下:
自定义数据类型
代码应用
//server端
#include "ros/ros.h"
#include "gm_ros_package/objectPosition.h"
#include "gm_ros_package/test.h"
bool process_position(gm_ros_package::test::Request &req,gm_ros_package::test::Response &res)
{
// ROS_INFO("x:%f,y:%f,z:%f",req.points.point[0].x,req.points.point[0].y,req.points.point[0].z);
for(int i =0;i < 2;i ++)
{
ROS_INFO("x:%f,y:%f,z:%f",req.points[i].x,req.points[i].y,req.points[i].z);
}
res.back = 12;
return true;
}
int main(int argc,char** argv)
{
ros::init(argc,argv,"recive_positon_node");
ros::NodeHandle nh;
ros::ServiceServer service = nh.advertiseService("object_position",process_position);
ROS_INFO("wait the position message!");
ros::spin();
return 0;
}
//client端
#include "ros/ros.h"
#include "gm_ros_package/objectPosition.h"
#include "gm_ros_package/Points.h"
#include "geometry_msgs/Point.h"
#include "gm_ros_package/test.h"
#include <iostream>
using namespace std;
int main(int argc,char** argv)
{
ros::init(argc,argv,"send_position_node");
ros::NodeHandle nh;
ros::ServiceClient client = nh.serviceClient<gm_ros_package::test>("object_position");
gm_ros_package::test position;
gm_ros_package::Points points;
geometry_msgs::Point point[2];
cout << "test" << endl;
point[0].x = 1.0;
point[0].y = 2.0;
point[0].z = 3.0;
point[1].x = 11.0;
point[1].y = 21.0;
point[1].z = 3.10;
cout << "test1" << endl;
std::vector<geometry_msgs::Point> ar(point,point+2);
// points.point = ar;
// position.request.points = points;
position.request.points = ar;
cout << "baxk" << endl;
if(client.call(position))
{
ROS_INFO("the progress is :%ld",position.response.back);
}
else
{
ROS_ERROR("Fail to call service");
}
}