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

geometry_msgs/Quaternion.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 1:098e75fd5ad2 1 #ifndef ros_geometry_msgs_Quaternion_h
nucho 1:098e75fd5ad2 2 #define ros_geometry_msgs_Quaternion_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
nucho 0:06fc856e99ca 9 namespace geometry_msgs
nucho 0:06fc856e99ca 10 {
nucho 0:06fc856e99ca 11
nucho 0:06fc856e99ca 12 class Quaternion : public ros::Msg
nucho 0:06fc856e99ca 13 {
nucho 0:06fc856e99ca 14 public:
nucho 0:06fc856e99ca 15 float x;
nucho 0:06fc856e99ca 16 float y;
nucho 0:06fc856e99ca 17 float z;
nucho 0:06fc856e99ca 18 float w;
nucho 0:06fc856e99ca 19
nucho 0:06fc856e99ca 20 virtual int serialize(unsigned char *outbuffer)
nucho 0:06fc856e99ca 21 {
nucho 0:06fc856e99ca 22 int offset = 0;
nucho 0:06fc856e99ca 23 long * val_x = (long *) &(this->x);
nucho 0:06fc856e99ca 24 long exp_x = (((*val_x)>>23)&255);
nucho 0:06fc856e99ca 25 if(exp_x != 0)
nucho 0:06fc856e99ca 26 exp_x += 1023-127;
nucho 0:06fc856e99ca 27 long sig_x = *val_x;
nucho 0:06fc856e99ca 28 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 29 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 30 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 31 *(outbuffer + offset++) = (sig_x<<5) & 0xff;
nucho 0:06fc856e99ca 32 *(outbuffer + offset++) = (sig_x>>3) & 0xff;
nucho 0:06fc856e99ca 33 *(outbuffer + offset++) = (sig_x>>11) & 0xff;
nucho 0:06fc856e99ca 34 *(outbuffer + offset++) = ((exp_x<<4) & 0xF0) | ((sig_x>>19)&0x0F);
nucho 0:06fc856e99ca 35 *(outbuffer + offset++) = (exp_x>>4) & 0x7F;
nucho 0:06fc856e99ca 36 if(this->x < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:06fc856e99ca 37 long * val_y = (long *) &(this->y);
nucho 0:06fc856e99ca 38 long exp_y = (((*val_y)>>23)&255);
nucho 0:06fc856e99ca 39 if(exp_y != 0)
nucho 0:06fc856e99ca 40 exp_y += 1023-127;
nucho 0:06fc856e99ca 41 long sig_y = *val_y;
nucho 0:06fc856e99ca 42 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 43 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 44 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 45 *(outbuffer + offset++) = (sig_y<<5) & 0xff;
nucho 0:06fc856e99ca 46 *(outbuffer + offset++) = (sig_y>>3) & 0xff;
nucho 0:06fc856e99ca 47 *(outbuffer + offset++) = (sig_y>>11) & 0xff;
nucho 0:06fc856e99ca 48 *(outbuffer + offset++) = ((exp_y<<4) & 0xF0) | ((sig_y>>19)&0x0F);
nucho 0:06fc856e99ca 49 *(outbuffer + offset++) = (exp_y>>4) & 0x7F;
nucho 0:06fc856e99ca 50 if(this->y < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:06fc856e99ca 51 long * val_z = (long *) &(this->z);
nucho 0:06fc856e99ca 52 long exp_z = (((*val_z)>>23)&255);
nucho 0:06fc856e99ca 53 if(exp_z != 0)
nucho 0:06fc856e99ca 54 exp_z += 1023-127;
nucho 0:06fc856e99ca 55 long sig_z = *val_z;
nucho 0:06fc856e99ca 56 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 57 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 58 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 59 *(outbuffer + offset++) = (sig_z<<5) & 0xff;
nucho 0:06fc856e99ca 60 *(outbuffer + offset++) = (sig_z>>3) & 0xff;
nucho 0:06fc856e99ca 61 *(outbuffer + offset++) = (sig_z>>11) & 0xff;
nucho 0:06fc856e99ca 62 *(outbuffer + offset++) = ((exp_z<<4) & 0xF0) | ((sig_z>>19)&0x0F);
nucho 0:06fc856e99ca 63 *(outbuffer + offset++) = (exp_z>>4) & 0x7F;
nucho 0:06fc856e99ca 64 if(this->z < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:06fc856e99ca 65 long * val_w = (long *) &(this->w);
nucho 0:06fc856e99ca 66 long exp_w = (((*val_w)>>23)&255);
nucho 0:06fc856e99ca 67 if(exp_w != 0)
nucho 0:06fc856e99ca 68 exp_w += 1023-127;
nucho 0:06fc856e99ca 69 long sig_w = *val_w;
nucho 0:06fc856e99ca 70 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 71 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 72 *(outbuffer + offset++) = 0;
nucho 0:06fc856e99ca 73 *(outbuffer + offset++) = (sig_w<<5) & 0xff;
nucho 0:06fc856e99ca 74 *(outbuffer + offset++) = (sig_w>>3) & 0xff;
nucho 0:06fc856e99ca 75 *(outbuffer + offset++) = (sig_w>>11) & 0xff;
nucho 0:06fc856e99ca 76 *(outbuffer + offset++) = ((exp_w<<4) & 0xF0) | ((sig_w>>19)&0x0F);
nucho 0:06fc856e99ca 77 *(outbuffer + offset++) = (exp_w>>4) & 0x7F;
nucho 0:06fc856e99ca 78 if(this->w < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:06fc856e99ca 79 return offset;
nucho 0:06fc856e99ca 80 }
nucho 0:06fc856e99ca 81
nucho 0:06fc856e99ca 82 virtual int deserialize(unsigned char *inbuffer)
nucho 0:06fc856e99ca 83 {
nucho 0:06fc856e99ca 84 int offset = 0;
nucho 0:06fc856e99ca 85 unsigned long * val_x = (unsigned long*) &(this->x);
nucho 0:06fc856e99ca 86 offset += 3;
nucho 0:06fc856e99ca 87 *val_x = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:06fc856e99ca 88 *val_x |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:06fc856e99ca 89 *val_x |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:06fc856e99ca 90 *val_x |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:06fc856e99ca 91 unsigned long exp_x = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:06fc856e99ca 92 exp_x |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:06fc856e99ca 93 if(exp_x !=0)
nucho 0:06fc856e99ca 94 *val_x |= ((exp_x)-1023+127)<<23;
nucho 0:06fc856e99ca 95 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;
nucho 0:06fc856e99ca 96 unsigned long * val_y = (unsigned long*) &(this->y);
nucho 0:06fc856e99ca 97 offset += 3;
nucho 0:06fc856e99ca 98 *val_y = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:06fc856e99ca 99 *val_y |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:06fc856e99ca 100 *val_y |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:06fc856e99ca 101 *val_y |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:06fc856e99ca 102 unsigned long exp_y = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:06fc856e99ca 103 exp_y |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:06fc856e99ca 104 if(exp_y !=0)
nucho 0:06fc856e99ca 105 *val_y |= ((exp_y)-1023+127)<<23;
nucho 0:06fc856e99ca 106 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;
nucho 0:06fc856e99ca 107 unsigned long * val_z = (unsigned long*) &(this->z);
nucho 0:06fc856e99ca 108 offset += 3;
nucho 0:06fc856e99ca 109 *val_z = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:06fc856e99ca 110 *val_z |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:06fc856e99ca 111 *val_z |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:06fc856e99ca 112 *val_z |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:06fc856e99ca 113 unsigned long exp_z = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:06fc856e99ca 114 exp_z |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:06fc856e99ca 115 if(exp_z !=0)
nucho 0:06fc856e99ca 116 *val_z |= ((exp_z)-1023+127)<<23;
nucho 0:06fc856e99ca 117 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->z = -this->z;
nucho 0:06fc856e99ca 118 unsigned long * val_w = (unsigned long*) &(this->w);
nucho 0:06fc856e99ca 119 offset += 3;
nucho 0:06fc856e99ca 120 *val_w = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:06fc856e99ca 121 *val_w |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:06fc856e99ca 122 *val_w |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:06fc856e99ca 123 *val_w |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:06fc856e99ca 124 unsigned long exp_w = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:06fc856e99ca 125 exp_w |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:06fc856e99ca 126 if(exp_w !=0)
nucho 0:06fc856e99ca 127 *val_w |= ((exp_w)-1023+127)<<23;
nucho 0:06fc856e99ca 128 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->w = -this->w;
nucho 0:06fc856e99ca 129 return offset;
nucho 0:06fc856e99ca 130 }
nucho 0:06fc856e99ca 131
nucho 0:06fc856e99ca 132 virtual const char * getType(){ return "geometry_msgs/Quaternion"; };
nucho 0:06fc856e99ca 133
nucho 0:06fc856e99ca 134 };
nucho 0:06fc856e99ca 135
nucho 0:06fc856e99ca 136 }
nucho 0:06fc856e99ca 137 #endif