Skip to content

Commit

Permalink
Make Player and Recorder Composable (#902) (#1419)
Browse files Browse the repository at this point in the history
* Squasgh to ease rebase

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Remove TODO for keyboard handlers

Signed-off-by: roncapat <[email protected]>
Signed-off-by: Patrick Roncagliolo <[email protected]>

* Change structure

Signed-off-by: roncapat <[email protected]>
Signed-off-by: Patrick Roncagliolo <[email protected]>

* Fix

Signed-off-by: roncapat <[email protected]>
Signed-off-by: Patrick Roncagliolo <[email protected]>

* Fix

Signed-off-by: roncapat <[email protected]>
Signed-off-by: Patrick Roncagliolo <[email protected]>

* QoS parsing

Signed-off-by: roncapat <[email protected]>
Signed-off-by: Patrick Roncagliolo <[email protected]>

* Fix

Signed-off-by: roncapat <[email protected]>
Signed-off-by: Patrick Roncagliolo <[email protected]>

* Uncrustify

Signed-off-by: roncapat <[email protected]>
Signed-off-by: Patrick Roncagliolo <[email protected]>

* Draft comparison of passed vs parsed params

Signed-off-by: roncapat <[email protected]>
Signed-off-by: Patrick Roncagliolo <[email protected]>

* Fix shared_from_this() issue, param file & paths

Signed-off-by: roncapat <[email protected]>
Signed-off-by: Patrick Roncagliolo <[email protected]>

* Fixes after rebase

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Fixes to handle durations

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Better test output

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Drafting record param test

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Fixing recorder issues

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Fixes

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Draft component load test

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Composition tests working

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Uncrustify

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Cpplint

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Cpplint

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Fix play_offset bug

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Fix storage defaults bug

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Get rid of temporal conversion crutches using map<string, Rosbag2QoS>

Signed-off-by: Michael Orlov <[email protected]>

* Use const ref to node in the options getter functions

- Also made a style clean up in getter functions

Signed-off-by: Michael Orlov <[email protected]>

* Cleanups in get_storage(/play/record)_options functions

Signed-off-by: Michael Orlov <[email protected]>

* Move RosBag2RecordTestFixture insight test_record_params.cpp

Signed-off-by: Michael Orlov <[email protected]>

* Rename overrides.yaml to the qos_profile_overrides.yaml file

Signed-off-by: Michael Orlov <[email protected]>

* Rename params_player.yaml to the player_node_params.yaml

Signed-off-by: Michael Orlov <[email protected]>

* Rename params_recorder.yaml to the recorder_node_params.yaml

Signed-off-by: Michael Orlov <[email protected]>

* Replace Rosbag2Duration by rclcpp::Duration

Signed-off-by: Michael Orlov <[email protected]>

* Cleanup in functions which are getting values from node parameters

- Address issues in duration and integer parameters conversion
- Introduce `declare_integer_node_params(..)` and
`get_duration_from_node_param(..)` helper functions

Signed-off-by: Michael Orlov <[email protected]>

* Bugfix. Adjust min-max ranges for get_duration_from_node_param(..)

- Also add expected range to the exception message

Signed-off-by: Michael Orlov <[email protected]>

* Initial comparisons

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Move component manager in fixture

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Uncrustify

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Remove residual AMENT_DEPS after merge

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Complete test_play_params

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Finish param tests

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Uncrustify

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Cleanups in player/recorder parameters and load components tests

Signed-off-by: Michael Orlov <[email protected]>

* Renames in player/recorder parameters and load components tests

Signed-off-by: Michael Orlov <[email protected]>

* Namespaced parameters

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Fix load_composable_components test

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Automatically start playback in "composable" Player constructor

- Added doxygen comments for Player's constructors

Signed-off-by: Michael Orlov <[email protected]>

* Add integration test for composable player

- Integration test will check that player can automatically play file
after composition

Signed-off-by: Michael Orlov <[email protected]>

* Add missing dependencies to the mock_player.hpp

Signed-off-by: Michael Orlov <[email protected]>

* Automatically start recording in "composable" Recorder constructor

- Added doxygen comments for Recorder's constructors

Signed-off-by: Michael Orlov <[email protected]>

* Adopt existent tests for auto starting recording after composition

- Add default "cdr" value for rmw_serialization_format node parameter

Signed-off-by: Michael Orlov <[email protected]>

* Add integration test for composable recorder

- Test verify that recorder can automatically start recording after
composition and record messages

Signed-off-by: Michael Orlov <[email protected]>

* Add missed parameters prefixes after rebase

Signed-off-by: Michael Orlov <[email protected]>

* Fix for failing test with wrong check for playback_until_timestamp

Signed-off-by: Michael Orlov <[email protected]>

* Fix for failing tests with wrong parameters deduction

- Adjust max bagfile size and duration due to the bug in the
 rc_yaml_param_parcer/parser.c
 Need to use srtoll() instead of the strol() in parser

Signed-off-by: Michael Orlov <[email protected]>

* Close recorder before trying to delete temp files on test destruction

Signed-off-by: Michael Orlov <[email protected]>

* Update rosbag2_transport/test/rosbag2_transport/composition_manager_test_fixture.hpp

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Update rosbag2_transport/test/rosbag2_transport/test_composable_recorder.cpp

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Update rosbag2_transport/CMakeLists.txt

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Update rosbag2_transport/CMakeLists.txt

Signed-off-by: Patrick Roncagliolo <[email protected]>

* Address warnings from Windows CI job in composable player and recorder

Signed-off-by: Michael Orlov <[email protected]>

* Update rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp

Co-authored-by: Michael Orlov <[email protected]>
Signed-off-by: Patrick Roncagliolo <[email protected]>

* Update rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp

Co-authored-by: Michael Orlov <[email protected]>
Signed-off-by: Patrick Roncagliolo <[email protected]>

---------

Signed-off-by: Patrick Roncagliolo <[email protected]>
Signed-off-by: roncapat <[email protected]>
Signed-off-by: Michael Orlov <[email protected]>
Co-authored-by: Michael Orlov <[email protected]>
Co-authored-by: Michael Orlov <[email protected]>
  • Loading branch information
3 people authored Dec 11, 2023
1 parent 0f3ebfe commit 0fcc839
Show file tree
Hide file tree
Showing 24 changed files with 1,553 additions and 74 deletions.
8 changes: 4 additions & 4 deletions rosbag2_storage/include/rosbag2_storage/qos.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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

Expand Down
38 changes: 37 additions & 1 deletion rosbag2_storage/include/rosbag2_storage/yaml.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
# pragma warning(pop)
#endif

#include "rclcpp/duration.hpp"
#include "rosbag2_storage/bag_metadata.hpp"
#include "rosbag2_storage/qos.hpp"

Expand All @@ -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);
}
}

Expand All @@ -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>
{
Expand Down
17 changes: 10 additions & 7 deletions rosbag2_storage/src/rosbag2_storage/qos.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#include <string>
#include <vector>


#include "rosbag2_storage/qos.hpp"
#include "rosbag2_storage/logging.hpp"
#include "rmw/qos_string_conversions.h"
Expand Down Expand Up @@ -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) {
Expand All @@ -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;
}
Expand Down
26 changes: 23 additions & 3 deletions rosbag2_transport/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand All @@ -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")
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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()
Expand Down
Original file line number Diff line number Diff line change
@@ -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_
18 changes: 15 additions & 3 deletions rosbag2_transport/include/rosbag2_transport/play_options.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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_
55 changes: 55 additions & 0 deletions rosbag2_transport/include/rosbag2_transport/player.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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,
Expand All @@ -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();

Expand Down Expand Up @@ -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_;
};
Expand Down
Loading

0 comments on commit 0fcc839

Please sign in to comment.