Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (4.11 KB)

1
#ifndef _ROS_geometry_msgs_Point_h
2
#define _ROS_geometry_msgs_Point_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 Point : public ros::Msg
13
  {
14
    public:
15
      float x;
16
      float y;
17
      float z;
18

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

    
67
    virtual int deserialize(unsigned char *inbuffer)
68
    {
69
      int offset = 0;
70
      uint32_t * val_x = (uint32_t*) &(this->x);
71
      offset += 3;
72
      *val_x = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
73
      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
74
      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
75
      *val_x |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
76
      uint32_t exp_x = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
77
      exp_x |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
78
      if(exp_x !=0)
79
        *val_x |= ((exp_x)-1023+127)<<23;
80
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;
81
      uint32_t * val_y = (uint32_t*) &(this->y);
82
      offset += 3;
83
      *val_y = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
84
      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
85
      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
86
      *val_y |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
87
      uint32_t exp_y = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
88
      exp_y |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
89
      if(exp_y !=0)
90
        *val_y |= ((exp_y)-1023+127)<<23;
91
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;
92
      uint32_t * val_z = (uint32_t*) &(this->z);
93
      offset += 3;
94
      *val_z = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
95
      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
96
      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
97
      *val_z |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
98
      uint32_t exp_z = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
99
      exp_z |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
100
      if(exp_z !=0)
101
        *val_z |= ((exp_z)-1023+127)<<23;
102
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->z = -this->z;
103
     return offset;
104
    }
105

    
106
    const char * getType(){ return "geometry_msgs/Point"; };
107
    const char * getMD5(){ return "4a842b65f413084dc2b10fb484ea7f17"; };
108

    
109
  };
110

    
111
}
112
#endif