WIP

3rdparty/data_tamer_parser/data_tamer_parser.hpp

@@ -425,7 +425,7 @@ inline bool ParseSnapshot(const Schema& schema, SnapshotView snapshot,
     const auto& field = schema.fields[i];
     if (GetBit(snapshot.active_mask, i))
     {
-      ParseSnapshotRecursive(field, schema.custom_types, buffer, callback_number, "");
+      ParseSnapshotRecursive(field, schema.custom_types, buffer, callback_number, {});
     }
   }
   return true;

plotjuggler_base/include/PlotJuggler/messageparser_base.h

@@ -8,12 +8,7 @@
 
 #include <QtPlugin>
 #include <QApplication>
-#include <array>
-#include <unordered_map>
-#include <unordered_set>
-#include <functional>
 #include <map>
-#include <set>
 #include "PlotJuggler/plotdata.h"
 #include "PlotJuggler/pj_plugin.h"
 
@@ -109,6 +104,16 @@ class MessageParser
     return _clamp_large_arrays;
   }
 
+  virtual bool useEmbeddedTimestamp() const
+  {
+    return _use_embedded_timestamp;
+  }
+
+  virtual void enableEmbeddedTimestamp(bool enable)
+  {
+    _use_embedded_timestamp = enable;
+  }
+
 protected:
   PlotDataMapRef& _plot_data;
   std::string _topic_name;
@@ -125,6 +130,7 @@ class MessageParser
 private:
   bool _clamp_large_arrays = false;
   unsigned _max_array_size = 10000;
+  bool _use_embedded_timestamp = false;
 };
 
 using MessageParserPtr = std::shared_ptr<MessageParser>;

plotjuggler_base/include/PlotJuggler/special_messages.h

@@ -4,6 +4,7 @@
 //These messages are exact equivalents of ROS messages
 
 
+#include <array>
 #include <vector>
 #include <string>
 #include <cstdint>
@@ -58,6 +59,15 @@ struct Vector3
   static const char* id() { return "geometry_msgs/Vector3"; }
 };
 
+struct Point
+{
+    double x;
+    double y;
+    double z;
+
+    static const char* id() { return "geometry_msgs/Point"; }
+};
+
 struct Quaternion
 {
   double x;
@@ -79,19 +89,59 @@ RPY QuaternionToRPY(Quaternion q);
 
 struct Transform
 {
-  Vector3 translation;
+  Point translation;
   Quaternion rotation;
 
   static const char* id() { return "geometry_msgs/Transform"; }
 };
 
 struct TransformStamped
 {
-  Header header;
-  std::string child_frame_id;
-  Transform transform;
+    Header header;
+    std::string child_frame_id;
+    Transform transform;
+
+    static const char* id() { return "geometry_msgs/TransformStamped"; }
+};
+
+struct Pose
+{
+    Vector3 position;
+    Quaternion orientation;
+
+    static const char* id() { return "geometry_msgs/Pose"; }
+};
+
+struct PoseStamped
+{
+    Header header;
+    Pose pose;
+
+    static const char* id() { return "geometry_msgs/PoseStamped"; }
+};
+
+struct PoseWithCovariance
+{
+    Pose pose;
+    std::array<double, 36> covariance;
 
-  static const char* id() { return "geometry_msgs/TransformStamped"; }
+    static const char* id() { return "geometry_msgs/PoseWithCovariance"; }
+};
+
+struct Twist
+{
+    Vector3  linear;
+    Vector3  angular;
+
+    static const char* id() { return "geometry_msgs/Twist"; }
+};
+
+struct TwistWithCovariance
+{
+    Twist twist;
+    std::array<double, 36> covariance;
+
+    static const char* id() { return "geometry_msgs/TwistWithCovariance"; }
 };
 
 struct TFMessage
@@ -102,6 +152,30 @@ struct TFMessage
 };
 //--------------------
 
+struct Imu
+{
+    Header header;
+    Quaternion orientation;
+    std::array<double, 9> orientation_covariance;
+    Vector3 angular_velocity;
+    std::array<double, 9> angular_velocity_covariance;
+    Vector3 linear_acceleration;
+    std::array<double, 9> linear_acceleration_covariance;
+
+    static const char* id() { return "sensor_msgs/Imu"; }
+};
+//--------------------
+struct Odometry
+{
+    Header header;
+    PoseWithCovariance pose;
+    TwistWithCovariance twist;
+
+    static const char* id() { return "nav_msgs/Odometry"; }
+};
+
+//--------------------
+
 struct JointState
 {
   Header header;

plotjuggler_plugins/DataLoadMCAP/dataload_mcap.cpp

@@ -14,6 +14,7 @@
 #include "mcap/reader.hpp"
 #include "dialog_mcap.h"
 
+#include <QElapsedTimer>
 #include <QStandardItemModel>
 
 DataLoadMCAP::DataLoadMCAP()
@@ -80,6 +81,10 @@ bool DataLoadMCAP::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_dat
 
   std::set<QString> notified_encoding_problem;
 
+
+  QElapsedTimer timer;
+  timer.start();
+
   for (const auto& [channel_id, channel_ptr] : reader.channels())
   {
     channels.insert( {channel_id, channel_ptr} );
@@ -193,15 +198,6 @@ bool DataLoadMCAP::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_dat
     MessageRef msg(msg_view.message.data, msg_view.message.dataSize);
     parser->parseMessage(msg, timestamp_sec);
 
-    // data tamer schema
-    if( channels_containing_datatamer_schema.count(msg_view.channel->id) != 0)
-    {
-
-
-    }
-
-    // regular message
-
     if (msg_count++ % 1000 == 0)
     {
       QApplication::processEvents();
@@ -213,6 +209,7 @@ bool DataLoadMCAP::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_dat
   }
 
   reader.close();
+  qDebug() << "Loaded file in " << timer.elapsed() << "milliseconds";
   return true;
 }
 

plotjuggler_plugins/DataLoadMCAP/dialog_mcap.cpp

@@ -4,6 +4,7 @@
 #include <QSettings>
 #include <QDialogButtonBox>
 #include <QPushButton>
+#include <QElapsedTimer>
 
 const QString DialogMCAP::prefix = "DialogLoadMCAP::";