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