ROS中自定义复杂数据类型
先说一下需求,想要服务的请求数据为一个point(x,y,z)的数组。具体的形式表示如:
[point1,point2,...]
geometry_msgs::Point
首先是对ROS官方提供的数据类型geometry::Point的认识和理解:
可以看到该数据类型格式如上图。
所以我们想是不是定义一个Point的数组就可以了。
自定义新的数据类型gm_ros_package::Points
来看一下自定义的数据类型的格式:
实现对geometry_msgs::Point 的一种封装。
在代码中定义:
gm_ros_package::Points points;
//points 就是geometry_msgs::Point的数组
//在赋值时许格外注意,使用vector进行赋值
自定义服务消息数据类型gm_ros_package::test
看一下服务消息数据类型的具体形式:
应用代码:
//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);
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];
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;
//特别注意这里赋值的形式
std::vector<geometry_msgs::Point> ar(point,point+2);
points.point = ar;
position.request.points = points;
if(client.call(position))
{
ROS_INFO("the progress is :%ld",position.response.back);
}
else
{
ROS_ERROR("Fail to call service");
}
}
总结
对于自己定义的复杂数据类型,当使用时,需要从最内层逐层的向外填充赋值。