root / quad2 / arduino / src / ros_lib / sensor_msgs / PointCloud2.h @ c1426757
History | View | Annotate | Download (6.19 KB)
1 |
#ifndef _ROS_sensor_msgs_PointCloud2_h
|
---|---|
2 |
#define _ROS_sensor_msgs_PointCloud2_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 "sensor_msgs/PointField.h" |
10 |
|
11 |
namespace sensor_msgs |
12 |
{ |
13 |
|
14 |
class PointCloud2 : public ros::Msg |
15 |
{ |
16 |
public:
|
17 |
std_msgs::Header header; |
18 |
uint32_t height; |
19 |
uint32_t width; |
20 |
uint8_t fields_length; |
21 |
sensor_msgs::PointField st_fields; |
22 |
sensor_msgs::PointField * fields; |
23 |
bool is_bigendian;
|
24 |
uint32_t point_step; |
25 |
uint32_t row_step; |
26 |
uint8_t data_length; |
27 |
uint8_t st_data; |
28 |
uint8_t * data; |
29 |
bool is_dense;
|
30 |
|
31 |
virtual int serialize(unsigned char *outbuffer) const |
32 |
{ |
33 |
int offset = 0; |
34 |
offset += this->header.serialize(outbuffer + offset); |
35 |
*(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF; |
36 |
*(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF; |
37 |
*(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF; |
38 |
*(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF; |
39 |
offset += sizeof(this->height);
|
40 |
*(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF; |
41 |
*(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF; |
42 |
*(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF; |
43 |
*(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF; |
44 |
offset += sizeof(this->width);
|
45 |
*(outbuffer + offset++) = fields_length; |
46 |
*(outbuffer + offset++) = 0;
|
47 |
*(outbuffer + offset++) = 0;
|
48 |
*(outbuffer + offset++) = 0;
|
49 |
for( uint8_t i = 0; i < fields_length; i++){ |
50 |
offset += this->fields[i].serialize(outbuffer + offset); |
51 |
} |
52 |
union {
|
53 |
bool real;
|
54 |
uint8_t base; |
55 |
} u_is_bigendian; |
56 |
u_is_bigendian.real = this->is_bigendian; |
57 |
*(outbuffer + offset + 0) = (u_is_bigendian.base >> (8 * 0)) & 0xFF; |
58 |
offset += sizeof(this->is_bigendian);
|
59 |
*(outbuffer + offset + 0) = (this->point_step >> (8 * 0)) & 0xFF; |
60 |
*(outbuffer + offset + 1) = (this->point_step >> (8 * 1)) & 0xFF; |
61 |
*(outbuffer + offset + 2) = (this->point_step >> (8 * 2)) & 0xFF; |
62 |
*(outbuffer + offset + 3) = (this->point_step >> (8 * 3)) & 0xFF; |
63 |
offset += sizeof(this->point_step);
|
64 |
*(outbuffer + offset + 0) = (this->row_step >> (8 * 0)) & 0xFF; |
65 |
*(outbuffer + offset + 1) = (this->row_step >> (8 * 1)) & 0xFF; |
66 |
*(outbuffer + offset + 2) = (this->row_step >> (8 * 2)) & 0xFF; |
67 |
*(outbuffer + offset + 3) = (this->row_step >> (8 * 3)) & 0xFF; |
68 |
offset += sizeof(this->row_step);
|
69 |
*(outbuffer + offset++) = data_length; |
70 |
*(outbuffer + offset++) = 0;
|
71 |
*(outbuffer + offset++) = 0;
|
72 |
*(outbuffer + offset++) = 0;
|
73 |
for( uint8_t i = 0; i < data_length; i++){ |
74 |
*(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; |
75 |
offset += sizeof(this->data[i]);
|
76 |
} |
77 |
union {
|
78 |
bool real;
|
79 |
uint8_t base; |
80 |
} u_is_dense; |
81 |
u_is_dense.real = this->is_dense; |
82 |
*(outbuffer + offset + 0) = (u_is_dense.base >> (8 * 0)) & 0xFF; |
83 |
offset += sizeof(this->is_dense);
|
84 |
return offset;
|
85 |
} |
86 |
|
87 |
virtual int deserialize(unsigned char *inbuffer) |
88 |
{ |
89 |
int offset = 0; |
90 |
offset += this->header.deserialize(inbuffer + offset); |
91 |
this->height |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
92 |
this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
93 |
this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
94 |
this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
95 |
offset += sizeof(this->height);
|
96 |
this->width |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
97 |
this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
98 |
this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
99 |
this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
100 |
offset += sizeof(this->width);
|
101 |
uint8_t fields_lengthT = *(inbuffer + offset++); |
102 |
if(fields_lengthT > fields_length)
|
103 |
this->fields = (sensor_msgs::PointField*)realloc(this->fields, fields_lengthT * sizeof(sensor_msgs::PointField));
|
104 |
offset += 3;
|
105 |
fields_length = fields_lengthT; |
106 |
for( uint8_t i = 0; i < fields_length; i++){ |
107 |
offset += this->st_fields.deserialize(inbuffer + offset); |
108 |
memcpy( &(this->fields[i]), &(this->st_fields), sizeof(sensor_msgs::PointField));
|
109 |
} |
110 |
union {
|
111 |
bool real;
|
112 |
uint8_t base; |
113 |
} u_is_bigendian; |
114 |
u_is_bigendian.base = 0;
|
115 |
u_is_bigendian.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); |
116 |
this->is_bigendian = u_is_bigendian.real; |
117 |
offset += sizeof(this->is_bigendian);
|
118 |
this->point_step |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
119 |
this->point_step |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
120 |
this->point_step |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
121 |
this->point_step |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
122 |
offset += sizeof(this->point_step);
|
123 |
this->row_step |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
124 |
this->row_step |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
125 |
this->row_step |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
126 |
this->row_step |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
127 |
offset += sizeof(this->row_step);
|
128 |
uint8_t data_lengthT = *(inbuffer + offset++); |
129 |
if(data_lengthT > data_length)
|
130 |
this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t));
|
131 |
offset += 3;
|
132 |
data_length = data_lengthT; |
133 |
for( uint8_t i = 0; i < data_length; i++){ |
134 |
this->st_data |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); |
135 |
offset += sizeof(this->st_data);
|
136 |
memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t));
|
137 |
} |
138 |
union {
|
139 |
bool real;
|
140 |
uint8_t base; |
141 |
} u_is_dense; |
142 |
u_is_dense.base = 0;
|
143 |
u_is_dense.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); |
144 |
this->is_dense = u_is_dense.real; |
145 |
offset += sizeof(this->is_dense);
|
146 |
return offset;
|
147 |
} |
148 |
|
149 |
const char * getType(){ return "sensor_msgs/PointCloud2"; }; |
150 |
const char * getMD5(){ return "1158d486dd51d683ce2f1be655c3c181"; }; |
151 |
|
152 |
}; |
153 |
|
154 |
} |
155 |
#endif
|