nucho / rosserial_mbed_lib

This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial.

Dependencies:  

Dependents:   rosserial_mbed robot_S2

std_msgs/UInt64MultiArray.h@1:ff0ec969dad1, 2011-10-16 (annotated)

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