nucho / rosserial_mbed_lib

This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial.

Dependencies:  

Dependents:   rosserial_mbed robot_S2

sensor_msgs/Imu.h@0:77afd7560544, 2011-08-19 (annotated)

Committer:
nucho
Date:
Fri Aug 19 09:06:30 2011 +0000
Revision:
0:77afd7560544
Child:
3:1cf99502f396

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


nucho

0:77afd7560544


     1

#ifndef ros_Imu_h





nucho

0:77afd7560544


     2

#define ros_Imu_h





nucho

0:77afd7560544


     3







nucho

0:77afd7560544


     4

#include <stdint.h>





nucho

0:77afd7560544


     5

#include <string.h>





nucho

0:77afd7560544


     6

#include <stdlib.h>





nucho

0:77afd7560544


     7

#include "../ros/msg.h"





nucho

0:77afd7560544


     8

#include "std_msgs/Header.h"





nucho

0:77afd7560544


     9

#include "geometry_msgs/Quaternion.h"





nucho

0:77afd7560544


    10

#include "geometry_msgs/Vector3.h"





nucho

0:77afd7560544


    11







nucho

0:77afd7560544


    12

namespace sensor_msgs





nucho

0:77afd7560544


    13

{





nucho

0:77afd7560544


    14







nucho

0:77afd7560544


    15

  class Imu : public ros::Msg





nucho

0:77afd7560544


    16

  {





nucho

0:77afd7560544


    17

    public:





nucho

0:77afd7560544


    18

      std_msgs::Header header;





nucho

0:77afd7560544


    19

      geometry_msgs::Quaternion orientation;





nucho

0:77afd7560544


    20

      float orientation_covariance[9];





nucho

0:77afd7560544


    21

      geometry_msgs::Vector3 angular_velocity;





nucho

0:77afd7560544


    22

      float angular_velocity_covariance[9];





nucho

0:77afd7560544


    23

      geometry_msgs::Vector3 linear_acceleration;





nucho

0:77afd7560544


    24

      float linear_acceleration_covariance[9];





nucho

0:77afd7560544


    25







nucho

0:77afd7560544


    26

    virtual int serialize(unsigned char *outbuffer)





nucho

0:77afd7560544


    27

    {





nucho

0:77afd7560544


    28

      int offset = 0;





nucho

0:77afd7560544


    29

      offset += this->header.serialize(outbuffer + offset);





nucho

0:77afd7560544


    30

      offset += this->orientation.serialize(outbuffer + offset);





nucho

0:77afd7560544


    31

      unsigned char * orientation_covariance_val = (unsigned char *) this->orientation_covariance;





nucho

0:77afd7560544


    32

      for( unsigned char i = 0; i < 9; i++){





nucho

0:77afd7560544


    33

      long * val_orientation_covariancei = (long *) &(this->orientation_covariance[i]);





nucho

0:77afd7560544


    34

      long exp_orientation_covariancei = (((*val_orientation_covariancei)>>23)&255);





nucho

0:77afd7560544


    35

      if(exp_orientation_covariancei != 0)





nucho

0:77afd7560544


    36

        exp_orientation_covariancei += 1023-127;





nucho

0:77afd7560544


    37

      long sig_orientation_covariancei = *val_orientation_covariancei;





nucho

0:77afd7560544


    38

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    39

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    40

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    41

      *(outbuffer + offset++) = (sig_orientation_covariancei<<5) & 0xff;





nucho

0:77afd7560544


    42

      *(outbuffer + offset++) = (sig_orientation_covariancei>>3) & 0xff;





nucho

0:77afd7560544


    43

      *(outbuffer + offset++) = (sig_orientation_covariancei>>11) & 0xff;





nucho

0:77afd7560544


    44

      *(outbuffer + offset++) = ((exp_orientation_covariancei<<4) & 0xF0) | ((sig_orientation_covariancei>>19)&0x0F);





nucho

0:77afd7560544


    45

      *(outbuffer + offset++) = (exp_orientation_covariancei>>4) & 0x7F;





nucho

0:77afd7560544


    46

      if(this->orientation_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;





nucho

0:77afd7560544


    47

      }





nucho

0:77afd7560544


    48

      offset += this->angular_velocity.serialize(outbuffer + offset);





nucho

0:77afd7560544


    49

      unsigned char * angular_velocity_covariance_val = (unsigned char *) this->angular_velocity_covariance;





nucho

0:77afd7560544


    50

      for( unsigned char i = 0; i < 9; i++){





nucho

0:77afd7560544


    51

      long * val_angular_velocity_covariancei = (long *) &(this->angular_velocity_covariance[i]);





nucho

0:77afd7560544


    52

      long exp_angular_velocity_covariancei = (((*val_angular_velocity_covariancei)>>23)&255);





nucho

0:77afd7560544


    53

      if(exp_angular_velocity_covariancei != 0)





nucho

0:77afd7560544


    54

        exp_angular_velocity_covariancei += 1023-127;





nucho

0:77afd7560544


    55

      long sig_angular_velocity_covariancei = *val_angular_velocity_covariancei;





nucho

0:77afd7560544


    56

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    57

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    58

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    59

      *(outbuffer + offset++) = (sig_angular_velocity_covariancei<<5) & 0xff;





nucho

0:77afd7560544


    60

      *(outbuffer + offset++) = (sig_angular_velocity_covariancei>>3) & 0xff;





nucho

0:77afd7560544


    61

      *(outbuffer + offset++) = (sig_angular_velocity_covariancei>>11) & 0xff;





nucho

0:77afd7560544


    62

      *(outbuffer + offset++) = ((exp_angular_velocity_covariancei<<4) & 0xF0) | ((sig_angular_velocity_covariancei>>19)&0x0F);





nucho

0:77afd7560544


    63

      *(outbuffer + offset++) = (exp_angular_velocity_covariancei>>4) & 0x7F;





nucho

0:77afd7560544


    64

      if(this->angular_velocity_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;





nucho

0:77afd7560544


    65

      }





nucho

0:77afd7560544


    66

      offset += this->linear_acceleration.serialize(outbuffer + offset);





nucho

0:77afd7560544


    67

      unsigned char * linear_acceleration_covariance_val = (unsigned char *) this->linear_acceleration_covariance;





nucho

0:77afd7560544


    68

      for( unsigned char i = 0; i < 9; i++){





nucho

0:77afd7560544


    69

      long * val_linear_acceleration_covariancei = (long *) &(this->linear_acceleration_covariance[i]);





nucho

0:77afd7560544


    70

      long exp_linear_acceleration_covariancei = (((*val_linear_acceleration_covariancei)>>23)&255);





nucho

0:77afd7560544


    71

      if(exp_linear_acceleration_covariancei != 0)





nucho

0:77afd7560544


    72

        exp_linear_acceleration_covariancei += 1023-127;





nucho

0:77afd7560544


    73

      long sig_linear_acceleration_covariancei = *val_linear_acceleration_covariancei;





nucho

0:77afd7560544


    74

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    75

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    76

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    77

      *(outbuffer + offset++) = (sig_linear_acceleration_covariancei<<5) & 0xff;





nucho

0:77afd7560544


    78

      *(outbuffer + offset++) = (sig_linear_acceleration_covariancei>>3) & 0xff;





nucho

0:77afd7560544


    79

      *(outbuffer + offset++) = (sig_linear_acceleration_covariancei>>11) & 0xff;





nucho

0:77afd7560544


    80

      *(outbuffer + offset++) = ((exp_linear_acceleration_covariancei<<4) & 0xF0) | ((sig_linear_acceleration_covariancei>>19)&0x0F);





nucho

0:77afd7560544


    81

      *(outbuffer + offset++) = (exp_linear_acceleration_covariancei>>4) & 0x7F;





nucho

0:77afd7560544


    82

      if(this->linear_acceleration_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;





nucho

0:77afd7560544


    83

      }





nucho

0:77afd7560544


    84

      return offset;





nucho

0:77afd7560544


    85

    }





nucho

0:77afd7560544


    86







nucho

0:77afd7560544


    87

    virtual int deserialize(unsigned char *inbuffer)





nucho

0:77afd7560544


    88

    {





nucho

0:77afd7560544


    89

      int offset = 0;





nucho

0:77afd7560544


    90

      offset += this->header.deserialize(inbuffer + offset);





nucho

0:77afd7560544


    91

      offset += this->orientation.deserialize(inbuffer + offset);





nucho

0:77afd7560544


    92

      unsigned char * orientation_covariance_val = (unsigned char *) this->orientation_covariance;





nucho

0:77afd7560544


    93

      for( unsigned char i = 0; i < 9; i++){





nucho

0:77afd7560544


    94

      unsigned long * val_orientation_covariancei = (unsigned long*) &(this->orientation_covariance[i]);





nucho

0:77afd7560544


    95

      offset += 3;





nucho

0:77afd7560544


    96

      *val_orientation_covariancei = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);





nucho

0:77afd7560544


    97

      *val_orientation_covariancei |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;





nucho

0:77afd7560544


    98

      *val_orientation_covariancei |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;





nucho

0:77afd7560544


    99

      *val_orientation_covariancei |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;





nucho

0:77afd7560544


   100

      unsigned long exp_orientation_covariancei = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;





nucho

0:77afd7560544


   101

      exp_orientation_covariancei |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;





nucho

0:77afd7560544


   102

      if(exp_orientation_covariancei !=0)





nucho

0:77afd7560544


   103

        *val_orientation_covariancei |= ((exp_orientation_covariancei)-1023+127)<<23;





nucho

0:77afd7560544


   104

      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->orientation_covariance[i] = -this->orientation_covariance[i];





nucho

0:77afd7560544


   105

      }





nucho

0:77afd7560544


   106

      offset += this->angular_velocity.deserialize(inbuffer + offset);





nucho

0:77afd7560544


   107

      unsigned char * angular_velocity_covariance_val = (unsigned char *) this->angular_velocity_covariance;





nucho

0:77afd7560544


   108

      for( unsigned char i = 0; i < 9; i++){





nucho

0:77afd7560544


   109

      unsigned long * val_angular_velocity_covariancei = (unsigned long*) &(this->angular_velocity_covariance[i]);





nucho

0:77afd7560544


   110

      offset += 3;





nucho

0:77afd7560544


   111

      *val_angular_velocity_covariancei = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);





nucho

0:77afd7560544


   112

      *val_angular_velocity_covariancei |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;





nucho

0:77afd7560544


   113

      *val_angular_velocity_covariancei |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;





nucho

0:77afd7560544


   114

      *val_angular_velocity_covariancei |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;





nucho

0:77afd7560544


   115

      unsigned long exp_angular_velocity_covariancei = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;





nucho

0:77afd7560544


   116

      exp_angular_velocity_covariancei |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;





nucho

0:77afd7560544


   117

      if(exp_angular_velocity_covariancei !=0)





nucho

0:77afd7560544


   118

        *val_angular_velocity_covariancei |= ((exp_angular_velocity_covariancei)-1023+127)<<23;





nucho

0:77afd7560544


   119

      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->angular_velocity_covariance[i] = -this->angular_velocity_covariance[i];





nucho

0:77afd7560544


   120

      }





nucho

0:77afd7560544


   121

      offset += this->linear_acceleration.deserialize(inbuffer + offset);





nucho

0:77afd7560544


   122

      unsigned char * linear_acceleration_covariance_val = (unsigned char *) this->linear_acceleration_covariance;





nucho

0:77afd7560544


   123

      for( unsigned char i = 0; i < 9; i++){





nucho

0:77afd7560544


   124

      unsigned long * val_linear_acceleration_covariancei = (unsigned long*) &(this->linear_acceleration_covariance[i]);





nucho

0:77afd7560544


   125

      offset += 3;





nucho

0:77afd7560544


   126

      *val_linear_acceleration_covariancei = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);





nucho

0:77afd7560544


   127

      *val_linear_acceleration_covariancei |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;





nucho

0:77afd7560544


   128

      *val_linear_acceleration_covariancei |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;





nucho

0:77afd7560544


   129

      *val_linear_acceleration_covariancei |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;





nucho

0:77afd7560544


   130

      unsigned long exp_linear_acceleration_covariancei = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;





nucho

0:77afd7560544


   131

      exp_linear_acceleration_covariancei |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;





nucho

0:77afd7560544


   132

      if(exp_linear_acceleration_covariancei !=0)





nucho

0:77afd7560544


   133

        *val_linear_acceleration_covariancei |= ((exp_linear_acceleration_covariancei)-1023+127)<<23;





nucho

0:77afd7560544


   134

      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->linear_acceleration_covariance[i] = -this->linear_acceleration_covariance[i];





nucho

0:77afd7560544


   135

      }





nucho

0:77afd7560544


   136

     return offset;





nucho

0:77afd7560544


   137

    }





nucho

0:77afd7560544


   138







nucho

0:77afd7560544


   139

   virtual const char * getType(){ return "sensor_msgs/Imu"; };





nucho

0:77afd7560544


   140







nucho

0:77afd7560544


   141

  };





nucho

0:77afd7560544


   142







nucho

0:77afd7560544


   143

}





nucho

0:77afd7560544


   144

#endif