root / quad2 / arduino / src / ros_lib / nav_msgs / Odometry.h @ c1426757
History | View | Annotate | Download (1.83 KB)
1 | c1426757 | Tom Mullins | #ifndef _ROS_nav_msgs_Odometry_h
|
---|---|---|---|
2 | #define _ROS_nav_msgs_Odometry_h
|
||
3 | |||
4 | #include <stdint.h> |
||
5 | #include <string.h> |
||
6 | #include <stdlib.h> |
||
7 | #include "ros/msg.h" |
||
8 | #include "std_msgs/Header.h" |
||
9 | #include "geometry_msgs/PoseWithCovariance.h" |
||
10 | #include "geometry_msgs/TwistWithCovariance.h" |
||
11 | |||
12 | namespace nav_msgs |
||
13 | { |
||
14 | |||
15 | class Odometry : public ros::Msg |
||
16 | { |
||
17 | public:
|
||
18 | std_msgs::Header header; |
||
19 | char * child_frame_id;
|
||
20 | geometry_msgs::PoseWithCovariance pose; |
||
21 | geometry_msgs::TwistWithCovariance twist; |
||
22 | |||
23 | virtual int serialize(unsigned char *outbuffer) const |
||
24 | { |
||
25 | int offset = 0; |
||
26 | offset += this->header.serialize(outbuffer + offset); |
||
27 | uint32_t * length_child_frame_id = (uint32_t *)(outbuffer + offset); |
||
28 | *length_child_frame_id = strlen( (const char*) this->child_frame_id); |
||
29 | offset += 4;
|
||
30 | memcpy(outbuffer + offset, this->child_frame_id, *length_child_frame_id); |
||
31 | offset += *length_child_frame_id; |
||
32 | offset += this->pose.serialize(outbuffer + offset); |
||
33 | offset += this->twist.serialize(outbuffer + offset); |
||
34 | return offset;
|
||
35 | } |
||
36 | |||
37 | virtual int deserialize(unsigned char *inbuffer) |
||
38 | { |
||
39 | int offset = 0; |
||
40 | offset += this->header.deserialize(inbuffer + offset); |
||
41 | uint32_t length_child_frame_id = *(uint32_t *)(inbuffer + offset); |
||
42 | offset += 4;
|
||
43 | for(unsigned int k= offset; k< offset+length_child_frame_id; ++k){ |
||
44 | inbuffer[k-1]=inbuffer[k];
|
||
45 | } |
||
46 | inbuffer[offset+length_child_frame_id-1]=0; |
||
47 | this->child_frame_id = (char *)(inbuffer + offset-1); |
||
48 | offset += length_child_frame_id; |
||
49 | offset += this->pose.deserialize(inbuffer + offset); |
||
50 | offset += this->twist.deserialize(inbuffer + offset); |
||
51 | return offset;
|
||
52 | } |
||
53 | |||
54 | const char * getType(){ return "nav_msgs/Odometry"; }; |
||
55 | const char * getMD5(){ return "cd5e73d190d741a2f92e81eda573aca7"; }; |
||
56 | |||
57 | }; |
||
58 | |||
59 | } |
||
60 | #endif |