Make Player and Recorder Composable (#902)

rosbag2_storage/include/rosbag2_storage/qos.hpp

@@ -15,9 +15,9 @@
 #ifndef ROSBAG2_STORAGE__QOS_HPP_
 #define ROSBAG2_STORAGE__QOS_HPP_
 
-#include <map>
 #include <string>
 #include <vector>
+#include <unordered_map>
 
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/qos.hpp"
@@ -179,11 +179,11 @@ struct ROSBAG2_STORAGE_PUBLIC convert<std::vector<rclcpp::QoS>>
 };
 
 template<>
-struct ROSBAG2_STORAGE_PUBLIC convert<std::map<std::string, rosbag2_storage::Rosbag2QoS>>
+struct ROSBAG2_STORAGE_PUBLIC convert<std::unordered_map<std::string, rclcpp::QoS>>
 {
-  static Node encode(const std::map<std::string, rosbag2_storage::Rosbag2QoS> & rhs);
+  static Node encode(const std::unordered_map<std::string, rclcpp::QoS> & rhs);
   static bool decode(
-    const Node & node, std::map<std::string, rosbag2_storage::Rosbag2QoS> & rhs, int version = 9);
+    const Node & node, std::unordered_map<std::string, rclcpp::QoS> & rhs, int version = 9);
 };
 }  // namespace YAML
 

rosbag2_storage/include/rosbag2_storage/yaml.hpp

@@ -30,6 +30,7 @@
 # pragma warning(pop)
 #endif
 
+#include "rclcpp/duration.hpp"
 #include "rosbag2_storage/bag_metadata.hpp"
 #include "rosbag2_storage/qos.hpp"
 
@@ -41,7 +42,7 @@ template<typename T>
 void optional_assign(const Node & node, std::string field, T & assign_to)
 {
   if (node[field]) {
-    assign_to = node[field].as<T>();
+    YAML::convert<T>::decode(node[field], assign_to);
   }
 }
 
@@ -66,6 +67,41 @@ struct convert<std::unordered_map<std::string, std::string>>
   }
 };
 
+template<>
+struct convert<rclcpp::Duration>
+{
+  static Node encode(const rclcpp::Duration & duration)
+  {
+    Node node;
+    node["sec"] = duration.nanoseconds() / 1000000000;
+    node["nsec"] = duration.nanoseconds() % 1000000000;
+    return node;
+  }
+
+  static bool decode(const Node & node, rclcpp::Duration & duration)
+  {
+    duration = rclcpp::Duration(node["sec"].as<int32_t>(), node["nsec"].as<uint32_t>());
+    return true;
+  }
+};
+
+template<>
+struct convert<std::chrono::milliseconds>
+{
+  static Node encode(const std::chrono::milliseconds & duration)
+  {
+    Node node;
+    node["milliseconds"] = duration.count();
+    return node;
+  }
+
+  static bool decode(const Node & node, std::chrono::milliseconds & duration)
+  {
+    duration = std::chrono::milliseconds(node["milliseconds"].as<std::chrono::milliseconds::rep>());
+    return true;
+  }
+};
+
 template<>
 struct convert<rosbag2_storage::TopicMetadata>
 {

rosbag2_storage/src/rosbag2_storage/qos.cpp

@@ -15,7 +15,6 @@
 #include <string>
 #include <vector>
 
-
 #include "rosbag2_storage/qos.hpp"
 #include "rosbag2_storage/logging.hpp"
 #include "rmw/qos_string_conversions.h"
@@ -275,8 +274,8 @@ bool convert<std::vector<rclcpp::QoS>>::decode(
   return true;
 }
 
-Node convert<std::map<std::string, rosbag2_storage::Rosbag2QoS>>::encode(
-  const std::map<std::string, rosbag2_storage::Rosbag2QoS> & rhs)
+Node convert<std::unordered_map<std::string, rclcpp::QoS>>::encode(
+  const std::unordered_map<std::string, rclcpp::QoS> & rhs)
 {
   Node node{NodeType::Sequence};
   for (const auto & [key, value] : rhs) {
@@ -285,17 +284,21 @@ Node convert<std::map<std::string, rosbag2_storage::Rosbag2QoS>>::encode(
   return node;
 }
 
-bool convert<std::map<std::string, rosbag2_storage::Rosbag2QoS>>::decode(
-  const Node & node, std::map<std::string, rosbag2_storage::Rosbag2QoS> & rhs, int version)
+bool convert<std::unordered_map<std::string, rclcpp::QoS>>::decode(
+  const Node & node, std::unordered_map<std::string, rclcpp::QoS> & rhs, int version)
 {
   if (!node.IsMap()) {
     return false;
   }
 
   rhs.clear();
   for (const auto & element : node) {
-    rhs[element.first.as<std::string>()] = decode_for_version<rosbag2_storage::Rosbag2QoS>(
-      element.second, version);
+    // Using rosbag2_storage::Rosbag2QoS for decoding because rclcpp::QoS is not default
+    // constructable. Note: It is safe to use upcast when inserting into the unordered_map
+    rhs.insert(
+      {element.first.as<std::string>(),
+        decode_for_version<rosbag2_storage::Rosbag2QoS>(element.second, version)
+      });
   }
   return true;
 }

rosbag2_transport/CMakeLists.txt

@@ -31,6 +31,7 @@ find_package(ament_cmake_ros REQUIRED)
 find_package(keyboard_handler REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(rcutils REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rosbag2_compression REQUIRED)
@@ -44,17 +45,20 @@ find_package(yaml_cpp_vendor REQUIRED)
 add_library(${PROJECT_NAME} SHARED
   src/rosbag2_transport/bag_rewrite.cpp
   src/rosbag2_transport/player.cpp
+  src/rosbag2_transport/play_options.cpp
   src/rosbag2_transport/reader_writer_factory.cpp
   src/rosbag2_transport/recorder.cpp
   src/rosbag2_transport/record_options.cpp
-  src/rosbag2_transport/topic_filter.cpp)
+  src/rosbag2_transport/topic_filter.cpp
+  src/rosbag2_transport/config_options_from_node_params.cpp)
 target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
 target_link_libraries(${PROJECT_NAME}
   keyboard_handler::keyboard_handler
   rcl::rcl
   rclcpp::rclcpp
+  ${rclcpp_components_TARGETS}
   rcutils::rcutils
   rmw::rmw
   rosbag2_compression::rosbag2_compression
@@ -65,6 +69,12 @@ target_link_libraries(${PROJECT_NAME}
   yaml-cpp
 )
 
+rclcpp_components_register_node(
+  ${PROJECT_NAME} PLUGIN "rosbag2_transport::Player" EXECUTABLE player)
+
+rclcpp_components_register_node(
+  ${PROJECT_NAME} PLUGIN "rosbag2_transport::Recorder" EXECUTABLE recorder)
+
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(${PROJECT_NAME} PRIVATE "ROSBAG2_TRANSPORT_BUILDING_LIBRARY")
@@ -89,6 +99,7 @@ ament_export_libraries(${PROJECT_NAME})
 ament_export_targets(export_${PROJECT_NAME})
 
 ament_export_dependencies(
+  rclcpp_components
   keyboard_handler
   rosbag2_cpp
   rosbag2_compression
@@ -109,6 +120,10 @@ function(create_tests_for_rmw_implementation)
     LINK_LIBS rosbag2_transport ${test_msgs_TARGETS} rosbag2_test_common::rosbag2_test_common
     ${SKIP_TEST})
 
+  rosbag2_transport_add_gmock(test_composable_player
+    test/rosbag2_transport/test_composable_player.cpp
+    LINK_LIBS rosbag2_transport ${test_msgs_TARGETS} rosbag2_test_common::rosbag2_test_common)
+
   rosbag2_transport_add_gmock(test_play_loop
     test/rosbag2_transport/test_play_loop.cpp
     LINK_LIBS rosbag2_transport ${test_msgs_TARGETS} rosbag2_test_common::rosbag2_test_common
@@ -203,10 +218,15 @@ function(create_tests_for_rmw_implementation)
     test/rosbag2_transport/test_record_services.cpp
     LINK_LIBS rosbag2_transport ${test_msgs_TARGETS} rosbag2_test_common::rosbag2_test_common)
 
-  rosbag2_transport_add_gmock(test_component_parameters
-      test/rosbag2_transport/test_composable_recorder.cpp
+  rosbag2_transport_add_gmock(test_composable_recorder
+    test/rosbag2_transport/test_composable_recorder.cpp
     LINK_LIBS rosbag2_transport ${test_msgs_TARGETS} rosbag2_test_common::rosbag2_test_common)
 
+  rosbag2_transport_add_gmock(test_load_composable_components
+    test/rosbag2_transport/test_load_composable_components.cpp
+    LINK_LIBS rosbag2_transport ${test_msgs_TARGETS} rosbag2_test_common::rosbag2_test_common
+    ${rclcpp_components_TARGETS})
+
   if(${rmw_implementation} MATCHES "rmw_cyclonedds(.*)")
     ament_add_test_label(test_play_services__rmw_cyclonedds_cpp xfail)
   endif()

rosbag2_transport/include/rosbag2_transport/config_options_from_node_params.hpp

@@ -0,0 +1,37 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROSBAG2_TRANSPORT__CONFIG_OPTIONS_FROM_NODE_PARAMS_HPP_
+#define ROSBAG2_TRANSPORT__CONFIG_OPTIONS_FROM_NODE_PARAMS_HPP_
+
+#include <memory>
+
+#include "rclcpp/node.hpp"
+#include "rosbag2_transport/play_options.hpp"
+#include "rosbag2_transport/record_options.hpp"
+#include "rosbag2_storage/storage_options.hpp"
+
+namespace rosbag2_transport
+{
+rosbag2_transport::PlayOptions
+get_play_options_from_node_params(rclcpp::Node & node);
+
+rosbag2_transport::RecordOptions
+get_record_options_from_node_params(rclcpp::Node & node);
+
+rosbag2_storage::StorageOptions
+get_storage_options_from_node_params(rclcpp::Node & node);
+}  // namespace rosbag2_transport
+
+#endif  // ROSBAG2_TRANSPORT__CONFIG_OPTIONS_FROM_NODE_PARAMS_HPP_

rosbag2_transport/include/rosbag2_transport/play_options.hpp

@@ -15,15 +15,16 @@
 #ifndef ROSBAG2_TRANSPORT__PLAY_OPTIONS_HPP_
 #define ROSBAG2_TRANSPORT__PLAY_OPTIONS_HPP_
 
+#include <chrono>
 #include <cstddef>
 #include <string>
 #include <unordered_map>
 #include <vector>
 
 #include "keyboard_handler/keyboard_handler.hpp"
-#include "rclcpp/duration.hpp"
-#include "rclcpp/time.hpp"
-#include "rclcpp/qos.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rosbag2_storage/yaml.hpp"
+#include "rosbag2_transport/visibility_control.hpp"
 
 namespace rosbag2_transport
 {
@@ -103,4 +104,15 @@ struct PlayOptions
 
 }  // namespace rosbag2_transport
 
+namespace YAML
+{
+template<>
+struct ROSBAG2_TRANSPORT_PUBLIC convert<rosbag2_transport::PlayOptions>
+{
+  static Node encode(const rosbag2_transport::PlayOptions & play_options);
+  static bool decode(const Node & node, rosbag2_transport::PlayOptions & play_options);
+};
+
+}  // namespace YAML
+
 #endif  // ROSBAG2_TRANSPORT__PLAY_OPTIONS_HPP_

rosbag2_transport/include/rosbag2_transport/player.hpp

@@ -80,18 +80,52 @@ class Player : public rclcpp::Node
   ROSBAG2_TRANSPORT_PUBLIC
   static constexpr callback_handle_t invalid_callback_handle = 0;
 
+  /// \brief Constructor and entry point for the composable player.
+  /// Will call Player(node_name, node_options) constructor with node_name = "rosbag2_player".
+  /// \param node_options Node options which will be used during construction of the underlying
+  /// node.
+  ROSBAG2_TRANSPORT_PUBLIC
+  explicit Player(const rclcpp::NodeOptions & node_options);
+
+  /// \brief Default constructor and entry point for the composable player.
+  /// Will construct Player class and initialize play_options, storage_options from node
+  /// parameters. At the end will call Player::play() to automatically start playback in a
+  /// separate thread.
+  /// \param node_name Name for the underlying node.
+  /// \param node_options Node options which will be used during construction of the underlying
+  /// node.
   ROSBAG2_TRANSPORT_PUBLIC
   explicit Player(
     const std::string & node_name = "rosbag2_player",
     const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
 
+  /// \brief Constructor which will construct Player class with provided parameters and default
+  /// rosbag2_cpp::reader and KeyboardHandler classes.
+  /// \note The KeyboardHandler class will be initialized with parameter which is disabling
+  /// signal handlers in it.
+  /// \param storage_options Storage options which will be applied to the rosbag2_cpp::reader
+  /// after construction.
+  /// \param play_options Playback settings for Player class.
+  /// \param node_name Name for the underlying node.
+  /// \param node_options Node options which will be used during construction of the underlying
+  /// node.
   ROSBAG2_TRANSPORT_PUBLIC
   Player(
     const rosbag2_storage::StorageOptions & storage_options,
     const rosbag2_transport::PlayOptions & play_options,
     const std::string & node_name = "rosbag2_player",
     const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
 
+  /// \brief Constructor which will construct Player class with provided parameters and default
+  /// KeyboardHandler class initialized with parameter which is disabling signal handlers in it.
+  /// \param reader Unique pointer to the rosbag2_cpp::Reader class which will be moved to the
+  /// internal instance of the Player class during construction.
+  /// \param storage_options Storage options which will be applied to the rosbag2_cpp::reader
+  /// after construction.
+  /// \param play_options Playback settings for Player class.
+  /// \param node_name Name for the underlying node.
+  /// \param node_options Node options which will be used during construction of the underlying
+  /// node.
   ROSBAG2_TRANSPORT_PUBLIC
   Player(
     std::unique_ptr<rosbag2_cpp::Reader> reader,
@@ -100,6 +134,16 @@ class Player : public rclcpp::Node
     const std::string & node_name = "rosbag2_player",
     const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
 
+  /// \brief Constructor which will construct Player class with provided parameters.
+  /// \param keyboard_handler Keyboard handler class uses to handle user input from keyboard.
+  /// \param reader Unique pointer to the rosbag2_cpp::Reader class which will be moved to the
+  /// internal instance of the Player class during construction.
+  /// \param storage_options Storage options which will be applied to the rosbag2_cpp::reader
+  /// after construction.
+  /// \param play_options Playback settings for Player class.
+  /// \param node_name Name for the underlying node.
+  /// \param node_options Node options which will be used during construction of the underlying
+  /// node.
   ROSBAG2_TRANSPORT_PUBLIC
   Player(
     std::unique_ptr<rosbag2_cpp::Reader> reader,
@@ -109,6 +153,7 @@ class Player : public rclcpp::Node
     const std::string & node_name = "rosbag2_player",
     const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
 
+  /// \brief Default destructor.
   ROSBAG2_TRANSPORT_PUBLIC
   virtual ~Player();
 
@@ -237,6 +282,16 @@ class Player : public rclcpp::Node
   ROSBAG2_TRANSPORT_PUBLIC
   size_t get_number_of_registered_on_play_msg_post_callbacks();
 
+  ROSBAG2_TRANSPORT_PUBLIC
+  /// \brief Getter for the currently stored storage options
+  /// \return Copy of the currently stored storage options
+  const rosbag2_storage::StorageOptions & get_storage_options();
+
+  ROSBAG2_TRANSPORT_PUBLIC
+  /// \brief Getter for the currently stored play options
+  /// \return Copy of the currently stored play options
+  const rosbag2_transport::PlayOptions & get_play_options();
+
 private:
   std::unique_ptr<PlayerImpl> pimpl_;
 };