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@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_NavSatFix_h





nucho

0:77afd7560544


     2

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

0:77afd7560544


     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

0:77afd7560544


    23

      unsigned char position_covariance_type;





nucho

0:77afd7560544


    24

      enum { COVARIANCE_TYPE_UNKNOWN = 0 };





nucho

0:77afd7560544


    25

      enum { COVARIANCE_TYPE_APPROXIMATED = 1 };





nucho

0:77afd7560544


    26

      enum { COVARIANCE_TYPE_DIAGONAL_KNOWN = 2 };





nucho

0:77afd7560544


    27

      enum { COVARIANCE_TYPE_KNOWN = 3 };





nucho

0:77afd7560544


    28







nucho

0:77afd7560544


    29

    virtual int serialize(unsigned char *outbuffer)





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

0:77afd7560544


    34

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





nucho

0:77afd7560544


    35

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





nucho

0:77afd7560544


    36

      if(exp_latitude != 0)





nucho

0:77afd7560544


    37

        exp_latitude += 1023-127;





nucho

0:77afd7560544


    38

      long 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

0:77afd7560544


    48

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





nucho

0:77afd7560544


    49

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





nucho

0:77afd7560544


    50

      if(exp_longitude != 0)





nucho

0:77afd7560544


    51

        exp_longitude += 1023-127;





nucho

0:77afd7560544


    52

      long 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

0:77afd7560544


    62

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





nucho

0:77afd7560544


    63

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





nucho

0:77afd7560544


    64

      if(exp_altitude != 0)





nucho

0:77afd7560544


    65

        exp_altitude += 1023-127;





nucho

0:77afd7560544


    66

      long 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

0:77afd7560544


    77

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





nucho

0:77afd7560544


    78

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





nucho

0:77afd7560544


    79

      long 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

0:77afd7560544


    82

      long 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

0:77afd7560544


    93

      union {





nucho

0:77afd7560544


    94

        unsigned char real;





nucho

0:77afd7560544


    95

        unsigned char base;





nucho

0:77afd7560544


    96

      } u_position_covariance_type;





nucho

0:77afd7560544


    97

      u_position_covariance_type.real = this->position_covariance_type;





nucho

0:77afd7560544


    98

      *(outbuffer + offset + 0) = (u_position_covariance_type.base >> (8 * 0)) & 0xFF;





nucho

0:77afd7560544


    99

      offset += sizeof(this->position_covariance_type);





nucho

0:77afd7560544


   100

      return offset;





nucho

0:77afd7560544


   101

    }





nucho

0:77afd7560544


   102







nucho

0:77afd7560544


   103

    virtual int deserialize(unsigned char *inbuffer)





nucho

0:77afd7560544


   104

    {





nucho

0:77afd7560544


   105

      int offset = 0;





nucho

0:77afd7560544


   106

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





nucho

0:77afd7560544


   107

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





nucho

0:77afd7560544


   108

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





nucho

0:77afd7560544


   109

      offset += 3;





nucho

0:77afd7560544


   110

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





nucho

0:77afd7560544


   111

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





nucho

0:77afd7560544


   112

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





nucho

0:77afd7560544


   113

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





nucho

0:77afd7560544


   114

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





nucho

0:77afd7560544


   115

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





nucho

0:77afd7560544


   116

      if(exp_latitude !=0)





nucho

0:77afd7560544


   117

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





nucho

0:77afd7560544


   118

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





nucho

0:77afd7560544


   119

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





nucho

0:77afd7560544


   120

      offset += 3;





nucho

0:77afd7560544


   121

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





nucho

0:77afd7560544


   122

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





nucho

0:77afd7560544


   123

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





nucho

0:77afd7560544


   124

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





nucho

0:77afd7560544


   125

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





nucho

0:77afd7560544


   126

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





nucho

0:77afd7560544


   127

      if(exp_longitude !=0)





nucho

0:77afd7560544


   128

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





nucho

0:77afd7560544


   129

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





nucho

0:77afd7560544


   130

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





nucho

0:77afd7560544


   131

      offset += 3;





nucho

0:77afd7560544


   132

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





nucho

0:77afd7560544


   133

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





nucho

0:77afd7560544


   134

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





nucho

0:77afd7560544


   135

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





nucho

0:77afd7560544


   136

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





nucho

0:77afd7560544


   137

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





nucho

0:77afd7560544


   138

      if(exp_altitude !=0)





nucho

0:77afd7560544


   139

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





nucho

0:77afd7560544


   140

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





nucho

0:77afd7560544


   141

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





nucho

0:77afd7560544


   142

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





nucho

0:77afd7560544


   143

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





nucho

0:77afd7560544


   144

      offset += 3;





nucho

0:77afd7560544


   145

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





nucho

0:77afd7560544


   146

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





nucho

0:77afd7560544


   147

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





nucho

0:77afd7560544


   148

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





nucho

0:77afd7560544


   149

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





nucho

0:77afd7560544


   150

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





nucho

0:77afd7560544


   151

      if(exp_position_covariancei !=0)





nucho

0:77afd7560544


   152

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





nucho

0:77afd7560544


   153

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





nucho

0:77afd7560544


   154

      }





nucho

0:77afd7560544


   155

      union {





nucho

0:77afd7560544


   156

        unsigned char real;





nucho

0:77afd7560544


   157

        unsigned char base;





nucho

0:77afd7560544


   158

      } u_position_covariance_type;





nucho

0:77afd7560544


   159

      u_position_covariance_type.base = 0;





nucho

0:77afd7560544


   160

      u_position_covariance_type.base |= ((typeof(u_position_covariance_type.base)) (*(inbuffer + offset + 0))) << (8 * 0);





nucho

0:77afd7560544


   161

      this->position_covariance_type = u_position_covariance_type.real;





nucho

0:77afd7560544


   162

      offset += sizeof(this->position_covariance_type);





nucho

0:77afd7560544


   163

     return offset;





nucho

0:77afd7560544


   164

    }





nucho

0:77afd7560544


   165







nucho

0:77afd7560544


   166

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





nucho

0:77afd7560544


   167







nucho

0:77afd7560544


   168

  };





nucho

0:77afd7560544


   169







nucho

0:77afd7560544


   170

}





nucho

0:77afd7560544


   171

#endif