diff --git a/rosbag2_transport/include/rosbag2_transport/recorder.hpp b/rosbag2_transport/include/rosbag2_transport/recorder.hpp index 27eb615474..4354aa8b16 100644 --- a/rosbag2_transport/include/rosbag2_transport/recorder.hpp +++ b/rosbag2_transport/include/rosbag2_transport/recorder.hpp @@ -30,6 +30,9 @@ #include "rosbag2_cpp/writer.hpp" +#include "rosbag2_interfaces/srv/is_paused.hpp" +#include "rosbag2_interfaces/srv/pause.hpp" +#include "rosbag2_interfaces/srv/resume.hpp" #include "rosbag2_interfaces/srv/snapshot.hpp" #include "rosbag2_interfaces/srv/split_bagfile.hpp" @@ -158,6 +161,9 @@ class Recorder : public rclcpp::Node std::string serialization_format_; std::unordered_map topic_qos_profile_overrides_; std::unordered_set topic_unknown_types_; + rclcpp::Service::SharedPtr srv_is_paused_; + rclcpp::Service::SharedPtr srv_pause_; + rclcpp::Service::SharedPtr srv_resume_; rclcpp::Service::SharedPtr srv_snapshot_; rclcpp::Service::SharedPtr srv_split_bagfile_; std::atomic paused_ = false; diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 72fe3aa594..84ae46b481 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -159,6 +159,36 @@ void Recorder::record() writer_->split_bagfile(); }); + srv_pause_ = create_service( + "~/pause", + [this]( + const std::shared_ptr/* request_header */, + const std::shared_ptr/* request */, + const std::shared_ptr/* response */) + { + pause(); + }); + + srv_resume_ = create_service( + "~/resume", + [this]( + const std::shared_ptr/* request_header */, + const std::shared_ptr/* request */, + const std::shared_ptr/* response */) + { + resume(); + }); + + srv_is_paused_ = create_service( + "~/is_paused", + [this]( + const std::shared_ptr/* request_header */, + const std::shared_ptr/* request */, + const std::shared_ptr response) + { + response->paused = is_paused(); + }); + // Start the thread that will publish events event_publisher_thread_ = std::thread(&Recorder::event_publisher_thread_main, this); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp index 66108d67a4..16d31835e7 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp @@ -21,6 +21,9 @@ #include "rclcpp/rclcpp.hpp" +#include "rosbag2_interfaces/srv/is_paused.hpp" +#include "rosbag2_interfaces/srv/pause.hpp" +#include "rosbag2_interfaces/srv/resume.hpp" #include "rosbag2_interfaces/srv/snapshot.hpp" #include "rosbag2_interfaces/srv/split_bagfile.hpp" #include "rosbag2_transport/recorder.hpp" @@ -36,6 +39,9 @@ using namespace ::testing; // NOLINT class RecordSrvsTest : public RecordIntegrationTestFixture { public: + using IsPaused = rosbag2_interfaces::srv::IsPaused; + using Pause = rosbag2_interfaces::srv::Pause; + using Resume = rosbag2_interfaces::srv::Resume; using Snapshot = rosbag2_interfaces::srv::Snapshot; using SplitBagfile = rosbag2_interfaces::srv::SplitBagfile; @@ -74,9 +80,11 @@ class RecordSrvsTest : public RecordIntegrationTestFixture pub_manager.setup_publisher(test_topic_, string_message, 10); const std::string ns = "/" + recorder_name_; + cli_is_paused_ = client_node_->create_client(ns + "/is_paused"); + cli_pause_ = client_node_->create_client(ns + "/pause"); + cli_resume_ = client_node_->create_client(ns + "/resume"); cli_snapshot_ = client_node_->create_client(ns + "/snapshot"); cli_split_bagfile_ = client_node_->create_client(ns + "/split_bagfile"); - exec_ = std::make_shared(); exec_->add_node(recorder_); @@ -133,6 +141,9 @@ class RecordSrvsTest : public RecordIntegrationTestFixture // Service clients rclcpp::Node::SharedPtr client_node_; + rclcpp::Client::SharedPtr cli_is_paused_; + rclcpp::Client::SharedPtr cli_pause_; + rclcpp::Client::SharedPtr cli_resume_; rclcpp::Client::SharedPtr cli_snapshot_; rclcpp::Client::SharedPtr cli_split_bagfile_; @@ -198,3 +209,20 @@ TEST_F(RecordSrvsTest, split_bagfile) EXPECT_EQ(closed_file, "BagFile0"); EXPECT_EQ(opened_file, "BagFile1"); } + +TEST_F(RecordSrvsTest, pause_resume) +{ + EXPECT_FALSE(recorder_->is_paused()); + auto is_paused_response = successful_service_request(cli_is_paused_); + EXPECT_FALSE(is_paused_response->paused); + + successful_service_request(cli_pause_); + EXPECT_TRUE(recorder_->is_paused()); + is_paused_response = successful_service_request(cli_is_paused_); + EXPECT_TRUE(is_paused_response->paused); + + successful_service_request(cli_resume_); + EXPECT_FALSE(recorder_->is_paused()); + is_paused_response = successful_service_request(cli_is_paused_); + EXPECT_FALSE(is_paused_response->paused); +}