root / quad2 / arduino / src / ros_lib / geometry_msgs / Pose2D.h @ c1426757
History | View | Annotate | Download (4.23 KB)
1 | c1426757 | Tom Mullins | #ifndef _ROS_geometry_msgs_Pose2D_h
|
---|---|---|---|
2 | #define _ROS_geometry_msgs_Pose2D_h
|
||
3 | |||
4 | #include <stdint.h> |
||
5 | #include <string.h> |
||
6 | #include <stdlib.h> |
||
7 | #include "ros/msg.h" |
||
8 | |||
9 | namespace geometry_msgs |
||
10 | { |
||
11 | |||
12 | class Pose2D : public ros::Msg |
||
13 | { |
||
14 | public:
|
||
15 | float x;
|
||
16 | float y;
|
||
17 | float theta;
|
||
18 | |||
19 | virtual int serialize(unsigned char *outbuffer) const |
||
20 | { |
||
21 | int offset = 0; |
||
22 | int32_t * val_x = (long *) &(this->x);
|
||
23 | int32_t exp_x = (((*val_x)>>23)&255); |
||
24 | if(exp_x != 0) |
||
25 | exp_x += 1023-127; |
||
26 | int32_t sig_x = *val_x; |
||
27 | *(outbuffer + offset++) = 0;
|
||
28 | *(outbuffer + offset++) = 0;
|
||
29 | *(outbuffer + offset++) = 0;
|
||
30 | *(outbuffer + offset++) = (sig_x<<5) & 0xff; |
||
31 | *(outbuffer + offset++) = (sig_x>>3) & 0xff; |
||
32 | *(outbuffer + offset++) = (sig_x>>11) & 0xff; |
||
33 | *(outbuffer + offset++) = ((exp_x<<4) & 0xF0) | ((sig_x>>19)&0x0F); |
||
34 | *(outbuffer + offset++) = (exp_x>>4) & 0x7F; |
||
35 | if(this->x < 0) *(outbuffer + offset -1) |= 0x80; |
||
36 | int32_t * val_y = (long *) &(this->y);
|
||
37 | int32_t exp_y = (((*val_y)>>23)&255); |
||
38 | if(exp_y != 0) |
||
39 | exp_y += 1023-127; |
||
40 | int32_t sig_y = *val_y; |
||
41 | *(outbuffer + offset++) = 0;
|
||
42 | *(outbuffer + offset++) = 0;
|
||
43 | *(outbuffer + offset++) = 0;
|
||
44 | *(outbuffer + offset++) = (sig_y<<5) & 0xff; |
||
45 | *(outbuffer + offset++) = (sig_y>>3) & 0xff; |
||
46 | *(outbuffer + offset++) = (sig_y>>11) & 0xff; |
||
47 | *(outbuffer + offset++) = ((exp_y<<4) & 0xF0) | ((sig_y>>19)&0x0F); |
||
48 | *(outbuffer + offset++) = (exp_y>>4) & 0x7F; |
||
49 | if(this->y < 0) *(outbuffer + offset -1) |= 0x80; |
||
50 | int32_t * val_theta = (long *) &(this->theta);
|
||
51 | int32_t exp_theta = (((*val_theta)>>23)&255); |
||
52 | if(exp_theta != 0) |
||
53 | exp_theta += 1023-127; |
||
54 | int32_t sig_theta = *val_theta; |
||
55 | *(outbuffer + offset++) = 0;
|
||
56 | *(outbuffer + offset++) = 0;
|
||
57 | *(outbuffer + offset++) = 0;
|
||
58 | *(outbuffer + offset++) = (sig_theta<<5) & 0xff; |
||
59 | *(outbuffer + offset++) = (sig_theta>>3) & 0xff; |
||
60 | *(outbuffer + offset++) = (sig_theta>>11) & 0xff; |
||
61 | *(outbuffer + offset++) = ((exp_theta<<4) & 0xF0) | ((sig_theta>>19)&0x0F); |
||
62 | *(outbuffer + offset++) = (exp_theta>>4) & 0x7F; |
||
63 | if(this->theta < 0) *(outbuffer + offset -1) |= 0x80; |
||
64 | return offset;
|
||
65 | } |
||
66 | |||
67 | virtual int deserialize(unsigned char *inbuffer) |
||
68 | { |
||
69 | int offset = 0; |
||
70 | uint32_t * val_x = (uint32_t*) &(this->x); |
||
71 | offset += 3;
|
||
72 | *val_x = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); |
||
73 | *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; |
||
74 | *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; |
||
75 | *val_x |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; |
||
76 | uint32_t exp_x = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; |
||
77 | exp_x |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; |
||
78 | if(exp_x !=0) |
||
79 | *val_x |= ((exp_x)-1023+127)<<23; |
||
80 | if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x; |
||
81 | uint32_t * val_y = (uint32_t*) &(this->y); |
||
82 | offset += 3;
|
||
83 | *val_y = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); |
||
84 | *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; |
||
85 | *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; |
||
86 | *val_y |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; |
||
87 | uint32_t exp_y = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; |
||
88 | exp_y |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; |
||
89 | if(exp_y !=0) |
||
90 | *val_y |= ((exp_y)-1023+127)<<23; |
||
91 | if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y; |
||
92 | uint32_t * val_theta = (uint32_t*) &(this->theta); |
||
93 | offset += 3;
|
||
94 | *val_theta = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); |
||
95 | *val_theta |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; |
||
96 | *val_theta |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; |
||
97 | *val_theta |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; |
||
98 | uint32_t exp_theta = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; |
||
99 | exp_theta |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; |
||
100 | if(exp_theta !=0) |
||
101 | *val_theta |= ((exp_theta)-1023+127)<<23; |
||
102 | if( ((*(inbuffer+offset++)) & 0x80) > 0) this->theta = -this->theta; |
||
103 | return offset;
|
||
104 | } |
||
105 | |||
106 | const char * getType(){ return "geometry_msgs/Pose2D"; }; |
||
107 | const char * getMD5(){ return "938fa65709584ad8e77d238529be13b8"; }; |
||
108 | |||
109 | }; |
||
110 | |||
111 | } |
||
112 | #endif |