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/CameraInfo.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_CameraInfo_h





nucho

3:1cf99502f396


     2

#define _ROS_sensor_msgs_CameraInfo_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/RegionOfInterest.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 CameraInfo : public ros::Msg





nucho

0:77afd7560544


    15

  {





nucho

0:77afd7560544


    16

    public:





nucho

0:77afd7560544


    17

      std_msgs::Header header;





nucho

3:1cf99502f396


    18

      uint32_t height;





nucho

3:1cf99502f396


    19

      uint32_t width;





nucho

0:77afd7560544


    20

      char * distortion_model;





nucho

3:1cf99502f396


    21

      uint8_t D_length;





nucho

0:77afd7560544


    22

      float st_D;





nucho

0:77afd7560544


    23

      float * D;





nucho

0:77afd7560544


    24

      float K[9];





nucho

0:77afd7560544


    25

      float R[9];





nucho

0:77afd7560544


    26

      float P[12];





nucho

3:1cf99502f396


    27

      uint32_t binning_x;





nucho

3:1cf99502f396


    28

      uint32_t binning_y;





nucho

0:77afd7560544


    29

      sensor_msgs::RegionOfInterest roi;





nucho

0:77afd7560544


    30







nucho

3:1cf99502f396


    31

    virtual int serialize(unsigned char *outbuffer) const





nucho

0:77afd7560544


    32

    {





nucho

0:77afd7560544


    33

      int offset = 0;





nucho

0:77afd7560544


    34

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





nucho

3:1cf99502f396


    35

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





nucho

3:1cf99502f396


    36

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





nucho

3:1cf99502f396


    37

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





nucho

3:1cf99502f396


    38

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





nucho

0:77afd7560544


    39

      offset += sizeof(this->height);





nucho

3:1cf99502f396


    40

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





nucho

3:1cf99502f396


    41

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





nucho

3:1cf99502f396


    42

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





nucho

3:1cf99502f396


    43

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





nucho

0:77afd7560544


    44

      offset += sizeof(this->width);





nucho

3:1cf99502f396


    45

      uint32_t * length_distortion_model = (uint32_t *)(outbuffer + offset);





nucho

0:77afd7560544


    46

      *length_distortion_model = strlen( (const char*) this->distortion_model);





nucho

0:77afd7560544


    47

      offset += 4;





nucho

0:77afd7560544


    48

      memcpy(outbuffer + offset, this->distortion_model, *length_distortion_model);





nucho

0:77afd7560544


    49

      offset += *length_distortion_model;





nucho

0:77afd7560544


    50

      *(outbuffer + offset++) = D_length;





nucho

0:77afd7560544


    51

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    52

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    53

      *(outbuffer + offset++) = 0;





nucho

3:1cf99502f396


    54

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





nucho

3:1cf99502f396


    55

      int32_t * val_Di = (long *) &(this->D[i]);





nucho

3:1cf99502f396


    56

      int32_t exp_Di = (((*val_Di)>>23)&255);





nucho

0:77afd7560544


    57

      if(exp_Di != 0)





nucho

0:77afd7560544


    58

        exp_Di += 1023-127;





nucho

3:1cf99502f396


    59

      int32_t sig_Di = *val_Di;





nucho

0:77afd7560544


    60

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    61

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    62

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    63

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





nucho

0:77afd7560544


    64

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





nucho

0:77afd7560544


    65

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





nucho

0:77afd7560544


    66

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





nucho

0:77afd7560544


    67

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





nucho

0:77afd7560544


    68

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





nucho

0:77afd7560544


    69

      }





nucho

0:77afd7560544


    70

      unsigned char * K_val = (unsigned char *) this->K;





nucho

3:1cf99502f396


    71

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





nucho

3:1cf99502f396


    72

      int32_t * val_Ki = (long *) &(this->K[i]);





nucho

3:1cf99502f396


    73

      int32_t exp_Ki = (((*val_Ki)>>23)&255);





nucho

0:77afd7560544


    74

      if(exp_Ki != 0)





nucho

0:77afd7560544


    75

        exp_Ki += 1023-127;





nucho

3:1cf99502f396


    76

      int32_t sig_Ki = *val_Ki;





nucho

0:77afd7560544


    77

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    78

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    79

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    80

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





nucho

0:77afd7560544


    81

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





nucho

0:77afd7560544


    82

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





nucho

0:77afd7560544


    83

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





nucho

0:77afd7560544


    84

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





nucho

0:77afd7560544


    85

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





nucho

0:77afd7560544


    86

      }





nucho

0:77afd7560544


    87

      unsigned char * R_val = (unsigned char *) this->R;





nucho

3:1cf99502f396


    88

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





nucho

3:1cf99502f396


    89

      int32_t * val_Ri = (long *) &(this->R[i]);





nucho

3:1cf99502f396


    90

      int32_t exp_Ri = (((*val_Ri)>>23)&255);





nucho

0:77afd7560544


    91

      if(exp_Ri != 0)





nucho

0:77afd7560544


    92

        exp_Ri += 1023-127;





nucho

3:1cf99502f396


    93

      int32_t sig_Ri = *val_Ri;





nucho

0:77afd7560544


    94

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    95

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    96

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    97

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





nucho

0:77afd7560544


    98

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





nucho

0:77afd7560544


    99

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





nucho

0:77afd7560544


   100

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





nucho

0:77afd7560544


   101

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





nucho

0:77afd7560544


   102

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





nucho

0:77afd7560544


   103

      }





nucho

0:77afd7560544


   104

      unsigned char * P_val = (unsigned char *) this->P;





nucho

3:1cf99502f396


   105

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





nucho

3:1cf99502f396


   106

      int32_t * val_Pi = (long *) &(this->P[i]);





nucho

3:1cf99502f396


   107

      int32_t exp_Pi = (((*val_Pi)>>23)&255);





nucho

0:77afd7560544


   108

      if(exp_Pi != 0)





nucho

0:77afd7560544


   109

        exp_Pi += 1023-127;





nucho

3:1cf99502f396


   110

      int32_t sig_Pi = *val_Pi;





nucho

0:77afd7560544


   111

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


   112

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


   113

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


   114

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





nucho

0:77afd7560544


   115

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





nucho

0:77afd7560544


   116

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





nucho

0:77afd7560544


   117

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





nucho

0:77afd7560544


   118

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





nucho

0:77afd7560544


   119

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





nucho

0:77afd7560544


   120

      }





nucho

3:1cf99502f396


   121

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





nucho

3:1cf99502f396


   122

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





nucho

3:1cf99502f396


   123

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





nucho

3:1cf99502f396


   124

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





nucho

0:77afd7560544


   125

      offset += sizeof(this->binning_x);





nucho

3:1cf99502f396


   126

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





nucho

3:1cf99502f396


   127

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





nucho

3:1cf99502f396


   128

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





nucho

3:1cf99502f396


   129

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





nucho

0:77afd7560544


   130

      offset += sizeof(this->binning_y);





nucho

0:77afd7560544


   131

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





nucho

0:77afd7560544


   132

      return offset;





nucho

0:77afd7560544


   133

    }





nucho

0:77afd7560544


   134







nucho

0:77afd7560544


   135

    virtual int deserialize(unsigned char *inbuffer)





nucho

0:77afd7560544


   136

    {





nucho

0:77afd7560544


   137

      int offset = 0;





nucho

0:77afd7560544


   138

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





nucho

3:1cf99502f396


   139

      this->height |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);





nucho

3:1cf99502f396


   140

      this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);





nucho

3:1cf99502f396


   141

      this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);





nucho

3:1cf99502f396


   142

      this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);





nucho

0:77afd7560544


   143

      offset += sizeof(this->height);





nucho

3:1cf99502f396


   144

      this->width |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);





nucho

3:1cf99502f396


   145

      this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);





nucho

3:1cf99502f396


   146

      this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);





nucho

3:1cf99502f396


   147

      this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);





nucho

0:77afd7560544


   148

      offset += sizeof(this->width);





nucho

0:77afd7560544


   149

      uint32_t length_distortion_model = *(uint32_t *)(inbuffer + offset);





nucho

0:77afd7560544


   150

      offset += 4;





nucho

0:77afd7560544


   151

      for(unsigned int k= offset; k< offset+length_distortion_model; ++k){





nucho

0:77afd7560544


   152

          inbuffer[k-1]=inbuffer[k];





nucho

3:1cf99502f396


   153

      }





nucho

0:77afd7560544


   154

      inbuffer[offset+length_distortion_model-1]=0;





nucho

0:77afd7560544


   155

      this->distortion_model = (char *)(inbuffer + offset-1);





nucho

0:77afd7560544


   156

      offset += length_distortion_model;





nucho

3:1cf99502f396


   157

      uint8_t D_lengthT = *(inbuffer + offset++);





nucho

0:77afd7560544


   158

      if(D_lengthT > D_length)





nucho

0:77afd7560544


   159

        this->D = (float*)realloc(this->D, D_lengthT * sizeof(float));





nucho

0:77afd7560544


   160

      offset += 3;





nucho

0:77afd7560544


   161

      D_length = D_lengthT;





nucho

3:1cf99502f396


   162

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





nucho

3:1cf99502f396


   163

      uint32_t * val_st_D = (uint32_t*) &(this->st_D);





nucho

0:77afd7560544


   164

      offset += 3;





nucho

3:1cf99502f396


   165

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





nucho

3:1cf99502f396


   166

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





nucho

3:1cf99502f396


   167

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





nucho

3:1cf99502f396


   168

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





nucho

3:1cf99502f396


   169

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





nucho

3:1cf99502f396


   170

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





nucho

0:77afd7560544


   171

      if(exp_st_D !=0)





nucho

0:77afd7560544


   172

        *val_st_D |= ((exp_st_D)-1023+127)<<23;





nucho

0:77afd7560544


   173

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





nucho

0:77afd7560544


   174

        memcpy( &(this->D[i]), &(this->st_D), sizeof(float));





nucho

0:77afd7560544


   175

      }





nucho

3:1cf99502f396


   176

      uint8_t * K_val = (uint8_t*) this->K;





nucho

3:1cf99502f396


   177

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





nucho

3:1cf99502f396


   178

      uint32_t * val_Ki = (uint32_t*) &(this->K[i]);





nucho

0:77afd7560544


   179

      offset += 3;





nucho

3:1cf99502f396


   180

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





nucho

3:1cf99502f396


   181

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





nucho

3:1cf99502f396


   182

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





nucho

3:1cf99502f396


   183

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





nucho

3:1cf99502f396


   184

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





nucho

3:1cf99502f396


   185

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





nucho

0:77afd7560544


   186

      if(exp_Ki !=0)





nucho

0:77afd7560544


   187

        *val_Ki |= ((exp_Ki)-1023+127)<<23;





nucho

0:77afd7560544


   188

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





nucho

0:77afd7560544


   189

      }





nucho

3:1cf99502f396


   190

      uint8_t * R_val = (uint8_t*) this->R;





nucho

3:1cf99502f396


   191

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





nucho

3:1cf99502f396


   192

      uint32_t * val_Ri = (uint32_t*) &(this->R[i]);





nucho

0:77afd7560544


   193

      offset += 3;





nucho

3:1cf99502f396


   194

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





nucho

3:1cf99502f396


   195

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





nucho

3:1cf99502f396


   196

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





nucho

3:1cf99502f396


   197

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





nucho

3:1cf99502f396


   198

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





nucho

3:1cf99502f396


   199

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





nucho

0:77afd7560544


   200

      if(exp_Ri !=0)





nucho

0:77afd7560544


   201

        *val_Ri |= ((exp_Ri)-1023+127)<<23;





nucho

0:77afd7560544


   202

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





nucho

0:77afd7560544


   203

      }





nucho

3:1cf99502f396


   204

      uint8_t * P_val = (uint8_t*) this->P;





nucho

3:1cf99502f396


   205

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





nucho

3:1cf99502f396


   206

      uint32_t * val_Pi = (uint32_t*) &(this->P[i]);





nucho

0:77afd7560544


   207

      offset += 3;





nucho

3:1cf99502f396


   208

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





nucho

3:1cf99502f396


   209

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





nucho

3:1cf99502f396


   210

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





nucho

3:1cf99502f396


   211

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





nucho

3:1cf99502f396


   212

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





nucho

3:1cf99502f396


   213

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





nucho

0:77afd7560544


   214

      if(exp_Pi !=0)





nucho

0:77afd7560544


   215

        *val_Pi |= ((exp_Pi)-1023+127)<<23;





nucho

0:77afd7560544


   216

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





nucho

0:77afd7560544


   217

      }





nucho

3:1cf99502f396


   218

      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);





nucho

3:1cf99502f396


   219

      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);





nucho

3:1cf99502f396


   220

      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);





nucho

3:1cf99502f396


   221

      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);





nucho

0:77afd7560544


   222

      offset += sizeof(this->binning_x);





nucho

3:1cf99502f396


   223

      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);





nucho

3:1cf99502f396


   224

      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);





nucho

3:1cf99502f396


   225

      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);





nucho

3:1cf99502f396


   226

      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);





nucho

0:77afd7560544


   227

      offset += sizeof(this->binning_y);





nucho

0:77afd7560544


   228

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





nucho

0:77afd7560544


   229

     return offset;





nucho

0:77afd7560544


   230

    }





nucho

0:77afd7560544


   231







nucho

3:1cf99502f396


   232

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





nucho

3:1cf99502f396


   233

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





nucho

0:77afd7560544


   234







nucho

0:77afd7560544


   235

  };





nucho

0:77afd7560544


   236







nucho

0:77afd7560544


   237

}





nucho

0:77afd7560544


   238

#endif