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

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_LaserScan_h
nucho 0:06fc856e99ca 2 #define ros_LaserScan_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
nucho 0:06fc856e99ca 10 namespace sensor_msgs
nucho 0:06fc856e99ca 11 {
nucho 0:06fc856e99ca 12
nucho 0:06fc856e99ca 13 class LaserScan : public ros::Msg
nucho 0:06fc856e99ca 14 {
nucho 0:06fc856e99ca 15 public:
nucho 0:06fc856e99ca 16 std_msgs::Header header;
nucho 0:06fc856e99ca 17 float angle_min;
nucho 0:06fc856e99ca 18 float angle_max;
nucho 0:06fc856e99ca 19 float angle_increment;
nucho 0:06fc856e99ca 20 float time_increment;
nucho 0:06fc856e99ca 21 float scan_time;
nucho 0:06fc856e99ca 22 float range_min;
nucho 0:06fc856e99ca 23 float range_max;
nucho 0:06fc856e99ca 24 unsigned char ranges_length;
nucho 0:06fc856e99ca 25 float st_ranges;
nucho 0:06fc856e99ca 26 float * ranges;
nucho 0:06fc856e99ca 27 unsigned char intensities_length;
nucho 0:06fc856e99ca 28 float st_intensities;
nucho 0:06fc856e99ca 29 float * intensities;
nucho 0:06fc856e99ca 30
nucho 0:06fc856e99ca 31 virtual int serialize(unsigned char *outbuffer)
nucho 0:06fc856e99ca 32 {
nucho 0:06fc856e99ca 33 int offset = 0;
nucho 0:06fc856e99ca 34 offset += this->header.serialize(outbuffer + offset);
nucho 0:06fc856e99ca 35 union {
nucho 0:06fc856e99ca 36 float real;
nucho 0:06fc856e99ca 37 unsigned long base;
nucho 0:06fc856e99ca 38 } u_angle_min;
nucho 0:06fc856e99ca 39 u_angle_min.real = this->angle_min;
nucho 0:06fc856e99ca 40 *(outbuffer + offset + 0) = (u_angle_min.base >> (8 * 0)) & 0xFF;
nucho 0:06fc856e99ca 41 *(outbuffer + offset + 1) = (u_angle_min.base >> (8 * 1)) & 0xFF;
nucho 0:06fc856e99ca 42 *(outbuffer + offset + 2) = (u_angle_min.base >> (8 * 2)) & 0xFF;
nucho 0:06fc856e99ca 43 *(outbuffer + offset + 3) = (u_angle_min.base >> (8 * 3)) & 0xFF;
nucho 0:06fc856e99ca 44 offset += sizeof(this->angle_min);
nucho 0:06fc856e99ca 45 union {
nucho 0:06fc856e99ca 46 float real;
nucho 0:06fc856e99ca 47 unsigned long base;
nucho 0:06fc856e99ca 48 } u_angle_max;
nucho 0:06fc856e99ca 49 u_angle_max.real = this->angle_max;
nucho 0:06fc856e99ca 50 *(outbuffer + offset + 0) = (u_angle_max.base >> (8 * 0)) & 0xFF;
nucho 0:06fc856e99ca 51 *(outbuffer + offset + 1) = (u_angle_max.base >> (8 * 1)) & 0xFF;
nucho 0:06fc856e99ca 52 *(outbuffer + offset + 2) = (u_angle_max.base >> (8 * 2)) & 0xFF;
nucho 0:06fc856e99ca 53 *(outbuffer + offset + 3) = (u_angle_max.base >> (8 * 3)) & 0xFF;
nucho 0:06fc856e99ca 54 offset += sizeof(this->angle_max);
nucho 0:06fc856e99ca 55 union {
nucho 0:06fc856e99ca 56 float real;
nucho 0:06fc856e99ca 57 unsigned long base;
nucho 0:06fc856e99ca 58 } u_angle_increment;
nucho 0:06fc856e99ca 59 u_angle_increment.real = this->angle_increment;
nucho 0:06fc856e99ca 60 *(outbuffer + offset + 0) = (u_angle_increment.base >> (8 * 0)) & 0xFF;
nucho 0:06fc856e99ca 61 *(outbuffer + offset + 1) = (u_angle_increment.base >> (8 * 1)) & 0xFF;
nucho 0:06fc856e99ca 62 *(outbuffer + offset + 2) = (u_angle_increment.base >> (8 * 2)) & 0xFF;
nucho 0:06fc856e99ca 63 *(outbuffer + offset + 3) = (u_angle_increment.base >> (8 * 3)) & 0xFF;
nucho 0:06fc856e99ca 64 offset += sizeof(this->angle_increment);
nucho 0:06fc856e99ca 65 union {
nucho 0:06fc856e99ca 66 float real;
nucho 0:06fc856e99ca 67 unsigned long base;
nucho 0:06fc856e99ca 68 } u_time_increment;
nucho 0:06fc856e99ca 69 u_time_increment.real = this->time_increment;
nucho 0:06fc856e99ca 70 *(outbuffer + offset + 0) = (u_time_increment.base >> (8 * 0)) & 0xFF;
nucho 0:06fc856e99ca 71 *(outbuffer + offset + 1) = (u_time_increment.base >> (8 * 1)) & 0xFF;
nucho 0:06fc856e99ca 72 *(outbuffer + offset + 2) = (u_time_increment.base >> (8 * 2)) & 0xFF;
nucho 0:06fc856e99ca 73 *(outbuffer + offset + 3) = (u_time_increment.base >> (8 * 3)) & 0xFF;
nucho 0:06fc856e99ca 74 offset += sizeof(this->time_increment);
nucho 0:06fc856e99ca 75 union {
nucho 0:06fc856e99ca 76 float real;
nucho 0:06fc856e99ca 77 unsigned long base;
nucho 0:06fc856e99ca 78 } u_scan_time;
nucho 0:06fc856e99ca 79 u_scan_time.real = this->scan_time;
nucho 0:06fc856e99ca 80 *(outbuffer + offset + 0) = (u_scan_time.base >> (8 * 0)) & 0xFF;
nucho 0:06fc856e99ca 81 *(outbuffer + offset + 1) = (u_scan_time.base >> (8 * 1)) & 0xFF;
nucho 0:06fc856e99ca 82 *(outbuffer + offset + 2) = (u_scan_time.base >> (8 * 2)) & 0xFF;
nucho 0:06fc856e99ca 83 *(outbuffer + offset + 3) = (u_scan_time.base >> (8 * 3)) & 0xFF;
nucho 0:06fc856e99ca 84 offset += sizeof(this->scan_time);
nucho 0:06fc856e99ca 85 union {
nucho 0:06fc856e99ca 86 float real;
nucho 0:06fc856e99ca 87 unsigned long base;
nucho 0:06fc856e99ca 88 } u_range_min;
nucho 0:06fc856e99ca 89 u_range_min.real = this->range_min;
nucho 0:06fc856e99ca 90 *(outbuffer + offset + 0) = (u_range_min.base >> (8 * 0)) & 0xFF;
nucho 0:06fc856e99ca 91 *(outbuffer + offset + 1) = (u_range_min.base >> (8 * 1)) & 0xFF;
nucho 0:06fc856e99ca 92 *(outbuffer + offset + 2) = (u_range_min.base >> (8 * 2)) & 0xFF;
nucho 0:06fc856e99ca 93 *(outbuffer + offset + 3) = (u_range_min.base >> (8 * 3)) & 0xFF;
nucho 0:06fc856e99ca 94 offset += sizeof(this->range_min);
nucho 0:06fc856e99ca 95 union {
nucho 0:06fc856e99ca 96 float real;
nucho 0:06fc856e99ca 97 unsigned long base;
nucho 0:06fc856e99ca 98 } u_range_max;
nucho 0:06fc856e99ca 99 u_range_max.real = this->range_max;
nucho 0:06fc856e99ca 100 *(outbuffer + offset + 0) = (u_range_max.base >> (8 * 0)) & 0xFF;
nucho 0:06fc856e99ca 101 *(outbuffer + offset + 1) = (u_range_max.base >> (8 * 1)) & 0xFF;
nucho 0:06fc856e99ca 102 *(outbuffer + offset + 2) = (u_range_max.base >> (8 * 2)) & 0xFF;
nucho 0:06fc856e99ca 103 *(outbuffer + offset + 3) = (u_range_max.base >> (8 * 3)) & 0xFF;
nucho 0:06fc856e99ca 104 offset += sizeof(this->range_max);
nucho 0:06fc856e99ca 105 *(outbuffer + offset++) = ranges_length;
nucho 0:06fc856e99ca 106 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 107 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 108 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 109 for( unsigned char i = 0; i < ranges_length; i++){
nucho 0:06fc856e99ca 110 union {
nucho 0:06fc856e99ca 111 float real;
nucho 0:06fc856e99ca 112 unsigned long base;
nucho 0:06fc856e99ca 113 } u_rangesi;
nucho 0:06fc856e99ca 114 u_rangesi.real = this->ranges[i];
nucho 0:06fc856e99ca 115 *(outbuffer + offset + 0) = (u_rangesi.base >> (8 * 0)) & 0xFF;
nucho 0:06fc856e99ca 116 *(outbuffer + offset + 1) = (u_rangesi.base >> (8 * 1)) & 0xFF;
nucho 0:06fc856e99ca 117 *(outbuffer + offset + 2) = (u_rangesi.base >> (8 * 2)) & 0xFF;
nucho 0:06fc856e99ca 118 *(outbuffer + offset + 3) = (u_rangesi.base >> (8 * 3)) & 0xFF;
nucho 0:06fc856e99ca 119 offset += sizeof(this->ranges[i]);
nucho 0:06fc856e99ca 120 }
nucho 0:06fc856e99ca 121 *(outbuffer + offset++) = intensities_length;
nucho 0:06fc856e99ca 122 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 123 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 124 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 125 for( unsigned char i = 0; i < intensities_length; i++){
nucho 0:06fc856e99ca 126 union {
nucho 0:06fc856e99ca 127 float real;
nucho 0:06fc856e99ca 128 unsigned long base;
nucho 0:06fc856e99ca 129 } u_intensitiesi;
nucho 0:06fc856e99ca 130 u_intensitiesi.real = this->intensities[i];
nucho 0:06fc856e99ca 131 *(outbuffer + offset + 0) = (u_intensitiesi.base >> (8 * 0)) & 0xFF;
nucho 0:06fc856e99ca 132 *(outbuffer + offset + 1) = (u_intensitiesi.base >> (8 * 1)) & 0xFF;
nucho 0:06fc856e99ca 133 *(outbuffer + offset + 2) = (u_intensitiesi.base >> (8 * 2)) & 0xFF;
nucho 0:06fc856e99ca 134 *(outbuffer + offset + 3) = (u_intensitiesi.base >> (8 * 3)) & 0xFF;
nucho 0:06fc856e99ca 135 offset += sizeof(this->intensities[i]);
nucho 0:06fc856e99ca 136 }
nucho 0:06fc856e99ca 137 return offset;
nucho 0:06fc856e99ca 138 }
nucho 0:06fc856e99ca 139
nucho 0:06fc856e99ca 140 virtual int deserialize(unsigned char *inbuffer)
nucho 0:06fc856e99ca 141 {
nucho 0:06fc856e99ca 142 int offset = 0;
nucho 0:06fc856e99ca 143 offset += this->header.deserialize(inbuffer + offset);
nucho 0:06fc856e99ca 144 union {
nucho 0:06fc856e99ca 145 float real;
nucho 0:06fc856e99ca 146 unsigned long base;
nucho 0:06fc856e99ca 147 } u_angle_min;
nucho 0:06fc856e99ca 148 u_angle_min.base = 0;
nucho 0:06fc856e99ca 149 u_angle_min.base |= ((typeof(u_angle_min.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 0:06fc856e99ca 150 u_angle_min.base |= ((typeof(u_angle_min.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 0:06fc856e99ca 151 u_angle_min.base |= ((typeof(u_angle_min.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 0:06fc856e99ca 152 u_angle_min.base |= ((typeof(u_angle_min.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 0:06fc856e99ca 153 this->angle_min = u_angle_min.real;
nucho 0:06fc856e99ca 154 offset += sizeof(this->angle_min);
nucho 0:06fc856e99ca 155 union {
nucho 0:06fc856e99ca 156 float real;
nucho 0:06fc856e99ca 157 unsigned long base;
nucho 0:06fc856e99ca 158 } u_angle_max;
nucho 0:06fc856e99ca 159 u_angle_max.base = 0;
nucho 0:06fc856e99ca 160 u_angle_max.base |= ((typeof(u_angle_max.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 0:06fc856e99ca 161 u_angle_max.base |= ((typeof(u_angle_max.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 0:06fc856e99ca 162 u_angle_max.base |= ((typeof(u_angle_max.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 0:06fc856e99ca 163 u_angle_max.base |= ((typeof(u_angle_max.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 0:06fc856e99ca 164 this->angle_max = u_angle_max.real;
nucho 0:06fc856e99ca 165 offset += sizeof(this->angle_max);
nucho 0:06fc856e99ca 166 union {
nucho 0:06fc856e99ca 167 float real;
nucho 0:06fc856e99ca 168 unsigned long base;
nucho 0:06fc856e99ca 169 } u_angle_increment;
nucho 0:06fc856e99ca 170 u_angle_increment.base = 0;
nucho 0:06fc856e99ca 171 u_angle_increment.base |= ((typeof(u_angle_increment.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 0:06fc856e99ca 172 u_angle_increment.base |= ((typeof(u_angle_increment.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 0:06fc856e99ca 173 u_angle_increment.base |= ((typeof(u_angle_increment.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 0:06fc856e99ca 174 u_angle_increment.base |= ((typeof(u_angle_increment.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 0:06fc856e99ca 175 this->angle_increment = u_angle_increment.real;
nucho 0:06fc856e99ca 176 offset += sizeof(this->angle_increment);
nucho 0:06fc856e99ca 177 union {
nucho 0:06fc856e99ca 178 float real;
nucho 0:06fc856e99ca 179 unsigned long base;
nucho 0:06fc856e99ca 180 } u_time_increment;
nucho 0:06fc856e99ca 181 u_time_increment.base = 0;
nucho 0:06fc856e99ca 182 u_time_increment.base |= ((typeof(u_time_increment.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 0:06fc856e99ca 183 u_time_increment.base |= ((typeof(u_time_increment.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 0:06fc856e99ca 184 u_time_increment.base |= ((typeof(u_time_increment.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 0:06fc856e99ca 185 u_time_increment.base |= ((typeof(u_time_increment.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 0:06fc856e99ca 186 this->time_increment = u_time_increment.real;
nucho 0:06fc856e99ca 187 offset += sizeof(this->time_increment);
nucho 0:06fc856e99ca 188 union {
nucho 0:06fc856e99ca 189 float real;
nucho 0:06fc856e99ca 190 unsigned long base;
nucho 0:06fc856e99ca 191 } u_scan_time;
nucho 0:06fc856e99ca 192 u_scan_time.base = 0;
nucho 0:06fc856e99ca 193 u_scan_time.base |= ((typeof(u_scan_time.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 0:06fc856e99ca 194 u_scan_time.base |= ((typeof(u_scan_time.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 0:06fc856e99ca 195 u_scan_time.base |= ((typeof(u_scan_time.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 0:06fc856e99ca 196 u_scan_time.base |= ((typeof(u_scan_time.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 0:06fc856e99ca 197 this->scan_time = u_scan_time.real;
nucho 0:06fc856e99ca 198 offset += sizeof(this->scan_time);
nucho 0:06fc856e99ca 199 union {
nucho 0:06fc856e99ca 200 float real;
nucho 0:06fc856e99ca 201 unsigned long base;
nucho 0:06fc856e99ca 202 } u_range_min;
nucho 0:06fc856e99ca 203 u_range_min.base = 0;
nucho 0:06fc856e99ca 204 u_range_min.base |= ((typeof(u_range_min.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 0:06fc856e99ca 205 u_range_min.base |= ((typeof(u_range_min.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 0:06fc856e99ca 206 u_range_min.base |= ((typeof(u_range_min.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 0:06fc856e99ca 207 u_range_min.base |= ((typeof(u_range_min.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 0:06fc856e99ca 208 this->range_min = u_range_min.real;
nucho 0:06fc856e99ca 209 offset += sizeof(this->range_min);
nucho 0:06fc856e99ca 210 union {
nucho 0:06fc856e99ca 211 float real;
nucho 0:06fc856e99ca 212 unsigned long base;
nucho 0:06fc856e99ca 213 } u_range_max;
nucho 0:06fc856e99ca 214 u_range_max.base = 0;
nucho 0:06fc856e99ca 215 u_range_max.base |= ((typeof(u_range_max.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 0:06fc856e99ca 216 u_range_max.base |= ((typeof(u_range_max.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 0:06fc856e99ca 217 u_range_max.base |= ((typeof(u_range_max.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 0:06fc856e99ca 218 u_range_max.base |= ((typeof(u_range_max.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 0:06fc856e99ca 219 this->range_max = u_range_max.real;
nucho 0:06fc856e99ca 220 offset += sizeof(this->range_max);
nucho 0:06fc856e99ca 221 unsigned char ranges_lengthT = *(inbuffer + offset++);
nucho 0:06fc856e99ca 222 if(ranges_lengthT > ranges_length)
nucho 0:06fc856e99ca 223 this->ranges = (float*)realloc(this->ranges, ranges_lengthT * sizeof(float));
nucho 0:06fc856e99ca 224 offset += 3;
nucho 0:06fc856e99ca 225 ranges_length = ranges_lengthT;
nucho 0:06fc856e99ca 226 for( unsigned char i = 0; i < ranges_length; i++){
nucho 0:06fc856e99ca 227 union {
nucho 0:06fc856e99ca 228 float real;
nucho 0:06fc856e99ca 229 unsigned long base;
nucho 0:06fc856e99ca 230 } u_st_ranges;
nucho 0:06fc856e99ca 231 u_st_ranges.base = 0;
nucho 0:06fc856e99ca 232 u_st_ranges.base |= ((typeof(u_st_ranges.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 0:06fc856e99ca 233 u_st_ranges.base |= ((typeof(u_st_ranges.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 0:06fc856e99ca 234 u_st_ranges.base |= ((typeof(u_st_ranges.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 0:06fc856e99ca 235 u_st_ranges.base |= ((typeof(u_st_ranges.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 0:06fc856e99ca 236 this->st_ranges = u_st_ranges.real;
nucho 0:06fc856e99ca 237 offset += sizeof(this->st_ranges);
nucho 0:06fc856e99ca 238 memcpy( &(this->ranges[i]), &(this->st_ranges), sizeof(float));
nucho 0:06fc856e99ca 239 }
nucho 0:06fc856e99ca 240 unsigned char intensities_lengthT = *(inbuffer + offset++);
nucho 0:06fc856e99ca 241 if(intensities_lengthT > intensities_length)
nucho 0:06fc856e99ca 242 this->intensities = (float*)realloc(this->intensities, intensities_lengthT * sizeof(float));
nucho 0:06fc856e99ca 243 offset += 3;
nucho 0:06fc856e99ca 244 intensities_length = intensities_lengthT;
nucho 0:06fc856e99ca 245 for( unsigned char i = 0; i < intensities_length; i++){
nucho 0:06fc856e99ca 246 union {
nucho 0:06fc856e99ca 247 float real;
nucho 0:06fc856e99ca 248 unsigned long base;
nucho 0:06fc856e99ca 249 } u_st_intensities;
nucho 0:06fc856e99ca 250 u_st_intensities.base = 0;
nucho 0:06fc856e99ca 251 u_st_intensities.base |= ((typeof(u_st_intensities.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 0:06fc856e99ca 252 u_st_intensities.base |= ((typeof(u_st_intensities.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 0:06fc856e99ca 253 u_st_intensities.base |= ((typeof(u_st_intensities.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 0:06fc856e99ca 254 u_st_intensities.base |= ((typeof(u_st_intensities.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 0:06fc856e99ca 255 this->st_intensities = u_st_intensities.real;
nucho 0:06fc856e99ca 256 offset += sizeof(this->st_intensities);
nucho 0:06fc856e99ca 257 memcpy( &(this->intensities[i]), &(this->st_intensities), sizeof(float));
nucho 0:06fc856e99ca 258 }
nucho 0:06fc856e99ca 259 return offset;
nucho 0:06fc856e99ca 260 }
nucho 0:06fc856e99ca 261
nucho 0:06fc856e99ca 262 virtual const char * getType(){ return "sensor_msgs/LaserScan"; };
nucho 0:06fc856e99ca 263
nucho 0:06fc856e99ca 264 };
nucho 0:06fc856e99ca 265
nucho 0:06fc856e99ca 266 }
nucho 0:06fc856e99ca 267 #endif