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/Float64MultiArray.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_Float64MultiArray_h
nucho 0:77afd7560544 2 #define ros_Float64MultiArray_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 Float64MultiArray : 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 double st_data;
nucho 1:ff0ec969dad1 17 double * 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 double real;
nucho 1:ff0ec969dad1 29 uint64_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 *(outbuffer + offset + 4) = (u_datai.base >> (8 * 4)) & 0xFF;
nucho 1:ff0ec969dad1 37 *(outbuffer + offset + 5) = (u_datai.base >> (8 * 5)) & 0xFF;
nucho 1:ff0ec969dad1 38 *(outbuffer + offset + 6) = (u_datai.base >> (8 * 6)) & 0xFF;
nucho 1:ff0ec969dad1 39 *(outbuffer + offset + 7) = (u_datai.base >> (8 * 7)) & 0xFF;
nucho 1:ff0ec969dad1 40 offset += sizeof(this->data[i]);
nucho 1:ff0ec969dad1 41 }
nucho 1:ff0ec969dad1 42 return offset;
nucho 1:ff0ec969dad1 43 }
nucho 1:ff0ec969dad1 44
nucho 1:ff0ec969dad1 45 virtual int deserialize(unsigned char *inbuffer) {
nucho 1:ff0ec969dad1 46 int offset = 0;
nucho 1:ff0ec969dad1 47 offset += this->layout.deserialize(inbuffer + offset);
nucho 1:ff0ec969dad1 48 unsigned char data_lengthT = *(inbuffer + offset++);
nucho 1:ff0ec969dad1 49 if (data_lengthT > data_length)
nucho 1:ff0ec969dad1 50 this->data = (double*)realloc(this->data, data_lengthT * sizeof(double));
nucho 1:ff0ec969dad1 51 offset += 3;
nucho 1:ff0ec969dad1 52 data_length = data_lengthT;
nucho 1:ff0ec969dad1 53 for ( unsigned char i = 0; i < data_length; i++) {
nucho 1:ff0ec969dad1 54 union {
nucho 1:ff0ec969dad1 55 double real;
nucho 1:ff0ec969dad1 56 uint64_t base;
nucho 1:ff0ec969dad1 57 } u_st_data;
nucho 1:ff0ec969dad1 58 u_st_data.base = 0;
nucho 1:ff0ec969dad1 59 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 1:ff0ec969dad1 60 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 1:ff0ec969dad1 61 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 1:ff0ec969dad1 62 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 1:ff0ec969dad1 63 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 4))) << (8 * 4);
nucho 1:ff0ec969dad1 64 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 5))) << (8 * 5);
nucho 1:ff0ec969dad1 65 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 6))) << (8 * 6);
nucho 1:ff0ec969dad1 66 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 7))) << (8 * 7);
nucho 1:ff0ec969dad1 67 this->st_data = u_st_data.real;
nucho 1:ff0ec969dad1 68 offset += sizeof(this->st_data);
nucho 1:ff0ec969dad1 69 memcpy( &(this->data[i]), &(this->st_data), sizeof(double));
nucho 1:ff0ec969dad1 70 }
nucho 1:ff0ec969dad1 71 return offset;
nucho 0:77afd7560544 72 }
nucho 0:77afd7560544 73
nucho 1:ff0ec969dad1 74 /*
nucho 1:ff0ec969dad1 75 public:
nucho 1:ff0ec969dad1 76 std_msgs::MultiArrayLayout layout;
nucho 1:ff0ec969dad1 77 unsigned char data_length;
nucho 1:ff0ec969dad1 78 float st_data;
nucho 1:ff0ec969dad1 79 float * data;
nucho 1:ff0ec969dad1 80
nucho 1:ff0ec969dad1 81 virtual int serialize(unsigned char *outbuffer)
nucho 1:ff0ec969dad1 82 {
nucho 1:ff0ec969dad1 83 int offset = 0;
nucho 1:ff0ec969dad1 84 offset += this->layout.serialize(outbuffer + offset);
nucho 1:ff0ec969dad1 85 *(outbuffer + offset++) = data_length;
nucho 1:ff0ec969dad1 86 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 87 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 88 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 89 for( unsigned char i = 0; i < data_length; i++){
nucho 1:ff0ec969dad1 90 long * val_datai = (long *) &(this->data[i]);
nucho 1:ff0ec969dad1 91 long exp_datai = (((*val_datai)>>23)&255);
nucho 1:ff0ec969dad1 92 if(exp_datai != 0)
nucho 1:ff0ec969dad1 93 exp_datai += 1023-127;
nucho 1:ff0ec969dad1 94 long sig_datai = *val_datai;
nucho 1:ff0ec969dad1 95 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 96 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 97 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 98 *(outbuffer + offset++) = (sig_datai<<5) & 0xff;
nucho 1:ff0ec969dad1 99 *(outbuffer + offset++) = (sig_datai>>3) & 0xff;
nucho 1:ff0ec969dad1 100 *(outbuffer + offset++) = (sig_datai>>11) & 0xff;
nucho 1:ff0ec969dad1 101 *(outbuffer + offset++) = ((exp_datai<<4) & 0xF0) | ((sig_datai>>19)&0x0F);
nucho 1:ff0ec969dad1 102 *(outbuffer + offset++) = (exp_datai>>4) & 0x7F;
nucho 1:ff0ec969dad1 103 if(this->data[i] < 0) *(outbuffer + offset -1) |= 0x80;
nucho 1:ff0ec969dad1 104 }
nucho 1:ff0ec969dad1 105 return offset;
nucho 0:77afd7560544 106 }
nucho 0:77afd7560544 107
nucho 1:ff0ec969dad1 108 virtual int deserialize(unsigned char *inbuffer)
nucho 1:ff0ec969dad1 109 {
nucho 1:ff0ec969dad1 110 int offset = 0;
nucho 1:ff0ec969dad1 111 offset += this->layout.deserialize(inbuffer + offset);
nucho 1:ff0ec969dad1 112 unsigned char data_lengthT = *(inbuffer + offset++);
nucho 1:ff0ec969dad1 113 if(data_lengthT > data_length)
nucho 1:ff0ec969dad1 114 this->data = (float*)realloc(this->data, data_lengthT * sizeof(float));
nucho 1:ff0ec969dad1 115 offset += 3;
nucho 1:ff0ec969dad1 116 data_length = data_lengthT;
nucho 1:ff0ec969dad1 117 for( unsigned char i = 0; i < data_length; i++){
nucho 1:ff0ec969dad1 118 unsigned long * val_st_data = (unsigned long*) &(this->st_data);
nucho 1:ff0ec969dad1 119 offset += 3;
nucho 1:ff0ec969dad1 120 *val_st_data = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 1:ff0ec969dad1 121 *val_st_data |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 1:ff0ec969dad1 122 *val_st_data |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 1:ff0ec969dad1 123 *val_st_data |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 1:ff0ec969dad1 124 unsigned long exp_st_data = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 1:ff0ec969dad1 125 exp_st_data |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 1:ff0ec969dad1 126 if(exp_st_data !=0)
nucho 1:ff0ec969dad1 127 *val_st_data |= ((exp_st_data)-1023+127)<<23;
nucho 1:ff0ec969dad1 128 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_data = -this->st_data;
nucho 1:ff0ec969dad1 129 memcpy( &(this->data[i]), &(this->st_data), sizeof(float));
nucho 1:ff0ec969dad1 130 }
nucho 1:ff0ec969dad1 131 return offset;
nucho 1:ff0ec969dad1 132 }
nucho 1:ff0ec969dad1 133 */
nucho 1:ff0ec969dad1 134 virtual const char * getType() {
nucho 1:ff0ec969dad1 135 return "std_msgs/Float64MultiArray";
nucho 1:ff0ec969dad1 136 };
nucho 0:77afd7560544 137
nucho 1:ff0ec969dad1 138 };
nucho 0:77afd7560544 139
nucho 0:77afd7560544 140 }
nucho 0:77afd7560544 141 #endif