add pal statistics

CMakeLists.txt

@@ -177,7 +177,6 @@ set(PLOTJUGGLER_BASE_SRC
     plotjuggler_base/src/plotpanner.cpp
     plotjuggler_base/src/timeseries_qwt.cpp
     plotjuggler_base/src/reactive_function.cpp
-    plotjuggler_base/src/special_messages.cpp
 )
 
 qt5_wrap_cpp(PLOTJUGGLER_BASE_MOCS

plotjuggler_plugins/ParserROS/CMakeLists.txt

@@ -8,6 +8,7 @@ add_subdirectory(rosx_introspection)
 include_directories( rosx_introspection/include )
 
 add_library(ParserROS1 SHARED
+    special_messages.cpp
     ros_parser.cpp
     ros1_parser.h )
 
@@ -17,6 +18,7 @@ target_link_libraries(ParserROS1
     plotjuggler_base)
 
 add_library(ParserROS2 SHARED
+    special_messages.cpp
     ros_parser.cpp
     ros2_parser.h )
 

plotjuggler_plugins/ParserROS/ros_parser.cpp

@@ -40,11 +40,11 @@ ParserROS::ParserROS(const std::string& topic_name, const std::string& type_name
   }
   else if (Msg::DataTamerSchemas::id() == type_name)
   {
-    _customized_parser = std::bind(&ParserROS::parseDataTamerSchemasMsg, this, _1, _2);
+    _customized_parser = std::bind(&ParserROS::parseDataTamerSchemas, this, _1, _2);
   }
   else if (Msg::DataTamerSnapshot::id() == type_name)
   {
-    _customized_parser = std::bind(&ParserROS::parseDataTamerSnapshotMsg, this, _1, _2);
+    _customized_parser = std::bind(&ParserROS::parseDataTamerSnapshot, this, _1, _2);
   }
   else if (Msg::Imu::id() == type_name)
   {
@@ -70,6 +70,16 @@ ParserROS::ParserROS(const std::string& topic_name, const std::string& type_name
   {
     _customized_parser = std::bind(&ParserROS::parseTransformStamped, this, _1, _2);
   }
+  else if (Msg::PalStatisticsNames::id() == type_name ||
+           type_name == "plotjuggler_msgs/StatisticsNames")
+  {
+    _customized_parser = std::bind(&ParserROS::parsePalStatisticsNames, this, _1, _2);
+  }
+  else if (Msg::PalStatisticsValues::id() == type_name  ||
+           type_name == "plotjuggler_msgs/StatisticsValues")
+  {
+    _customized_parser = std::bind(&ParserROS::parsePalStatisticsValues, this, _1, _2);
+  }
 }
 
 bool ParserROS::parseMessage(const PJ::MessageRef serialized_msg, double& timestamp)
@@ -128,37 +138,40 @@ void ParserROS::setLargeArraysPolicy(bool clamp, unsigned max_size)
 
 //------------------------------------------------------------------------
 
-Msg::Header ParserROS::parseHeader(const std::string& prefix, double& timestamp)
+Msg::Header ParserROS::readHeader(double& timestamp)
 {
   Msg::Header header;
-  const bool is_ros1 = dynamic_cast<ROS_Deserializer*>(_deserializer.get()) != nullptr;
   // only ROS1 as the files header.seq
-  if (is_ros1)
+  if (dynamic_cast<ROS_Deserializer*>(_deserializer.get()) != nullptr)
   {
     header.seq = _deserializer->deserializeUInt32();
   }
 
   header.stamp.sec = _deserializer->deserializeUInt32();
   header.stamp.nanosec = _deserializer->deserializeUInt32();
 
-  const double ts = double(header.stamp.sec) + 1e-9 * double(header.stamp.nanosec);
   if (useEmbeddedTimestamp())
   {
-    timestamp = ts;
+    timestamp = double(header.stamp.sec) + 1e-9 * double(header.stamp.nanosec);
   }
-
   _deserializer->deserializeString(header.frame_id);
 
-  getSeries(prefix + "/header/stamp").pushBack({ timestamp, ts });
-  if (is_ros1)
+  return header;
+}
+
+void ParserROS::parseHeader(const std::string& prefix, double& timestamp)
+{
+  const auto header = readHeader(timestamp);
+
+  getSeries(prefix + "/header/stamp").pushBack({ timestamp, header.stamp.toSec() });
+  getStringSeries(prefix + "/header/frame_id").pushBack({ timestamp, header.frame_id });
+  if (dynamic_cast<ROS_Deserializer*>(_deserializer.get()) != nullptr)
   {
     getSeries(prefix + "/header/seq").pushBack({ timestamp, double(header.seq) });
   }
-  getStringSeries(prefix + "/header/frame_id").pushBack({ timestamp, header.frame_id });
-
-  return header;
 }
 
+
 void ParserROS::parseVector3(const std::string& prefix, double& timestamp)
 {
   auto x = _deserializer->deserialize(FLOAT64).convert<double>();
@@ -409,7 +422,7 @@ void ParserROS::parseTF2Msg(const std::string& prefix, double& timestamp)
 
   for (size_t i = 0; i < transform_size; i++)
   {
-    auto header = parseHeader(prefix, timestamp);
+    const auto header = readHeader(timestamp);
     std::string child_frame_id;
     _deserializer->deserializeString(child_frame_id);
 
@@ -426,7 +439,7 @@ void ParserROS::parseTF2Msg(const std::string& prefix, double& timestamp)
   }
 }
 
-void ParserROS::parseDataTamerSchemasMsg(const std::string& prefix, double& timestamp)
+void ParserROS::parseDataTamerSchemas(const std::string& prefix, double& timestamp)
 {
   const size_t vector_size = _deserializer->deserializeUInt32();
 
@@ -445,7 +458,7 @@ void ParserROS::parseDataTamerSchemasMsg(const std::string& prefix, double& time
   }
 }
 
-void ParserROS::parseDataTamerSnapshotMsg(const std::string& prefix, double& timestamp)
+void ParserROS::parseDataTamerSnapshot(const std::string& prefix, double& timestamp)
 {
   DataTamerParser::SnapshotView snapshot;
 
@@ -478,3 +491,44 @@ void ParserROS::parseDataTamerSnapshotMsg(const std::string& prefix, double& tim
 
   DataTamerParser::ParseSnapshot(dt_schema, snapshot, callback);
 }
+
+static std::unordered_map<uint32_t, std::vector<std::string>> _pal_statistics_names;
+
+void ParserROS::parsePalStatisticsNames(const std::string &prefix, double &timestamp)
+{
+  const auto header = readHeader(timestamp);
+  std::vector<std::string> names;
+  const size_t vector_size = _deserializer->deserializeUInt32();
+  names.resize(vector_size);
+  for(auto& name: names)
+  {
+    _deserializer->deserializeString(name);
+  }
+  uint32_t names_version = _deserializer->deserializeUInt32();
+  _pal_statistics_names[names_version] = std::move(names);
+}
+
+void ParserROS::parsePalStatisticsValues(const std::string &prefix, double &timestamp)
+{
+  const auto header = readHeader(timestamp);
+  std::vector<double> values;
+  const size_t vector_size = _deserializer->deserializeUInt32();
+  values.resize(vector_size);
+
+  for(auto& value: values)
+  {
+    value = _deserializer->deserialize(BuiltinType::FLOAT64).convert<double>();
+  }
+  uint32_t names_version = _deserializer->deserializeUInt32();
+  auto it = _pal_statistics_names.find(names_version);
+  if( it != _pal_statistics_names.end() )
+  {
+    const auto& names = it->second;
+    const size_t N = std::min(names.size(), values.size());
+    for(size_t i=0; i<N; i++)
+    {
+      auto& series = getSeries(fmt::format("{}/{}", prefix, names[i]));
+      series.pushBack({timestamp, values[i]});
+    }
+  }
+}

plotjuggler_plugins/ParserROS/ros_parser.h

@@ -3,8 +3,8 @@
 
 #include "PlotJuggler/fmt/core.h"
 #include "PlotJuggler/messageparser_base.h"
-#include "PlotJuggler/special_messages.h"
 #include "rosx_introspection/ros_parser.hpp"
+#include "special_messages.h"
 
 class ParserROS : public PJ::MessageParser
 {
@@ -23,7 +23,8 @@ class ParserROS : public PJ::MessageParser
   RosMsgParser::FlatMessage _flat_msg;
   std::string _topic;
 
-  PJ::Msg::Header parseHeader(const std::string& prefix, double& timestamp);
+  PJ::Msg::Header readHeader(double& timestamp);
+  void parseHeader(const std::string& prefix, double& timestamp);
 
   template <size_t N>
   void parseCovariance(const std::string& prefix, double& timestamp);
@@ -49,8 +50,11 @@ class ParserROS : public PJ::MessageParser
   void parseJointStateMsg(const std::string& prefix, double& timestamp);
   void parseTF2Msg(const std::string& prefix, double& timestamp);
 
-  void parseDataTamerSchemasMsg(const std::string& prefix, double& timestamp);
-  void parseDataTamerSnapshotMsg(const std::string& prefix, double& timestamp);
+  void parseDataTamerSchemas(const std::string& prefix, double& timestamp);
+  void parseDataTamerSnapshot(const std::string& prefix, double& timestamp);
+
+  void parsePalStatisticsNames(const std::string& prefix, double& timestamp);
+  void parsePalStatisticsValues(const std::string& prefix, double& timestamp);
 
   std::function<void(const std::string& prefix, double&)> _customized_parser;
 

plotjuggler_base/src/special_messages.cpp → plotjuggler_plugins/ParserROS/special_messages.cpp

@@ -1,4 +1,4 @@
-#include "PlotJuggler/special_messages.h"
+#include "special_messages.h"
 #include <cmath>
 
 PJ::Msg::RPY PJ::Msg::QuaternionToRPY(PJ::Msg::Quaternion q)

plotjuggler_base/include/PlotJuggler/special_messages.h → plotjuggler_plugins/ParserROS/special_messages.h

@@ -3,7 +3,6 @@
 
 //These messages are exact equivalents of ROS messages
 
-
 #include <array>
 #include <vector>
 #include <string>
@@ -207,6 +206,25 @@ struct DataTamerSnapshot
   static const char* id() { return "data_tamer_msgs/Snapshot"; }
 };
 
+//--------------------
+
+struct PalStatisticsNames
+{
+  Header header;
+  std::vector<std::string> names;
+  uint32_t names_version;
+
+  static const char* id() { return "pal_statistics_msgs/StatisticsNames"; }
+};
+
+struct PalStatisticsValues
+{
+  Header header;
+  std::vector<double> names;
+  uint32_t names_version;
+
+  static const char* id() { return "pal_statistics_msgs/StatisticsValues"; }
+};
 }
 
 #endif // SPECIAL_MESSAGES_H