root / quad2 / arduino / src / ros_lib / nav_msgs / GridCells.h @ c1426757
History | View | Annotate | Download (3.56 KB)
1 |
#ifndef _ROS_nav_msgs_GridCells_h
|
---|---|
2 |
#define _ROS_nav_msgs_GridCells_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/Point.h" |
10 |
|
11 |
namespace nav_msgs |
12 |
{ |
13 |
|
14 |
class GridCells : public ros::Msg |
15 |
{ |
16 |
public:
|
17 |
std_msgs::Header header; |
18 |
float cell_width;
|
19 |
float cell_height;
|
20 |
uint8_t cells_length; |
21 |
geometry_msgs::Point st_cells; |
22 |
geometry_msgs::Point * cells; |
23 |
|
24 |
virtual int serialize(unsigned char *outbuffer) const |
25 |
{ |
26 |
int offset = 0; |
27 |
offset += this->header.serialize(outbuffer + offset); |
28 |
union {
|
29 |
float real;
|
30 |
uint32_t base; |
31 |
} u_cell_width; |
32 |
u_cell_width.real = this->cell_width; |
33 |
*(outbuffer + offset + 0) = (u_cell_width.base >> (8 * 0)) & 0xFF; |
34 |
*(outbuffer + offset + 1) = (u_cell_width.base >> (8 * 1)) & 0xFF; |
35 |
*(outbuffer + offset + 2) = (u_cell_width.base >> (8 * 2)) & 0xFF; |
36 |
*(outbuffer + offset + 3) = (u_cell_width.base >> (8 * 3)) & 0xFF; |
37 |
offset += sizeof(this->cell_width);
|
38 |
union {
|
39 |
float real;
|
40 |
uint32_t base; |
41 |
} u_cell_height; |
42 |
u_cell_height.real = this->cell_height; |
43 |
*(outbuffer + offset + 0) = (u_cell_height.base >> (8 * 0)) & 0xFF; |
44 |
*(outbuffer + offset + 1) = (u_cell_height.base >> (8 * 1)) & 0xFF; |
45 |
*(outbuffer + offset + 2) = (u_cell_height.base >> (8 * 2)) & 0xFF; |
46 |
*(outbuffer + offset + 3) = (u_cell_height.base >> (8 * 3)) & 0xFF; |
47 |
offset += sizeof(this->cell_height);
|
48 |
*(outbuffer + offset++) = cells_length; |
49 |
*(outbuffer + offset++) = 0;
|
50 |
*(outbuffer + offset++) = 0;
|
51 |
*(outbuffer + offset++) = 0;
|
52 |
for( uint8_t i = 0; i < cells_length; i++){ |
53 |
offset += this->cells[i].serialize(outbuffer + offset); |
54 |
} |
55 |
return offset;
|
56 |
} |
57 |
|
58 |
virtual int deserialize(unsigned char *inbuffer) |
59 |
{ |
60 |
int offset = 0; |
61 |
offset += this->header.deserialize(inbuffer + offset); |
62 |
union {
|
63 |
float real;
|
64 |
uint32_t base; |
65 |
} u_cell_width; |
66 |
u_cell_width.base = 0;
|
67 |
u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
68 |
u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
69 |
u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
70 |
u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
71 |
this->cell_width = u_cell_width.real; |
72 |
offset += sizeof(this->cell_width);
|
73 |
union {
|
74 |
float real;
|
75 |
uint32_t base; |
76 |
} u_cell_height; |
77 |
u_cell_height.base = 0;
|
78 |
u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
79 |
u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
80 |
u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
81 |
u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
82 |
this->cell_height = u_cell_height.real; |
83 |
offset += sizeof(this->cell_height);
|
84 |
uint8_t cells_lengthT = *(inbuffer + offset++); |
85 |
if(cells_lengthT > cells_length)
|
86 |
this->cells = (geometry_msgs::Point*)realloc(this->cells, cells_lengthT * sizeof(geometry_msgs::Point));
|
87 |
offset += 3;
|
88 |
cells_length = cells_lengthT; |
89 |
for( uint8_t i = 0; i < cells_length; i++){ |
90 |
offset += this->st_cells.deserialize(inbuffer + offset); |
91 |
memcpy( &(this->cells[i]), &(this->st_cells), sizeof(geometry_msgs::Point));
|
92 |
} |
93 |
return offset;
|
94 |
} |
95 |
|
96 |
const char * getType(){ return "nav_msgs/GridCells"; }; |
97 |
const char * getMD5(){ return "b9e4f5df6d28e272ebde00a3994830f5"; }; |
98 |
|
99 |
}; |
100 |
|
101 |
} |
102 |
#endif
|