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