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/Float32MultiArray.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_Float32MultiArray_h
nucho 0:06fc856e99ca 2 #define ros_Float32MultiArray_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 Float32MultiArray : 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 float st_data;
nucho 1:098e75fd5ad2 17 float * 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 float real;
nucho 1:098e75fd5ad2 29 uint32_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 offset += sizeof(this->data[i]);
nucho 1:098e75fd5ad2 37 }
nucho 1:098e75fd5ad2 38 return offset;
nucho 0:06fc856e99ca 39 }
nucho 0:06fc856e99ca 40
nucho 1:098e75fd5ad2 41 virtual int deserialize(unsigned char *inbuffer) {
nucho 1:098e75fd5ad2 42 int offset = 0;
nucho 1:098e75fd5ad2 43 offset += this->layout.deserialize(inbuffer + offset);
nucho 1:098e75fd5ad2 44 unsigned char data_lengthT = *(inbuffer + offset++);
nucho 1:098e75fd5ad2 45 if (data_lengthT > data_length)
nucho 1:098e75fd5ad2 46 this->data = (float*)realloc(this->data, data_lengthT * sizeof(float));
nucho 1:098e75fd5ad2 47 offset += 3;
nucho 1:098e75fd5ad2 48 data_length = data_lengthT;
nucho 1:098e75fd5ad2 49 for ( unsigned char i = 0; i < data_length; i++) {
nucho 1:098e75fd5ad2 50 union {
nucho 1:098e75fd5ad2 51 float real;
nucho 1:098e75fd5ad2 52 uint32_t base;
nucho 1:098e75fd5ad2 53 } u_st_data;
nucho 1:098e75fd5ad2 54 u_st_data.base = 0;
nucho 1:098e75fd5ad2 55 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 1:098e75fd5ad2 56 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 1:098e75fd5ad2 57 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 1:098e75fd5ad2 58 u_st_data.base |= ((typeof(u_st_data.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 1:098e75fd5ad2 59 this->st_data = u_st_data.real;
nucho 1:098e75fd5ad2 60 offset += sizeof(this->st_data);
nucho 1:098e75fd5ad2 61 memcpy( &(this->data[i]), &(this->st_data), sizeof(float));
nucho 1:098e75fd5ad2 62 }
nucho 1:098e75fd5ad2 63 return offset;
nucho 0:06fc856e99ca 64 }
nucho 0:06fc856e99ca 65
nucho 1:098e75fd5ad2 66 virtual const char * getType() {
nucho 1:098e75fd5ad2 67 return "std_msgs/Float32MultiArray";
nucho 1:098e75fd5ad2 68 };
nucho 0:06fc856e99ca 69
nucho 1:098e75fd5ad2 70 };
nucho 0:06fc856e99ca 71
nucho 0:06fc856e99ca 72 }
nucho 0:06fc856e99ca 73 #endif