Statistics
| Branch: | Revision:

root / quad2 / arduino / src / ros_lib / geometry_msgs / Quaternion.h @ c1426757

History | View | Annotate | Download (5.31 KB)

1
#ifndef _ROS_geometry_msgs_Quaternion_h
2
#define _ROS_geometry_msgs_Quaternion_h
3

    
4
#include <stdint.h>
5
#include <string.h>
6
#include <stdlib.h>
7
#include "ros/msg.h"
8

    
9
namespace geometry_msgs
10
{
11

    
12
  class Quaternion : public ros::Msg
13
  {
14
    public:
15
      float x;
16
      float y;
17
      float z;
18
      float w;
19

    
20
    virtual int serialize(unsigned char *outbuffer) const
21
    {
22
      int offset = 0;
23
      int32_t * val_x = (long *) &(this->x);
24
      int32_t exp_x = (((*val_x)>>23)&255);
25
      if(exp_x != 0)
26
        exp_x += 1023-127;
27
      int32_t sig_x = *val_x;
28
      *(outbuffer + offset++) = 0;
29
      *(outbuffer + offset++) = 0;
30
      *(outbuffer + offset++) = 0;
31
      *(outbuffer + offset++) = (sig_x<<5) & 0xff;
32
      *(outbuffer + offset++) = (sig_x>>3) & 0xff;
33
      *(outbuffer + offset++) = (sig_x>>11) & 0xff;
34
      *(outbuffer + offset++) = ((exp_x<<4) & 0xF0) | ((sig_x>>19)&0x0F);
35
      *(outbuffer + offset++) = (exp_x>>4) & 0x7F;
36
      if(this->x < 0) *(outbuffer + offset -1) |= 0x80;
37
      int32_t * val_y = (long *) &(this->y);
38
      int32_t exp_y = (((*val_y)>>23)&255);
39
      if(exp_y != 0)
40
        exp_y += 1023-127;
41
      int32_t sig_y = *val_y;
42
      *(outbuffer + offset++) = 0;
43
      *(outbuffer + offset++) = 0;
44
      *(outbuffer + offset++) = 0;
45
      *(outbuffer + offset++) = (sig_y<<5) & 0xff;
46
      *(outbuffer + offset++) = (sig_y>>3) & 0xff;
47
      *(outbuffer + offset++) = (sig_y>>11) & 0xff;
48
      *(outbuffer + offset++) = ((exp_y<<4) & 0xF0) | ((sig_y>>19)&0x0F);
49
      *(outbuffer + offset++) = (exp_y>>4) & 0x7F;
50
      if(this->y < 0) *(outbuffer + offset -1) |= 0x80;
51
      int32_t * val_z = (long *) &(this->z);
52
      int32_t exp_z = (((*val_z)>>23)&255);
53
      if(exp_z != 0)
54
        exp_z += 1023-127;
55
      int32_t sig_z = *val_z;
56
      *(outbuffer + offset++) = 0;
57
      *(outbuffer + offset++) = 0;
58
      *(outbuffer + offset++) = 0;
59
      *(outbuffer + offset++) = (sig_z<<5) & 0xff;
60
      *(outbuffer + offset++) = (sig_z>>3) & 0xff;
61
      *(outbuffer + offset++) = (sig_z>>11) & 0xff;
62
      *(outbuffer + offset++) = ((exp_z<<4) & 0xF0) | ((sig_z>>19)&0x0F);
63
      *(outbuffer + offset++) = (exp_z>>4) & 0x7F;
64
      if(this->z < 0) *(outbuffer + offset -1) |= 0x80;
65
      int32_t * val_w = (long *) &(this->w);
66
      int32_t exp_w = (((*val_w)>>23)&255);
67
      if(exp_w != 0)
68
        exp_w += 1023-127;
69
      int32_t sig_w = *val_w;
70
      *(outbuffer + offset++) = 0;
71
      *(outbuffer + offset++) = 0;
72
      *(outbuffer + offset++) = 0;
73
      *(outbuffer + offset++) = (sig_w<<5) & 0xff;
74
      *(outbuffer + offset++) = (sig_w>>3) & 0xff;
75
      *(outbuffer + offset++) = (sig_w>>11) & 0xff;
76
      *(outbuffer + offset++) = ((exp_w<<4) & 0xF0) | ((sig_w>>19)&0x0F);
77
      *(outbuffer + offset++) = (exp_w>>4) & 0x7F;
78
      if(this->w < 0) *(outbuffer + offset -1) |= 0x80;
79
      return offset;
80
    }
81

    
82
    virtual int deserialize(unsigned char *inbuffer)
83
    {
84
      int offset = 0;
85
      uint32_t * val_x = (uint32_t*) &(this->x);
86
      offset += 3;
87
      *val_x = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
88
      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
89
      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
90
      *val_x |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
91
      uint32_t exp_x = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
92
      exp_x |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
93
      if(exp_x !=0)
94
        *val_x |= ((exp_x)-1023+127)<<23;
95
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;
96
      uint32_t * val_y = (uint32_t*) &(this->y);
97
      offset += 3;
98
      *val_y = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
99
      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
100
      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
101
      *val_y |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
102
      uint32_t exp_y = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
103
      exp_y |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
104
      if(exp_y !=0)
105
        *val_y |= ((exp_y)-1023+127)<<23;
106
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;
107
      uint32_t * val_z = (uint32_t*) &(this->z);
108
      offset += 3;
109
      *val_z = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
110
      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
111
      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
112
      *val_z |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
113
      uint32_t exp_z = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
114
      exp_z |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
115
      if(exp_z !=0)
116
        *val_z |= ((exp_z)-1023+127)<<23;
117
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->z = -this->z;
118
      uint32_t * val_w = (uint32_t*) &(this->w);
119
      offset += 3;
120
      *val_w = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
121
      *val_w |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
122
      *val_w |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
123
      *val_w |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
124
      uint32_t exp_w = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
125
      exp_w |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
126
      if(exp_w !=0)
127
        *val_w |= ((exp_w)-1023+127)<<23;
128
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->w = -this->w;
129
     return offset;
130
    }
131

    
132
    const char * getType(){ return "geometry_msgs/Quaternion"; };
133
    const char * getMD5(){ return "a779879fadf0160734f906b8c19c7004"; };
134

    
135
  };
136

    
137
}
138
#endif