Go to the documentation of this file.00001 #ifndef FIVEAI_STAMPED_DIAGNOSTIC_H
00002 #define FIVEAI_STAMPED_DIAGNOSTIC_H
00003
00004 #include <string>
00005 #include <vector>
00006 #include <map>
00007
00008 #include <ros/types.h>
00009 #include <ros/serialization.h>
00010 #include "ros_parser_base.h"
00011 #include <absl/strings/str_cat.h>
00012 #include <absl/strings/charconv.h>
00013
00014 struct StampedDiagnostic_
00015 {
00016 uint8_t status;
00017 ros::Time stamp;
00018 std::string key;
00019 std::string value;
00020 };
00021
00022 struct NodeDiagnostics_
00023 {
00024 std::vector< StampedDiagnostic_ > diagnostics;
00025 };
00026
00027
00028 namespace ros
00029 {
00030 namespace serialization
00031 {
00032
00033 template<> struct Serializer< ::StampedDiagnostic_ >
00034 {
00035 template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
00036 {
00037 stream.next(m.status);
00038 stream.next(m.stamp);
00039 stream.next(m.key);
00040 stream.next(m.value);
00041 }
00042 ROS_DECLARE_ALLINONE_SERIALIZER
00043 };
00044
00045 template<> struct Serializer< ::NodeDiagnostics_ >
00046 {
00047 template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
00048 {
00049 stream.next(m.diagnostics);
00050 }
00051 ROS_DECLARE_ALLINONE_SERIALIZER
00052 };
00053
00054 }
00055 }
00056
00057
00058
00059
00060 class FiveAiDiagnosticMsg: public RosParserBase
00061 {
00062 public:
00063
00064 FiveAiDiagnosticMsg() = default;
00065
00066 static const std::string& getCompatibleKey()
00067 {
00068 static std::string temp = "b47994f5f7cab18367c65bedb56d7f75";
00069 return temp;
00070 }
00071
00072 const std::unordered_set<std::string>& getCompatibleKeys() const override
00073 {
00074 static std::unordered_set<std::string> temp = { getCompatibleKey() };
00075 return temp;
00076 }
00077
00078 virtual void pushMessageRef(const std::string& ,
00079 const MessageRef& msg,
00080 double timestamp) override
00081 {
00082 NodeDiagnostics_ diagnostic;
00083 ros::serialization::IStream is( const_cast<uint8_t*>(msg.data()), msg.size() );
00084 ros::serialization::deserialize(is, diagnostic);
00085
00086 for( const auto& it: diagnostic.diagnostics)
00087 {
00088 if( _use_header_stamp )
00089 {
00090 timestamp = it.stamp.toSec();
00091 }
00092 const char *start_ptr = it.value.data();
00093 double val = 0;
00094 auto res = absl::from_chars (start_ptr, start_ptr + it.value.size(), val);
00095 if( start_ptr == res.ptr ) continue;
00096
00097 auto data_it = _data.find( it.key );
00098 if( data_it == _data.end() )
00099 {
00100 data_it = _data.emplace( std::piecewise_construct,
00101 std::forward_as_tuple(it.key),
00102 std::forward_as_tuple(it.key)
00103 ).first;
00104 }
00105 data_it->second.pushBack( { timestamp, val } );
00106 }
00107 }
00108
00109 void extractData(PlotDataMapRef& plot_map, const std::string& prefix) override
00110 {
00111 for (auto& it: _data)
00112 {
00113 appendData(plot_map,
00114 absl::StrCat(prefix, "/", it.first),
00115 it.second);
00116 }
00117 }
00118
00119 private:
00120 std::unordered_map<std::string,PlotData> _data;
00121 };
00122
00123
00124
00125 #endif // FIVEAI_STAMPED_DIAGNOSTIC_H