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/NavSatFix.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_NavSatFix_h





nucho

3:1cf99502f396


     2

#define _ROS_sensor_msgs_NavSatFix_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 "sensor_msgs/NavSatStatus.h"





nucho

0:77afd7560544


    10







nucho

0:77afd7560544


    11

namespace sensor_msgs





nucho

0:77afd7560544


    12

{





nucho

0:77afd7560544


    13







nucho

0:77afd7560544


    14

  class NavSatFix : public ros::Msg





nucho

0:77afd7560544


    15

  {





nucho

0:77afd7560544


    16

    public:





nucho

0:77afd7560544


    17

      std_msgs::Header header;





nucho

0:77afd7560544


    18

      sensor_msgs::NavSatStatus status;





nucho

0:77afd7560544


    19

      float latitude;





nucho

0:77afd7560544


    20

      float longitude;





nucho

0:77afd7560544


    21

      float altitude;





nucho

0:77afd7560544


    22

      float position_covariance[9];





nucho

3:1cf99502f396


    23

      uint8_t position_covariance_type;





nucho

3:1cf99502f396


    24

      enum { COVARIANCE_TYPE_UNKNOWN =  0 };





nucho

3:1cf99502f396


    25

      enum { COVARIANCE_TYPE_APPROXIMATED =  1 };





nucho

3:1cf99502f396


    26

      enum { COVARIANCE_TYPE_DIAGONAL_KNOWN =  2 };





nucho

3:1cf99502f396


    27

      enum { COVARIANCE_TYPE_KNOWN =  3 };





nucho

0:77afd7560544


    28







nucho

3:1cf99502f396


    29

    virtual int serialize(unsigned char *outbuffer) const





nucho

0:77afd7560544


    30

    {





nucho

0:77afd7560544


    31

      int offset = 0;





nucho

0:77afd7560544


    32

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





nucho

0:77afd7560544


    33

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





nucho

3:1cf99502f396


    34

      int32_t * val_latitude = (long *) &(this->latitude);





nucho

3:1cf99502f396


    35

      int32_t exp_latitude = (((*val_latitude)>>23)&255);





nucho

0:77afd7560544


    36

      if(exp_latitude != 0)





nucho

0:77afd7560544


    37

        exp_latitude += 1023-127;





nucho

3:1cf99502f396


    38

      int32_t sig_latitude = *val_latitude;





nucho

0:77afd7560544


    39

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    40

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    41

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    42

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





nucho

0:77afd7560544


    43

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





nucho

0:77afd7560544


    44

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





nucho

0:77afd7560544


    45

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





nucho

0:77afd7560544


    46

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





nucho

0:77afd7560544


    47

      if(this->latitude < 0) *(outbuffer + offset -1) |= 0x80;





nucho

3:1cf99502f396


    48

      int32_t * val_longitude = (long *) &(this->longitude);





nucho

3:1cf99502f396


    49

      int32_t exp_longitude = (((*val_longitude)>>23)&255);





nucho

0:77afd7560544


    50

      if(exp_longitude != 0)





nucho

0:77afd7560544


    51

        exp_longitude += 1023-127;





nucho

3:1cf99502f396


    52

      int32_t sig_longitude = *val_longitude;





nucho

0:77afd7560544


    53

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    54

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    55

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    56

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





nucho

0:77afd7560544


    57

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





nucho

0:77afd7560544


    58

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





nucho

0:77afd7560544


    59

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





nucho

0:77afd7560544


    60

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





nucho

0:77afd7560544


    61

      if(this->longitude < 0) *(outbuffer + offset -1) |= 0x80;





nucho

3:1cf99502f396


    62

      int32_t * val_altitude = (long *) &(this->altitude);





nucho

3:1cf99502f396


    63

      int32_t exp_altitude = (((*val_altitude)>>23)&255);





nucho

0:77afd7560544


    64

      if(exp_altitude != 0)





nucho

0:77afd7560544


    65

        exp_altitude += 1023-127;





nucho

3:1cf99502f396


    66

      int32_t sig_altitude = *val_altitude;





nucho

0:77afd7560544


    67

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    68

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    69

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    70

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





nucho

0:77afd7560544


    71

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





nucho

0:77afd7560544


    72

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





nucho

0:77afd7560544


    73

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





nucho

0:77afd7560544


    74

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





nucho

0:77afd7560544


    75

      if(this->altitude < 0) *(outbuffer + offset -1) |= 0x80;





nucho

0:77afd7560544


    76

      unsigned char * position_covariance_val = (unsigned char *) this->position_covariance;





nucho

3:1cf99502f396


    77

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





nucho

3:1cf99502f396


    78

      int32_t * val_position_covariancei = (long *) &(this->position_covariance[i]);





nucho

3:1cf99502f396


    79

      int32_t exp_position_covariancei = (((*val_position_covariancei)>>23)&255);





nucho

0:77afd7560544


    80

      if(exp_position_covariancei != 0)





nucho

0:77afd7560544


    81

        exp_position_covariancei += 1023-127;





nucho

3:1cf99502f396


    82

      int32_t sig_position_covariancei = *val_position_covariancei;





nucho

0:77afd7560544


    83

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    84

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    85

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    86

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





nucho

0:77afd7560544


    87

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





nucho

0:77afd7560544


    88

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





nucho

0:77afd7560544


    89

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





nucho

0:77afd7560544


    90

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





nucho

0:77afd7560544


    91

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





nucho

0:77afd7560544


    92

      }





nucho

3:1cf99502f396


    93

      *(outbuffer + offset + 0) = (this->position_covariance_type >> (8 * 0)) & 0xFF;





nucho

0:77afd7560544


    94

      offset += sizeof(this->position_covariance_type);





nucho

0:77afd7560544


    95

      return offset;





nucho

0:77afd7560544


    96

    }





nucho

0:77afd7560544


    97







nucho

0:77afd7560544


    98

    virtual int deserialize(unsigned char *inbuffer)





nucho

0:77afd7560544


    99

    {





nucho

0:77afd7560544


   100

      int offset = 0;





nucho

0:77afd7560544


   101

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





nucho

0:77afd7560544


   102

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





nucho

3:1cf99502f396


   103

      uint32_t * val_latitude = (uint32_t*) &(this->latitude);





nucho

0:77afd7560544


   104

      offset += 3;





nucho

3:1cf99502f396


   105

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





nucho

3:1cf99502f396


   106

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





nucho

3:1cf99502f396


   107

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





nucho

3:1cf99502f396


   108

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





nucho

3:1cf99502f396


   109

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





nucho

3:1cf99502f396


   110

      exp_latitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;





nucho

0:77afd7560544


   111

      if(exp_latitude !=0)





nucho

0:77afd7560544


   112

        *val_latitude |= ((exp_latitude)-1023+127)<<23;





nucho

0:77afd7560544


   113

      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->latitude = -this->latitude;





nucho

3:1cf99502f396


   114

      uint32_t * val_longitude = (uint32_t*) &(this->longitude);





nucho

0:77afd7560544


   115

      offset += 3;





nucho

3:1cf99502f396


   116

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





nucho

3:1cf99502f396


   117

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





nucho

3:1cf99502f396


   118

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





nucho

3:1cf99502f396


   119

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





nucho

3:1cf99502f396


   120

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





nucho

3:1cf99502f396


   121

      exp_longitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;





nucho

0:77afd7560544


   122

      if(exp_longitude !=0)





nucho

0:77afd7560544


   123

        *val_longitude |= ((exp_longitude)-1023+127)<<23;





nucho

0:77afd7560544


   124

      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->longitude = -this->longitude;





nucho

3:1cf99502f396


   125

      uint32_t * val_altitude = (uint32_t*) &(this->altitude);





nucho

0:77afd7560544


   126

      offset += 3;





nucho

3:1cf99502f396


   127

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





nucho

3:1cf99502f396


   128

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





nucho

3:1cf99502f396


   129

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





nucho

3:1cf99502f396


   130

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





nucho

3:1cf99502f396


   131

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





nucho

3:1cf99502f396


   132

      exp_altitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;





nucho

0:77afd7560544


   133

      if(exp_altitude !=0)





nucho

0:77afd7560544


   134

        *val_altitude |= ((exp_altitude)-1023+127)<<23;





nucho

0:77afd7560544


   135

      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->altitude = -this->altitude;





nucho

3:1cf99502f396


   136

      uint8_t * position_covariance_val = (uint8_t*) this->position_covariance;





nucho

3:1cf99502f396


   137

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





nucho

3:1cf99502f396


   138

      uint32_t * val_position_covariancei = (uint32_t*) &(this->position_covariance[i]);





nucho

0:77afd7560544


   139

      offset += 3;





nucho

3:1cf99502f396


   140

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





nucho

3:1cf99502f396


   141

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





nucho

3:1cf99502f396


   142

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





nucho

3:1cf99502f396


   143

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





nucho

3:1cf99502f396


   144

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





nucho

3:1cf99502f396


   145

      exp_position_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;





nucho

0:77afd7560544


   146

      if(exp_position_covariancei !=0)





nucho

0:77afd7560544


   147

        *val_position_covariancei |= ((exp_position_covariancei)-1023+127)<<23;





nucho

0:77afd7560544


   148

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





nucho

0:77afd7560544


   149

      }





nucho

3:1cf99502f396


   150

      this->position_covariance_type |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);





nucho

0:77afd7560544


   151

      offset += sizeof(this->position_covariance_type);





nucho

0:77afd7560544


   152

     return offset;





nucho

0:77afd7560544


   153

    }





nucho

0:77afd7560544


   154







nucho

3:1cf99502f396


   155

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





nucho

3:1cf99502f396


   156

    virtual const char * getMD5(){ return "2d3a8cd499b9b4a0249fb98fd05cfa48"; };





nucho

0:77afd7560544


   157







nucho

0:77afd7560544


   158

  };





nucho

0:77afd7560544


   159







nucho

0:77afd7560544


   160

}





nucho

0:77afd7560544


   161

#endif