Go to the documentation of this file.00001 #ifndef _ROS_wiimote_State_h
00002 #define _ROS_wiimote_State_h
00003
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "std_msgs/Header.h"
00009 #include "geometry_msgs/Vector3.h"
00010 #include "wiimote/IrSourceInfo.h"
00011 #include "ros/time.h"
00012
00013 namespace wiimote
00014 {
00015
00016 class State : public ros::Msg
00017 {
00018 public:
00019 std_msgs::Header header;
00020 geometry_msgs::Vector3 angular_velocity_zeroed;
00021 geometry_msgs::Vector3 angular_velocity_raw;
00022 float angular_velocity_covariance[9];
00023 geometry_msgs::Vector3 linear_acceleration_zeroed;
00024 geometry_msgs::Vector3 linear_acceleration_raw;
00025 float linear_acceleration_covariance[9];
00026 geometry_msgs::Vector3 nunchuk_acceleration_zeroed;
00027 geometry_msgs::Vector3 nunchuk_acceleration_raw;
00028 float nunchuk_joystick_zeroed[2];
00029 float nunchuk_joystick_raw[2];
00030 bool buttons[11];
00031 bool nunchuk_buttons[2];
00032 bool LEDs[4];
00033 bool rumble;
00034 uint8_t ir_tracking_length;
00035 wiimote::IrSourceInfo st_ir_tracking;
00036 wiimote::IrSourceInfo * ir_tracking;
00037 float raw_battery;
00038 float percent_battery;
00039 ros::Time zeroing_time;
00040 uint64_t errors;
00041 enum { INVALID = -1 };
00042 enum { INVALID_FLOAT = -1.0 };
00043 enum { MSG_BTN_1 = 0 };
00044 enum { MSG_BTN_2 = 1 };
00045 enum { MSG_BTN_A = 2 };
00046 enum { MSG_BTN_B = 3 };
00047 enum { MSG_BTN_PLUS = 4 };
00048 enum { MSG_BTN_MINUS = 5 };
00049 enum { MSG_BTN_LEFT = 6 };
00050 enum { MSG_BTN_RIGHT = 7 };
00051 enum { MSG_BTN_UP = 8 };
00052 enum { MSG_BTN_DOWN = 9 };
00053 enum { MSG_BTN_HOME = 10 };
00054 enum { MSG_BTN_Z = 0 };
00055 enum { MSG_BTN_C = 1 };
00056 enum { MSG_CLASSIC_BTN_X = 0 };
00057 enum { MSG_CLASSIC_BTN_Y = 1 };
00058 enum { MSG_CLASSIC_BTN_A = 2 };
00059 enum { MSG_CLASSIC_BTN_B = 3 };
00060 enum { MSG_CLASSIC_BTN_PLUS = 4 };
00061 enum { MSG_CLASSIC_BTN_MINUS = 5 };
00062 enum { MSG_CLASSIC_BTN_LEFT = 6 };
00063 enum { MSG_CLASSIC_BTN_RIGHT = 7 };
00064 enum { MSG_CLASSIC_BTN_UP = 8 };
00065 enum { MSG_CLASSIC_BTN_DOWN = 9 };
00066 enum { MSG_CLASSIC_BTN_HOME = 10 };
00067 enum { MSG_CLASSIC_BTN_L = 11 };
00068 enum { MSG_CLASSIC_BTN_R = 12 };
00069 enum { MSG_CLASSIC_BTN_ZL = 13 };
00070 enum { MSG_CLASSIC_BTN_ZR = 14 };
00071
00072 virtual int serialize(unsigned char *outbuffer) const
00073 {
00074 int offset = 0;
00075 offset += this->header.serialize(outbuffer + offset);
00076 offset += this->angular_velocity_zeroed.serialize(outbuffer + offset);
00077 offset += this->angular_velocity_raw.serialize(outbuffer + offset);
00078 for( uint8_t i = 0; i < 9; i++){
00079 int32_t * val_angular_velocity_covariancei = (int32_t *) &(this->angular_velocity_covariance[i]);
00080 int32_t exp_angular_velocity_covariancei = (((*val_angular_velocity_covariancei)>>23)&255);
00081 if(exp_angular_velocity_covariancei != 0)
00082 exp_angular_velocity_covariancei += 1023-127;
00083 int32_t sig_angular_velocity_covariancei = *val_angular_velocity_covariancei;
00084 *(outbuffer + offset++) = 0;
00085 *(outbuffer + offset++) = 0;
00086 *(outbuffer + offset++) = 0;
00087 *(outbuffer + offset++) = (sig_angular_velocity_covariancei<<5) & 0xff;
00088 *(outbuffer + offset++) = (sig_angular_velocity_covariancei>>3) & 0xff;
00089 *(outbuffer + offset++) = (sig_angular_velocity_covariancei>>11) & 0xff;
00090 *(outbuffer + offset++) = ((exp_angular_velocity_covariancei<<4) & 0xF0) | ((sig_angular_velocity_covariancei>>19)&0x0F);
00091 *(outbuffer + offset++) = (exp_angular_velocity_covariancei>>4) & 0x7F;
00092 if(this->angular_velocity_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
00093 }
00094 offset += this->linear_acceleration_zeroed.serialize(outbuffer + offset);
00095 offset += this->linear_acceleration_raw.serialize(outbuffer + offset);
00096 for( uint8_t i = 0; i < 9; i++){
00097 int32_t * val_linear_acceleration_covariancei = (int32_t *) &(this->linear_acceleration_covariance[i]);
00098 int32_t exp_linear_acceleration_covariancei = (((*val_linear_acceleration_covariancei)>>23)&255);
00099 if(exp_linear_acceleration_covariancei != 0)
00100 exp_linear_acceleration_covariancei += 1023-127;
00101 int32_t sig_linear_acceleration_covariancei = *val_linear_acceleration_covariancei;
00102 *(outbuffer + offset++) = 0;
00103 *(outbuffer + offset++) = 0;
00104 *(outbuffer + offset++) = 0;
00105 *(outbuffer + offset++) = (sig_linear_acceleration_covariancei<<5) & 0xff;
00106 *(outbuffer + offset++) = (sig_linear_acceleration_covariancei>>3) & 0xff;
00107 *(outbuffer + offset++) = (sig_linear_acceleration_covariancei>>11) & 0xff;
00108 *(outbuffer + offset++) = ((exp_linear_acceleration_covariancei<<4) & 0xF0) | ((sig_linear_acceleration_covariancei>>19)&0x0F);
00109 *(outbuffer + offset++) = (exp_linear_acceleration_covariancei>>4) & 0x7F;
00110 if(this->linear_acceleration_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
00111 }
00112 offset += this->nunchuk_acceleration_zeroed.serialize(outbuffer + offset);
00113 offset += this->nunchuk_acceleration_raw.serialize(outbuffer + offset);
00114 for( uint8_t i = 0; i < 2; i++){
00115 union {
00116 float real;
00117 uint32_t base;
00118 } u_nunchuk_joystick_zeroedi;
00119 u_nunchuk_joystick_zeroedi.real = this->nunchuk_joystick_zeroed[i];
00120 *(outbuffer + offset + 0) = (u_nunchuk_joystick_zeroedi.base >> (8 * 0)) & 0xFF;
00121 *(outbuffer + offset + 1) = (u_nunchuk_joystick_zeroedi.base >> (8 * 1)) & 0xFF;
00122 *(outbuffer + offset + 2) = (u_nunchuk_joystick_zeroedi.base >> (8 * 2)) & 0xFF;
00123 *(outbuffer + offset + 3) = (u_nunchuk_joystick_zeroedi.base >> (8 * 3)) & 0xFF;
00124 offset += sizeof(this->nunchuk_joystick_zeroed[i]);
00125 }
00126 for( uint8_t i = 0; i < 2; i++){
00127 union {
00128 float real;
00129 uint32_t base;
00130 } u_nunchuk_joystick_rawi;
00131 u_nunchuk_joystick_rawi.real = this->nunchuk_joystick_raw[i];
00132 *(outbuffer + offset + 0) = (u_nunchuk_joystick_rawi.base >> (8 * 0)) & 0xFF;
00133 *(outbuffer + offset + 1) = (u_nunchuk_joystick_rawi.base >> (8 * 1)) & 0xFF;
00134 *(outbuffer + offset + 2) = (u_nunchuk_joystick_rawi.base >> (8 * 2)) & 0xFF;
00135 *(outbuffer + offset + 3) = (u_nunchuk_joystick_rawi.base >> (8 * 3)) & 0xFF;
00136 offset += sizeof(this->nunchuk_joystick_raw[i]);
00137 }
00138 for( uint8_t i = 0; i < 11; i++){
00139 union {
00140 bool real;
00141 uint8_t base;
00142 } u_buttonsi;
00143 u_buttonsi.real = this->buttons[i];
00144 *(outbuffer + offset + 0) = (u_buttonsi.base >> (8 * 0)) & 0xFF;
00145 offset += sizeof(this->buttons[i]);
00146 }
00147 for( uint8_t i = 0; i < 2; i++){
00148 union {
00149 bool real;
00150 uint8_t base;
00151 } u_nunchuk_buttonsi;
00152 u_nunchuk_buttonsi.real = this->nunchuk_buttons[i];
00153 *(outbuffer + offset + 0) = (u_nunchuk_buttonsi.base >> (8 * 0)) & 0xFF;
00154 offset += sizeof(this->nunchuk_buttons[i]);
00155 }
00156 for( uint8_t i = 0; i < 4; i++){
00157 union {
00158 bool real;
00159 uint8_t base;
00160 } u_LEDsi;
00161 u_LEDsi.real = this->LEDs[i];
00162 *(outbuffer + offset + 0) = (u_LEDsi.base >> (8 * 0)) & 0xFF;
00163 offset += sizeof(this->LEDs[i]);
00164 }
00165 union {
00166 bool real;
00167 uint8_t base;
00168 } u_rumble;
00169 u_rumble.real = this->rumble;
00170 *(outbuffer + offset + 0) = (u_rumble.base >> (8 * 0)) & 0xFF;
00171 offset += sizeof(this->rumble);
00172 *(outbuffer + offset++) = ir_tracking_length;
00173 *(outbuffer + offset++) = 0;
00174 *(outbuffer + offset++) = 0;
00175 *(outbuffer + offset++) = 0;
00176 for( uint8_t i = 0; i < ir_tracking_length; i++){
00177 offset += this->ir_tracking[i].serialize(outbuffer + offset);
00178 }
00179 union {
00180 float real;
00181 uint32_t base;
00182 } u_raw_battery;
00183 u_raw_battery.real = this->raw_battery;
00184 *(outbuffer + offset + 0) = (u_raw_battery.base >> (8 * 0)) & 0xFF;
00185 *(outbuffer + offset + 1) = (u_raw_battery.base >> (8 * 1)) & 0xFF;
00186 *(outbuffer + offset + 2) = (u_raw_battery.base >> (8 * 2)) & 0xFF;
00187 *(outbuffer + offset + 3) = (u_raw_battery.base >> (8 * 3)) & 0xFF;
00188 offset += sizeof(this->raw_battery);
00189 union {
00190 float real;
00191 uint32_t base;
00192 } u_percent_battery;
00193 u_percent_battery.real = this->percent_battery;
00194 *(outbuffer + offset + 0) = (u_percent_battery.base >> (8 * 0)) & 0xFF;
00195 *(outbuffer + offset + 1) = (u_percent_battery.base >> (8 * 1)) & 0xFF;
00196 *(outbuffer + offset + 2) = (u_percent_battery.base >> (8 * 2)) & 0xFF;
00197 *(outbuffer + offset + 3) = (u_percent_battery.base >> (8 * 3)) & 0xFF;
00198 offset += sizeof(this->percent_battery);
00199 *(outbuffer + offset + 0) = (this->zeroing_time.sec >> (8 * 0)) & 0xFF;
00200 *(outbuffer + offset + 1) = (this->zeroing_time.sec >> (8 * 1)) & 0xFF;
00201 *(outbuffer + offset + 2) = (this->zeroing_time.sec >> (8 * 2)) & 0xFF;
00202 *(outbuffer + offset + 3) = (this->zeroing_time.sec >> (8 * 3)) & 0xFF;
00203 offset += sizeof(this->zeroing_time.sec);
00204 *(outbuffer + offset + 0) = (this->zeroing_time.nsec >> (8 * 0)) & 0xFF;
00205 *(outbuffer + offset + 1) = (this->zeroing_time.nsec >> (8 * 1)) & 0xFF;
00206 *(outbuffer + offset + 2) = (this->zeroing_time.nsec >> (8 * 2)) & 0xFF;
00207 *(outbuffer + offset + 3) = (this->zeroing_time.nsec >> (8 * 3)) & 0xFF;
00208 offset += sizeof(this->zeroing_time.nsec);
00209 union {
00210 uint64_t real;
00211 uint32_t base;
00212 } u_errors;
00213 u_errors.real = this->errors;
00214 *(outbuffer + offset + 0) = (u_errors.base >> (8 * 0)) & 0xFF;
00215 *(outbuffer + offset + 1) = (u_errors.base >> (8 * 1)) & 0xFF;
00216 *(outbuffer + offset + 2) = (u_errors.base >> (8 * 2)) & 0xFF;
00217 *(outbuffer + offset + 3) = (u_errors.base >> (8 * 3)) & 0xFF;
00218 offset += sizeof(this->errors);
00219 return offset;
00220 }
00221
00222 virtual int deserialize(unsigned char *inbuffer)
00223 {
00224 int offset = 0;
00225 offset += this->header.deserialize(inbuffer + offset);
00226 offset += this->angular_velocity_zeroed.deserialize(inbuffer + offset);
00227 offset += this->angular_velocity_raw.deserialize(inbuffer + offset);
00228 for( uint8_t i = 0; i < 9; i++){
00229 uint32_t * val_angular_velocity_covariancei = (uint32_t*) &(this->angular_velocity_covariance[i]);
00230 offset += 3;
00231 *val_angular_velocity_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00232 *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00233 *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00234 *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00235 uint32_t exp_angular_velocity_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00236 exp_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00237 if(exp_angular_velocity_covariancei !=0)
00238 *val_angular_velocity_covariancei |= ((exp_angular_velocity_covariancei)-1023+127)<<23;
00239 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->angular_velocity_covariance[i] = -this->angular_velocity_covariance[i];
00240 }
00241 offset += this->linear_acceleration_zeroed.deserialize(inbuffer + offset);
00242 offset += this->linear_acceleration_raw.deserialize(inbuffer + offset);
00243 for( uint8_t i = 0; i < 9; i++){
00244 uint32_t * val_linear_acceleration_covariancei = (uint32_t*) &(this->linear_acceleration_covariance[i]);
00245 offset += 3;
00246 *val_linear_acceleration_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00247 *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00248 *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00249 *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00250 uint32_t exp_linear_acceleration_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00251 exp_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00252 if(exp_linear_acceleration_covariancei !=0)
00253 *val_linear_acceleration_covariancei |= ((exp_linear_acceleration_covariancei)-1023+127)<<23;
00254 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->linear_acceleration_covariance[i] = -this->linear_acceleration_covariance[i];
00255 }
00256 offset += this->nunchuk_acceleration_zeroed.deserialize(inbuffer + offset);
00257 offset += this->nunchuk_acceleration_raw.deserialize(inbuffer + offset);
00258 for( uint8_t i = 0; i < 2; i++){
00259 union {
00260 float real;
00261 uint32_t base;
00262 } u_nunchuk_joystick_zeroedi;
00263 u_nunchuk_joystick_zeroedi.base = 0;
00264 u_nunchuk_joystick_zeroedi.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00265 u_nunchuk_joystick_zeroedi.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00266 u_nunchuk_joystick_zeroedi.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00267 u_nunchuk_joystick_zeroedi.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00268 this->nunchuk_joystick_zeroed[i] = u_nunchuk_joystick_zeroedi.real;
00269 offset += sizeof(this->nunchuk_joystick_zeroed[i]);
00270 }
00271 for( uint8_t i = 0; i < 2; i++){
00272 union {
00273 float real;
00274 uint32_t base;
00275 } u_nunchuk_joystick_rawi;
00276 u_nunchuk_joystick_rawi.base = 0;
00277 u_nunchuk_joystick_rawi.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00278 u_nunchuk_joystick_rawi.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00279 u_nunchuk_joystick_rawi.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00280 u_nunchuk_joystick_rawi.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00281 this->nunchuk_joystick_raw[i] = u_nunchuk_joystick_rawi.real;
00282 offset += sizeof(this->nunchuk_joystick_raw[i]);
00283 }
00284 for( uint8_t i = 0; i < 11; i++){
00285 union {
00286 bool real;
00287 uint8_t base;
00288 } u_buttonsi;
00289 u_buttonsi.base = 0;
00290 u_buttonsi.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00291 this->buttons[i] = u_buttonsi.real;
00292 offset += sizeof(this->buttons[i]);
00293 }
00294 for( uint8_t i = 0; i < 2; i++){
00295 union {
00296 bool real;
00297 uint8_t base;
00298 } u_nunchuk_buttonsi;
00299 u_nunchuk_buttonsi.base = 0;
00300 u_nunchuk_buttonsi.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00301 this->nunchuk_buttons[i] = u_nunchuk_buttonsi.real;
00302 offset += sizeof(this->nunchuk_buttons[i]);
00303 }
00304 for( uint8_t i = 0; i < 4; i++){
00305 union {
00306 bool real;
00307 uint8_t base;
00308 } u_LEDsi;
00309 u_LEDsi.base = 0;
00310 u_LEDsi.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00311 this->LEDs[i] = u_LEDsi.real;
00312 offset += sizeof(this->LEDs[i]);
00313 }
00314 union {
00315 bool real;
00316 uint8_t base;
00317 } u_rumble;
00318 u_rumble.base = 0;
00319 u_rumble.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00320 this->rumble = u_rumble.real;
00321 offset += sizeof(this->rumble);
00322 uint8_t ir_tracking_lengthT = *(inbuffer + offset++);
00323 if(ir_tracking_lengthT > ir_tracking_length)
00324 this->ir_tracking = (wiimote::IrSourceInfo*)realloc(this->ir_tracking, ir_tracking_lengthT * sizeof(wiimote::IrSourceInfo));
00325 offset += 3;
00326 ir_tracking_length = ir_tracking_lengthT;
00327 for( uint8_t i = 0; i < ir_tracking_length; i++){
00328 offset += this->st_ir_tracking.deserialize(inbuffer + offset);
00329 memcpy( &(this->ir_tracking[i]), &(this->st_ir_tracking), sizeof(wiimote::IrSourceInfo));
00330 }
00331 union {
00332 float real;
00333 uint32_t base;
00334 } u_raw_battery;
00335 u_raw_battery.base = 0;
00336 u_raw_battery.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00337 u_raw_battery.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00338 u_raw_battery.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00339 u_raw_battery.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00340 this->raw_battery = u_raw_battery.real;
00341 offset += sizeof(this->raw_battery);
00342 union {
00343 float real;
00344 uint32_t base;
00345 } u_percent_battery;
00346 u_percent_battery.base = 0;
00347 u_percent_battery.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00348 u_percent_battery.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00349 u_percent_battery.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00350 u_percent_battery.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00351 this->percent_battery = u_percent_battery.real;
00352 offset += sizeof(this->percent_battery);
00353 this->zeroing_time.sec = ((uint32_t) (*(inbuffer + offset)));
00354 this->zeroing_time.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00355 this->zeroing_time.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00356 this->zeroing_time.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00357 offset += sizeof(this->zeroing_time.sec);
00358 this->zeroing_time.nsec = ((uint32_t) (*(inbuffer + offset)));
00359 this->zeroing_time.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00360 this->zeroing_time.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00361 this->zeroing_time.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00362 offset += sizeof(this->zeroing_time.nsec);
00363 union {
00364 uint64_t real;
00365 uint32_t base;
00366 } u_errors;
00367 u_errors.base = 0;
00368 u_errors.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00369 u_errors.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00370 u_errors.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00371 u_errors.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00372 this->errors = u_errors.real;
00373 offset += sizeof(this->errors);
00374 return offset;
00375 }
00376
00377 const char * getType(){ return "wiimote/State"; };
00378 const char * getMD5(){ return "a69651e8129655c6ed3c5039e468362c"; };
00379
00380 };
00381
00382 }
00383 #endif