root / quad2 / arduino / src / ros_lib / sensor_msgs / JointState.h @ c1426757
History | View | Annotate | Download (8.09 KB)
1 |
#ifndef _ROS_sensor_msgs_JointState_h
|
---|---|
2 |
#define _ROS_sensor_msgs_JointState_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 JointState : public ros::Msg |
14 |
{ |
15 |
public:
|
16 |
std_msgs::Header header; |
17 |
uint8_t name_length; |
18 |
char* st_name;
|
19 |
char* * name;
|
20 |
uint8_t position_length; |
21 |
float st_position;
|
22 |
float * position;
|
23 |
uint8_t velocity_length; |
24 |
float st_velocity;
|
25 |
float * velocity;
|
26 |
uint8_t effort_length; |
27 |
float st_effort;
|
28 |
float * effort;
|
29 |
|
30 |
virtual int serialize(unsigned char *outbuffer) const |
31 |
{ |
32 |
int offset = 0; |
33 |
offset += this->header.serialize(outbuffer + offset); |
34 |
*(outbuffer + offset++) = name_length; |
35 |
*(outbuffer + offset++) = 0;
|
36 |
*(outbuffer + offset++) = 0;
|
37 |
*(outbuffer + offset++) = 0;
|
38 |
for( uint8_t i = 0; i < name_length; i++){ |
39 |
uint32_t * length_namei = (uint32_t *)(outbuffer + offset); |
40 |
*length_namei = strlen( (const char*) this->name[i]); |
41 |
offset += 4;
|
42 |
memcpy(outbuffer + offset, this->name[i], *length_namei); |
43 |
offset += *length_namei; |
44 |
} |
45 |
*(outbuffer + offset++) = position_length; |
46 |
*(outbuffer + offset++) = 0;
|
47 |
*(outbuffer + offset++) = 0;
|
48 |
*(outbuffer + offset++) = 0;
|
49 |
for( uint8_t i = 0; i < position_length; i++){ |
50 |
int32_t * val_positioni = (long *) &(this->position[i]);
|
51 |
int32_t exp_positioni = (((*val_positioni)>>23)&255); |
52 |
if(exp_positioni != 0) |
53 |
exp_positioni += 1023-127; |
54 |
int32_t sig_positioni = *val_positioni; |
55 |
*(outbuffer + offset++) = 0;
|
56 |
*(outbuffer + offset++) = 0;
|
57 |
*(outbuffer + offset++) = 0;
|
58 |
*(outbuffer + offset++) = (sig_positioni<<5) & 0xff; |
59 |
*(outbuffer + offset++) = (sig_positioni>>3) & 0xff; |
60 |
*(outbuffer + offset++) = (sig_positioni>>11) & 0xff; |
61 |
*(outbuffer + offset++) = ((exp_positioni<<4) & 0xF0) | ((sig_positioni>>19)&0x0F); |
62 |
*(outbuffer + offset++) = (exp_positioni>>4) & 0x7F; |
63 |
if(this->position[i] < 0) *(outbuffer + offset -1) |= 0x80; |
64 |
} |
65 |
*(outbuffer + offset++) = velocity_length; |
66 |
*(outbuffer + offset++) = 0;
|
67 |
*(outbuffer + offset++) = 0;
|
68 |
*(outbuffer + offset++) = 0;
|
69 |
for( uint8_t i = 0; i < velocity_length; i++){ |
70 |
int32_t * val_velocityi = (long *) &(this->velocity[i]);
|
71 |
int32_t exp_velocityi = (((*val_velocityi)>>23)&255); |
72 |
if(exp_velocityi != 0) |
73 |
exp_velocityi += 1023-127; |
74 |
int32_t sig_velocityi = *val_velocityi; |
75 |
*(outbuffer + offset++) = 0;
|
76 |
*(outbuffer + offset++) = 0;
|
77 |
*(outbuffer + offset++) = 0;
|
78 |
*(outbuffer + offset++) = (sig_velocityi<<5) & 0xff; |
79 |
*(outbuffer + offset++) = (sig_velocityi>>3) & 0xff; |
80 |
*(outbuffer + offset++) = (sig_velocityi>>11) & 0xff; |
81 |
*(outbuffer + offset++) = ((exp_velocityi<<4) & 0xF0) | ((sig_velocityi>>19)&0x0F); |
82 |
*(outbuffer + offset++) = (exp_velocityi>>4) & 0x7F; |
83 |
if(this->velocity[i] < 0) *(outbuffer + offset -1) |= 0x80; |
84 |
} |
85 |
*(outbuffer + offset++) = effort_length; |
86 |
*(outbuffer + offset++) = 0;
|
87 |
*(outbuffer + offset++) = 0;
|
88 |
*(outbuffer + offset++) = 0;
|
89 |
for( uint8_t i = 0; i < effort_length; i++){ |
90 |
int32_t * val_efforti = (long *) &(this->effort[i]);
|
91 |
int32_t exp_efforti = (((*val_efforti)>>23)&255); |
92 |
if(exp_efforti != 0) |
93 |
exp_efforti += 1023-127; |
94 |
int32_t sig_efforti = *val_efforti; |
95 |
*(outbuffer + offset++) = 0;
|
96 |
*(outbuffer + offset++) = 0;
|
97 |
*(outbuffer + offset++) = 0;
|
98 |
*(outbuffer + offset++) = (sig_efforti<<5) & 0xff; |
99 |
*(outbuffer + offset++) = (sig_efforti>>3) & 0xff; |
100 |
*(outbuffer + offset++) = (sig_efforti>>11) & 0xff; |
101 |
*(outbuffer + offset++) = ((exp_efforti<<4) & 0xF0) | ((sig_efforti>>19)&0x0F); |
102 |
*(outbuffer + offset++) = (exp_efforti>>4) & 0x7F; |
103 |
if(this->effort[i] < 0) *(outbuffer + offset -1) |= 0x80; |
104 |
} |
105 |
return offset;
|
106 |
} |
107 |
|
108 |
virtual int deserialize(unsigned char *inbuffer) |
109 |
{ |
110 |
int offset = 0; |
111 |
offset += this->header.deserialize(inbuffer + offset); |
112 |
uint8_t name_lengthT = *(inbuffer + offset++); |
113 |
if(name_lengthT > name_length)
|
114 |
this->name = (char**)realloc(this->name, name_lengthT * sizeof(char*)); |
115 |
offset += 3;
|
116 |
name_length = name_lengthT; |
117 |
for( uint8_t i = 0; i < name_length; i++){ |
118 |
uint32_t length_st_name = *(uint32_t *)(inbuffer + offset); |
119 |
offset += 4;
|
120 |
for(unsigned int k= offset; k< offset+length_st_name; ++k){ |
121 |
inbuffer[k-1]=inbuffer[k];
|
122 |
} |
123 |
inbuffer[offset+length_st_name-1]=0; |
124 |
this->st_name = (char *)(inbuffer + offset-1); |
125 |
offset += length_st_name; |
126 |
memcpy( &(this->name[i]), &(this->st_name), sizeof(char*)); |
127 |
} |
128 |
uint8_t position_lengthT = *(inbuffer + offset++); |
129 |
if(position_lengthT > position_length)
|
130 |
this->position = (float*)realloc(this->position, position_lengthT * sizeof(float)); |
131 |
offset += 3;
|
132 |
position_length = position_lengthT; |
133 |
for( uint8_t i = 0; i < position_length; i++){ |
134 |
uint32_t * val_st_position = (uint32_t*) &(this->st_position); |
135 |
offset += 3;
|
136 |
*val_st_position = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); |
137 |
*val_st_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; |
138 |
*val_st_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; |
139 |
*val_st_position |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; |
140 |
uint32_t exp_st_position = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; |
141 |
exp_st_position |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; |
142 |
if(exp_st_position !=0) |
143 |
*val_st_position |= ((exp_st_position)-1023+127)<<23; |
144 |
if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_position = -this->st_position; |
145 |
memcpy( &(this->position[i]), &(this->st_position), sizeof(float)); |
146 |
} |
147 |
uint8_t velocity_lengthT = *(inbuffer + offset++); |
148 |
if(velocity_lengthT > velocity_length)
|
149 |
this->velocity = (float*)realloc(this->velocity, velocity_lengthT * sizeof(float)); |
150 |
offset += 3;
|
151 |
velocity_length = velocity_lengthT; |
152 |
for( uint8_t i = 0; i < velocity_length; i++){ |
153 |
uint32_t * val_st_velocity = (uint32_t*) &(this->st_velocity); |
154 |
offset += 3;
|
155 |
*val_st_velocity = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); |
156 |
*val_st_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; |
157 |
*val_st_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; |
158 |
*val_st_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; |
159 |
uint32_t exp_st_velocity = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; |
160 |
exp_st_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; |
161 |
if(exp_st_velocity !=0) |
162 |
*val_st_velocity |= ((exp_st_velocity)-1023+127)<<23; |
163 |
if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_velocity = -this->st_velocity; |
164 |
memcpy( &(this->velocity[i]), &(this->st_velocity), sizeof(float)); |
165 |
} |
166 |
uint8_t effort_lengthT = *(inbuffer + offset++); |
167 |
if(effort_lengthT > effort_length)
|
168 |
this->effort = (float*)realloc(this->effort, effort_lengthT * sizeof(float)); |
169 |
offset += 3;
|
170 |
effort_length = effort_lengthT; |
171 |
for( uint8_t i = 0; i < effort_length; i++){ |
172 |
uint32_t * val_st_effort = (uint32_t*) &(this->st_effort); |
173 |
offset += 3;
|
174 |
*val_st_effort = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); |
175 |
*val_st_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; |
176 |
*val_st_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; |
177 |
*val_st_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; |
178 |
uint32_t exp_st_effort = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; |
179 |
exp_st_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; |
180 |
if(exp_st_effort !=0) |
181 |
*val_st_effort |= ((exp_st_effort)-1023+127)<<23; |
182 |
if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_effort = -this->st_effort; |
183 |
memcpy( &(this->effort[i]), &(this->st_effort), sizeof(float)); |
184 |
} |
185 |
return offset;
|
186 |
} |
187 |
|
188 |
const char * getType(){ return "sensor_msgs/JointState"; }; |
189 |
const char * getMD5(){ return "3066dcd76a6cfaef579bd0f34173e9fd"; }; |
190 |
|
191 |
}; |
192 |
|
193 |
} |
194 |
#endif
|