root / quad2 / arduino / src / ros_lib / sensor_msgs / PointCloud2.h @ c1426757
History | View | Annotate | Download (6.19 KB)
1 | c1426757 | Tom Mullins | #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 |