root / quad2 / arduino / src / ros_lib / sensor_msgs / Joy.h @ c1426757
History | View | Annotate | Download (3.84 KB)
1 | c1426757 | Tom Mullins | #ifndef _ROS_sensor_msgs_Joy_h
|
---|---|---|---|
2 | #define _ROS_sensor_msgs_Joy_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 | |||
10 | namespace sensor_msgs |
||
11 | { |
||
12 | |||
13 | class Joy : public ros::Msg |
||
14 | { |
||
15 | public:
|
||
16 | std_msgs::Header header; |
||
17 | uint8_t axes_length; |
||
18 | float st_axes;
|
||
19 | float * axes;
|
||
20 | uint8_t buttons_length; |
||
21 | int32_t st_buttons; |
||
22 | int32_t * buttons; |
||
23 | |||
24 | virtual int serialize(unsigned char *outbuffer) const |
||
25 | { |
||
26 | int offset = 0; |
||
27 | offset += this->header.serialize(outbuffer + offset); |
||
28 | *(outbuffer + offset++) = axes_length; |
||
29 | *(outbuffer + offset++) = 0;
|
||
30 | *(outbuffer + offset++) = 0;
|
||
31 | *(outbuffer + offset++) = 0;
|
||
32 | for( uint8_t i = 0; i < axes_length; i++){ |
||
33 | union {
|
||
34 | float real;
|
||
35 | uint32_t base; |
||
36 | } u_axesi; |
||
37 | u_axesi.real = this->axes[i]; |
||
38 | *(outbuffer + offset + 0) = (u_axesi.base >> (8 * 0)) & 0xFF; |
||
39 | *(outbuffer + offset + 1) = (u_axesi.base >> (8 * 1)) & 0xFF; |
||
40 | *(outbuffer + offset + 2) = (u_axesi.base >> (8 * 2)) & 0xFF; |
||
41 | *(outbuffer + offset + 3) = (u_axesi.base >> (8 * 3)) & 0xFF; |
||
42 | offset += sizeof(this->axes[i]);
|
||
43 | } |
||
44 | *(outbuffer + offset++) = buttons_length; |
||
45 | *(outbuffer + offset++) = 0;
|
||
46 | *(outbuffer + offset++) = 0;
|
||
47 | *(outbuffer + offset++) = 0;
|
||
48 | for( uint8_t i = 0; i < buttons_length; i++){ |
||
49 | union {
|
||
50 | int32_t real; |
||
51 | uint32_t base; |
||
52 | } u_buttonsi; |
||
53 | u_buttonsi.real = this->buttons[i]; |
||
54 | *(outbuffer + offset + 0) = (u_buttonsi.base >> (8 * 0)) & 0xFF; |
||
55 | *(outbuffer + offset + 1) = (u_buttonsi.base >> (8 * 1)) & 0xFF; |
||
56 | *(outbuffer + offset + 2) = (u_buttonsi.base >> (8 * 2)) & 0xFF; |
||
57 | *(outbuffer + offset + 3) = (u_buttonsi.base >> (8 * 3)) & 0xFF; |
||
58 | offset += sizeof(this->buttons[i]);
|
||
59 | } |
||
60 | return offset;
|
||
61 | } |
||
62 | |||
63 | virtual int deserialize(unsigned char *inbuffer) |
||
64 | { |
||
65 | int offset = 0; |
||
66 | offset += this->header.deserialize(inbuffer + offset); |
||
67 | uint8_t axes_lengthT = *(inbuffer + offset++); |
||
68 | if(axes_lengthT > axes_length)
|
||
69 | this->axes = (float*)realloc(this->axes, axes_lengthT * sizeof(float)); |
||
70 | offset += 3;
|
||
71 | axes_length = axes_lengthT; |
||
72 | for( uint8_t i = 0; i < axes_length; i++){ |
||
73 | union {
|
||
74 | float real;
|
||
75 | uint32_t base; |
||
76 | } u_st_axes; |
||
77 | u_st_axes.base = 0;
|
||
78 | u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
79 | u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
||
80 | u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
||
81 | u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
||
82 | this->st_axes = u_st_axes.real; |
||
83 | offset += sizeof(this->st_axes);
|
||
84 | memcpy( &(this->axes[i]), &(this->st_axes), sizeof(float)); |
||
85 | } |
||
86 | uint8_t buttons_lengthT = *(inbuffer + offset++); |
||
87 | if(buttons_lengthT > buttons_length)
|
||
88 | this->buttons = (int32_t*)realloc(this->buttons, buttons_lengthT * sizeof(int32_t));
|
||
89 | offset += 3;
|
||
90 | buttons_length = buttons_lengthT; |
||
91 | for( uint8_t i = 0; i < buttons_length; i++){ |
||
92 | union {
|
||
93 | int32_t real; |
||
94 | uint32_t base; |
||
95 | } u_st_buttons; |
||
96 | u_st_buttons.base = 0;
|
||
97 | u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
98 | u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
||
99 | u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
||
100 | u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
||
101 | this->st_buttons = u_st_buttons.real; |
||
102 | offset += sizeof(this->st_buttons);
|
||
103 | memcpy( &(this->buttons[i]), &(this->st_buttons), sizeof(int32_t));
|
||
104 | } |
||
105 | return offset;
|
||
106 | } |
||
107 | |||
108 | const char * getType(){ return "sensor_msgs/Joy"; }; |
||
109 | const char * getMD5(){ return "5a9ea5f83505693b71e785041e67a8bb"; }; |
||
110 | |||
111 | }; |
||
112 | |||
113 | } |
||
114 | #endif |