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

std_msgs/Float32MultiArray.h@1:ff0ec969dad1, 2011-10-16 (annotated)

Committer:
nucho
Date:
Sun Oct 16 07:19:36 2011 +0000
Revision:
1:ff0ec969dad1
Parent:
0:77afd7560544
Child:
3:1cf99502f396
This program supported the revision of 143 of rosserial.

And the bug fix of receive of array data.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nucho 0:77afd7560544 1 #ifndef ros_Float32MultiArray_h
nucho 0:77afd7560544 2 #define ros_Float32MultiArray_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 0:77afd7560544 7 #include "../ros/msg.h"
nucho 0:77afd7560544 8 #include "std_msgs/MultiArrayLayout.h"
nucho 0:77afd7560544 9
nucho 1:ff0ec969dad1 10 namespace std_msgs {
nucho 0:77afd7560544 11
nucho 1:ff0ec969dad1 12 class Float32MultiArray : public ros::Msg {
nucho 1:ff0ec969dad1 13 public:
nucho 1:ff0ec969dad1 14 std_msgs::MultiArrayLayout layout;
nucho 1:ff0ec969dad1 15 unsigned char data_length;
nucho 1:ff0ec969dad1 16 float st_data;
nucho 1:ff0ec969dad1 17 float * data;
nucho 0:77afd7560544 18
nucho 1:ff0ec969dad1 19 virtual int serialize(unsigned char *outbuffer) {
nucho 1:ff0ec969dad1 20 int offset = 0;
nucho 1:ff0ec969dad1 21 offset += this->layout.serialize(outbuffer + offset);
nucho 1:ff0ec969dad1 22 *(outbuffer + offset++) = data_length;
nucho 1:ff0ec969dad1 23 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 24 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 25 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 26 for ( unsigned char i = 0; i < data_length; i++) {
nucho 1:ff0ec969dad1 27 union {
nucho 1:ff0ec969dad1 28 float real;
nucho 1:ff0ec969dad1 29 uint32_t base;
nucho 1:ff0ec969dad1 30 } u_datai;
nucho 1:ff0ec969dad1 31 u_datai.real = this->data[i];
nucho 1:ff0ec969dad1 32 *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF;
nucho 1:ff0ec969dad1 33 *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF;
nucho 1:ff0ec969dad1 34 *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF;
nucho 1:ff0ec969dad1 35 *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF;
nucho 1:ff0ec969dad1 36 offset += sizeof(this->data[i]);
nucho 1:ff0ec969dad1 37 }
nucho 1:ff0ec969dad1 38 return offset;
nucho 0:77afd7560544 39 }
nucho 0:77afd7560544 40
nucho 1:ff0ec969dad1 41 virtual int deserialize(unsigned char *inbuffer) {
nucho 1:ff0ec969dad1 42 int offset = 0;
nucho 1:ff0ec969dad1 43 offset += this->layout.deserialize(inbuffer + offset);
nucho 1:ff0ec969dad1 44 unsigned char data_lengthT = *(inbuffer + offset++);
nucho 1:ff0ec969dad1 45 if (data_lengthT > data_length)
nucho 1:ff0ec969dad1 46 this->data = (float*)realloc(this->data, data_lengthT * sizeof(float));
nucho 1:ff0ec969dad1 47 offset += 3;
nucho 1:ff0ec969dad1 48 data_length = data_lengthT;
nucho 1:ff0ec969dad1 49 for ( unsigned char i = 0; i < data_length; i++) {
nucho 1:ff0ec969dad1 50 union {
nucho 1:ff0ec969dad1 51 float real;
nucho 1:ff0ec969dad1 52 uint32_t base;
nucho 1:ff0ec969dad1 53 } u_st_data;
nucho 1:ff0ec969dad1 54 u_st_data.base = 0;
nucho 1:ff0ec969dad1 55 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 1:ff0ec969dad1 56 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 1:ff0ec969dad1 57 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 1:ff0ec969dad1 58 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 1:ff0ec969dad1 59 this->st_data = u_st_data.real;
nucho 1:ff0ec969dad1 60 offset += sizeof(this->st_data);
nucho 1:ff0ec969dad1 61 memcpy( &(this->data[i]), &(this->st_data), sizeof(float));
nucho 1:ff0ec969dad1 62 }
nucho 1:ff0ec969dad1 63 return offset;
nucho 0:77afd7560544 64 }
nucho 0:77afd7560544 65
nucho 1:ff0ec969dad1 66 virtual const char * getType() {
nucho 1:ff0ec969dad1 67 return "std_msgs/Float32MultiArray";
nucho 1:ff0ec969dad1 68 };
nucho 0:77afd7560544 69
nucho 1:ff0ec969dad1 70 };
nucho 0:77afd7560544 71
nucho 0:77afd7560544 72 }
nucho 0:77afd7560544 73 #endif