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@4:684f39d0c346, 2012-02-29 (annotated)

Committer:
nucho
Date:
Wed Feb 29 23:00:21 2012 +0000
Revision:
4:684f39d0c346
Parent:
3:1cf99502f396 

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


nucho

3:1cf99502f396


     1

#ifndef _ROS_sensor_msgs_Imu_h





nucho

3:1cf99502f396


     2

#define _ROS_sensor_msgs_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

3:1cf99502f396


     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

3:1cf99502f396


    26

    virtual int serialize(unsigned char *outbuffer) const





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

3:1cf99502f396


    32

      for( uint8_t i = 0; i < 9; i++){





nucho

3:1cf99502f396


    33

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





nucho

3:1cf99502f396


    34

      int32_t 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

3:1cf99502f396


    37

      int32_t 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

3:1cf99502f396


    50

      for( uint8_t i = 0; i < 9; i++){





nucho

3:1cf99502f396


    51

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





nucho

3:1cf99502f396


    52

      int32_t 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

3:1cf99502f396


    55

      int32_t 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

3:1cf99502f396


    68

      for( uint8_t i = 0; i < 9; i++){





nucho

3:1cf99502f396


    69

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





nucho

3:1cf99502f396


    70

      int32_t 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

3:1cf99502f396


    73

      int32_t 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

3:1cf99502f396


    92

      uint8_t * orientation_covariance_val = (uint8_t*) this->orientation_covariance;





nucho

3:1cf99502f396


    93

      for( uint8_t i = 0; i < 9; i++){





nucho

3:1cf99502f396


    94

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





nucho

0:77afd7560544


    95

      offset += 3;





nucho

3:1cf99502f396


    96

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





nucho

3:1cf99502f396


    97

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





nucho

3:1cf99502f396


    98

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





nucho

3:1cf99502f396


    99

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





nucho

3:1cf99502f396


   100

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





nucho

3:1cf99502f396


   101

      exp_orientation_covariancei |= ((uint32_t)(*(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

3:1cf99502f396


   107

      uint8_t * angular_velocity_covariance_val = (uint8_t*) this->angular_velocity_covariance;





nucho

3:1cf99502f396


   108

      for( uint8_t i = 0; i < 9; i++){





nucho

3:1cf99502f396


   109

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





nucho

0:77afd7560544


   110

      offset += 3;





nucho

3:1cf99502f396


   111

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





nucho

3:1cf99502f396


   112

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





nucho

3:1cf99502f396


   113

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





nucho

3:1cf99502f396


   114

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





nucho

3:1cf99502f396


   115

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





nucho

3:1cf99502f396


   116

      exp_angular_velocity_covariancei |= ((uint32_t)(*(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

3:1cf99502f396


   122

      uint8_t * linear_acceleration_covariance_val = (uint8_t*) this->linear_acceleration_covariance;





nucho

3:1cf99502f396


   123

      for( uint8_t i = 0; i < 9; i++){





nucho

3:1cf99502f396


   124

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





nucho

0:77afd7560544


   125

      offset += 3;





nucho

3:1cf99502f396


   126

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





nucho

3:1cf99502f396


   127

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





nucho

3:1cf99502f396


   128

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





nucho

3:1cf99502f396


   129

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





nucho

3:1cf99502f396


   130

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





nucho

3:1cf99502f396


   131

      exp_linear_acceleration_covariancei |= ((uint32_t)(*(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

3:1cf99502f396


   139

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





nucho

3:1cf99502f396


   140

    virtual const char * getMD5(){ return "6a62c6daae103f4ff57a132d6f95cec2"; };





nucho

0:77afd7560544


   141







nucho

0:77afd7560544


   142

  };





nucho

0:77afd7560544


   143







nucho

0:77afd7560544


   144

}





nucho

0:77afd7560544


   145

#endif