fixes for ROS1

facontidavide · Dec 12, 2023 · f6ba71f · f6ba71f
1 parent 8dfd8e4
commit f6ba71f
Show file tree

Hide file tree

Showing 7 changed files with 52 additions and 44 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -14,6 +14,7 @@ if( CATKIN_DEVEL_PREFIX OR catkin_FOUND OR CATKIN_BUILD_BINARY_PACKAGE)
         rosbag_storage
         roscpp
         roscpp_serialization
+        ros_type_introspection
         tf2_ros
         plotjuggler )
 

diff --git a/package.xml b/package.xml
@@ -17,6 +17,7 @@
     <depend condition="$ROS_VERSION == 1">rosbag_storage</depend>
     <depend condition="$ROS_VERSION == 1">roscpp</depend>
     <depend condition="$ROS_VERSION == 1">roscpp_serialization</depend>
+    <depend condition="$ROS_VERSION == 1">ros_type_introspection</depend>
 
     <!-- ROS2 dependencies -->
     <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>

diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt
@@ -28,43 +28,45 @@ SET( COMMON_SRC
 
 if(COMPILING_WITH_CATKIN)
 
+    include_directories(
+        ${catkin_INCLUDE_DIRS}  )
+
      list(APPEND COMMON_SRC
         qnodedialog.cpp
         rule_editing.cpp
-        ros_parsers/ros1_parser.cpp
         )
 
     add_library( commonROS STATIC ${COMMON_SRC} ${COMMON_UI_SRC} )
     target_compile_definitions(commonROS PUBLIC DISABLE_RULE_EDITING)
     target_link_libraries( commonROS PRIVATE
         ${Qt5Widgets_LIBRARIES}
         ${Qt5Xml_LIBRARIES}
-        ${catkin_LIBRARIES}
-        ros_type_introspection_static )
+        ${catkin_LIBRARIES})
 
     #########
     add_library( DataLoadROS SHARED
         DataLoadROS/dataload_ros.cpp)
     target_link_libraries( DataLoadROS  commonROS)
     #############
-    add_library( DataStreamROS SHARED
-        DataStreamROS/datastream_ROS.cpp )
-    target_link_libraries( DataStreamROS  commonROS )
-    #############
-    add_library( RosTopicPublisher SHARED
-        TopicPublisherROS/statepublisher_rostopic.cpp)
-    target_link_libraries( RosTopicPublisher commonROS )
-    #############
-    QT5_WRAP_UI ( ROSOUT_UI RosoutPublisher/logwidget.ui )
-    add_library( RosoutPublisher SHARED ${ROSOUT_UI}
-        RosoutPublisher/rosout_publisher.cpp
-        RosoutPublisher/logs_table_model.cpp
-        RosoutPublisher/logwidget.cpp
-        RosoutPublisher/modelfilter.cpp
-        )
-    target_link_libraries( RosoutPublisher commonROS )
+    # add_library( DataStreamROS SHARED
+    #     DataStreamROS/datastream_ROS.cpp )
+    # target_link_libraries( DataStreamROS  commonROS )
+    # #############
+    # add_library( RosTopicPublisher SHARED
+    #     TopicPublisherROS/statepublisher_rostopic.cpp)
+    # target_link_libraries( RosTopicPublisher commonROS )
+    # #############
+    # QT5_WRAP_UI ( ROSOUT_UI RosoutPublisher/logwidget.ui )
+    # add_library( RosoutPublisher SHARED ${ROSOUT_UI}
+    #     RosoutPublisher/rosout_publisher.cpp
+    #     RosoutPublisher/logs_table_model.cpp
+    #     RosoutPublisher/logwidget.cpp
+    #     RosoutPublisher/modelfilter.cpp
+    #     )
+    # target_link_libraries( RosoutPublisher commonROS )
 #######################################################################
 elseif(COMPILING_WITH_AMENT)
+
     include_directories( ${CMAKE_CURRENT_SOURCE_DIR} )
 
     list( APPEND COMMON_SRC ros_parsers/ros2_parser.cpp )
@@ -118,9 +120,9 @@ endif()
 if(COMPILING_WITH_CATKIN)
     install(TARGETS
         DataLoadROS
-        DataStreamROS
-        RosTopicPublisher
-        RosoutPublisher
+        # DataStreamROS
+        # RosTopicPublisher
+        # RosoutPublisher
         DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/
         )
 elseif(COMPILING_WITH_AMENT)

diff --git a/plugins/DataLoadROS/dataload_ros.cpp b/plugins/DataLoadROS/dataload_ros.cpp
@@ -45,7 +45,7 @@ const std::vector<const char*>& DataLoadROS::compatibleFileExtensions() const
 }
 
 std::vector<std::pair<QString, QString>>
-DataLoadROS::getAllTopics(const rosbag::Bag* bag, RosCompositeParser& parser)
+DataLoadROS::getAllTopics(const rosbag::Bag* bag, PJ::CompositeParser& parser)
 {
   std::vector<std::pair<QString, QString>> all_topics;
   rosbag::View bag_view(*bag, ros::TIME_MIN, ros::TIME_MAX, true);
@@ -62,8 +62,8 @@ DataLoadROS::getAllTopics(const rosbag::Bag* bag, RosCompositeParser& parser)
     all_topics.push_back(std::make_pair(QString(topic.c_str()), QString(datatype.c_str())));
     try
     {
-      parser.registerMessageType(topic, datatype, definition);
-
+      auto ros_parser = CreateParserROS(*parserFactories(), topic, datatype, definition, *_plot_map);
+      parser.addParser(topic, ros_parser);
       RosIntrospectionFactory::registerMessage(topic, md5sum, datatype, definition);
     }
     catch (std::exception& ex)
@@ -107,6 +107,7 @@ DataLoadROS::getAllTopics(const rosbag::Bag* bag, RosCompositeParser& parser)
 
 bool DataLoadROS::readDataFromFile(PJ::FileLoadInfo* info, PJ::PlotDataMapRef& plot_map)
 {
+  _plot_map = &plot_map;
   auto temp_bag = std::make_shared<rosbag::Bag>();
 
   try
@@ -119,7 +120,7 @@ bool DataLoadROS::readDataFromFile(PJ::FileLoadInfo* info, PJ::PlotDataMapRef& p
     return false;
   }
 
-  RosCompositeParser ros_parser(plot_map);
+  PJ::CompositeParser ros_parser;
   auto all_topics = getAllTopics(temp_bag.get(), ros_parser);
 
   //----------------------------------
@@ -182,7 +183,7 @@ bool DataLoadROS::readDataFromFile(PJ::FileLoadInfo* info, PJ::PlotDataMapRef& p
   QElapsedTimer timer;
   timer.start();
 
-  PlotDataAny& plot_consecutive = plot_map.addUserDefined("plotjuggler::rosbag1::consecutive_messages")->second;
+  PJ::PlotDataAny& plot_consecutive = plot_map.addUserDefined("plotjuggler::rosbag1::consecutive_messages")->second;
 
   std::vector<uint8_t> buffer;
 
@@ -215,21 +216,21 @@ bool DataLoadROS::readDataFromFile(PJ::FileLoadInfo* info, PJ::PlotDataMapRef& p
     ros::serialization::OStream stream(buffer.data(), msg_size);
     msg_instance.write(stream);
 
-    MessageRef msg_serialized(buffer.data(), buffer.size());
+    PJ::MessageRef msg_serialized(buffer.data(), buffer.size());
 
     double tmp_timestamp = msg_time;
     ros_parser.parseMessage(topic_name, msg_serialized, tmp_timestamp);
 
     //------ save msg reference in PlotAny ----
-    auto data_point = PlotDataAny::Point(tmp_timestamp, std::any(msg_instance));
+    auto data_point = PJ::PlotDataAny::Point(tmp_timestamp, std::any(msg_instance));
     plot_consecutive.pushBack(data_point);
 
     auto plot_pair = plot_map.user_defined.find(topic_name);
     if (plot_pair == plot_map.user_defined.end())
     {
       plot_pair = plot_map.addUserDefined(topic_name);
     }
-    PlotDataAny& plot_raw = plot_pair->second;
+    PJ::PlotDataAny& plot_raw = plot_pair->second;
     plot_raw.pushBack(data_point);
   }
 

diff --git a/plugins/DataLoadROS/dataload_ros.h b/plugins/DataLoadROS/dataload_ros.h
@@ -9,13 +9,13 @@
 
 #include <PlotJuggler/dataloader_base.h>
 #include "dialog_select_ros_topics.h"
-#include "ros1_parsers/ros1_parser.h"
+#include "ros_parsers/ros1_parser.h"
+#include "parser_configuration.h"
 
 class DataLoadROS : public PJ::DataLoader
 {
   Q_OBJECT
-  Q_PLUGIN_METADATA(IID "facontidavide.PlotJuggler3.DataLoader"
-                        "../dataloader.json")
+  Q_PLUGIN_METADATA(IID "facontidavide.PlotJuggler3.ROSDataLoader")
   Q_INTERFACES(PJ::DataLoader)
 
 public:
@@ -45,9 +45,11 @@ class DataLoadROS : public PJ::DataLoader
 private:
   std::vector<const char*> _extensions;
 
-  RosParserConfig _config;
+  PJ::RosParserConfig _config;
 
-  std::vector<std::pair<QString, QString>> getAllTopics(const rosbag::Bag* bag, RosCompositeParser &parser);
+  std::vector<std::pair<QString, QString>> getAllTopics(const rosbag::Bag* bag, PJ::CompositeParser &parser);
+
+  PJ::PlotDataMapRef* _plot_map = nullptr;
 };
 
 #endif  // DATALOAD_CSV_H
diff --git a/plugins/ros_parsers/ros1_parser.cpp b/plugins/ros_parsers/ros1_parser.cpp
diff --git a/plugins/ros_parsers/ros1_parser.h b/plugins/ros_parsers/ros1_parser.h
@@ -1,12 +1,15 @@
 #pragma once
 
-#include <PlotJuggler/plotdata.h>
 #include <PlotJuggler/messageparser_base.h>
-#include "ros_type_introspection/ros_introspection.hpp"
-#include "parser_configuration.h"
 
-std::shared_ptr<PJ::MessageParser>
+
+inline std::shared_ptr<PJ::MessageParser>
 CreateParserROS(const PJ::ParserFactories &factories,
-                const std::string &topic_name,
-                const std::string &type_name,
-                PJ::PlotDataMapRef &data);
+                const std::string& topic_name,
+                const std::string& type_name,
+                const std::string& definition,
+                PJ::PlotDataMapRef& data)
+{
+  return factories.at("ros1msg")->createParser(topic_name, type_name, definition, data);
+}
+