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