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/PointCloud2.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_PointCloud2_h





nucho

3:1cf99502f396


     2

#define _ROS_sensor_msgs_PointCloud2_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/PointField.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 PointCloud2 : 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

3:1cf99502f396


    20

      uint8_t fields_length;





nucho

0:77afd7560544


    21

      sensor_msgs::PointField st_fields;





nucho

0:77afd7560544


    22

      sensor_msgs::PointField * fields;





nucho

0:77afd7560544


    23

      bool is_bigendian;





nucho

3:1cf99502f396


    24

      uint32_t point_step;





nucho

3:1cf99502f396


    25

      uint32_t row_step;





nucho

3:1cf99502f396


    26

      uint8_t data_length;





nucho

3:1cf99502f396


    27

      uint8_t st_data;





nucho

3:1cf99502f396


    28

      uint8_t * data;





nucho

0:77afd7560544


    29

      bool is_dense;





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

0:77afd7560544


    45

      *(outbuffer + offset++) = fields_length;





nucho

0:77afd7560544


    46

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    47

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    48

      *(outbuffer + offset++) = 0;





nucho

3:1cf99502f396


    49

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





nucho

0:77afd7560544


    50

      offset += this->fields[i].serialize(outbuffer + offset);





nucho

0:77afd7560544


    51

      }





nucho

0:77afd7560544


    52

      union {





nucho

0:77afd7560544


    53

        bool real;





nucho

3:1cf99502f396


    54

        uint8_t base;





nucho

0:77afd7560544


    55

      } u_is_bigendian;





nucho

0:77afd7560544


    56

      u_is_bigendian.real = this->is_bigendian;





nucho

0:77afd7560544


    57

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





nucho

0:77afd7560544


    58

      offset += sizeof(this->is_bigendian);





nucho

3:1cf99502f396


    59

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





nucho

3:1cf99502f396


    60

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





nucho

3:1cf99502f396


    61

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





nucho

3:1cf99502f396


    62

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





nucho

0:77afd7560544


    63

      offset += sizeof(this->point_step);





nucho

3:1cf99502f396


    64

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





nucho

3:1cf99502f396


    65

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





nucho

3:1cf99502f396


    66

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





nucho

3:1cf99502f396


    67

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





nucho

0:77afd7560544


    68

      offset += sizeof(this->row_step);





nucho

0:77afd7560544


    69

      *(outbuffer + offset++) = data_length;





nucho

0:77afd7560544


    70

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    71

      *(outbuffer + offset++) = 0;





nucho

0:77afd7560544


    72

      *(outbuffer + offset++) = 0;





nucho

3:1cf99502f396


    73

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





nucho

3:1cf99502f396


    74

      *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF;





nucho

0:77afd7560544


    75

      offset += sizeof(this->data[i]);





nucho

0:77afd7560544


    76

      }





nucho

0:77afd7560544


    77

      union {





nucho

0:77afd7560544


    78

        bool real;





nucho

3:1cf99502f396


    79

        uint8_t base;





nucho

0:77afd7560544


    80

      } u_is_dense;





nucho

0:77afd7560544


    81

      u_is_dense.real = this->is_dense;





nucho

0:77afd7560544


    82

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





nucho

0:77afd7560544


    83

      offset += sizeof(this->is_dense);





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

3:1cf99502f396


    91

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





nucho

3:1cf99502f396


    92

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





nucho

3:1cf99502f396


    93

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





nucho

3:1cf99502f396


    94

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





nucho

0:77afd7560544


    95

      offset += sizeof(this->height);





nucho

3:1cf99502f396


    96

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





nucho

3:1cf99502f396


    97

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





nucho

3:1cf99502f396


    98

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





nucho

3:1cf99502f396


    99

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





nucho

0:77afd7560544


   100

      offset += sizeof(this->width);





nucho

3:1cf99502f396


   101

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





nucho

0:77afd7560544


   102

      if(fields_lengthT > fields_length)





nucho

0:77afd7560544


   103

        this->fields = (sensor_msgs::PointField*)realloc(this->fields, fields_lengthT * sizeof(sensor_msgs::PointField));





nucho

0:77afd7560544


   104

      offset += 3;





nucho

0:77afd7560544


   105

      fields_length = fields_lengthT;





nucho

3:1cf99502f396


   106

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





nucho

0:77afd7560544


   107

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





nucho

0:77afd7560544


   108

        memcpy( &(this->fields[i]), &(this->st_fields), sizeof(sensor_msgs::PointField));





nucho

0:77afd7560544


   109

      }





nucho

0:77afd7560544


   110

      union {





nucho

0:77afd7560544


   111

        bool real;





nucho

3:1cf99502f396


   112

        uint8_t base;





nucho

0:77afd7560544


   113

      } u_is_bigendian;





nucho

0:77afd7560544


   114

      u_is_bigendian.base = 0;





nucho

3:1cf99502f396


   115

      u_is_bigendian.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);





nucho

0:77afd7560544


   116

      this->is_bigendian = u_is_bigendian.real;





nucho

0:77afd7560544


   117

      offset += sizeof(this->is_bigendian);





nucho

3:1cf99502f396


   118

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





nucho

3:1cf99502f396


   119

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





nucho

3:1cf99502f396


   120

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





nucho

3:1cf99502f396


   121

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





nucho

0:77afd7560544


   122

      offset += sizeof(this->point_step);





nucho

3:1cf99502f396


   123

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





nucho

3:1cf99502f396


   124

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





nucho

3:1cf99502f396


   125

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





nucho

3:1cf99502f396


   126

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





nucho

0:77afd7560544


   127

      offset += sizeof(this->row_step);





nucho

3:1cf99502f396


   128

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





nucho

0:77afd7560544


   129

      if(data_lengthT > data_length)





nucho

3:1cf99502f396


   130

        this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t));





nucho

0:77afd7560544


   131

      offset += 3;





nucho

0:77afd7560544


   132

      data_length = data_lengthT;





nucho

3:1cf99502f396


   133

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





nucho

3:1cf99502f396


   134

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





nucho

0:77afd7560544


   135

      offset += sizeof(this->st_data);





nucho

3:1cf99502f396


   136

        memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t));





nucho

0:77afd7560544


   137

      }





nucho

0:77afd7560544


   138

      union {





nucho

0:77afd7560544


   139

        bool real;





nucho

3:1cf99502f396


   140

        uint8_t base;





nucho

0:77afd7560544


   141

      } u_is_dense;





nucho

0:77afd7560544


   142

      u_is_dense.base = 0;





nucho

3:1cf99502f396


   143

      u_is_dense.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);





nucho

0:77afd7560544


   144

      this->is_dense = u_is_dense.real;





nucho

0:77afd7560544


   145

      offset += sizeof(this->is_dense);





nucho

0:77afd7560544


   146

     return offset;





nucho

0:77afd7560544


   147

    }





nucho

0:77afd7560544


   148







nucho

3:1cf99502f396


   149

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





nucho

3:1cf99502f396


   150

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





nucho

0:77afd7560544


   151







nucho

0:77afd7560544


   152

  };





nucho

0:77afd7560544


   153







nucho

0:77afd7560544


   154

}





nucho

0:77afd7560544


   155

#endif