Go to the documentation of this file.00001 #ifndef PAL_STATISTICS_MSG_H
00002 #define PAL_STATISTICS_MSG_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 <std_msgs/Header.h>
00012 #include <absl/strings/str_cat.h>
00013 #include <absl/strings/charconv.h>
00014
00015 struct PalStatisticsNames_
00016 {
00017
00018
00019
00020 std_msgs::Header header;
00021 std::vector<std::string> names;
00022 uint32_t names_version;
00023 };
00024
00025 struct PalStatisticsValues_
00026 {
00027
00028
00029
00030 std_msgs::Header header;
00031 std::vector<double> values;
00032 uint32_t names_version;
00033 };
00034
00035
00036
00037 namespace ros
00038 {
00039 namespace serialization
00040 {
00041
00042 template<> struct Serializer< ::PalStatisticsNames_ >
00043 {
00044 template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
00045 {
00046 stream.next(m.header);
00047 stream.next(m.names);
00048 stream.next(m.names_version);
00049 }
00050 ROS_DECLARE_ALLINONE_SERIALIZER
00051 };
00052
00053 template<> struct Serializer< ::PalStatisticsValues_ >
00054 {
00055 template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
00056 {
00057 stream.next(m.header);
00058 stream.next(m.values);
00059 stream.next(m.names_version);
00060 }
00061 ROS_DECLARE_ALLINONE_SERIALIZER
00062 };
00063
00064 }
00065 }
00066
00067
00068
00069 static std::unordered_map<uint32_t, std::vector<std::string> > _stored_pal_statistics_names;
00070
00071 class PalStatisticsNamesParser: public RosParserBase
00072 {
00073 public:
00074
00075 PalStatisticsNamesParser() = default;
00076
00077 static const std::string& getCompatibleKey()
00078 {
00079 static std::string temp = "bece3d42a81d5c50cd68f110cf17bf55";
00080 return temp;
00081 }
00082
00083 const std::unordered_set<std::string>& getCompatibleKeys() const override
00084 {
00085 static std::unordered_set<std::string> temp = { getCompatibleKey() };
00086 return temp;
00087 }
00088
00089 virtual void pushMessageRef(const std::string& ,
00090 const MessageRef& msg,
00091 double) override
00092 {
00093 PalStatisticsNames_ pal_names;
00094 ros::serialization::IStream is( const_cast<uint8_t*>(msg.data()), msg.size() );
00095 ros::serialization::deserialize(is, pal_names);
00096 _stored_pal_statistics_names.insert( {pal_names.names_version, std::move(pal_names.names) });
00097 }
00098
00099 void extractData(PlotDataMapRef& , const std::string& ) override
00100 { }
00101 };
00102
00103
00104 class PalStatisticsValuesParser: public RosParserBase
00105 {
00106 public:
00107
00108 PalStatisticsValuesParser() = default;
00109
00110 static const std::string& getCompatibleKey()
00111 {
00112 static std::string temp = "44646896ace86f96c24fbb63054eeee8";
00113 return temp;
00114 }
00115
00116 const std::unordered_set<std::string>& getCompatibleKeys() const override
00117 {
00118 static std::unordered_set<std::string> temp = { getCompatibleKey() };
00119 return temp;
00120 }
00121
00122 virtual void pushMessageRef(const std::string& ,
00123 const MessageRef& msg,
00124 double timestamp) override
00125 {
00126 PalStatisticsValues_ pal_msg;
00127 ros::serialization::IStream is( const_cast<uint8_t*>(msg.data()), msg.size() );
00128 ros::serialization::deserialize(is, pal_msg);
00129
00130 auto& values = _data[ pal_msg.names_version ];
00131
00132 if( _use_header_stamp )
00133 {
00134 timestamp = pal_msg.header.stamp.toSec();
00135 }
00136
00137 for( size_t index = 0; index < pal_msg.values.size(); index++)
00138 {
00139 if( index >= values.size() )
00140 {
00141 values.emplace_back( "placeholder" );
00142 }
00143 values[index].pushBack( { timestamp, pal_msg.values[index] } );
00144 }
00145 }
00146
00147 void extractData(PlotDataMapRef& plot_map, const std::string& prefix) override
00148 {
00149 for ( auto& it_version: _data)
00150 {
00151 const auto& names = _stored_pal_statistics_names[ it_version.first ];
00152 auto& vect = it_version.second;
00153 for ( size_t index = 0; index < vect.size(); index++ )
00154 {
00155 appendData(plot_map, absl::StrCat(prefix, "/", names.at(index) ), vect[index]);
00156 }
00157 }
00158 }
00159
00160 private:
00161 std::unordered_map<uint32_t, std::vector< PlotData > > _data;
00162
00163 };
00164
00165
00166
00167 #endif // PAL_STATISTICS_MSG_H