modify for Hydro version

Dependencies:   MODSERIAL

Fork of rosserial_mbed_lib by nucho

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nucho 3:1cf99502f396 1 #ifndef _ROS_SERVICE_RequestParam_h
nucho 3:1cf99502f396 2 #define _ROS_SERVICE_RequestParam_h
nucho 0:77afd7560544 3 #include <stdint.h>
nucho 0:77afd7560544 4 #include <string.h>
nucho 0:77afd7560544 5 #include <stdlib.h>
nucho 3:1cf99502f396 6 #include "ros/msg.h"
nucho 0:77afd7560544 7
nucho 0:77afd7560544 8 namespace rosserial_msgs
nucho 0:77afd7560544 9 {
nucho 0:77afd7560544 10
nucho 0:77afd7560544 11 static const char REQUESTPARAM[] = "rosserial_msgs/RequestParam";
nucho 0:77afd7560544 12
nucho 0:77afd7560544 13 class RequestParamRequest : public ros::Msg
nucho 0:77afd7560544 14 {
nucho 0:77afd7560544 15 public:
nucho 0:77afd7560544 16 char * name;
nucho 0:77afd7560544 17
nucho 3:1cf99502f396 18 virtual int serialize(unsigned char *outbuffer) const
nucho 0:77afd7560544 19 {
nucho 0:77afd7560544 20 int offset = 0;
jjzak 5:8cd48977ec68 21 uint32_t length_name = strlen( (const char*) this->name);
jjzak 5:8cd48977ec68 22 memcpy(outbuffer + offset, &length_name, sizeof(uint32_t));
nucho 0:77afd7560544 23 offset += 4;
jjzak 5:8cd48977ec68 24 memcpy(outbuffer + offset, this->name, length_name);
jjzak 5:8cd48977ec68 25 offset += length_name;
nucho 0:77afd7560544 26 return offset;
nucho 0:77afd7560544 27 }
nucho 0:77afd7560544 28
nucho 0:77afd7560544 29 virtual int deserialize(unsigned char *inbuffer)
nucho 0:77afd7560544 30 {
nucho 0:77afd7560544 31 int offset = 0;
jjzak 5:8cd48977ec68 32 uint32_t length_name;
jjzak 5:8cd48977ec68 33 memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t));
nucho 0:77afd7560544 34 offset += 4;
nucho 0:77afd7560544 35 for(unsigned int k= offset; k< offset+length_name; ++k){
nucho 0:77afd7560544 36 inbuffer[k-1]=inbuffer[k];
nucho 3:1cf99502f396 37 }
nucho 0:77afd7560544 38 inbuffer[offset+length_name-1]=0;
nucho 0:77afd7560544 39 this->name = (char *)(inbuffer + offset-1);
nucho 0:77afd7560544 40 offset += length_name;
nucho 0:77afd7560544 41 return offset;
nucho 0:77afd7560544 42 }
nucho 0:77afd7560544 43
nucho 0:77afd7560544 44 virtual const char * getType(){ return REQUESTPARAM; };
nucho 3:1cf99502f396 45 virtual const char * getMD5(){ return "c1f3d28f1b044c871e6eff2e9fc3c667"; };
nucho 0:77afd7560544 46
nucho 0:77afd7560544 47 };
nucho 0:77afd7560544 48
nucho 0:77afd7560544 49 class RequestParamResponse : public ros::Msg
nucho 0:77afd7560544 50 {
nucho 0:77afd7560544 51 public:
nucho 3:1cf99502f396 52 uint8_t ints_length;
nucho 3:1cf99502f396 53 int32_t st_ints;
nucho 3:1cf99502f396 54 int32_t * ints;
nucho 3:1cf99502f396 55 uint8_t floats_length;
nucho 0:77afd7560544 56 float st_floats;
nucho 0:77afd7560544 57 float * floats;
nucho 3:1cf99502f396 58 uint8_t strings_length;
nucho 0:77afd7560544 59 char* st_strings;
nucho 0:77afd7560544 60 char* * strings;
nucho 0:77afd7560544 61
nucho 3:1cf99502f396 62 virtual int serialize(unsigned char *outbuffer) const
nucho 0:77afd7560544 63 {
nucho 0:77afd7560544 64 int offset = 0;
nucho 0:77afd7560544 65 *(outbuffer + offset++) = ints_length;
nucho 0:77afd7560544 66 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 67 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 68 *(outbuffer + offset++) = 0;
nucho 3:1cf99502f396 69 for( uint8_t i = 0; i < ints_length; i++){
nucho 0:77afd7560544 70 union {
nucho 3:1cf99502f396 71 int32_t real;
nucho 3:1cf99502f396 72 uint32_t base;
nucho 0:77afd7560544 73 } u_intsi;
nucho 0:77afd7560544 74 u_intsi.real = this->ints[i];
nucho 0:77afd7560544 75 *(outbuffer + offset + 0) = (u_intsi.base >> (8 * 0)) & 0xFF;
nucho 0:77afd7560544 76 *(outbuffer + offset + 1) = (u_intsi.base >> (8 * 1)) & 0xFF;
nucho 0:77afd7560544 77 *(outbuffer + offset + 2) = (u_intsi.base >> (8 * 2)) & 0xFF;
nucho 0:77afd7560544 78 *(outbuffer + offset + 3) = (u_intsi.base >> (8 * 3)) & 0xFF;
nucho 0:77afd7560544 79 offset += sizeof(this->ints[i]);
nucho 0:77afd7560544 80 }
nucho 0:77afd7560544 81 *(outbuffer + offset++) = floats_length;
nucho 0:77afd7560544 82 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 83 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 84 *(outbuffer + offset++) = 0;
nucho 3:1cf99502f396 85 for( uint8_t i = 0; i < floats_length; i++){
nucho 0:77afd7560544 86 union {
nucho 0:77afd7560544 87 float real;
nucho 3:1cf99502f396 88 uint32_t base;
nucho 0:77afd7560544 89 } u_floatsi;
nucho 0:77afd7560544 90 u_floatsi.real = this->floats[i];
nucho 0:77afd7560544 91 *(outbuffer + offset + 0) = (u_floatsi.base >> (8 * 0)) & 0xFF;
nucho 0:77afd7560544 92 *(outbuffer + offset + 1) = (u_floatsi.base >> (8 * 1)) & 0xFF;
nucho 0:77afd7560544 93 *(outbuffer + offset + 2) = (u_floatsi.base >> (8 * 2)) & 0xFF;
nucho 0:77afd7560544 94 *(outbuffer + offset + 3) = (u_floatsi.base >> (8 * 3)) & 0xFF;
nucho 0:77afd7560544 95 offset += sizeof(this->floats[i]);
nucho 0:77afd7560544 96 }
nucho 0:77afd7560544 97 *(outbuffer + offset++) = strings_length;
nucho 0:77afd7560544 98 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 99 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 100 *(outbuffer + offset++) = 0;
nucho 3:1cf99502f396 101 for( uint8_t i = 0; i < strings_length; i++){
jjzak 5:8cd48977ec68 102 uint32_t length_stringsi = strlen( (const char*) this->strings[i]);
jjzak 5:8cd48977ec68 103 memcpy(outbuffer + offset, &length_stringsi, sizeof(uint32_t));
nucho 0:77afd7560544 104 offset += 4;
jjzak 5:8cd48977ec68 105 memcpy(outbuffer + offset, this->strings[i], length_stringsi);
jjzak 5:8cd48977ec68 106 offset += length_stringsi;
nucho 0:77afd7560544 107 }
nucho 0:77afd7560544 108 return offset;
nucho 0:77afd7560544 109 }
nucho 0:77afd7560544 110
nucho 0:77afd7560544 111 virtual int deserialize(unsigned char *inbuffer)
nucho 0:77afd7560544 112 {
nucho 0:77afd7560544 113 int offset = 0;
nucho 3:1cf99502f396 114 uint8_t ints_lengthT = *(inbuffer + offset++);
nucho 0:77afd7560544 115 if(ints_lengthT > ints_length)
nucho 3:1cf99502f396 116 this->ints = (int32_t*)realloc(this->ints, ints_lengthT * sizeof(int32_t));
nucho 0:77afd7560544 117 offset += 3;
nucho 0:77afd7560544 118 ints_length = ints_lengthT;
nucho 3:1cf99502f396 119 for( uint8_t i = 0; i < ints_length; i++){
nucho 0:77afd7560544 120 union {
nucho 3:1cf99502f396 121 int32_t real;
nucho 3:1cf99502f396 122 uint32_t base;
nucho 0:77afd7560544 123 } u_st_ints;
nucho 0:77afd7560544 124 u_st_ints.base = 0;
nucho 3:1cf99502f396 125 u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 3:1cf99502f396 126 u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 3:1cf99502f396 127 u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 3:1cf99502f396 128 u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 0:77afd7560544 129 this->st_ints = u_st_ints.real;
nucho 0:77afd7560544 130 offset += sizeof(this->st_ints);
nucho 3:1cf99502f396 131 memcpy( &(this->ints[i]), &(this->st_ints), sizeof(int32_t));
nucho 0:77afd7560544 132 }
nucho 3:1cf99502f396 133 uint8_t floats_lengthT = *(inbuffer + offset++);
nucho 0:77afd7560544 134 if(floats_lengthT > floats_length)
nucho 0:77afd7560544 135 this->floats = (float*)realloc(this->floats, floats_lengthT * sizeof(float));
nucho 0:77afd7560544 136 offset += 3;
nucho 0:77afd7560544 137 floats_length = floats_lengthT;
nucho 3:1cf99502f396 138 for( uint8_t i = 0; i < floats_length; i++){
nucho 0:77afd7560544 139 union {
nucho 0:77afd7560544 140 float real;
nucho 3:1cf99502f396 141 uint32_t base;
nucho 0:77afd7560544 142 } u_st_floats;
nucho 0:77afd7560544 143 u_st_floats.base = 0;
nucho 3:1cf99502f396 144 u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 3:1cf99502f396 145 u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 3:1cf99502f396 146 u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 3:1cf99502f396 147 u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 0:77afd7560544 148 this->st_floats = u_st_floats.real;
nucho 0:77afd7560544 149 offset += sizeof(this->st_floats);
nucho 0:77afd7560544 150 memcpy( &(this->floats[i]), &(this->st_floats), sizeof(float));
nucho 0:77afd7560544 151 }
nucho 3:1cf99502f396 152 uint8_t strings_lengthT = *(inbuffer + offset++);
nucho 0:77afd7560544 153 if(strings_lengthT > strings_length)
nucho 0:77afd7560544 154 this->strings = (char**)realloc(this->strings, strings_lengthT * sizeof(char*));
nucho 0:77afd7560544 155 offset += 3;
nucho 0:77afd7560544 156 strings_length = strings_lengthT;
nucho 3:1cf99502f396 157 for( uint8_t i = 0; i < strings_length; i++){
jjzak 5:8cd48977ec68 158 uint32_t length_st_strings;
jjzak 5:8cd48977ec68 159 memcpy(&length_st_strings, (inbuffer + offset), sizeof(uint32_t));
nucho 0:77afd7560544 160 offset += 4;
nucho 0:77afd7560544 161 for(unsigned int k= offset; k< offset+length_st_strings; ++k){
nucho 0:77afd7560544 162 inbuffer[k-1]=inbuffer[k];
nucho 3:1cf99502f396 163 }
nucho 0:77afd7560544 164 inbuffer[offset+length_st_strings-1]=0;
nucho 0:77afd7560544 165 this->st_strings = (char *)(inbuffer + offset-1);
nucho 0:77afd7560544 166 offset += length_st_strings;
nucho 0:77afd7560544 167 memcpy( &(this->strings[i]), &(this->st_strings), sizeof(char*));
nucho 0:77afd7560544 168 }
nucho 0:77afd7560544 169 return offset;
nucho 0:77afd7560544 170 }
nucho 0:77afd7560544 171
nucho 3:1cf99502f396 172 virtual const char * getType(){ return REQUESTPARAM; };
nucho 3:1cf99502f396 173 virtual const char * getMD5(){ return "9f0e98bda65981986ddf53afa7a40e49"; };
nucho 3:1cf99502f396 174
nucho 3:1cf99502f396 175 };
nucho 0:77afd7560544 176
nucho 3:1cf99502f396 177 class RequestParam {
nucho 3:1cf99502f396 178 public:
nucho 3:1cf99502f396 179 typedef RequestParamRequest Request;
nucho 3:1cf99502f396 180 typedef RequestParamResponse Response;
nucho 0:77afd7560544 181 };
nucho 0:77afd7560544 182
nucho 0:77afd7560544 183 }
jjzak 5:8cd48977ec68 184 #endif
jjzak 5:8cd48977ec68 185