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