root / quad2 / arduino / src / ros_lib / sensor_msgs / PointCloud.h @ c1426757
History | View | Annotate | Download (2.6 KB)
1 | c1426757 | Tom Mullins | #ifndef _ROS_sensor_msgs_PointCloud_h
|
---|---|---|---|
2 | #define _ROS_sensor_msgs_PointCloud_h
|
||
3 | |||
4 | #include <stdint.h> |
||
5 | #include <string.h> |
||
6 | #include <stdlib.h> |
||
7 | #include "ros/msg.h" |
||
8 | #include "std_msgs/Header.h" |
||
9 | #include "geometry_msgs/Point32.h" |
||
10 | #include "sensor_msgs/ChannelFloat32.h" |
||
11 | |||
12 | namespace sensor_msgs |
||
13 | { |
||
14 | |||
15 | class PointCloud : public ros::Msg |
||
16 | { |
||
17 | public:
|
||
18 | std_msgs::Header header; |
||
19 | uint8_t points_length; |
||
20 | geometry_msgs::Point32 st_points; |
||
21 | geometry_msgs::Point32 * points; |
||
22 | uint8_t channels_length; |
||
23 | sensor_msgs::ChannelFloat32 st_channels; |
||
24 | sensor_msgs::ChannelFloat32 * channels; |
||
25 | |||
26 | virtual int serialize(unsigned char *outbuffer) const |
||
27 | { |
||
28 | int offset = 0; |
||
29 | offset += this->header.serialize(outbuffer + offset); |
||
30 | *(outbuffer + offset++) = points_length; |
||
31 | *(outbuffer + offset++) = 0;
|
||
32 | *(outbuffer + offset++) = 0;
|
||
33 | *(outbuffer + offset++) = 0;
|
||
34 | for( uint8_t i = 0; i < points_length; i++){ |
||
35 | offset += this->points[i].serialize(outbuffer + offset); |
||
36 | } |
||
37 | *(outbuffer + offset++) = channels_length; |
||
38 | *(outbuffer + offset++) = 0;
|
||
39 | *(outbuffer + offset++) = 0;
|
||
40 | *(outbuffer + offset++) = 0;
|
||
41 | for( uint8_t i = 0; i < channels_length; i++){ |
||
42 | offset += this->channels[i].serialize(outbuffer + offset); |
||
43 | } |
||
44 | return offset;
|
||
45 | } |
||
46 | |||
47 | virtual int deserialize(unsigned char *inbuffer) |
||
48 | { |
||
49 | int offset = 0; |
||
50 | offset += this->header.deserialize(inbuffer + offset); |
||
51 | uint8_t points_lengthT = *(inbuffer + offset++); |
||
52 | if(points_lengthT > points_length)
|
||
53 | this->points = (geometry_msgs::Point32*)realloc(this->points, points_lengthT * sizeof(geometry_msgs::Point32));
|
||
54 | offset += 3;
|
||
55 | points_length = points_lengthT; |
||
56 | for( uint8_t i = 0; i < points_length; i++){ |
||
57 | offset += this->st_points.deserialize(inbuffer + offset); |
||
58 | memcpy( &(this->points[i]), &(this->st_points), sizeof(geometry_msgs::Point32));
|
||
59 | } |
||
60 | uint8_t channels_lengthT = *(inbuffer + offset++); |
||
61 | if(channels_lengthT > channels_length)
|
||
62 | this->channels = (sensor_msgs::ChannelFloat32*)realloc(this->channels, channels_lengthT * sizeof(sensor_msgs::ChannelFloat32));
|
||
63 | offset += 3;
|
||
64 | channels_length = channels_lengthT; |
||
65 | for( uint8_t i = 0; i < channels_length; i++){ |
||
66 | offset += this->st_channels.deserialize(inbuffer + offset); |
||
67 | memcpy( &(this->channels[i]), &(this->st_channels), sizeof(sensor_msgs::ChannelFloat32));
|
||
68 | } |
||
69 | return offset;
|
||
70 | } |
||
71 | |||
72 | const char * getType(){ return "sensor_msgs/PointCloud"; }; |
||
73 | const char * getMD5(){ return "d8e9c3f5afbdd8a130fd1d2763945fca"; }; |
||
74 | |||
75 | }; |
||
76 | |||
77 | } |
||
78 | #endif |