Go to the documentation of this file.00001 #ifndef ODOMETRY_MSG_H
00002 #define ODOMETRY_MSG_H
00003
00004
00005 #include <nav_msgs/Odometry.h>
00006 #include "ros_parser_base.h"
00007
00008 class OdometryMsgParser: public RosParserBase
00009 {
00010 public:
00011
00012 OdometryMsgParser()
00013 {
00014 _data.emplace_back( "/header/seq" );
00015 _data.emplace_back( "/header/stamp" );
00016
00017 _data.emplace_back( "/pose/position/x" );
00018 _data.emplace_back( "/pose/position/y" );
00019 _data.emplace_back( "/pose/position/z" );
00020
00021 _data.emplace_back( "/pose/orientation/quat_x" );
00022 _data.emplace_back( "/pose/orientation/quat_y" );
00023 _data.emplace_back( "/pose/orientation/quat_z" );
00024 _data.emplace_back( "/pose/orientation/quat_w" );
00025 _data.emplace_back( "/pose/orientation/yaw_degrees" );
00026
00027 _data.emplace_back( "/twist/linear/x" );
00028 _data.emplace_back( "/twist/linear/y" );
00029 _data.emplace_back( "/twist/linear/z" );
00030
00031 _data.emplace_back( "/twist/angular/x" );
00032 _data.emplace_back( "/twist/angular/y" );
00033 _data.emplace_back( "/twist/angular/z" );
00034 }
00035
00036 static const std::string& getCompatibleKey()
00037 {
00038 static std::string str = ros::message_traits::MD5Sum<nav_msgs::Odometry>::value();
00039 return str;
00040 }
00041
00042 const std::unordered_set<std::string>& getCompatibleKeys() const override
00043 {
00044 static std::unordered_set<std::string> temp = { getCompatibleKey() };
00045 return temp;
00046 }
00047
00048 virtual void pushMessageRef(const std::string& ,
00049 const MessageRef& msg,
00050 double timestamp) override
00051 {
00052 nav_msgs::Odometry odom;
00053 ros::serialization::IStream is( const_cast<uint8_t*>(msg.data()), msg.size() );
00054 ros::serialization::deserialize(is, odom);
00055
00056 if( _use_header_stamp )
00057 {
00058 timestamp = odom.header.stamp.toSec();
00059 }
00060
00061
00062 _data[0].pushBack( {timestamp, static_cast<double>(odom.header.seq)} );
00063 _data[1].pushBack( {timestamp, odom.header.stamp.toSec()} );
00064
00065 _data[2].pushBack( {timestamp, odom.pose.pose.position.x} );
00066 _data[3].pushBack( {timestamp, odom.pose.pose.position.y} );
00067 _data[4].pushBack( {timestamp, odom.pose.pose.position.z} );
00068
00069
00070 const auto& q = odom.pose.pose.orientation;
00071 _data[5].pushBack( {timestamp, q.x} );
00072 _data[6].pushBack( {timestamp, q.y} );
00073 _data[7].pushBack( {timestamp, q.z} );
00074 _data[8].pushBack( {timestamp, q.w} );
00075
00076
00077 double siny_cosp = +2.0 * (q.w * q.z + q.x * q.y);
00078 double cosy_cosp = +1.0 - 2.0 * (q.y * q.y + q.z * q.z);
00079 double yaw = atan2(siny_cosp, cosy_cosp) * 180 / M_PI;
00080
00081 if( _data[9].size() == 0 )
00082 {
00083 _yaw_warp = 0;
00084 }
00085 else{
00086 double prev_yaw = _data[9].back().y - _yaw_warp;
00087 if( prev_yaw - yaw > 355 )
00088 _yaw_warp += 360;
00089 else if( yaw - prev_yaw > 355)
00090 _yaw_warp -= 360;
00091 }
00092
00093 _data[9].pushBack( {timestamp, yaw + _yaw_warp} );
00094
00095 _data[10].pushBack( {timestamp, odom.twist.twist.linear.x} );
00096 _data[11].pushBack( {timestamp, odom.twist.twist.linear.y} );
00097 _data[12].pushBack( {timestamp, odom.twist.twist.linear.z} );
00098
00099 _data[13].pushBack( {timestamp, odom.twist.twist.angular.x} );
00100 _data[14].pushBack( {timestamp, odom.twist.twist.angular.y} );
00101 _data[15].pushBack( {timestamp, odom.twist.twist.angular.z} );
00102
00103 }
00104
00105 void extractData(PlotDataMapRef& plot_map, const std::string& prefix) override
00106 {
00107 for (auto& it: _data)
00108 {
00109 appendData(plot_map, prefix + it.name(), it);
00110 }
00111 }
00112
00113 private:
00114 std::vector<PlotData> _data;
00115 double _yaw_warp;
00116 };
00117
00118 #endif // ODOMETRY_MSG_H