State.h
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


ric_mc
Author(s): RoboTiCan
autogenerated on Fri May 20 2016 03:48:56