Go to the documentation of this file.00001 #ifndef ROS_MESSAGEPARSER_H
00002 #define ROS_MESSAGEPARSER_H
00003
00004 #include "PlotJuggler/messageparser_base.h"
00005 #include <ros/types.h>
00006 #include <ros/serialization.h>
00007 #include <ros/builtin_message_traits.h>
00008 #include <ros/message_operations.h>
00009 #include <std_msgs/Header.h>
00010
00011
00012 class RosParserBase : public MessageParser
00013 {
00014 public:
00015
00016 void setUseHeaderStamp( bool use )
00017 {
00018 _use_header_stamp = use;
00019 }
00020
00021 protected:
00022 bool _use_header_stamp;
00023
00024 };
00025
00026 template <typename MainType, typename SubType, class ChildParser>
00027 class RosMessageStampedParser: public RosParserBase
00028 {
00029 public:
00030 RosMessageStampedParser( const char* child_prefix):
00031 _child_prefix(child_prefix)
00032 {
00033 _data.emplace_back( "/header/seq" );
00034 _data.emplace_back( "/header/stamp" );
00035 }
00036
00037
00038 const std::unordered_set<std::string>& getCompatibleKeys() const override
00039 {
00040 static std::unordered_set<std::string> compatible_key =
00041 { ros::message_traits::MD5Sum<MainType>::value() };
00042 return compatible_key;
00043 }
00044
00045 virtual void pushMessageRef(const std::string& key,
00046 const MessageRef& buffer,
00047 double timestamp) override
00048 {
00049 std_msgs::Header header;
00050 ros::serialization::IStream is( const_cast<uint8_t*>(buffer.data()), buffer.size() );
00051 ros::serialization::deserialize(is, header);
00052
00053 size_t header_size = sizeof( ros::Time ) + 8 + header.frame_id.size();
00054
00055 _data[0].pushBack( {timestamp, header.seq} );
00056 _data[1].pushBack( {timestamp, header.stamp.toSec()} );
00057
00058 MessageRef sub_buffer( buffer.data(), header_size);
00059
00060 _child_parser.pushMessageRef( key, sub_buffer, timestamp );
00061 }
00062
00063 void extractData(PlotDataMapRef& plot_map, const std::string& prefix) override
00064 {
00065 for (auto& it: _data)
00066 {
00067 MessageParser::appendData(plot_map, prefix + it.name(), it);
00068 }
00069 _child_parser.extractData(plot_map, prefix + _child_prefix);
00070 }
00071 private:
00072 std::vector<PlotData> _data;
00073 ChildParser _child_parser;
00074 std::string _child_prefix;
00075
00076 };
00077
00078
00079
00080 #endif // ROS_MESSAGEPARSER_H