Go to the documentation of this file.00001 #ifndef DIAGNOSTIC_MSG_H
00002 #define DIAGNOSTIC_MSG_H
00003
00004 #include "ros_parser_base.h"
00005 #include <diagnostic_msgs/DiagnosticArray.h>
00006 #include <absl/strings/str_cat.h>
00007 #include <absl/strings/charconv.h>
00008
00009 class DiagnosticMsg: public RosParserBase
00010 {
00011 public:
00012
00013 DiagnosticMsg()
00014 {
00015 _header_data.emplace_back( "/header/seq" );
00016 _header_data.emplace_back( "/header/stamp" );
00017 }
00018
00019 static const std::string& getCompatibleKey()
00020 {
00021 static std::string str = ros::message_traits::MD5Sum<diagnostic_msgs::DiagnosticArray>::value();
00022 return str;
00023 }
00024
00025 const std::unordered_set<std::string>& getCompatibleKeys() const override
00026 {
00027 static std::unordered_set<std::string> temp = { getCompatibleKey() };
00028 return temp;
00029 }
00030
00031 virtual void pushMessageRef(const std::string& ,
00032 const MessageRef& msg,
00033 double timestamp) override
00034 {
00035 diagnostic_msgs::DiagnosticArray status_array;
00036 ros::serialization::IStream is( const_cast<uint8_t*>(msg.data()), msg.size() );
00037 ros::serialization::deserialize(is, status_array);
00038
00039 if( _use_header_stamp )
00040 {
00041 timestamp = status_array.header.stamp.toSec();
00042 }
00043
00044 _header_data[0].pushBack( {timestamp, (double)status_array.header.seq} );
00045 _header_data[1].pushBack( {timestamp, status_array.header.stamp.toSec()} );
00046
00047 for( const auto& status: status_array.status)
00048 {
00049 for( const auto& kv: status.values)
00050 {
00051 const char *start_ptr = kv.value.data();
00052 double val = 0;
00053 auto res = absl::from_chars (start_ptr, start_ptr + kv.value.size(), val);
00054 if( start_ptr == res.ptr ) continue;
00055
00056 std::string status_prefix;
00057 if( status.hardware_id.empty()){
00058 status_prefix = absl::StrCat( "/", status.name, "/",
00059 kv.key );
00060 }
00061 else {
00062 status_prefix = absl::StrCat( "/",status.hardware_id, "/",
00063 status.name, "/",
00064 kv.key );
00065 }
00066 auto it = _data.find(status_prefix);
00067 if( it == _data.end() )
00068 {
00069 it = _data.emplace( std::piecewise_construct,
00070 std::forward_as_tuple(status_prefix),
00071 std::forward_as_tuple(status_prefix)
00072 ).first;
00073 }
00074 it->second.pushBack( { timestamp, val } );
00075 }
00076 }
00077
00078 }
00079
00080 void extractData(PlotDataMapRef& plot_map, const std::string& prefix) override
00081 {
00082 for (auto& it: _header_data)
00083 {
00084 appendData(plot_map, prefix + it.name(), it);
00085 }
00086 for (auto& it: _data)
00087 {
00088 appendData(plot_map, prefix + it.first, it.second);
00089 }
00090 }
00091
00092 private:
00093 std::vector<PlotData> _header_data;
00094 std::unordered_map<std::string,PlotData> _data;
00095 };
00096
00097
00098
00099 #endif // DIAGNOSTIC_MSG_H