From b398a6bb29f667ad5d15a05f2d8fe82cea84d80f Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Fri, 3 Jan 2025 00:22:15 +0100 Subject: [PATCH] Add service to stop recording - Set bag name when recording is started (#6) Added feature: add service to stop recording --------- Signed-off-by: Beniamino Pozzan Co-authored-by: Bernd Pfrommer --- README.md | 10 +++- .../composable_recorder.hpp | 9 +++- src/composable_recorder.cpp | 51 ++++++++++++++++--- 3 files changed, 59 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 7f63128..a891bf0 100644 --- a/README.md +++ b/README.md @@ -45,11 +45,17 @@ Alternatively, use the python script: ``` ros2 run rosbag2_composable_recorder start_recording.py ``` -To stop the recording you have to kill (Ctrl-C) the recording driver. +To stop the recording, either kill (Ctrl-C) the recording driver or call the stop service: +``` +ros2 service call /stop_recording std_srvs/srv/Trigger +``` ## Parameters -- ``bag_name``: (default: empty) prefix of directory name used for bag. +- ``bag_name``: (default: empty) prefix of directory name used for + bag. Note: if you stop the recording and restart, the recorder node + will throw an exception and exit because the bag already + exists. Use ``bag_prefix`` in this case. - ``bag_prefix``: (default: ``rosbag_2``) prefix of directory name used for storage. A timestamp will be appended. This parameter is only used when no ``bag_name`` is specified. diff --git a/include/rosbag2_composable_recorder/composable_recorder.hpp b/include/rosbag2_composable_recorder/composable_recorder.hpp index e5e6dc7..ffa0a15 100644 --- a/include/rosbag2_composable_recorder/composable_recorder.hpp +++ b/include/rosbag2_composable_recorder/composable_recorder.hpp @@ -35,9 +35,16 @@ class ComposableRecorder : public rosbag2_transport::Recorder const std::shared_ptr req, std::shared_ptr res); + bool stopRecording( + const std::shared_ptr req, + std::shared_ptr res); + // ---- variables - rclcpp::Service::SharedPtr service_; + rclcpp::Service::SharedPtr start_service_; + rclcpp::Service::SharedPtr stop_service_; bool isRecording_{false}; + std::string bag_name_; + std::string bag_prefix_; }; } // namespace rosbag2_composable_recorder diff --git a/src/composable_recorder.cpp b/src/composable_recorder.cpp index 8746728..6ad2f96 100644 --- a/src/composable_recorder.cpp +++ b/src/composable_recorder.cpp @@ -37,7 +37,9 @@ ComposableRecorder::ComposableRecorder(const rclcpp::NodeOptions & options) : rosbag2_transport::Recorder( std::make_shared(), rosbag2_storage::StorageOptions(), rosbag2_transport::RecordOptions(), "recorder", - rclcpp::NodeOptions(options).start_parameter_event_publisher(false)) + rclcpp::NodeOptions(options).start_parameter_event_publisher(false)), + bag_name_(declare_parameter("bag_name", "")), + bag_prefix_(declare_parameter("bag_prefix", "rosbag2_")) { std::vector topics = declare_parameter>("topics", std::vector()); @@ -52,12 +54,6 @@ ComposableRecorder::ComposableRecorder(const rclcpp::NodeOptions & options) #endif sopt.storage_id = declare_parameter("storage_id", "sqlite3"); sopt.max_cache_size = declare_parameter("max_cache_size", 100 * 1024 * 1024); - const std::string bag_name = declare_parameter("bag_name", ""); - if (!bag_name.empty()) { - sopt.uri = bag_name; - } else { - sopt.uri = declare_parameter("bag_prefix", "rosbag2_") + get_time_stamp(); - } // set recorder options #ifdef USE_GET_RECORD_OPTIONS @@ -86,11 +82,15 @@ ComposableRecorder::ComposableRecorder(const rclcpp::NodeOptions & options) if (declare_parameter("start_recording_immediately", false)) { record(); } else { - service_ = create_service( + start_service_ = create_service( "start_recording", std::bind( &ComposableRecorder::startRecording, this, std::placeholders::_1, std::placeholders::_2)); } + stop_service_ = create_service( + "stop_recording", + std::bind( + &ComposableRecorder::stopRecording, this, std::placeholders::_1, std::placeholders::_2)); } bool ComposableRecorder::startRecording( @@ -104,6 +104,16 @@ bool ComposableRecorder::startRecording( res->message = "already recording!"; } else { RCLCPP_INFO(get_logger(), "starting recording..."); +#ifdef USE_GET_STORAGE_OPTIONS + rosbag2_storage::StorageOptions & sopt = get_storage_options(); +#else + rosbag2_storage::StorageOptions & sopt = storage_options_; +#endif + if (!bag_name_.empty()) { + sopt.uri = bag_name_; + } else { + sopt.uri = bag_prefix_ + get_time_stamp(); + } try { record(); isRecording_ = true; @@ -118,6 +128,31 @@ bool ComposableRecorder::startRecording( return (true); } +bool ComposableRecorder::stopRecording( + const std::shared_ptr req, + std::shared_ptr res) +{ + (void)req; + res->success = false; + if (!isRecording_) { + RCLCPP_WARN(get_logger(), "not recording yet!"); + res->message = "not recording yet!"; + } else { + RCLCPP_INFO(get_logger(), "stopping recording..."); + try { + stop(); + isRecording_ = false; + RCLCPP_INFO(get_logger(), "stopped recording successfully"); + res->success = true; + res->message = "stopped recoding!"; + } catch (const std::runtime_error & e) { + RCLCPP_WARN(get_logger(), "cannot toggle recording!"); + res->message = "runtime error occurred: " + std::string(e.what()); + } + } + return (true); +} + ComposableRecorder::~ComposableRecorder() {} } // namespace rosbag2_composable_recorder