nucho / Mbed 2 deprecated rosserial_mbed

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

Dependencies:   rosserial_mbed_lib mbed Servo

sensor_msgs/NavSatFix.h@0:06fc856e99ca, 2011-08-19 (annotated)

Committer:
nucho
Date:
Fri Aug 19 09:06:16 2011 +0000
Revision:
0:06fc856e99ca
Child:
3:dff241b66f84

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


nucho

0:06fc856e99ca


     1

#ifndef ros_NavSatFix_h





nucho

0:06fc856e99ca


     2

#define ros_NavSatFix_h





nucho

0:06fc856e99ca


     3







nucho

0:06fc856e99ca


     4

#include <stdint.h>





nucho

0:06fc856e99ca


     5

#include <string.h>





nucho

0:06fc856e99ca


     6

#include <stdlib.h>





nucho

0:06fc856e99ca


     7

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





nucho

0:06fc856e99ca


     8

#include "std_msgs/Header.h"





nucho

0:06fc856e99ca


     9

#include "sensor_msgs/NavSatStatus.h"





nucho

0:06fc856e99ca


    10







nucho

0:06fc856e99ca


    11

namespace sensor_msgs





nucho

0:06fc856e99ca


    12

{





nucho

0:06fc856e99ca


    13







nucho

0:06fc856e99ca


    14

  class NavSatFix : public ros::Msg





nucho

0:06fc856e99ca


    15

  {





nucho

0:06fc856e99ca


    16

    public:





nucho

0:06fc856e99ca


    17

      std_msgs::Header header;





nucho

0:06fc856e99ca


    18

      sensor_msgs::NavSatStatus status;





nucho

0:06fc856e99ca


    19

      float latitude;





nucho

0:06fc856e99ca


    20

      float longitude;





nucho

0:06fc856e99ca


    21

      float altitude;





nucho

0:06fc856e99ca


    22

      float position_covariance[9];





nucho

0:06fc856e99ca


    23

      unsigned char position_covariance_type;





nucho

0:06fc856e99ca


    24

      enum { COVARIANCE_TYPE_UNKNOWN = 0 };





nucho

0:06fc856e99ca


    25

      enum { COVARIANCE_TYPE_APPROXIMATED = 1 };





nucho

0:06fc856e99ca


    26

      enum { COVARIANCE_TYPE_DIAGONAL_KNOWN = 2 };





nucho

0:06fc856e99ca


    27

      enum { COVARIANCE_TYPE_KNOWN = 3 };





nucho

0:06fc856e99ca


    28







nucho

0:06fc856e99ca


    29

    virtual int serialize(unsigned char *outbuffer)





nucho

0:06fc856e99ca


    30

    {





nucho

0:06fc856e99ca


    31

      int offset = 0;





nucho

0:06fc856e99ca


    32

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





nucho

0:06fc856e99ca


    33

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





nucho

0:06fc856e99ca


    34

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





nucho

0:06fc856e99ca


    35

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





nucho

0:06fc856e99ca


    36

      if(exp_latitude != 0)





nucho

0:06fc856e99ca


    37

        exp_latitude += 1023-127;





nucho

0:06fc856e99ca


    38

      long sig_latitude = *val_latitude;





nucho

0:06fc856e99ca


    39

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    40

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    41

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    42

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





nucho

0:06fc856e99ca


    43

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





nucho

0:06fc856e99ca


    44

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





nucho

0:06fc856e99ca


    45

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





nucho

0:06fc856e99ca


    46

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





nucho

0:06fc856e99ca


    47

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





nucho

0:06fc856e99ca


    48

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





nucho

0:06fc856e99ca


    49

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





nucho

0:06fc856e99ca


    50

      if(exp_longitude != 0)





nucho

0:06fc856e99ca


    51

        exp_longitude += 1023-127;





nucho

0:06fc856e99ca


    52

      long sig_longitude = *val_longitude;





nucho

0:06fc856e99ca


    53

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    54

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    55

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    56

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





nucho

0:06fc856e99ca


    57

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





nucho

0:06fc856e99ca


    58

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





nucho

0:06fc856e99ca


    59

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





nucho

0:06fc856e99ca


    60

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





nucho

0:06fc856e99ca


    61

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





nucho

0:06fc856e99ca


    62

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





nucho

0:06fc856e99ca


    63

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





nucho

0:06fc856e99ca


    64

      if(exp_altitude != 0)





nucho

0:06fc856e99ca


    65

        exp_altitude += 1023-127;





nucho

0:06fc856e99ca


    66

      long sig_altitude = *val_altitude;





nucho

0:06fc856e99ca


    67

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    68

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    69

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    70

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





nucho

0:06fc856e99ca


    71

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





nucho

0:06fc856e99ca


    72

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





nucho

0:06fc856e99ca


    73

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





nucho

0:06fc856e99ca


    74

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





nucho

0:06fc856e99ca


    75

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





nucho

0:06fc856e99ca


    76

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





nucho

0:06fc856e99ca


    77

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





nucho

0:06fc856e99ca


    78

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





nucho

0:06fc856e99ca


    79

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





nucho

0:06fc856e99ca


    80

      if(exp_position_covariancei != 0)





nucho

0:06fc856e99ca


    81

        exp_position_covariancei += 1023-127;





nucho

0:06fc856e99ca


    82

      long sig_position_covariancei = *val_position_covariancei;





nucho

0:06fc856e99ca


    83

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    84

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    85

      *(outbuffer + offset++) = 0;





nucho

0:06fc856e99ca


    86

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





nucho

0:06fc856e99ca


    87

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





nucho

0:06fc856e99ca


    88

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





nucho

0:06fc856e99ca


    89

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





nucho

0:06fc856e99ca


    90

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





nucho

0:06fc856e99ca


    91

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





nucho

0:06fc856e99ca


    92

      }





nucho

0:06fc856e99ca


    93

      union {





nucho

0:06fc856e99ca


    94

        unsigned char real;





nucho

0:06fc856e99ca


    95

        unsigned char base;





nucho

0:06fc856e99ca


    96

      } u_position_covariance_type;





nucho

0:06fc856e99ca


    97

      u_position_covariance_type.real = this->position_covariance_type;





nucho

0:06fc856e99ca


    98

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





nucho

0:06fc856e99ca


    99

      offset += sizeof(this->position_covariance_type);





nucho

0:06fc856e99ca


   100

      return offset;





nucho

0:06fc856e99ca


   101

    }





nucho

0:06fc856e99ca


   102







nucho

0:06fc856e99ca


   103

    virtual int deserialize(unsigned char *inbuffer)





nucho

0:06fc856e99ca


   104

    {





nucho

0:06fc856e99ca


   105

      int offset = 0;





nucho

0:06fc856e99ca


   106

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





nucho

0:06fc856e99ca


   107

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





nucho

0:06fc856e99ca


   108

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





nucho

0:06fc856e99ca


   109

      offset += 3;





nucho

0:06fc856e99ca


   110

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





nucho

0:06fc856e99ca


   111

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





nucho

0:06fc856e99ca


   112

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





nucho

0:06fc856e99ca


   113

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





nucho

0:06fc856e99ca


   114

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





nucho

0:06fc856e99ca


   115

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





nucho

0:06fc856e99ca


   116

      if(exp_latitude !=0)





nucho

0:06fc856e99ca


   117

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





nucho

0:06fc856e99ca


   118

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





nucho

0:06fc856e99ca


   119

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





nucho

0:06fc856e99ca


   120

      offset += 3;





nucho

0:06fc856e99ca


   121

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





nucho

0:06fc856e99ca


   122

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





nucho

0:06fc856e99ca


   123

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





nucho

0:06fc856e99ca


   124

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





nucho

0:06fc856e99ca


   125

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





nucho

0:06fc856e99ca


   126

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





nucho

0:06fc856e99ca


   127

      if(exp_longitude !=0)





nucho

0:06fc856e99ca


   128

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





nucho

0:06fc856e99ca


   129

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





nucho

0:06fc856e99ca


   130

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





nucho

0:06fc856e99ca


   131

      offset += 3;





nucho

0:06fc856e99ca


   132

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





nucho

0:06fc856e99ca


   133

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





nucho

0:06fc856e99ca


   134

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





nucho

0:06fc856e99ca


   135

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





nucho

0:06fc856e99ca


   136

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





nucho

0:06fc856e99ca


   137

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





nucho

0:06fc856e99ca


   138

      if(exp_altitude !=0)





nucho

0:06fc856e99ca


   139

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





nucho

0:06fc856e99ca


   140

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





nucho

0:06fc856e99ca


   141

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





nucho

0:06fc856e99ca


   142

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





nucho

0:06fc856e99ca


   143

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





nucho

0:06fc856e99ca


   144

      offset += 3;





nucho

0:06fc856e99ca


   145

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





nucho

0:06fc856e99ca


   146

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





nucho

0:06fc856e99ca


   147

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





nucho

0:06fc856e99ca


   148

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





nucho

0:06fc856e99ca


   149

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





nucho

0:06fc856e99ca


   150

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





nucho

0:06fc856e99ca


   151

      if(exp_position_covariancei !=0)





nucho

0:06fc856e99ca


   152

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





nucho

0:06fc856e99ca


   153

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





nucho

0:06fc856e99ca


   154

      }





nucho

0:06fc856e99ca


   155

      union {





nucho

0:06fc856e99ca


   156

        unsigned char real;





nucho

0:06fc856e99ca


   157

        unsigned char base;





nucho

0:06fc856e99ca


   158

      } u_position_covariance_type;





nucho

0:06fc856e99ca


   159

      u_position_covariance_type.base = 0;





nucho

0:06fc856e99ca


   160

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





nucho

0:06fc856e99ca


   161

      this->position_covariance_type = u_position_covariance_type.real;





nucho

0:06fc856e99ca


   162

      offset += sizeof(this->position_covariance_type);





nucho

0:06fc856e99ca


   163

     return offset;





nucho

0:06fc856e99ca


   164

    }





nucho

0:06fc856e99ca


   165







nucho

0:06fc856e99ca


   166

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





nucho

0:06fc856e99ca


   167







nucho

0:06fc856e99ca


   168

  };





nucho

0:06fc856e99ca


   169







nucho

0:06fc856e99ca


   170

}





nucho

0:06fc856e99ca


   171

#endif