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/UInt64MultiArray.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_UInt64MultiArray_h
nucho 0:06fc856e99ca 2 #define ros_UInt64MultiArray_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 UInt64MultiArray : 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 uint64_t st_data;
nucho 1:098e75fd5ad2 17 uint64_t * 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 uint64_t 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 0:06fc856e99ca 43 }
nucho 0:06fc856e99ca 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 = (uint64_t*)realloc(this->data, data_lengthT * sizeof(uint64_t));
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 uint64_t 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(uint64_t));
nucho 1:098e75fd5ad2 70 }
nucho 1:098e75fd5ad2 71 return offset;
nucho 1:098e75fd5ad2 72 }
nucho 1:098e75fd5ad2 73 /*
nucho 1:098e75fd5ad2 74 public:
nucho 1:098e75fd5ad2 75 std_msgs::MultiArrayLayout layout;
nucho 1:098e75fd5ad2 76 unsigned char data_length;
nucho 1:098e75fd5ad2 77 long st_data;
nucho 1:098e75fd5ad2 78 long * data;
nucho 1:098e75fd5ad2 79
nucho 1:098e75fd5ad2 80 virtual int serialize(unsigned char *outbuffer)
nucho 1:098e75fd5ad2 81 {
nucho 1:098e75fd5ad2 82 int offset = 0;
nucho 1:098e75fd5ad2 83 offset += this->layout.serialize(outbuffer + offset);
nucho 1:098e75fd5ad2 84 *(outbuffer + offset++) = data_length;
nucho 1:098e75fd5ad2 85 *(outbuffer + offset++) = 0;
nucho 1:098e75fd5ad2 86 *(outbuffer + offset++) = 0;
nucho 1:098e75fd5ad2 87 *(outbuffer + offset++) = 0;
nucho 1:098e75fd5ad2 88 for( unsigned char i = 0; i < data_length; i++){
nucho 1:098e75fd5ad2 89 *(outbuffer + offset++) = (data[i] >> (8 * 0)) & 0xFF;
nucho 1:098e75fd5ad2 90 *(outbuffer + offset++) = (data[i] >> (8 * 1)) & 0xFF;
nucho 1:098e75fd5ad2 91 *(outbuffer + offset++) = (data[i] >> (8 * 2)) & 0xFF;
nucho 1:098e75fd5ad2 92 *(outbuffer + offset++) = (data[i] >> (8 * 3)) & 0xFF;
nucho 1:098e75fd5ad2 93 *(outbuffer + offset++) = (data[i] > 0) ? 0: 255;
nucho 1:098e75fd5ad2 94 *(outbuffer + offset++) = (data[i] > 0) ? 0: 255;
nucho 1:098e75fd5ad2 95 *(outbuffer + offset++) = (data[i] > 0) ? 0: 255;
nucho 1:098e75fd5ad2 96 *(outbuffer + offset++) = (data[i] > 0) ? 0: 255;
nucho 1:098e75fd5ad2 97 }
nucho 1:098e75fd5ad2 98 return offset;
nucho 0:06fc856e99ca 99 }
nucho 0:06fc856e99ca 100
nucho 1:098e75fd5ad2 101 virtual int deserialize(unsigned char *inbuffer)
nucho 1:098e75fd5ad2 102 {
nucho 1:098e75fd5ad2 103 int offset = 0;
nucho 1:098e75fd5ad2 104 offset += this->layout.deserialize(inbuffer + offset);
nucho 1:098e75fd5ad2 105 unsigned char data_lengthT = *(inbuffer + offset++);
nucho 1:098e75fd5ad2 106 if(data_lengthT > data_length)
nucho 1:098e75fd5ad2 107 this->data = (long*)realloc(this->data, data_lengthT * sizeof(long));
nucho 1:098e75fd5ad2 108 offset += 3;
nucho 1:098e75fd5ad2 109 data_length = data_lengthT;
nucho 1:098e75fd5ad2 110 for( unsigned char i = 0; i < data_length; i++){
nucho 1:098e75fd5ad2 111 st_data = 0;
nucho 1:098e75fd5ad2 112 st_data += ((long)(*(inbuffer + offset++))) >> (8 * 0);
nucho 1:098e75fd5ad2 113 st_data += ((long)(*(inbuffer + offset++))) >> (8 * 1);
nucho 1:098e75fd5ad2 114 st_data += ((long)(*(inbuffer + offset++))) >> (8 * 2);
nucho 1:098e75fd5ad2 115 st_data += ((long)(*(inbuffer + offset++))) >> (8 * 3);
nucho 1:098e75fd5ad2 116 offset += 4;
nucho 1:098e75fd5ad2 117 memcpy( &(this->data[i]), &(this->st_data), sizeof(long));
nucho 1:098e75fd5ad2 118 }
nucho 1:098e75fd5ad2 119 return offset;
nucho 1:098e75fd5ad2 120 }
nucho 1:098e75fd5ad2 121 */
nucho 1:098e75fd5ad2 122 virtual const char * getType() {
nucho 1:098e75fd5ad2 123 return "std_msgs/UInt64MultiArray";
nucho 1:098e75fd5ad2 124 };
nucho 0:06fc856e99ca 125
nucho 1:098e75fd5ad2 126 };
nucho 0:06fc856e99ca 127
nucho 0:06fc856e99ca 128 }
nucho 0:06fc856e99ca 129 #endif