nucho / Mbed 2 deprecated rosserial_mbed

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

std_msgs/Float64MultiArray.h@1:098e75fd5ad2, 2011-10-16 (annotated)

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