jjzak / rosserial_mbed_lib

modify for Hydro version

Dependencies:   MODSERIAL

Fork of rosserial_mbed_lib by nucho

sensor_msgs/LaserEcho.h@5:8cd48977ec68, 2013-10-26 (annotated)

Committer:
jjzak
Date:
Sat Oct 26 15:38:35 2013 +0000
Revision:
5:8cd48977ec68
modify for Hydro version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jjzak 5:8cd48977ec68 1 #ifndef _ROS_sensor_msgs_LaserEcho_h
jjzak 5:8cd48977ec68 2 #define _ROS_sensor_msgs_LaserEcho_h
jjzak 5:8cd48977ec68 3
jjzak 5:8cd48977ec68 4 #include <stdint.h>
jjzak 5:8cd48977ec68 5 #include <string.h>
jjzak 5:8cd48977ec68 6 #include <stdlib.h>
jjzak 5:8cd48977ec68 7 #include "ros/msg.h"
jjzak 5:8cd48977ec68 8
jjzak 5:8cd48977ec68 9 namespace sensor_msgs
jjzak 5:8cd48977ec68 10 {
jjzak 5:8cd48977ec68 11
jjzak 5:8cd48977ec68 12 class LaserEcho : public ros::Msg
jjzak 5:8cd48977ec68 13 {
jjzak 5:8cd48977ec68 14 public:
jjzak 5:8cd48977ec68 15 uint8_t echoes_length;
jjzak 5:8cd48977ec68 16 float st_echoes;
jjzak 5:8cd48977ec68 17 float * echoes;
jjzak 5:8cd48977ec68 18
jjzak 5:8cd48977ec68 19 virtual int serialize(unsigned char *outbuffer) const
jjzak 5:8cd48977ec68 20 {
jjzak 5:8cd48977ec68 21 int offset = 0;
jjzak 5:8cd48977ec68 22 *(outbuffer + offset++) = echoes_length;
jjzak 5:8cd48977ec68 23 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 24 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 25 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 26 for( uint8_t i = 0; i < echoes_length; i++){
jjzak 5:8cd48977ec68 27 union {
jjzak 5:8cd48977ec68 28 float real;
jjzak 5:8cd48977ec68 29 uint32_t base;
jjzak 5:8cd48977ec68 30 } u_echoesi;
jjzak 5:8cd48977ec68 31 u_echoesi.real = this->echoes[i];
jjzak 5:8cd48977ec68 32 *(outbuffer + offset + 0) = (u_echoesi.base >> (8 * 0)) & 0xFF;
jjzak 5:8cd48977ec68 33 *(outbuffer + offset + 1) = (u_echoesi.base >> (8 * 1)) & 0xFF;
jjzak 5:8cd48977ec68 34 *(outbuffer + offset + 2) = (u_echoesi.base >> (8 * 2)) & 0xFF;
jjzak 5:8cd48977ec68 35 *(outbuffer + offset + 3) = (u_echoesi.base >> (8 * 3)) & 0xFF;
jjzak 5:8cd48977ec68 36 offset += sizeof(this->echoes[i]);
jjzak 5:8cd48977ec68 37 }
jjzak 5:8cd48977ec68 38 return offset;
jjzak 5:8cd48977ec68 39 }
jjzak 5:8cd48977ec68 40
jjzak 5:8cd48977ec68 41 virtual int deserialize(unsigned char *inbuffer)
jjzak 5:8cd48977ec68 42 {
jjzak 5:8cd48977ec68 43 int offset = 0;
jjzak 5:8cd48977ec68 44 uint8_t echoes_lengthT = *(inbuffer + offset++);
jjzak 5:8cd48977ec68 45 if(echoes_lengthT > echoes_length)
jjzak 5:8cd48977ec68 46 this->echoes = (float*)realloc(this->echoes, echoes_lengthT * sizeof(float));
jjzak 5:8cd48977ec68 47 offset += 3;
jjzak 5:8cd48977ec68 48 echoes_length = echoes_lengthT;
jjzak 5:8cd48977ec68 49 for( uint8_t i = 0; i < echoes_length; i++){
jjzak 5:8cd48977ec68 50 union {
jjzak 5:8cd48977ec68 51 float real;
jjzak 5:8cd48977ec68 52 uint32_t base;
jjzak 5:8cd48977ec68 53 } u_st_echoes;
jjzak 5:8cd48977ec68 54 u_st_echoes.base = 0;
jjzak 5:8cd48977ec68 55 u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
jjzak 5:8cd48977ec68 56 u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
jjzak 5:8cd48977ec68 57 u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
jjzak 5:8cd48977ec68 58 u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
jjzak 5:8cd48977ec68 59 this->st_echoes = u_st_echoes.real;
jjzak 5:8cd48977ec68 60 offset += sizeof(this->st_echoes);
jjzak 5:8cd48977ec68 61 memcpy( &(this->echoes[i]), &(this->st_echoes), sizeof(float));
jjzak 5:8cd48977ec68 62 }
jjzak 5:8cd48977ec68 63 return offset;
jjzak 5:8cd48977ec68 64 }
jjzak 5:8cd48977ec68 65
jjzak 5:8cd48977ec68 66 const char * getType(){ return "sensor_msgs/LaserEcho"; };
jjzak 5:8cd48977ec68 67 const char * getMD5(){ return "8bc5ae449b200fba4d552b4225586696"; };
jjzak 5:8cd48977ec68 68
jjzak 5:8cd48977ec68 69 };
jjzak 5:8cd48977ec68 70
jjzak 5:8cd48977ec68 71 }
jjzak 5:8cd48977ec68 72 #endif