├── hector_recorder ├── src │ ├── modified │ │ ├── .clang-format │ │ └── recorder_impl.cpp │ ├── main.cpp │ ├── arg_parser.cpp │ ├── utils.cpp │ └── terminal_ui.cpp ├── include │ └── hector_recorder │ │ ├── modified │ │ ├── .clang-format │ │ ├── logging.hpp │ │ └── recorder_impl.hpp │ │ ├── arg_parser.hpp │ │ ├── utils.h │ │ ├── terminal_ui.hpp │ │ └── topic_information.hpp ├── env_hooks │ └── 50.scripts.bash.in ├── package.xml ├── config │ └── default.yaml ├── CMakeLists.txt └── scripts │ └── hector_recorder_completion.bash ├── .gitignore ├── media └── teaser.gif ├── hector_recorder_msgs ├── msg │ ├── RecorderStatus.msg │ └── TopicInfo.msg ├── CMakeLists.txt ├── package.xml └── LICENSE ├── .clang-format ├── README.md └── LICENSE /hector_recorder/src/modified/.clang-format: -------------------------------------------------------------------------------- 1 | DisableFormat: true -------------------------------------------------------------------------------- /hector_recorder/include/hector_recorder/modified/.clang-format: -------------------------------------------------------------------------------- 1 | DisableFormat: true -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | .idea/ 3 | cmake-build-debug/ 4 | react/node_modules/ 5 | react/build/ -------------------------------------------------------------------------------- /media/teaser.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tu-darmstadt-ros-pkg/hector_recorder/HEAD/media/teaser.gif -------------------------------------------------------------------------------- /hector_recorder/env_hooks/50.scripts.bash.in: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | source @CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/scripts/hector_recorder_completion.bash -------------------------------------------------------------------------------- /hector_recorder_msgs/msg/RecorderStatus.msg: -------------------------------------------------------------------------------- 1 | string output_dir 2 | string config_path 3 | string[] files 4 | hector_recorder_msgs/TopicInfo[] topics 5 | builtin_interfaces/Duration duration 6 | int64 size -------------------------------------------------------------------------------- /hector_recorder_msgs/msg/TopicInfo.msg: -------------------------------------------------------------------------------- 1 | string topic 2 | # string[] types 3 | int64 msg_count 4 | float64 frequency # Hz 5 | int64 size # bytes 6 | int64 bandwidth # bytes/s 7 | # uint8 publishers 8 | # uint8 subscribers -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: LLVM 2 | AllowShortBlocksOnASingleLine: Always 3 | AllowShortLoopsOnASingleLine: true 4 | AlwaysBreakTemplateDeclarations: Yes 5 | BreakBeforeBraces: Linux 6 | ColumnLimit: 100 7 | PenaltyBreakComment: 1000 8 | PenaltyExcessCharacter: 10 9 | SpaceAfterTemplateKeyword: false 10 | SpaceInEmptyBlock: true 11 | SpacesInConditionalStatement: true 12 | SpacesInParentheses: true 13 | UseTab: Never 14 | -------------------------------------------------------------------------------- /hector_recorder_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(hector_recorder_msgs) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rosidl_default_generators REQUIRED) 11 | find_package(std_msgs REQUIRED) 12 | 13 | rosidl_generate_interfaces(${PROJECT_NAME} 14 | "msg/RecorderStatus.msg" 15 | "msg/TopicInfo.msg" 16 | DEPENDENCIES std_msgs) 17 | 18 | ament_export_dependencies(rosidl_default_runtime) 19 | 20 | # Ensure interfaces are installed 21 | install( 22 | DIRECTORY msg 23 | DESTINATION share/${PROJECT_NAME} 24 | ) 25 | 26 | ament_package() 27 | -------------------------------------------------------------------------------- /hector_recorder/include/hector_recorder/arg_parser.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "CLI11.hpp" 4 | #include "rosbag2_storage/storage_options.hpp" 5 | #include "rosbag2_transport/record_options.hpp" 6 | #include 7 | 8 | #include 9 | 10 | namespace hector_recorder 11 | { 12 | 13 | class ArgParser 14 | { 15 | 16 | public: 17 | ArgParser() = default; 18 | ~ArgParser() = default; 19 | 20 | void parseCommandLineArguments( int argc, char **argv, CustomOptions &custom_options, 21 | rosbag2_storage::StorageOptions &storage_options, 22 | rosbag2_transport::RecordOptions &record_options ); 23 | 24 | private: 25 | CLI::App parser; 26 | }; 27 | 28 | } // namespace hector_recorder -------------------------------------------------------------------------------- /hector_recorder_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | hector_recorder_msgs 5 | 0.0.0 6 | TODO: Package description 7 | Jonathan Lichtenfeld 8 | Apache-2.0 9 | 10 | ament_cmake 11 | rosidl_default_generators 12 | rosidl_default_runtime 13 | rosidl_interface_packages 14 | 15 | std_msgs 16 | 17 | ament_lint_auto 18 | ament_lint_common 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /hector_recorder/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | hector_recorder 5 | 0.0.0 6 | Server for recording ROS2 bag files 7 | Jonathan Lichtenfeld 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rosbag2_cpp 14 | rosbag2_transport 15 | rosbag2_storage 16 | rosbag2_interfaces 17 | hector_recorder_msgs 18 | libncurses-dev 19 | yaml-cpp 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /hector_recorder/include/hector_recorder/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "rosbag2_transport/recorder.hpp" 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace fs = std::filesystem; 19 | 20 | namespace hector_recorder 21 | { 22 | struct CustomOptions { 23 | std::string node_name; 24 | std::string config_path; 25 | std::string status_topic = "recorder_status"; 26 | bool publish_status; 27 | }; 28 | 29 | std::string getAbsolutePath( const std::string &path ); 30 | static std::string make_timestamped_folder_name(); 31 | static bool is_rosbag_dir( const fs::path &dir ); 32 | static fs::path find_rosbag_ancestor( const fs::path &dir ); 33 | std::string resolveOutputDirectory( const std::string &output_dir ); 34 | 35 | std::string formatMemory( uint64_t bytes ); 36 | std::string rateToString( double rate ); 37 | std::string bandwidthToString( double bandwidth ); 38 | int calculateRequiredLines( const std::vector &lines ); 39 | 40 | bool parseYamlConfig( CustomOptions &custom_options, rosbag2_transport::RecordOptions &record_options, 41 | rosbag2_storage::StorageOptions &storage_options ); 42 | std::string clipString( const std::string &str, int max_length ); 43 | void ensureLeadingSlash( std::vector &vector ); 44 | static std::string expandUserAndEnv( std::string s ); 45 | std::string resolveOutputUriToAbsolute( std::string &uri ); 46 | } // namespace hector_recorder -------------------------------------------------------------------------------- /hector_recorder/config/default.yaml: -------------------------------------------------------------------------------- 1 | node_name: "hector_recorder" 2 | output: "/home/user/bags" 3 | storage_id: "sqlite3" 4 | #max_bag_size: 0 5 | #max_bag_size_gb: 1.0 # alternative to max_bag_size; use either this or max_bag_size not both 6 | #max_bag_duration: 0 7 | #max_cache_size: 0 8 | #storage_preset_profile: "" 9 | #storage_config_uri: "" 10 | #snapshot_mode: false 11 | #start_time_ns: -1 12 | #end_time_ns: -1 13 | #custom_data: 14 | # "key1": "value1" 15 | # "key2": "value2" 16 | all_topics: true 17 | #all_topics: false 18 | #all_services: false 19 | #is_discovery_disabled: false 20 | #topics: 21 | #- "/topic1" 22 | #- "/topic2" 23 | #topic_types: 24 | #- "std_msgs/String" 25 | #services: 26 | #- "/service1" 27 | #- "/service2" 28 | #exclude_topics: 29 | #- "/excluded_topic" 30 | #exclude_topic_types: 31 | #- "std_msgs/Int32" 32 | #exclude_service_events: 33 | #- "/excluded_service_event" 34 | rmw_serialization_format: "cdr" 35 | #topic_polling_interval: 100 36 | #regex: "" 37 | #exclude_regex: "" 38 | #node_prefix: "" 39 | #compression_mode: "" 40 | #compression_format: "" 41 | #compression_queue_size: 0 42 | #compression_threads: 0 43 | #compression_threads_priority: 0 44 | #topic_qos_profile_overrides: 45 | #"/topic1": 46 | # history_depth: 10 47 | # reliability: "reliable" 48 | # durability: "volatile" 49 | #"/topic2": 50 | # history_depth: 5 51 | # reliability: "best_effort" 52 | # durability: "transient_local" 53 | #include_hidden_topics: false 54 | #include_unpublished_topics: false 55 | #ignore_leaf_topics: false 56 | #start_paused: false 57 | #use_sim_time: false 58 | #disable_keyboard_controls: false 59 | #publish_status: true 60 | #publish_status_topic: "recorder_status" 61 | -------------------------------------------------------------------------------- /hector_recorder/include/hector_recorder/modified/logging.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018, Bosch Software Innovations GmbH. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ROSBAG2_TRANSPORT__LOGGING_HPP_ 16 | #define ROSBAG2_TRANSPORT__LOGGING_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "rcutils/logging_macros.h" 22 | 23 | #define ROSBAG2_TRANSPORT_PACKAGE_NAME "ROSBAG2_TRANSPORT" 24 | 25 | // *INDENT-OFF* 26 | 27 | #define ROSBAG2_TRANSPORT_LOG_INFO(...) \ 28 | RCUTILS_LOG_INFO_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) 29 | 30 | #define ROSBAG2_TRANSPORT_LOG_INFO_STREAM(args) do { \ 31 | std::stringstream __ss; \ 32 | __ss << args; \ 33 | RCUTILS_LOG_INFO_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ 34 | } while (0) 35 | 36 | #define ROSBAG2_TRANSPORT_LOG_ERROR(...) \ 37 | RCUTILS_LOG_ERROR_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) 38 | 39 | #define ROSBAG2_TRANSPORT_LOG_ERROR_STREAM(args) do { \ 40 | std::stringstream __ss; \ 41 | __ss << args; \ 42 | RCUTILS_LOG_ERROR_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ 43 | } while (0) 44 | 45 | #define ROSBAG2_TRANSPORT_LOG_WARN(...) \ 46 | RCUTILS_LOG_WARN_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) 47 | 48 | #define ROSBAG2_TRANSPORT_LOG_WARN_STREAM(args) do { \ 49 | std::stringstream __ss; \ 50 | __ss << args; \ 51 | RCUTILS_LOG_WARN_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ 52 | } while (0) 53 | 54 | #define ROSBAG2_TRANSPORT_LOG_DEBUG(...) \ 55 | RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) 56 | 57 | #define ROSBAG2_TRANSPORT_LOG_DEBUG_STREAM(args) do { \ 58 | std::stringstream __ss; \ 59 | __ss << args; \ 60 | RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ 61 | } while (0) 62 | 63 | // *INDENT-ON* 64 | 65 | #endif // ROSBAG2_TRANSPORT__LOGGING_HPP_ 66 | -------------------------------------------------------------------------------- /hector_recorder/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(hector_recorder) 3 | 4 | # Find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rosbag2_cpp REQUIRED) 8 | find_package(rosbag2_transport REQUIRED) 9 | find_package(rosbag2_storage REQUIRED) 10 | find_package(rosbag2_interfaces REQUIRED) 11 | find_package(hector_recorder_msgs REQUIRED) 12 | find_package(fmt REQUIRED) 13 | find_package(Curses REQUIRED) 14 | find_package(yaml-cpp REQUIRED) 15 | 16 | set(LIBRARY_NAME hector_recorder_lib) 17 | # set(EXECUTABLE_NAME server) 18 | 19 | # Add library 20 | add_library(${LIBRARY_NAME} SHARED 21 | src/arg_parser.cpp 22 | src/modified/recorder_impl.cpp 23 | src/utils.cpp 24 | ) 25 | 26 | # Include directories 27 | target_include_directories(${LIBRARY_NAME} PUBLIC 28 | $ 29 | $) 30 | 31 | include_directories(include include/modified) 32 | 33 | # Link library 34 | ament_target_dependencies(${LIBRARY_NAME} PUBLIC 35 | rclcpp 36 | rosbag2_cpp 37 | rosbag2_transport 38 | rosbag2_storage 39 | rosbag2_interfaces 40 | hector_recorder_msgs 41 | ) 42 | 43 | add_executable(record 44 | src/main.cpp 45 | src/terminal_ui.cpp 46 | ) 47 | 48 | ament_target_dependencies(record 49 | rclcpp 50 | hector_recorder_msgs 51 | fmt 52 | ) 53 | 54 | target_link_libraries(record 55 | ${LIBRARY_NAME} 56 | ${rclcpp_LIBRARIES} 57 | fmt::fmt 58 | ${CURSES_LIBRARIES} 59 | yaml-cpp 60 | ) 61 | 62 | # Install 63 | install( 64 | DIRECTORY include/ 65 | DESTINATION include/${PROJECT_NAME} 66 | ) 67 | 68 | install (DIRECTORY config 69 | DESTINATION share/${PROJECT_NAME}/ 70 | ) 71 | 72 | install( 73 | TARGETS ${LIBRARY_NAME} 74 | EXPORT export_${PROJECT_NAME} 75 | LIBRARY DESTINATION lib 76 | ARCHIVE DESTINATION lib 77 | RUNTIME DESTINATION bin 78 | ) 79 | 80 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 81 | 82 | # Export information to downstream packages 83 | ament_export_dependencies( 84 | rclcpp 85 | rosbag2_cpp) 86 | 87 | 88 | # Install environment hooks 89 | ament_environment_hooks(env_hooks/50.scripts.bash.in) 90 | 91 | # Install directories 92 | install(DIRECTORY 93 | scripts 94 | env_hooks 95 | DESTINATION share/${PROJECT_NAME} 96 | ) 97 | 98 | ament_package() 99 | 100 | install(TARGETS record 101 | DESTINATION lib/${PROJECT_NAME}) -------------------------------------------------------------------------------- /hector_recorder/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "CLI11.hpp" 2 | #include "hector_recorder/arg_parser.hpp" 3 | #include "hector_recorder/terminal_ui.hpp" 4 | #include "hector_recorder/utils.h" 5 | #include "rclcpp/rclcpp.hpp" 6 | #include 7 | #include 8 | 9 | using namespace hector_recorder; 10 | 11 | int main( int argc, char **argv ) 12 | { 13 | ArgParser arg_parser; 14 | CustomOptions custom_options; 15 | rosbag2_storage::StorageOptions storage_options; 16 | rosbag2_transport::RecordOptions record_options; 17 | 18 | try { 19 | arg_parser.parseCommandLineArguments( argc, argv, custom_options, storage_options, 20 | record_options ); 21 | } catch ( const std::exception &e ) { 22 | RCLCPP_ERROR( rclcpp::get_logger( "hector_recorder.config.cli" ), "Error parsing CLI args: %s", 23 | e.what() ); 24 | return 0; 25 | } 26 | 27 | if ( !custom_options.config_path.empty() ) { 28 | if ( !hector_recorder::parseYamlConfig( custom_options, record_options, storage_options ) ) { 29 | return 0; // Exit if config parsing fails 30 | }; 31 | } 32 | 33 | if ( !record_options.all_topics && !record_options.all_services && 34 | record_options.topics.empty() && record_options.services.empty() && 35 | record_options.topic_types.empty() && record_options.regex.empty() ) { 36 | RCLCPP_ERROR( rclcpp::get_logger( "hector_recorder.config" ), 37 | "Need to specify at least one option out of --all, --all-topics, --all-services, " 38 | "--services, --topics, --topic-types, --regex or specify them in --config" ); 39 | return 0; // Exit if no valid recording options are provided 40 | } 41 | 42 | // Ensure that topics and services start with a slash 43 | ensureLeadingSlash( record_options.topics ); 44 | ensureLeadingSlash( record_options.services ); 45 | ensureLeadingSlash( record_options.exclude_topics ); 46 | ensureLeadingSlash( record_options.exclude_service_events ); 47 | 48 | // Add ROS-args: Disable stdout ros logs to prevent interference with terminal UI 49 | std::vector argv_storage( argv, argv + argc ); 50 | argv_storage.push_back( "--ros-args" ); 51 | argv_storage.push_back( "--disable-stdout-logs" ); 52 | 53 | std::vector new_argv; 54 | new_argv.reserve( argv_storage.size() ); 55 | for ( auto &s : argv_storage ) new_argv.push_back( s.data() ); 56 | 57 | int new_argc = static_cast( new_argv.size() ); 58 | rclcpp::init( new_argc, new_argv.data() ); 59 | 60 | auto ui_node = std::make_shared( custom_options, storage_options, record_options ); 61 | 62 | rclcpp::spin( ui_node ); 63 | rclcpp::shutdown(); 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /hector_recorder/include/hector_recorder/terminal_ui.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TERMINAL_UI_HPP 2 | #define TERMINAL_UI_HPP 3 | 4 | #include "hector_recorder/modified/recorder_impl.hpp" 5 | #include "hector_recorder/utils.h" 6 | #include "hector_recorder_msgs/msg/recorder_status.hpp" 7 | #include "hector_recorder_msgs/msg/topic_info.hpp" 8 | #include "rclcpp/rclcpp.hpp" 9 | #include "rosbag2_cpp/writers/sequential_writer.hpp" 10 | #include "rosbag2_storage/storage_options.hpp" 11 | #include "rosbag2_transport/record_options.hpp" 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace hector_recorder 21 | { 22 | 23 | class TerminalUI : public rclcpp::Node 24 | { 25 | public: 26 | TerminalUI( const hector_recorder::CustomOptions &custom_options, 27 | const rosbag2_storage::StorageOptions &storage_options, 28 | const rosbag2_transport::RecordOptions &record_options ); 29 | 30 | ~TerminalUI(); 31 | 32 | private: 33 | void initializeRecorder(); 34 | void initializeUI(); 35 | void drawBorders(); 36 | void cleanupUI(); 37 | void startRecording(); 38 | void stopRecording(); 39 | void updateUI(); 40 | void updateGeneralInfo(); 41 | void updateWarnings(); 42 | void updateSystemResources(); 43 | void updateTable(); 44 | std::vector> 45 | sortTopics( const std::unordered_map &topics_info ); 46 | void adjustColumnWidths(); 47 | void distributeRemainingWidth( size_t longest_topic_name, size_t longest_topic_type ); 48 | void renderTable(); 49 | void renderHeaders(); 50 | void renderSeperatorLine(); 51 | void renderTopicRow( const std::pair &topic ); 52 | std::tuple getTopicDetails( const std::string &topic_name ); 53 | void publishRecorderStatus(); 54 | 55 | std::unique_ptr recorder_; 56 | rosbag2_storage::StorageOptions storage_options_; 57 | rosbag2_transport::RecordOptions record_options_; 58 | size_t ui_topics_max_length_; 59 | size_t ui_topics_type_max_length_; 60 | std::vector> sorted_topics_; 61 | std::vector headers_; 62 | std::vector column_widths_; 63 | int max_width_; 64 | int row_; 65 | int total_width_; 66 | std::mutex data_mutex_; 67 | rclcpp::TimerBase::SharedPtr ui_refresh_timer_; 68 | rclcpp::TimerBase::SharedPtr status_pub_timer_; 69 | std::string config_path_; 70 | bool publish_status_; 71 | 72 | rclcpp::Publisher::SharedPtr status_pub_; 73 | 74 | WINDOW *generalInfoWin_; 75 | WINDOW *warningsWin_; 76 | WINDOW *resourcesWin_; 77 | WINDOW *tableWin_; 78 | }; 79 | 80 | std::string formatMemory( uint64_t bytes ); 81 | std::string rateToString( double rate ); 82 | std::string bandwidthToString( double bandwidth ); 83 | 84 | } // namespace hector_recorder 85 | 86 | #endif // TERMINAL_UI_HPP 87 | -------------------------------------------------------------------------------- /hector_recorder/include/hector_recorder/topic_information.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace hector_recorder 10 | { 11 | 12 | namespace ba = boost::accumulators; 13 | 14 | class TopicInformation 15 | { 16 | public: 17 | TopicInformation() 18 | : time_intervals_acc_( ba::tag::rolling_window::window_size = 100 ), 19 | size_acc_( ba::tag::rolling_window::window_size = 100 ), 20 | last_msg_stamp_( std::chrono::nanoseconds::zero() ) 21 | { 22 | } 23 | 24 | void update_statistics( const std::chrono::nanoseconds ×tamp, size_t size ) 25 | { 26 | if ( last_msg_stamp_ != std::chrono::nanoseconds::zero() ) { 27 | time_intervals_acc_( timestamp - last_msg_stamp_ ); 28 | } 29 | last_msg_stamp_ = timestamp; 30 | last_update_ = std::chrono::steady_clock::now(); 31 | 32 | size_acc_( size ); 33 | size_ += size; 34 | 35 | message_count_++; 36 | } 37 | 38 | void update_publisher_info( const std::string &type, int count, const std::string &qos_profile ) 39 | { 40 | topic_type_ = type; 41 | publisher_count_ = count; 42 | qos_ = qos_profile; 43 | } 44 | 45 | const std::string &topic_type() const { return topic_type_; } 46 | 47 | int publisher_count() const { return publisher_count_; } 48 | 49 | const std::string &qos_reliability() const { return qos_; } 50 | 51 | size_t message_count() const { return message_count_; } 52 | 53 | size_t size() const { return size_; } 54 | 55 | double mean_frequency() const 56 | { 57 | if ( message_count_ < 2 ) { 58 | return 0.0; 59 | } 60 | 61 | auto now = std::chrono::steady_clock::now(); 62 | auto diff = now - last_update_; 63 | double diff_in_seconds = std::chrono::duration_cast( diff ).count(); 64 | if ( diff_in_seconds > 3.0 ) { 65 | return 0.0; 66 | } 67 | 68 | return 1.0 / ( ba::rolling_mean( time_intervals_acc_ ).count() / 1e9 ); // nanoseconds to seconds 69 | } 70 | 71 | size_t bandwidth() const 72 | { 73 | if ( message_count_ < 2 ) { 74 | return 0; 75 | } 76 | 77 | auto now = std::chrono::steady_clock::now(); 78 | auto diff = now - last_update_; 79 | double diff_in_seconds = std::chrono::duration_cast( diff ).count(); 80 | if ( diff_in_seconds > 3.0 ) { 81 | return 0; 82 | } 83 | 84 | double freq = mean_frequency(); 85 | return ba::rolling_mean( size_acc_ ) * freq; 86 | } 87 | 88 | private: 89 | size_t message_count_ = 0; 90 | size_t size_ = 0; 91 | std::string topic_type_; 92 | int publisher_count_ = 0; 93 | std::string qos_; 94 | ba::accumulator_set> time_intervals_acc_; 95 | ba::accumulator_set> size_acc_; 96 | std::chrono::nanoseconds last_msg_stamp_; 97 | std::chrono::time_point last_update_; 98 | }; 99 | 100 | } // namespace hector_recorder 101 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # hector_recorder 2 | 3 | ![hector_recorder teaser](media/teaser.gif) 4 | 5 | A terminal UI for recording ROS2 bags (strongly inspired by [rosbag_fancy](https://github.com/xqms/rosbag_fancy)). 6 | 7 | Some of its features: 8 | - Sort by message count, topic type, frequency, bandwith, duration, disk size... 9 | - Adaptive display based on terminal window size 10 | - Same arguments as ```rosbag2``` ```(ros2 bag record)``` 11 | - Colors indicate 'no msgs received' (yellow) and 'no publisher' (red) 12 | - Specify arguments per command line or YAML file 13 | - Publishes status topic (optional) 14 | 15 | If you are familiar with `ros2 bag record`, you can use `hector_recorder` as a drop-in replacement with additional convenience. 16 | 17 | ## Requirements 18 | 19 | - ROS 2 (tested for jazzy) 20 | - ncurses 21 | - fmt 22 | - yaml-cpp 23 | ``` 24 | sudo apt update && 25 | sudo apt install libncurses-dev libfmt-dev libyaml-cpp-dev 26 | ``` 27 | 28 | ## Build 29 | 30 | ```bash 31 | # clone this repo into your ros2 workspace, then build: 32 | colcon build --packages-select hector_recorder hector_recorder_msgs 33 | source install/setup.bash 34 | ``` 35 | 36 | ## Usage 37 | ```bash 38 | bag_recorder 39 | ``` 40 | Place all ROS arguments at the end of the command line. 41 | ```bash 42 | bag_recorder --ros-args -r __ns:=my_namespace 43 | ``` 44 | We support almost all ```ros2 bag record``` arguments as explained in the [official documentation](https://github.com/ros2/rosbag2?tab=readme-ov-file#record). 45 | In addition, there is: 46 | 47 | --max-bag-size-gb Specify the split size in GB instead of bytes 48 | --publish-status If true, recorder stats will be published on a topic 49 | --config Load all parameters from a YAML file (see below for more details) 50 | 51 | By pressing keys 1-8 you can sort the table by the respective column. 52 | 53 | ### Examples 54 | - Record everything (all topics & services): 55 | ```bash 56 | bag_recorder --all 57 | ``` 58 | 59 | - Record specific topics: 60 | ```bash 61 | bag_recorder --topics /tf /odom 62 | ``` 63 | 64 | - Load from YAML: 65 | ```bash 66 | bag_recorder --config /path/to/config.yaml 67 | ``` 68 | 69 | ### Config file 70 | All arguments can be specified either via command line or in a config file. 71 | 72 | Example: 73 | ```yaml 74 | node_name: "my_node_name" # defaults to 'hector_recorder' 75 | output: "/tmp/bags" # will be normalized, timestamp subdir if directory 76 | topics: 77 | - "/tf" 78 | - "/odom" 79 | max_bag_duration: 60 # split the bag at 60s 80 | publish_status: true # publish hector_recorder_msgs status 81 | ``` 82 | 83 | See here for all available parameters and their default values: 84 | [hector_recorder/config/default.yaml](hector_recorder/config/default.yaml) 85 | 86 | ### Directory resolution 87 | - If ```--output/-o``` is not specified, a timestamped folder in the current directory is created. 88 | - ```-o some_dir``` creates ```some_dir``` (works with absolute/relative paths) 89 | - If you want to have timestamped bag files in a specified log dir (useful for automatic logging), you can append a slash: 90 | ```-o some_dir/``` creates ```some_dir/rosbag_``` 91 | 92 | ### TODOs 93 | - [ ] Add/test qos-profile-overrides-path 94 | 95 | 96 | #### Acknowledgement 97 | This project includes components from: 98 | - ROS 2 [rosbag2](https://github.com/ros2/rosbag2) 99 | - [CLI11](https://github.com/CLIUtils/CLI11) by Henry Schreiner (BSD-3-Clause) -------------------------------------------------------------------------------- /hector_recorder/scripts/hector_recorder_completion.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | bag_recorder() { 4 | ros2 run hector_recorder record "$@" 5 | } 6 | 7 | # Autocompletion-function 8 | _bag_recorder_completion() { 9 | local cur prev opts 10 | COMPREPLY=() 11 | 12 | # Get current and previous word (bash-completion helper) 13 | if declare -F _get_comp_words_by_ref >/dev/null 2>&1; then 14 | _get_comp_words_by_ref -n : cur prev 15 | else 16 | cur="${COMP_WORDS[COMP_CWORD]}" 17 | prev="${COMP_WORDS[COMP_CWORD-1]}" 18 | fi 19 | 20 | # List of all args for the command. 21 | opts=( 22 | -o --output 23 | -s --storage 24 | -t --topics 25 | --services 26 | --topic-types 27 | -a --all 28 | --all-topics 29 | --all-services 30 | -e --regex 31 | --exclude-regex 32 | --exclude-topic-types 33 | --exclude-topics 34 | --exclude-services 35 | --include-unpublished-topics 36 | --include-hidden-topics 37 | --no-discovery 38 | -p --polling-interval 39 | --publish-status 40 | --ignore-leaf-topics 41 | -f --serialization-format 42 | -b --max-bag-size 43 | --gb --max-bag-size-gb 44 | -d --max-bag-duration 45 | --max-cache-size 46 | --disable-keyboard-controls 47 | --start-paused 48 | --use-sim-time 49 | --node-name 50 | --custom-data 51 | --snapshot-mode 52 | --compression-queue-size 53 | --compression-threads 54 | --compression-threads-priority 55 | --compression-mode 56 | --compression-format 57 | -c --config 58 | -h --help 59 | --publish-status-topic 60 | --ros-args 61 | ) 62 | 63 | # Options that accept multiple values 64 | is_multi_value_option() { 65 | case "$1" in 66 | -t|--topics|--exclude-topics|--services|--exclude-services|--topic-types|--exclude-topic-types) 67 | return 0 ;; 68 | *) return 1 ;; 69 | esac 70 | } 71 | 72 | # Determine if we are currently completing values for a multi-value option 73 | local current_option="" 74 | local i 75 | for (( i=COMP_CWORD-1; i >= 0; i-- )); do 76 | local word="${COMP_WORDS[i]}" 77 | if [[ "$word" == -* ]]; then 78 | if is_multi_value_option "$word"; then 79 | current_option="$word" 80 | fi 81 | break 82 | fi 83 | done 84 | 85 | # Path completions for specific options 86 | case "$prev" in 87 | -o|--output|-s|--storage) 88 | COMPREPLY=( $(compgen -f -- "$cur") ) 89 | return 0 90 | ;; 91 | -c|--config) 92 | COMPREPLY=( $(compgen -f -- "$cur" | xargs -I {} bash -c '[[ -f "{}" ]] && echo "{}"') ) 93 | return 0 94 | ;; 95 | esac 96 | 97 | # If starting a new option, suggest options 98 | if [[ "$cur" == -* ]]; then 99 | COMPREPLY=( $(compgen -W "${opts[*]}" -- "$cur") ) 100 | return 0 101 | fi 102 | 103 | # Completions for multi-value options 104 | case "$current_option" in 105 | -t|--topics|--exclude-topics) 106 | COMPREPLY=( $(compgen -W "$(ros2 topic list 2>/dev/null)" -- "$cur") ) 107 | return 0 108 | ;; 109 | --services|--exclude-services) 110 | COMPREPLY=( $(compgen -W "$(ros2 service list 2>/dev/null)" -- "$cur") ) 111 | return 0 112 | ;; 113 | --topic-types|--exclude-topic-types) 114 | COMPREPLY=( $(compgen -W "$(ros2 topic list -t 2>/dev/null | awk '{print $2}' | sort -u)" -- "$cur") ) 115 | return 0 116 | ;; 117 | esac 118 | 119 | # Default: no completion 120 | return 0 121 | } 122 | 123 | # Autocomplete-registration 124 | complete -F _bag_recorder_completion bag_recorder -------------------------------------------------------------------------------- /hector_recorder/include/hector_recorder/modified/recorder_impl.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Changelog Jonathan Lichtenfeld 12.6.2024: 16 | // - Extract RecorderImpl from rosbag2_transport/recorder.cpp to its own file 17 | // - Add get_topics_info(), get_bagfile_duration() 18 | 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "rcutils/allocator.h" 31 | 32 | #include "rclcpp/logging.hpp" 33 | #include "rclcpp/clock.hpp" 34 | 35 | #include "rmw/types.h" 36 | 37 | #include "rosbag2_cpp/bag_events.hpp" 38 | #include "rosbag2_cpp/writer.hpp" 39 | #include "rosbag2_cpp/service_utils.hpp" 40 | 41 | #include "rosbag2_interfaces/srv/is_paused.hpp" 42 | #include "rosbag2_interfaces/srv/pause.hpp" 43 | #include "rosbag2_interfaces/srv/resume.hpp" 44 | #include "rosbag2_interfaces/srv/snapshot.hpp" 45 | #include "rosbag2_interfaces/srv/split_bagfile.hpp" 46 | 47 | #include "rosbag2_interfaces/msg/write_split_event.hpp" 48 | 49 | #include "rosbag2_storage/yaml.hpp" 50 | #include "rosbag2_storage/qos.hpp" 51 | 52 | #include "logging.hpp" 53 | #include "rosbag2_transport/config_options_from_node_params.hpp" 54 | #include "rosbag2_transport/topic_filter.hpp" 55 | #include "rosbag2_transport/visibility_control.hpp" 56 | 57 | #include "hector_recorder/topic_information.hpp" 58 | 59 | 60 | namespace hector_recorder 61 | { 62 | 63 | class RecorderImpl 64 | { 65 | public: 66 | RecorderImpl( 67 | rclcpp::Node * owner, 68 | std::shared_ptr writer, 69 | const rosbag2_storage::StorageOptions & storage_options, 70 | const rosbag2_transport::RecordOptions & record_options); 71 | 72 | ~RecorderImpl(); 73 | 74 | void record(); 75 | 76 | /// @brief Stopping recording and closing writer. 77 | /// The record() can be called again after stop(). 78 | void stop(); 79 | 80 | void split(); 81 | 82 | const rosbag2_cpp::Writer & get_writer_handle(); 83 | 84 | /// Pause the recording. 85 | void pause(); 86 | 87 | /// Start discovery 88 | void start_discovery(); 89 | 90 | /// Stop discovery 91 | void stop_discovery(); 92 | 93 | const std::unordered_map & get_topics_info(); 94 | 95 | const rclcpp::Duration get_bagfile_duration() const; 96 | 97 | const uint64_t get_bagfile_size() const; 98 | 99 | const std::vector & get_files() const; 100 | 101 | std::unordered_map get_requested_or_available_topics(); 102 | 103 | bool is_recording() const { return in_recording_; } 104 | 105 | /// Public members for access by wrapper 106 | std::unordered_set topics_warned_about_incompatibility_; 107 | std::shared_ptr writer_; 108 | rosbag2_storage::StorageOptions storage_options_; 109 | rosbag2_transport::RecordOptions record_options_; 110 | std::unordered_map> subscriptions_; 111 | 112 | void update_topic_publisher_info(); 113 | 114 | private: 115 | void topics_discovery(); 116 | 117 | std::unordered_map 118 | get_missing_topics(const std::unordered_map & all_topics); 119 | 120 | std::vector get_unknown_topics() const; 121 | 122 | void subscribe_topics( 123 | const std::unordered_map & topics_and_types); 124 | 125 | void subscribe_topic(const rosbag2_storage::TopicMetadata & topic); 126 | 127 | std::shared_ptr create_subscription( 128 | const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos); 129 | 130 | void update_topic_statistics(const std::string & topic_name, std::chrono::nanoseconds stamp, int size); 131 | 132 | /** 133 | * Find the QoS profile that should be used for subscribing. 134 | * 135 | * Uses the override from record_options, if it is specified for this topic. 136 | * Otherwise, falls back to Rosbag2QoS::adapt_request_to_offers 137 | * 138 | * \param topic_name The full name of the topic, with namespace (ex. /arm/joint_status). 139 | * \return The QoS profile to be used for subscribing. 140 | */ 141 | rclcpp::QoS subscription_qos_for_topic(const std::string & topic_name) const; 142 | 143 | // Get all currently offered QoS profiles for a topic. 144 | std::vector offered_qos_profiles_for_topic( 145 | const std::vector & topics_endpoint_info) const; 146 | 147 | void warn_if_new_qos_for_subscribed_topic(const std::string & topic_name); 148 | 149 | void event_publisher_thread_main(); 150 | bool event_publisher_thread_should_wake(); 151 | 152 | rclcpp::Node * node; 153 | std::unique_ptr topic_filter_; 154 | std::future discovery_future_; 155 | std::string serialization_format_; 156 | std::unordered_map topic_qos_profile_overrides_; 157 | std::unordered_set topic_unknown_types_; 158 | 159 | std::mutex start_stop_transition_mutex_; 160 | std::mutex discovery_mutex_; 161 | std::atomic stop_discovery_ = false; 162 | std::atomic_uchar paused_ = 0; 163 | std::atomic in_recording_ = false; 164 | 165 | // Variables for event publishing 166 | rclcpp::Publisher::SharedPtr split_event_pub_; 167 | std::atomic event_publisher_thread_should_exit_ = false; 168 | std::atomic write_split_has_occurred_ = false; 169 | rosbag2_cpp::bag_events::BagSplitInfo bag_split_info_; 170 | std::mutex event_publisher_thread_mutex_; 171 | std::condition_variable event_publisher_thread_wake_cv_; 172 | std::thread event_publisher_thread_; 173 | 174 | // Topic metadata 175 | std::unordered_map topics_info_; 176 | rclcpp::Time first_stamp_; 177 | bool first_msg_received_ = false; 178 | std::vector files_; 179 | }; 180 | 181 | std::string type_hash_to_string(const rosidl_type_hash_t & type_hash); 182 | // Retrieve the type description hash from endpoint info. 183 | std::string type_description_hash_for_topic( 184 | const std::vector & topics_endpoint_info); 185 | std::string reliability_to_string( 186 | const rclcpp::ReliabilityPolicy & reliability); 187 | 188 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /hector_recorder_msgs/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /hector_recorder/src/arg_parser.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace hector_recorder; 4 | 5 | void ArgParser::parseCommandLineArguments( int argc, char **argv, CustomOptions &custom_options, 6 | rosbag2_storage::StorageOptions &storage_options, 7 | rosbag2_transport::RecordOptions &record_options ) 8 | { 9 | // Find --ros-args in argv 10 | int cli_argc = argc; 11 | for ( int i = 1; i < argc; ++i ) { 12 | if ( std::string( argv[i] ) == "--ros-args" ) { 13 | cli_argc = i; // Only parse up to --ros-args 14 | break; 15 | } 16 | } 17 | 18 | // Default values for custom options 19 | custom_options.node_name = "hector_recorder"; 20 | custom_options.config_path = ""; 21 | custom_options.publish_status = false; 22 | 23 | // Tmp values 24 | std::string output_dir; 25 | std::string storage_id; 26 | bool all_flag = false; 27 | 28 | std::vector writer_choices = { "sqlite3", "mcap" }; 29 | std::string default_writer = "sqlite3"; 30 | 31 | float max_bag_size_gb = 0.0; 32 | 33 | argv = parser.ensure_utf8( argv ); 34 | 35 | // Create a group for all manual options 36 | auto manual_group = parser.add_option_group( "manual", "Manual configuration options" ); 37 | 38 | // Add all your options to the group instead of directly to parser 39 | parser 40 | .add_option( 41 | "-o,--output", 42 | output_dir, "Destination of the bagfile to create, defaults to a timestamped folder in the current directory." ) 43 | ->default_val( "" ); 44 | 45 | parser 46 | .add_option( "-s,--storage", storage_id, 47 | "Storage identifier to be used, defaults to '" + default_writer + "'." ) 48 | ->default_val( default_writer ) 49 | ->check( CLI::IsMember( writer_choices ) ); 50 | 51 | parser 52 | .add_option( "-t,--topics", record_options.topics, "Space-delimited list of topics to record." ) 53 | ->expected( 1, -1 ) 54 | ->delimiter( ' ' ); 55 | 56 | parser 57 | .add_option( "--services", record_options.services, 58 | "Space-delimited list of services to record." ) 59 | ->expected( 1, -1 ) 60 | ->delimiter( ' ' ); 61 | 62 | parser 63 | .add_option( "--topic-types", record_options.topic_types, 64 | "Space-delimited list of topic types to record." ) 65 | ->expected( 1, -1 ) 66 | ->delimiter( ' ' ); 67 | 68 | parser 69 | .add_flag( "-a,--a,--all", all_flag, "Record all topics and services. (Exclude hidden topic)" ) 70 | ->default_val( false ); 71 | 72 | parser 73 | .add_flag( "--all-topics", record_options.all_topics, 74 | "Record all topics (Exclude hidden topic)." ) 75 | ->default_val( false ); 76 | 77 | parser 78 | .add_flag( "--all-services", record_options.all_services, 79 | "Record all services via service event topics." ) 80 | ->default_val( false ); 81 | 82 | parser 83 | .add_option( "-e,--e,--regex", record_options.regex, 84 | "Record only topics and services containing provided regular expression. Note: " 85 | "--all, --all-topics, --all-services or --all-actions will override --regex." ) 86 | ->default_val( "" ); 87 | 88 | parser 89 | .add_option( "--exclude-regex", record_options.exclude_regex, 90 | "Exclude topics and services containing provided regular expression. Works on " 91 | "top of --all, --all-topics, --all-services, --all-actions, --topics, " 92 | "--services, --actions or --regex." ) 93 | ->default_val( "" ); 94 | 95 | parser 96 | .add_option( "--exclude-topic-types", record_options.exclude_topic_types, 97 | "Space-delimited list of topic types not being recorded. Works on top of --all, " 98 | "--all-topics, --topics or --regex." ) 99 | ->expected( 1, -1 ) 100 | ->delimiter( ' ' ) 101 | ->default_val( "" ); 102 | 103 | parser 104 | .add_option( "--exclude-topics", record_options.exclude_topics, 105 | "Space-delimited list of topics not being recorded. Works on top of --all, " 106 | "--all-topics, --topics or --regex." ) 107 | ->expected( 1, -1 ) 108 | ->delimiter( ' ' ); 109 | 110 | parser 111 | .add_option( "--exclude-services", record_options.exclude_service_events, 112 | "Space-delimited list of services not being recorded. Works on top of --all, " 113 | "--all-services, --services or --regex." ) 114 | ->expected( 1, -1 ) 115 | ->delimiter( ' ' ); 116 | 117 | // Discovery behavior 118 | 119 | parser 120 | .add_flag( 121 | "--include-unpublished-topics", record_options.include_unpublished_topics, 122 | "Discover and record topics which have no publisher. Subscriptions on such topics will " 123 | "be made with default QoS unless otherwise specified in a QoS overrides file." ) 124 | ->default_val( false ); 125 | 126 | parser 127 | .add_flag( "--include-hidden-topics", record_options.include_hidden_topics, 128 | "Discover and record hidden topics as well. These are topics used internally by " 129 | "ROS 2 implementation." ) 130 | ->default_val( false ); 131 | 132 | parser 133 | .add_flag( "--no-discovery", record_options.is_discovery_disabled, 134 | "Disables topic auto discovery during recording: only topics present at startup " 135 | "will be recorded." ) 136 | ->default_val( false ); 137 | 138 | parser 139 | .add_option( "-p,--p,--polling-interval", record_options.topic_polling_interval, 140 | "Time in ms to wait between querying available topics for recording. It has no " 141 | "effect if --no-discovery is enabled." ) 142 | ->default_val( 100 ); 143 | 144 | parser 145 | .add_flag( "--publish-status", custom_options.publish_status, 146 | "Publish status to /recorder_status topic (default: false)." ) 147 | ->default_val( false ); 148 | 149 | parser 150 | .add_flag( "--ignore-leaf-topics", record_options.ignore_leaf_topics, 151 | "Ignore topics without a subscription." ) 152 | ->default_val( false ); 153 | 154 | // parser.add_option("--qos-profile-overrides-path", record_options.topic_qos_profile_overrides_path, "Path to a yaml file defining overrides of the QoS profile for specific topics.") 155 | // ->default_val(""); // TODO 156 | 157 | // Core Config 158 | 159 | parser 160 | .add_option( "-f,--f,--serialization-format", record_options.rmw_serialization_format, 161 | "The rmw serialization format in which the messages are saved." ) 162 | ->default_val( rmw_get_serialization_format() ); 163 | //->check(CLI::IsMember(serialization_choices)); 164 | 165 | auto max_size_opt = parser 166 | .add_option( "-b,--b,--max-bag-size", storage_options.max_bagfile_size, 167 | "Maximum size in bytes before the bagfile will be split. " 168 | "Default: %(default)d, recording written in single " 169 | "bagfile and splitting is disabled." ) 170 | ->default_val( 0 ); 171 | 172 | auto max_size_gb_opt = 173 | parser 174 | .add_option( 175 | "--gb,--max-bag-size-gb", max_bag_size_gb, 176 | "Maximum size in gigabytes before the bagfile will be split. Default: 0 (disabled)." ) 177 | ->default_val( 0.0 ); 178 | 179 | max_size_opt->excludes( max_size_gb_opt ); 180 | max_size_gb_opt->excludes( max_size_opt ); 181 | 182 | parser 183 | .add_option( "-d,--d,--max-bag-duration", storage_options.max_bagfile_duration, 184 | "Maximum duration in seconds before the bagfile will be split. " 185 | "Default: %(default)d, recording written in single bagfile and splitting is " 186 | "disabled. If both splitting by size and duration are enabled, the bag will " 187 | "split at whichever threshold is reached first." ) 188 | ->default_val( 0 ); 189 | 190 | parser 191 | .add_option( "--max-cache-size", storage_options.max_cache_size, 192 | "Maximum size (in bytes) of messages to hold in each buffer of cache. " 193 | "Default: %(default)d. The cache is handled through double buffering, which " 194 | "means that in pessimistic case up to twice the parameter value of memory is " 195 | "needed. A rule of thumb is to cache an order of magnitude corresponding to " 196 | "about one second of total recorded data volume. " 197 | "If the value specified is 0, then every message is directly written to disk." ) 198 | ->default_val( 100 * 1024 * 1024 ); 199 | 200 | parser 201 | .add_flag( "--disable-keyboard-controls", record_options.disable_keyboard_controls, 202 | "Disables keyboard controls for recorder." ) 203 | ->default_val( false ); 204 | 205 | parser 206 | .add_flag( "--start-paused", record_options.start_paused, 207 | "Start the recorder in a paused state." ) 208 | ->default_val( false ); 209 | 210 | parser 211 | .add_flag( "--use-sim-time", record_options.use_sim_time, 212 | "Use simulation time for message timestamps by subscribing to the /clock topic. " 213 | "Until first /clock message is received, no messages will be written to bag." ) 214 | ->default_val( false ); 215 | 216 | parser 217 | .add_option( "--node-name", custom_options.node_name, 218 | "Specify the recorder node name. Default is hector_recorder." ) 219 | ->default_val( "hector_recorder" ); 220 | 221 | parser 222 | .add_option( "--custom-data", storage_options.custom_data, 223 | "Space-delimited list of key=value pairs. Store the custom data in metadata " 224 | "under the 'rosbag2_bagfile_information/custom_data'. The key=value pair can " 225 | "appear more than once. The last value will override the former ones." ) 226 | ->expected( 1, -1 ) 227 | ->delimiter( ' ' ); 228 | 229 | parser 230 | .add_flag( "--snapshot-mode", storage_options.snapshot_mode, 231 | "Enable snapshot mode. Messages will not be written to the bagfile until the " 232 | "'/rosbag2_recorder/snapshot' service is called. e.g. \n ros2 service call " 233 | "/rosbag2_recorder/snapshot rosbag2_interfaces/Snapshot" ) 234 | ->default_val( false ); 235 | 236 | parser 237 | .add_option( "--compression-queue-size", record_options.compression_queue_size, 238 | "Number of files or messages that may be queued for compression before being " 239 | "dropped. Default is %(default)d." ) 240 | ->default_val( 1 ); 241 | 242 | parser 243 | .add_option( "--compression-threads", record_options.compression_threads, 244 | "Number of files or messages that may be compressed in parallel. Default is " 245 | "%(default)d, which will be interpreted as the number of CPU cores." ) 246 | ->default_val( 0 ); 247 | 248 | parser 249 | .add_option( 250 | "--compression-threads-priority", record_options.compression_threads_priority, 251 | "Compression threads scheduling priority. \nFor Windows the valid values are:" 252 | " THREAD_PRIORITY_LOWEST=-2, THREAD_PRIORITY_BELOW_NORMAL=-1 and" 253 | " THREAD_PRIORITY_NORMAL=0. Please refer to" 254 | " https://learn.microsoft.com/en-us/windows/win32/api/processthreadsapi/" 255 | "nf-processthreadsapi-setthreadpriority" 256 | " for details.\n" 257 | "For POSIX compatible OSes this is the 'nice' value. The nice value range is" 258 | " -20 to +19 where -20 is highest, 0 default and +19 is lowest." 259 | " Please refer to https://man7.org/linux/man-pages/man2/nice.2.html for details.\n" 260 | "Default is %(default)d." ) 261 | ->default_val( 0 ); 262 | 263 | parser 264 | .add_option( "--compression-mode", record_options.compression_mode, 265 | "Choose mode of compression for the storage. Default: %(default)s." ) 266 | ->default_val( "none" ) 267 | ->check( CLI::IsMember( { "none", "file", "message" } ) ); 268 | 269 | parser 270 | .add_option( "--compression-format", record_options.compression_format, 271 | "Choose the compression format/algorithm. Has no effect if no compression mode " 272 | "is chosen. Default: %(default)s." ) 273 | ->default_val( "" ); 274 | //->check(CLI::IsMember(get_registered_compressors())); 275 | 276 | parser 277 | .add_option( "--publish-status-topic", custom_options.status_topic, 278 | "The topic to publish the recorder status on. Default: %(default)s." ) 279 | ->default_val( "recorder_status" ); 280 | 281 | auto config_opt = 282 | parser 283 | .add_option( "-c,--c,--config", custom_options.config_path, 284 | "Path to a YAML configuration file. Mutually exclusive with other options." ) 285 | ->default_val( "" ) 286 | ->check( CLI::ExistingFile ); 287 | 288 | // Make config mutually exclusive with every other option except itself 289 | for ( auto &opt : parser.get_options() ) { 290 | if ( opt != config_opt ) { 291 | config_opt->excludes( opt ); 292 | } 293 | } 294 | 295 | try { 296 | // Use only the arguments before --ros-args for CLI11 297 | parser.parse( cli_argc, argv ); 298 | } catch ( const CLI::ParseError &e ) { 299 | std::exit( parser.exit( e ) ); 300 | } 301 | 302 | std::string final_output_dir; 303 | final_output_dir = hector_recorder::resolveOutputDirectory( output_dir ); 304 | // custom_options.resolved_output_dir = final_output_dir; 305 | storage_options.uri = final_output_dir; 306 | 307 | if ( all_flag ) { 308 | record_options.all_topics = true; 309 | record_options.all_services = true; 310 | } 311 | 312 | // logic to handle max_bag_size_gb 313 | if ( max_bag_size_gb > 0.0 ) { 314 | uint64_t max_bag_size_bytes_from_gb = 315 | static_cast( max_bag_size_gb * 1024 * 1024 * 1024 ); 316 | storage_options.max_bagfile_size = 317 | max_bag_size_bytes_from_gb; // Use the value from --max-bag-size-gb 318 | } 319 | } -------------------------------------------------------------------------------- /hector_recorder/src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "hector_recorder/utils.h" 2 | #include 3 | 4 | namespace hector_recorder 5 | { 6 | std::string getAbsolutePath( const std::string &path ) 7 | { 8 | try { 9 | return std::filesystem::canonical( path ); 10 | } catch ( const std::filesystem::filesystem_error &e ) { 11 | throw std::runtime_error( "Error resolving absolute path for '" + path + "': " + e.what() ); 12 | } 13 | } 14 | 15 | static std::string make_timestamped_folder_name() 16 | { 17 | auto now = std::chrono::system_clock::now(); 18 | std::time_t t = std::chrono::system_clock::to_time_t( now ); 19 | std::tm lt{}; 20 | #if defined( _WIN32 ) 21 | localtime_s( <, &t ); 22 | #else 23 | localtime_r( &t, < ); 24 | #endif 25 | std::ostringstream oss; 26 | oss << "rosbag2_" << std::put_time( <, "%Y_%m_%d-%H_%M_%S" ); 27 | return oss.str(); 28 | } 29 | 30 | static bool is_rosbag_dir( const fs::path &dir ) 31 | { 32 | const fs::path meta = dir / "metadata.yaml"; 33 | return fs::exists( meta ) && fs::is_regular_file( meta ); 34 | } 35 | 36 | static fs::path find_rosbag_ancestor( const fs::path &dir ) 37 | { 38 | if ( dir.empty() ) 39 | return {}; 40 | 41 | fs::path p = dir; 42 | while ( true ) { 43 | if ( fs::exists( p ) && fs::is_directory( p ) && is_rosbag_dir( p ) ) { 44 | return p; 45 | } 46 | fs::path parent = p.parent_path(); 47 | 48 | // Important: If parent is the same as p, we are at the root directory 49 | if ( parent == p || parent.empty() ) { 50 | break; 51 | } 52 | p = parent; 53 | } 54 | return {}; 55 | } 56 | 57 | std::string resolveOutputDirectory( const std::string &output_dir ) 58 | { 59 | const fs::path cwd = fs::current_path(); 60 | const std::string ts = make_timestamped_folder_name(); 61 | const bool had_trailing_sep = !output_dir.empty() && ( output_dir.back() == '/' ); 62 | 63 | fs::path target; // ← This directory will be created by rosbag2 64 | 65 | if ( output_dir.empty() ) { 66 | // No output_dir → CWD/rosbag2_ 67 | target = cwd / ts; 68 | } else { 69 | fs::path p = fs::path( output_dir ).lexically_normal(); 70 | const bool exists = fs::exists( p ); 71 | 72 | if ( exists ) { 73 | if ( !fs::is_directory( p ) ) { 74 | throw std::runtime_error( "Specified output path exists but is not a directory: " + 75 | p.string() ); 76 | } 77 | // Container-Directory cannot be a rosbag directory 78 | if ( is_rosbag_dir( p ) ) { 79 | throw std::runtime_error( "Cannot use an existing rosbag directory as container: " + 80 | p.string() ); 81 | } 82 | // Existing directory → always timestamped rosbag directory 83 | target = p / ts; 84 | } else { 85 | fs::path par = p.parent_path(); 86 | if ( par.empty() ) { 87 | // Only one name → Bag under CWD/ 88 | target = cwd / p; 89 | } else { 90 | if ( fs::exists( par ) && !fs::is_directory( par ) ) { 91 | throw std::runtime_error( "Specified output parent exists but is not a directory: " + 92 | par.string() ); 93 | } 94 | // Trailing Slash signals Container-Semantic → timestamped directory 95 | // Else: Just use the name as bag directory 96 | target = had_trailing_sep ? ( p / ts ) : p; 97 | } 98 | } 99 | } 100 | 101 | const fs::path container = target.parent_path(); 102 | 103 | // Impede placing a new rosbag inside an existing rosbag directory 104 | if ( !container.empty() ) { 105 | if ( fs::path bad = find_rosbag_ancestor( container ); !bad.empty() ) { 106 | throw std::runtime_error( "Cannot place a new rosbag inside an existing rosbag directory: " + 107 | bad.string() ); 108 | } 109 | } 110 | 111 | // Target directory must not exist yet 112 | if ( fs::exists( target ) ) { 113 | throw std::runtime_error( "Target directory already exists: " + target.string() ); 114 | } 115 | 116 | // Create the parent directory if it does not exist 117 | try { 118 | if ( !container.empty() && !fs::exists( container ) ) { 119 | fs::create_directories( container ); 120 | } 121 | } catch ( const fs::filesystem_error &e ) { 122 | throw std::runtime_error( "Failed to create parent directories for '" + container.string() + 123 | "': " + e.what() ); 124 | } 125 | 126 | return target.string(); 127 | } 128 | 129 | /** 130 | * @brief Formats memory size in human-readable units (B, KiB, MiB, etc.). 131 | * @param bytes The memory size in bytes. 132 | * @return A formatted string representing the memory size. 133 | */ 134 | std::string formatMemory( uint64_t bytes ) 135 | { 136 | if ( bytes < ( 1ull << 10 ) ) 137 | return fmt::format( "{:.1f} B", static_cast( bytes ) ); 138 | else if ( bytes < ( 1ull << 20 ) ) 139 | return fmt::format( "{:.1f} KiB", static_cast( bytes ) / ( 1ull << 10 ) ); 140 | else if ( bytes < ( 1ull << 30 ) ) 141 | return fmt::format( "{:.1f} MiB", static_cast( bytes ) / ( 1ull << 20 ) ); 142 | else if ( bytes < ( 1ull << 40 ) ) 143 | return fmt::format( "{:.1f} GiB", static_cast( bytes ) / ( 1ull << 30 ) ); 144 | else 145 | return fmt::format( "{:.1f} TiB", static_cast( bytes ) / ( 1ull << 40 ) ); 146 | } 147 | 148 | /** 149 | * @brief Converts a frequency value to a human-readable string (Hz, kHz, MHz). 150 | * @param rate The frequency in Hz. 151 | * @return A formatted string representing the frequency. 152 | */ 153 | std::string rateToString( double rate ) 154 | { 155 | if ( rate < 1000.0 ) 156 | return fmt::format( "{:.1f} Hz", rate ); 157 | else if ( rate < 1e6 ) 158 | return fmt::format( "{:.1f} kHz", rate / 1e3 ); 159 | else 160 | return fmt::format( "{:.1f} MHz", rate / 1e6 ); 161 | } 162 | 163 | /** 164 | * @brief Converts a bytes per second to a human-readable string (B/s, KB/s, MB/s). 165 | * @param bandwidth The bandwidth in bytes per second. 166 | * @return A formatted string representing the bandwidth. 167 | */ 168 | std::string bandwidthToString( double bandwidth ) 169 | { 170 | if ( bandwidth < 1000.0 ) 171 | return fmt::format( "{:.1f} B/s", bandwidth ); 172 | else if ( bandwidth < 1000.0 * 1000.0 ) 173 | return fmt::format( "{:.1f} kB/s", bandwidth / 1000.0 ); 174 | else if ( bandwidth < 1000.0 * 1000.0 * 1000.0 ) 175 | return fmt::format( "{:.1f} MB/s", bandwidth / ( 1000.0 * 1000.0 ) ); 176 | else 177 | return fmt::format( "{:.1f} GB/s", bandwidth / ( 1000.0 * 1000.0 * 1000.0 ) ); 178 | } 179 | 180 | int calculateRequiredLines( const std::vector &lines ) 181 | { 182 | return static_cast( lines.size() ) + 2; // +2 for borders 183 | } 184 | 185 | bool parseYamlConfig( CustomOptions &custom_options, rosbag2_transport::RecordOptions &record_options, 186 | rosbag2_storage::StorageOptions &storage_options ) 187 | { 188 | try { 189 | YAML::Node config = YAML::LoadFile( custom_options.config_path ); 190 | 191 | if ( config["node_name"] ) { 192 | custom_options.node_name = config["node_name"].as(); 193 | } 194 | if ( config["output"] ) { 195 | storage_options.uri = resolveOutputDirectory( config["output"].as() ); 196 | } else { 197 | storage_options.uri = resolveOutputDirectory( "" ); // Default to current working directory 198 | } 199 | if ( config["storage_id"] ) { 200 | storage_options.storage_id = config["storage_id"].as(); 201 | } 202 | // mutually exclusive 203 | if ( config["max_bag_size"] && config["max_bag_size_gb"] ) { 204 | throw std::runtime_error( "Conflicting configuration detected: Both 'max_bag_size' and " 205 | "'max_bag_size_gb' are defined. Please specify only one." ); 206 | } else if ( config["max_bag_size"] ) { 207 | storage_options.max_bagfile_size = config["max_bag_size"].as(); 208 | } else if ( config["max_bag_size_gb"] ) { 209 | uint64_t max_bag_size_bytes = 210 | static_cast( config["max_bag_size_gb"].as() * 1024 * 1024 * 1024 ); 211 | storage_options.max_bagfile_size = max_bag_size_bytes; 212 | } 213 | if ( config["max_bag_duration"] ) { 214 | storage_options.max_bagfile_duration = config["max_bag_duration"].as(); 215 | } else { 216 | storage_options.max_bagfile_duration = 0; 217 | } 218 | if ( config["max_cache_size"] ) { 219 | storage_options.max_cache_size = config["max_cache_size"].as(); 220 | } else { 221 | storage_options.max_cache_size = 0; 222 | } 223 | if ( config["storage_preset_profile"] ) { 224 | storage_options.storage_preset_profile = config["storage_preset_profile"].as(); 225 | } 226 | if ( config["storage_config_uri"] ) { 227 | storage_options.storage_config_uri = config["storage_config_uri"].as(); 228 | } 229 | if ( config["snapshot_mode"] ) { 230 | storage_options.snapshot_mode = config["snapshot_mode"].as(); 231 | } else { 232 | storage_options.snapshot_mode = false; 233 | } 234 | if ( config["start_time_ns"] ) { 235 | storage_options.start_time_ns = config["start_time_ns"].as(); 236 | } else { 237 | storage_options.start_time_ns = -1; 238 | } 239 | if ( config["end_time_ns"] ) { 240 | storage_options.end_time_ns = config["end_time_ns"].as(); 241 | } else { 242 | storage_options.end_time_ns = -1; 243 | } 244 | if ( config["custom_data"] ) { 245 | storage_options.custom_data.clear(); 246 | for ( const auto &pair : config["custom_data"] ) { 247 | if ( pair.size() != 2 ) { 248 | std::cerr << "Error: Custom data must be a map of key-value pairs." << std::endl; 249 | return false; // Invalid custom data format 250 | } 251 | storage_options.custom_data[pair[0].as()] = pair[1].as(); 252 | } 253 | } 254 | 255 | if ( config["all_topics"] ) { 256 | record_options.all_topics = config["all_topics"].as(); 257 | } else { 258 | record_options.all_topics = false; 259 | } 260 | if ( config["all_services"] ) { 261 | record_options.all_services = config["all_services"].as(); 262 | } else { 263 | record_options.all_services = false; 264 | } 265 | if ( config["is_discovery_disabled"] ) { 266 | record_options.is_discovery_disabled = config["is_discovery_disabled"].as(); 267 | } else { 268 | record_options.is_discovery_disabled = false; 269 | } 270 | if ( config["topics"] ) { 271 | record_options.topics = config["topics"].as>(); 272 | } 273 | if ( config["topic_types"] ) { 274 | record_options.topic_types = config["topic_types"].as>(); 275 | } 276 | if ( config["services"] ) { 277 | record_options.services = config["services"].as>(); 278 | } 279 | if ( config["exclude_topics"] ) { 280 | record_options.exclude_topics = config["exclude_topics"].as>(); 281 | } 282 | if ( config["exclude_topic_types"] ) { 283 | record_options.exclude_topic_types = 284 | config["exclude_topic_types"].as>(); 285 | } 286 | if ( config["exclude_service_events"] ) { 287 | record_options.exclude_service_events = 288 | config["exclude_service_events"].as>(); 289 | } 290 | if ( config["rmw_serialization_format"] ) { 291 | record_options.rmw_serialization_format = config["rmw_serialization_format"].as(); 292 | } else { 293 | record_options.rmw_serialization_format = rmw_get_serialization_format(); 294 | } 295 | if ( config["topic_polling_interval"] ) { 296 | record_options.topic_polling_interval = 297 | std::chrono::milliseconds( config["topic_polling_interval"].as() ); 298 | } else { 299 | record_options.topic_polling_interval = std::chrono::milliseconds( 100 ); 300 | } 301 | if ( config["regex"] ) { 302 | record_options.regex = config["regex"].as(); 303 | } 304 | if ( config["exclude_regex"] ) { 305 | record_options.exclude_regex = config["exclude_regex"].as(); 306 | } 307 | if ( config["node_prefix"] ) { 308 | record_options.node_prefix = config["node_prefix"].as(); 309 | } 310 | if ( config["compression_mode"] ) { 311 | record_options.compression_mode = config["compression_mode"].as(); 312 | } 313 | if ( config["compression_format"] ) { 314 | record_options.compression_format = config["compression_format"].as(); 315 | } 316 | if ( config["compression_queue_size"] ) { 317 | record_options.compression_queue_size = config["compression_queue_size"].as(); 318 | } else { 319 | record_options.compression_queue_size = 0; 320 | } 321 | if ( config["compression_threads"] ) { 322 | record_options.compression_threads = config["compression_threads"].as(); 323 | } else { 324 | record_options.compression_threads = 0; 325 | } 326 | if ( config["compression_threads_priority"] ) { 327 | record_options.compression_threads_priority = 328 | config["compression_threads_priority"].as(); 329 | } else { 330 | record_options.compression_threads_priority = 0; 331 | } 332 | if ( config["topic_qos_profile_overrides"] ) { 333 | // TODO: Implement topic_qos_profile_overrides parsing 334 | /* 335 | record_options.topic_qos_profile_overrides.clear(); 336 | 337 | // Parse the QoS overrides as a map 338 | auto qos_overrides = config["topic_qos_profile_overrides"].as>(); 339 | 340 | for (const auto &pair : qos_overrides) 341 | { 342 | const std::string &topic_name = pair.first; 343 | const YAML::Node &qos_node = pair.second; 344 | 345 | // Default QoSInitialization 346 | rclcpp::QoSInitialization qos_init = rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default); 347 | rmw_qos_profile_t rmw_qos = rmw_qos_profile_default; 348 | 349 | // Parse QoS settings 350 | if (qos_node["history_depth"]) 351 | { 352 | rmw_qos.depth = qos_node["history_depth"].as(); 353 | } 354 | if (qos_node["reliability"]) 355 | { 356 | std::string reliability = qos_node["reliability"].as(); 357 | if (reliability == "reliable") 358 | { 359 | rmw_qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; 360 | } 361 | else if (reliability == "best_effort") 362 | { 363 | rmw_qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; 364 | } 365 | else 366 | { 367 | throw std::runtime_error("Invalid reliability setting: " + reliability); 368 | } 369 | } 370 | if (qos_node["durability"]) 371 | { 372 | std::string durability = qos_node["durability"].as(); 373 | if (durability == "volatile") 374 | { 375 | rmw_qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; 376 | } 377 | else if (durability == "transient_local") 378 | { 379 | rmw_qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; 380 | } 381 | else 382 | { 383 | throw std::runtime_error("Invalid durability setting: " + durability); 384 | } 385 | } 386 | 387 | // Create QoS profile and add it to the map 388 | rclcpp::QoS qos_profile(qos_init, rmw_qos); 389 | record_options.topic_qos_profile_overrides[topic_name] = qos_profile; 390 | } 391 | */ 392 | } 393 | if ( config["include_hidden_topics"] ) { 394 | record_options.include_hidden_topics = config["include_hidden_topics"].as(); 395 | } else { 396 | record_options.include_hidden_topics = false; 397 | } 398 | if ( config["include_unpublished_topics"] ) { 399 | record_options.include_unpublished_topics = config["include_unpublished_topics"].as(); 400 | } else { 401 | record_options.include_unpublished_topics = false; 402 | } 403 | if ( config["ignore_leaf_topics"] ) { 404 | record_options.ignore_leaf_topics = config["ignore_leaf_topics"].as(); 405 | } else { 406 | record_options.ignore_leaf_topics = false; 407 | } 408 | if ( config["start_paused"] ) { 409 | record_options.start_paused = config["start_paused"].as(); 410 | } else { 411 | record_options.start_paused = false; 412 | } 413 | if ( config["use_sim_time"] ) { 414 | record_options.use_sim_time = config["use_sim_time"].as(); 415 | } else { 416 | record_options.use_sim_time = false; 417 | } 418 | if ( config["disable_keyboard_controls"] ) { 419 | record_options.disable_keyboard_controls = config["disable_keyboard_controls"].as(); 420 | } else { 421 | record_options.disable_keyboard_controls = false; 422 | } 423 | if ( config["publish_status"] ) { 424 | custom_options.publish_status = config["publish_status"].as(); 425 | } 426 | if ( config["publish_status_topic"] ) { 427 | custom_options.status_topic = config["publish_status_topic"].as(); 428 | } 429 | 430 | if ( record_options.rmw_serialization_format.empty() ) { 431 | record_options.rmw_serialization_format = 432 | rmw_get_serialization_format(); // Default to the current RMW serialization format 433 | } 434 | 435 | return true; 436 | } catch ( const std::exception &e ) { 437 | RCLCPP_ERROR( rclcpp::get_logger( "hector_recorder.config.yaml" ), 438 | "Error parsing YAML config: %s", e.what() ); 439 | return false; 440 | } 441 | } 442 | 443 | std::string clipString( const std::string &str, int max_length ) 444 | { 445 | if ( static_cast( str.size() ) <= max_length ) { 446 | return str; 447 | } 448 | 449 | // Find the position of the last '/' 450 | size_t last_slash_pos = str.rfind( '/' ); 451 | if ( last_slash_pos == std::string::npos || last_slash_pos == 0 ) { 452 | // If no '/' is found or it's the first character, fallback to simple clipping 453 | return str.substr( 0, max_length - 3 ) + "..."; 454 | } 455 | 456 | std::string suffix = str.substr( last_slash_pos ); // Include the '/' in the suffix 457 | int suffix_length = static_cast( suffix.size() ); 458 | int prefix_length = max_length - suffix_length - 3; // Account for "..." 459 | 460 | if ( prefix_length <= 0 ) { 461 | // If the suffix alone exceeds the max length, truncate it 462 | return "..." + suffix.substr( suffix.size() - ( max_length - 3 ) ); 463 | } 464 | 465 | std::string prefix = str.substr( 0, prefix_length ); 466 | return prefix + "..." + suffix; 467 | } 468 | 469 | void ensureLeadingSlash( std::vector &vector ) 470 | { 471 | if ( !vector.empty() ) { 472 | for ( auto &string : vector ) { 473 | if ( string.find( "/" ) != 0 ) { 474 | string = "/" + string; 475 | } 476 | } 477 | } 478 | } 479 | 480 | static std::string expandUserAndEnv( std::string s ) 481 | { 482 | // ~ → $HOME 483 | if ( !s.empty() && s[0] == '~' ) { 484 | const char *home = std::getenv( "HOME" ); 485 | if ( home && s.size() == 1 ) { 486 | s = home; 487 | } else if ( home && s.size() > 1 && s[1] == '/' ) { 488 | s = std::string( home ) + s.substr( 1 ); 489 | } 490 | // (If "~user" needed, implement lookup; omitted for simplicity.) 491 | } 492 | 493 | // ${VAR} → env, then $VAR → env 494 | auto replace_env = []( const std::string &in, const std::regex &re ) { 495 | std::string out; 496 | std::sregex_iterator it( in.begin(), in.end(), re ), end; 497 | size_t last = 0; 498 | out.reserve( in.size() ); 499 | for ( ; it != end; ++it ) { 500 | out.append( in, last, it->position() - last ); 501 | std::string key = it->size() > 1 ? ( *it )[1].str() : ""; 502 | const char *val = key.empty() ? nullptr : std::getenv( key.c_str() ); 503 | out += ( val ? val : "" ); 504 | last = it->position() + it->length(); 505 | } 506 | out.append( in, last, std::string::npos ); 507 | return out; 508 | }; 509 | 510 | // ${VAR} 511 | s = replace_env( s, std::regex( R"(\$\{([A-Za-z_][A-Za-z0-9_]*)\})" ) ); 512 | // $VAR 513 | s = replace_env( s, std::regex( R"(\$([A-Za-z_][A-Za-z0-9_]*))" ) ); 514 | 515 | return s; 516 | } 517 | 518 | std::string resolveOutputUriToAbsolute( std::string &uri ) 519 | { 520 | 521 | std::string expanded = expandUserAndEnv( uri ); 522 | fs::path p( expanded ); 523 | 524 | // Make absolute and normalize. weakly_canonical doesn't require existence. 525 | // If you *want* to preserve symlinks, use fs::absolute(p) instead. 526 | fs::path abs = fs::absolute( p ); 527 | fs::path norm = fs::weakly_canonical( abs ); 528 | 529 | // If weakly_canonical fails (non-existent parent chains), fallback to abs 530 | uri = norm.empty() ? abs.string() : norm.string(); 531 | return uri; 532 | } 533 | 534 | } // namespace hector_recorder -------------------------------------------------------------------------------- /hector_recorder/src/terminal_ui.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file terminal_ui.cpp 3 | * @brief Implementation of the TerminalUI class, which provides a terminal-based user interface for 4 | * monitoring and controlling a ROS 2 recorder. 5 | * 6 | * This file contains the implementation of the TerminalUI class, which uses ncurses to display 7 | * information about the recorder's status, system resources, and recorded topics in a terminal 8 | * window. It also handles the initialization and cleanup of the recorder and UI components. 9 | */ 10 | 11 | #include "hector_recorder/terminal_ui.hpp" 12 | #include "hector_recorder/utils.h" 13 | #include "rclcpp/rclcpp.hpp" 14 | #include "rosbag2_cpp/writers/sequential_writer.hpp" 15 | #include "rosbag2_storage/storage_options.hpp" 16 | #include "rosbag2_transport/record_options.hpp" 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | using namespace std::chrono_literals; 29 | using namespace hector_recorder; 30 | 31 | namespace 32 | { 33 | // Innerwidth (without borders) 34 | inline int interior_width( WINDOW *win ) { return getmaxx( win ) - 2; } 35 | 36 | // Column gap: total columns added between two printed columns. 37 | constexpr int kColumnGap = 3; 38 | inline int sep_offset() { return ( kColumnGap / 2 ) + 1; } 39 | 40 | // Tablewidth from column widths + gaps (kColumnGap per gap) + trailing '---' (3) 41 | inline int compute_table_width( const std::vector &widths, size_t ncols ) 42 | { 43 | int sum = 0; 44 | for ( int w : widths ) sum += w; 45 | int gaps = ( ncols > 0 ) ? static_cast( ncols - 1 ) * kColumnGap : 0; 46 | int trailing = 3; // last '---' for last column 47 | return sum + gaps + trailing; 48 | } 49 | 50 | // Clip with "..." at the end if truncated 51 | inline std::string clip_with_ellipsis( const std::string &s, int width ) 52 | { 53 | if ( width <= 0 ) 54 | return ""; 55 | if ( (int)s.size() <= width ) 56 | return s; 57 | if ( width <= 3 ) 58 | return std::string( std::max( 0, width ), '.' ); 59 | return s.substr( 0, width - 3 ) + "..."; 60 | } 61 | 62 | // Global sort state (1-based column index). Default: sort by Size (column 5) desc 63 | std::atomic g_sort_column{ 5 }; 64 | std::atomic g_sort_desc{ true }; 65 | 66 | } // namespace 67 | 68 | TerminalUI::TerminalUI( const CustomOptions &custom_options, 69 | const rosbag2_storage::StorageOptions &storage_options, 70 | const rosbag2_transport::RecordOptions &record_options ) 71 | : Node( custom_options.node_name ), config_path_( custom_options.config_path ), 72 | publish_status_( custom_options.publish_status ) 73 | { 74 | storage_options_ = storage_options; 75 | record_options_ = record_options; 76 | 77 | initializeRecorder(); 78 | initializeUI(); 79 | 80 | ui_refresh_timer_ = this->create_wall_timer( 100ms, [this]() { this->updateUI(); } ); 81 | if ( publish_status_ ) { 82 | status_pub_ = this->create_publisher( 83 | custom_options.status_topic, 10 ); 84 | status_pub_timer_ = this->create_wall_timer( 1s, [this]() { this->publishRecorderStatus(); } ); 85 | } 86 | 87 | startRecording(); 88 | } 89 | 90 | TerminalUI::~TerminalUI() 91 | { 92 | stopRecording(); 93 | cleanupUI(); 94 | } 95 | 96 | void TerminalUI::initializeRecorder() 97 | { 98 | if ( initscr() == nullptr ) { 99 | throw std::runtime_error( "Error initializing ncurses. Terminal may not support it." ); 100 | } 101 | cbreak(); 102 | auto writer_impl = std::make_unique(); 103 | auto writer = std::make_shared( std::move( writer_impl ) ); 104 | recorder_ = std::make_unique( this, writer, storage_options_, record_options_ ); 105 | } 106 | 107 | void TerminalUI::initializeUI() 108 | { 109 | initscr(); 110 | cbreak(); 111 | noecho(); 112 | curs_set( 0 ); 113 | start_color(); 114 | 115 | use_default_colors(); 116 | 117 | init_pair( 1, COLOR_GREEN, -1 ); 118 | init_pair( 2, COLOR_RED, -1 ); 119 | init_pair( 3, COLOR_YELLOW, -1 ); 120 | 121 | int height, width; 122 | getmaxyx( stdscr, height, width ); 123 | 124 | int general_info_height = 3; // Placeholder for general info height, dynamically adjusted later 125 | generalInfoWin_ = newwin( general_info_height, width, 0, 0 ); 126 | 127 | // Remaining height for tableWin_ 128 | int table_height = height - general_info_height; 129 | tableWin_ = newwin( table_height, width, general_info_height, 0 ); 130 | 131 | // enable non-blocking input and function keys 132 | nodelay( tableWin_, TRUE ); 133 | keypad( tableWin_, TRUE ); 134 | 135 | scrollok( tableWin_, true ); 136 | 137 | // Draw borders for all windows 138 | for ( auto win : { generalInfoWin_, tableWin_ } ) { 139 | box( win, 0, 0 ); 140 | wrefresh( win ); 141 | } 142 | } 143 | 144 | void TerminalUI::cleanupUI() 145 | { 146 | delwin( generalInfoWin_ ); 147 | delwin( tableWin_ ); 148 | endwin(); 149 | } 150 | 151 | void TerminalUI::startRecording() 152 | { 153 | if ( !recorder_ ) { 154 | RCLCPP_ERROR( this->get_logger(), "Recorder is not initialized. Cannot start recording." ); 155 | return; 156 | } 157 | recorder_->record(); 158 | RCLCPP_INFO( this->get_logger(), "Recording started." ); 159 | } 160 | 161 | void TerminalUI::stopRecording() 162 | { 163 | if ( recorder_ ) { 164 | recorder_->stop(); 165 | RCLCPP_INFO( this->get_logger(), "Recording stopped." ); 166 | } 167 | } 168 | 169 | void TerminalUI::updateUI() 170 | { 171 | std::lock_guard lock( data_mutex_ ); 172 | 173 | // Handle user input for sorting 174 | int ch = wgetch( tableWin_ ); 175 | if ( ch != ERR ) { 176 | if ( ch >= '1' && ch <= '8' ) { 177 | const int col = ch - '0'; 178 | const int prev = g_sort_column.load(); 179 | if ( prev != col ) { 180 | g_sort_column.store( col ); 181 | g_sort_desc.store( ( col == 1 || col == 6 ) ? false : true ); 182 | } else { 183 | g_sort_desc.store( !g_sort_desc.load() ); 184 | } 185 | } 186 | } 187 | 188 | if ( !recorder_ ) { 189 | werase( generalInfoWin_ ); 190 | box( generalInfoWin_, 0, 0 ); 191 | mvwprintw( generalInfoWin_, 1, 1, "Recorder not initialized." ); 192 | wrefresh( generalInfoWin_ ); 193 | return; 194 | } 195 | 196 | updateGeneralInfo(); 197 | 198 | // Dynamically adjust the size of tableWin_ based on the terminal size 199 | int general_info_height, general_info_width; 200 | getmaxyx( generalInfoWin_, general_info_height, general_info_width ); 201 | int terminal_height, terminal_width; 202 | getmaxyx( stdscr, terminal_height, terminal_width ); 203 | int table_height = terminal_height - general_info_height; 204 | if ( table_height > 0 ) { 205 | wresize( tableWin_, table_height, terminal_width ); 206 | mvwin( tableWin_, general_info_height, 0 ); 207 | } 208 | 209 | updateTable(); 210 | } 211 | 212 | void TerminalUI::updateGeneralInfo() 213 | { 214 | std::vector lines; 215 | lines.push_back( 216 | fmt::format( "Bagfile Path: {}", resolveOutputUriToAbsolute( storage_options_.uri ) ) ); 217 | if ( !config_path_.empty() ) { 218 | lines.push_back( fmt::format( "Config Path: {}", config_path_ ) ); 219 | } 220 | lines.push_back( 221 | fmt::format( "Duration: {} s | Size: {} / {} | Files: {}", 222 | static_cast( recorder_->get_bagfile_duration().seconds() ), 223 | formatMemory( recorder_->get_bagfile_size() ), 224 | formatMemory( std::filesystem::space( storage_options_.uri ).available ), 225 | recorder_->get_files().size() ) ); 226 | 227 | // Calculate the required height based on the content 228 | int required_height = calculateRequiredLines( lines ); 229 | 230 | // Update the height of the generalInfoWin_ if it has changed 231 | int current_height, current_width; 232 | getmaxyx( generalInfoWin_, current_height, current_width ); 233 | 234 | if ( current_height != required_height ) { 235 | wresize( generalInfoWin_, required_height, current_width ); 236 | mvwin( generalInfoWin_, 0, 0 ); 237 | } 238 | 239 | // Clear the old content and draw the border 240 | werase( generalInfoWin_ ); 241 | box( generalInfoWin_, 0, 0 ); 242 | 243 | const int inner = interior_width( generalInfoWin_ ); 244 | 245 | if ( inner > 0 ) { 246 | // Write the content to the window, clipping if necessary 247 | for ( size_t i = 0; i < lines.size(); ++i ) { 248 | const std::string clipped = clip_with_ellipsis( lines[i], inner ); 249 | mvwprintw( generalInfoWin_, static_cast( i ) + 1, 1, "%-*s", inner, clipped.c_str() ); 250 | } 251 | } 252 | 253 | // Refresh the window to display the new content 254 | wrefresh( generalInfoWin_ ); 255 | } 256 | 257 | void TerminalUI::updateTable() 258 | { 259 | werase( tableWin_ ); 260 | box( tableWin_, 0, 0 ); 261 | 262 | const auto &topics_info = recorder_->get_topics_info(); 263 | max_width_ = getmaxx( tableWin_ ); 264 | ui_topics_max_length_ = 265 | static_cast( max_width_ * 0.3 ); // topic name should not exceed 30% of the window width 266 | ui_topics_type_max_length_ = 267 | static_cast( max_width_ * 0.2 ); // topic type should not exceed 20% of the window width 268 | row_ = 1; 269 | 270 | if ( topics_info.empty() ) { 271 | mvwprintw( tableWin_, row_++, 1, "No topics or services selected." ); 272 | } else { 273 | // Sort Topics by bandwidth, name, and frequency 274 | sorted_topics_ = sortTopics( topics_info ); 275 | 276 | // Topic, Msg, Freq, Bandwidth, Size, Type, Pub, QoS 277 | headers_ = { "Topic", "Msgs", "Freq", "Bandwidth", "Size", "Type", "Pub", "QoS" }; 278 | column_widths_.resize( headers_.size(), 0 ); 279 | for ( size_t i = 0; i < headers_.size(); ++i ) { column_widths_[i] = headers_[i].size(); } 280 | 281 | // Adjust ColumnWidths and ensure Table fits the maximum width of the terminal 282 | adjustColumnWidths(); 283 | 284 | renderTable(); 285 | } 286 | 287 | wrefresh( tableWin_ ); 288 | } 289 | 290 | std::vector> 291 | TerminalUI::sortTopics( const std::unordered_map &topics_info ) 292 | { 293 | std::vector> sorted_topics( topics_info.begin(), 294 | topics_info.end() ); 295 | 296 | // Capture current sort state once 297 | int sort_col = g_sort_column.load(); // 1..8 298 | bool sort_desc = g_sort_desc.load(); 299 | 300 | std::sort( sorted_topics.begin(), sorted_topics.end(), 301 | [sort_col, sort_desc]( const auto &a, const auto &b ) { 302 | const auto &A = a.second; 303 | const auto &B = b.second; 304 | 305 | auto cmp_str = [&]( const std::string &x, const std::string &y ) { 306 | if ( x == y ) 307 | return 0; 308 | return ( x < y ) ? -1 : 1; 309 | }; 310 | auto cmp_uint64 = [&]( uint64_t x, uint64_t y ) { 311 | if ( x == y ) 312 | return 0; 313 | return ( x < y ) ? -1 : 1; 314 | }; 315 | auto cmp_double = [&]( double x, double y ) { 316 | if ( x == y ) 317 | return 0; 318 | return ( x < y ) ? -1 : 1; 319 | }; 320 | 321 | int order = 0; 322 | switch ( sort_col ) { 323 | case 1: // Topic name 324 | order = cmp_str( a.first, b.first ); 325 | break; 326 | case 2: // Msgs (message_count) 327 | order = cmp_uint64( A.message_count(), B.message_count() ); 328 | break; 329 | case 3: // Freq 330 | order = cmp_double( A.mean_frequency(), B.mean_frequency() ); 331 | break; 332 | case 4: // Bandwidth 333 | order = cmp_double( A.bandwidth(), B.bandwidth() ); 334 | break; 335 | case 5: // Size (bytes) 336 | order = cmp_uint64( A.size(), B.size() ); 337 | break; 338 | case 6: // Type 339 | order = cmp_str( A.topic_type(), B.topic_type() ); 340 | break; 341 | case 7: // Pub (publisher_count) 342 | order = cmp_uint64( A.publisher_count(), B.publisher_count() ); 343 | break; 344 | case 8: // QoS (string) 345 | order = cmp_str( A.qos_reliability(), B.qos_reliability() ); 346 | break; 347 | default: 348 | // fallback: by size desc, name asc, freq desc 349 | if ( A.size() != B.size() ) 350 | return A.size() > B.size(); 351 | if ( a.first != b.first ) 352 | return a.first < b.first; 353 | return A.mean_frequency() > B.mean_frequency(); 354 | } 355 | 356 | if ( order == 0 ) { 357 | if ( A.size() != B.size() ) 358 | return A.size() > B.size(); 359 | if ( a.first != b.first ) 360 | return a.first < b.first; 361 | return A.mean_frequency() > B.mean_frequency(); 362 | } 363 | 364 | if ( sort_desc ) 365 | return order > 0; 366 | else 367 | return order < 0; 368 | } ); 369 | 370 | return sorted_topics; 371 | } 372 | 373 | void TerminalUI::adjustColumnWidths() 374 | { 375 | auto updateColumnWidth = [&]( size_t index, int value, int max_length = INT_MAX ) { 376 | column_widths_[index] = std::min( std::max( column_widths_[index], value ), max_length ); 377 | }; 378 | 379 | size_t longest_topic_name = column_widths_[0]; 380 | size_t longest_topic_type = ( column_widths_.size() > 5 ) ? column_widths_[5] : 0; 381 | 382 | for ( const auto &topic : sorted_topics_ ) { 383 | const auto &info = topic.second; 384 | 385 | // Update longest topic name 386 | longest_topic_name = std::max( longest_topic_name, topic.first.size() ); 387 | 388 | // Update topic name 389 | updateColumnWidth( 0, static_cast( topic.first.size() ), ui_topics_max_length_ ); 390 | 391 | // Update message count 392 | updateColumnWidth( 1, static_cast( std::to_string( info.message_count() ).size() ) ); 393 | 394 | // Update frequency 395 | updateColumnWidth( 2, static_cast( rateToString( info.mean_frequency() ).size() ) ); 396 | 397 | // Update bandwidth 398 | updateColumnWidth( 3, static_cast( bandwidthToString( info.bandwidth() ).size() ) ); 399 | 400 | // Update size 401 | updateColumnWidth( 4, static_cast( formatMemory( info.size() ).size() ) ); 402 | 403 | // Update topic type 404 | longest_topic_type = std::max( longest_topic_type, info.topic_type().size() ); 405 | updateColumnWidth( 5, static_cast( info.topic_type().size() ), ui_topics_type_max_length_ ); 406 | 407 | // Update publisher count 408 | updateColumnWidth( 6, static_cast( std::to_string( info.publisher_count() ).size() ) ); 409 | 410 | // Update QoS 411 | updateColumnWidth( 7, static_cast( info.qos_reliability().size() ) ); 412 | } 413 | 414 | // Calculate total width 415 | total_width_ = compute_table_width( column_widths_, headers_.size() ); 416 | 417 | // Ensure table fits within the maximum width of the terminal window 418 | const int inner = interior_width( tableWin_ ); 419 | while ( total_width_ > inner && !headers_.empty() ) { 420 | headers_.pop_back(); 421 | column_widths_.pop_back(); 422 | total_width_ = compute_table_width( column_widths_, headers_.size() ); 423 | } 424 | 425 | // Distribute remaining width 426 | distributeRemainingWidth( longest_topic_name, longest_topic_type ); 427 | } 428 | 429 | void TerminalUI::distributeRemainingWidth( size_t longest_topic_name, size_t longest_topic_type ) 430 | { 431 | if ( headers_.empty() ) 432 | return; 433 | 434 | const int inner = interior_width( tableWin_ ); 435 | total_width_ = compute_table_width( column_widths_, headers_.size() ); 436 | 437 | if ( total_width_ >= inner ) 438 | return; 439 | 440 | int remaining_width = inner - total_width_; 441 | 442 | auto growUpTo = [&]( size_t idx, size_t longest_size, int budget, size_t &max_length ) -> int { 443 | int needed = static_cast( longest_size ) - column_widths_[idx]; 444 | int give = std::max( 0, std::min( needed, budget ) ); 445 | column_widths_[idx] += give; 446 | max_length += give; 447 | return give; 448 | }; 449 | 450 | bool need_name = column_widths_[0] < static_cast( longest_topic_name ); 451 | bool need_type = headers_.size() > 5 && column_widths_[5] < static_cast( longest_topic_type ); 452 | 453 | if ( need_name && need_type ) { 454 | int give_name = remaining_width * 3 / 5; 455 | int used_name = growUpTo( 0, longest_topic_name, give_name, ui_topics_max_length_ ); 456 | remaining_width -= used_name; 457 | 458 | int used_type = growUpTo( 5, longest_topic_type, remaining_width, ui_topics_type_max_length_ ); 459 | remaining_width -= used_type; 460 | } else if ( need_name ) { 461 | int used = growUpTo( 0, longest_topic_name, remaining_width, ui_topics_max_length_ ); 462 | remaining_width -= used; 463 | } else if ( need_type ) { 464 | int used = growUpTo( 5, longest_topic_type, remaining_width, ui_topics_type_max_length_ ); 465 | remaining_width -= used; 466 | } 467 | 468 | total_width_ = compute_table_width( column_widths_, headers_.size() ); 469 | } 470 | 471 | void TerminalUI::renderTable() 472 | { 473 | // Render Headers 474 | renderHeaders(); 475 | 476 | // Render Separator Line 477 | renderSeperatorLine(); 478 | 479 | // Render Topics 480 | for ( const auto &topic : sorted_topics_ ) { renderTopicRow( topic ); } 481 | } 482 | 483 | void TerminalUI::renderHeaders() 484 | { 485 | for ( size_t i = 0, col = 1; i < headers_.size(); ++i ) { 486 | std::string label = headers_[i]; 487 | int col_no = static_cast( i ) + 1; 488 | const std::string to_print = clip_with_ellipsis( label, column_widths_[i] ); 489 | 490 | // Draw header with green text and bold when it's the sort column 491 | if ( g_sort_column.load() == col_no ) { 492 | wattron( tableWin_, COLOR_PAIR( 1 ) | A_BOLD ); 493 | mvwprintw( tableWin_, row_, col, "%-*s", column_widths_[i], to_print.c_str() ); 494 | wattroff( tableWin_, COLOR_PAIR( 1 ) | A_BOLD ); 495 | } else { 496 | wattron( tableWin_, COLOR_PAIR( 1 ) ); 497 | mvwprintw( tableWin_, row_, col, "%-*s", column_widths_[i], to_print.c_str() ); 498 | wattroff( tableWin_, COLOR_PAIR( 1 ) ); 499 | } 500 | 501 | col += column_widths_[i]; 502 | if ( i < headers_.size() - 1 ) { 503 | col += kColumnGap; 504 | mvwaddch( tableWin_, row_, col - sep_offset(), '|' ); 505 | } 506 | } 507 | row_++; 508 | } 509 | 510 | void TerminalUI::renderSeperatorLine() 511 | { 512 | const int inner = interior_width( tableWin_ ); 513 | 514 | const int base_width = std::min( total_width_, inner ); 515 | mvwhline( tableWin_, row_, 1, '-', base_width ); 516 | 517 | // Add '+' as column separators 518 | for ( size_t i = 0, col = 1; i < headers_.size(); ++i ) { 519 | col += column_widths_[i]; 520 | if ( i < headers_.size() - 1 ) { 521 | int plus_x = col + ( kColumnGap / 2 ); 522 | if ( plus_x >= 1 && plus_x <= inner ) 523 | mvwaddch( tableWin_, row_, plus_x, '+' ); 524 | col += kColumnGap; 525 | } 526 | } 527 | 528 | row_++; 529 | } 530 | 531 | void TerminalUI::renderTopicRow( const std::pair &topic ) 532 | { 533 | const auto &info = topic.second; 534 | int col = 1; 535 | 536 | // Row coloring (foreground text only): 537 | // - red text if no publishers 538 | // - yellow text if there are publishers but no messages received yet 539 | int active_color_pair = 0; 540 | if ( info.publisher_count() == 0 ) { 541 | active_color_pair = 2; // red text 542 | } else if ( info.publisher_count() > 0 && info.message_count() == 0 ) { 543 | active_color_pair = 3; // yellow text 544 | } 545 | if ( active_color_pair != 0 ) { 546 | wattron( tableWin_, COLOR_PAIR( active_color_pair ) ); 547 | } 548 | 549 | // Populate table columns 550 | // Topic, Msg, Freq, Bandwidth, Size, Type, Pub, QoS 551 | const std::vector column_data = { 552 | clipString( topic.first, column_widths_[0] ), // Topic name 553 | std::to_string( info.message_count() ), // Message count 554 | rateToString( info.mean_frequency() ), // Frequency 555 | bandwidthToString( info.bandwidth() ), // Bandwidth 556 | formatMemory( info.size() ), // Size 557 | clipString( info.topic_type(), column_widths_[5] ), // Topic type 558 | std::to_string( info.publisher_count() ), // Publisher count 559 | info.qos_reliability() // QoS 560 | }; 561 | 562 | for ( size_t i = 0; i < headers_.size(); ++i ) { 563 | mvwprintw( tableWin_, row_, col, "%-*s", column_widths_[i], column_data[i].c_str() ); 564 | 565 | col += column_widths_[i]; 566 | if ( i < headers_.size() - 1 ) { 567 | col += kColumnGap; 568 | mvwaddch( tableWin_, row_, col - sep_offset(), '|' ); 569 | } 570 | } 571 | 572 | if ( active_color_pair != 0 ) { 573 | wattroff( tableWin_, COLOR_PAIR( active_color_pair ) ); 574 | } 575 | 576 | row_++; 577 | } 578 | 579 | void TerminalUI::publishRecorderStatus() 580 | { 581 | if ( !recorder_ ) { 582 | return; 583 | } 584 | 585 | std::lock_guard lock( data_mutex_ ); 586 | 587 | hector_recorder_msgs::msg::RecorderStatus status_msg; 588 | status_msg.output_dir = storage_options_.uri; 589 | status_msg.config_path = config_path_; 590 | status_msg.files = recorder_->get_files(); 591 | status_msg.duration = recorder_->get_bagfile_duration(); 592 | status_msg.size = recorder_->get_bagfile_size(); 593 | for ( const auto &topic_info : recorder_->get_topics_info() ) { 594 | hector_recorder_msgs::msg::TopicInfo topic_msg; 595 | topic_msg.topic = topic_info.first; 596 | topic_msg.msg_count = topic_info.second.message_count(); 597 | topic_msg.frequency = topic_info.second.mean_frequency(); 598 | topic_msg.bandwidth = topic_info.second.bandwidth(); 599 | topic_msg.size = topic_info.second.size(); 600 | status_msg.topics.push_back( topic_msg ); 601 | } 602 | 603 | status_pub_->publish( status_msg ); 604 | } 605 | -------------------------------------------------------------------------------- /hector_recorder/src/modified/recorder_impl.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Changelog Jonathan Lichtenfeld 12.6.2024: 16 | // - Extract RecorderImpl from rosbag2_transport/recorder.cpp to its own file 17 | // - Add get_topics_names_to_info() 18 | // - Add get_bagfile_duration() 19 | 20 | 21 | #include "hector_recorder/modified/recorder_impl.hpp" 22 | #include "rosbag2_cpp/writers/sequential_writer.hpp" 23 | 24 | namespace hector_recorder 25 | { 26 | 27 | RecorderImpl::RecorderImpl( 28 | rclcpp::Node * owner, 29 | std::shared_ptr writer, 30 | const rosbag2_storage::StorageOptions & storage_options, 31 | const rosbag2_transport::RecordOptions & record_options) 32 | : writer_(std::move(writer)), 33 | storage_options_(storage_options), 34 | record_options_(record_options), 35 | node(owner) 36 | { 37 | if (record_options_.use_sim_time && record_options_.is_discovery_disabled) { 38 | throw std::runtime_error( 39 | "use_sim_time and is_discovery_disabled both set, but are incompatible settings. " 40 | "The /clock topic needs to be discovered to record with sim time."); 41 | } 42 | 43 | topic_filter_ = std::make_unique(record_options, node->get_node_graph_interface()); 44 | 45 | for (auto & topic : record_options_.topics) { 46 | topic = rclcpp::expand_topic_or_service_name( 47 | topic, node->get_name(), 48 | node->get_namespace(), false); 49 | } 50 | 51 | for (auto & exclude_topic : record_options_.exclude_topics) { 52 | exclude_topic = rclcpp::expand_topic_or_service_name( 53 | exclude_topic, node->get_name(), 54 | node->get_namespace(), false); 55 | } 56 | 57 | for (auto & service : record_options_.services) { 58 | service = rclcpp::expand_topic_or_service_name( 59 | service, node->get_name(), 60 | node->get_namespace(), false); 61 | } 62 | 63 | for (auto & exclude_service_event_topic : record_options_.exclude_service_events) { 64 | exclude_service_event_topic = rclcpp::expand_topic_or_service_name( 65 | exclude_service_event_topic, node->get_name(), 66 | node->get_namespace(), false); 67 | } 68 | } 69 | 70 | RecorderImpl::~RecorderImpl() 71 | { 72 | stop(); 73 | } 74 | 75 | void RecorderImpl::stop() 76 | { 77 | std::lock_guard state_lock(start_stop_transition_mutex_); 78 | if (!in_recording_) { 79 | RCLCPP_DEBUG(node->get_logger(), "Recording has already been stopped or not running."); 80 | return; 81 | } 82 | 83 | stop_discovery(); 84 | pause(); 85 | subscriptions_.clear(); 86 | writer_->close(); // Call writer->close() to finalize current bag file and write metadata 87 | 88 | { 89 | std::lock_guard lock(event_publisher_thread_mutex_); 90 | event_publisher_thread_should_exit_ = true; 91 | } 92 | event_publisher_thread_wake_cv_.notify_all(); 93 | if (event_publisher_thread_.joinable()) { 94 | event_publisher_thread_.join(); 95 | } 96 | in_recording_ = false; 97 | RCLCPP_INFO(node->get_logger(), "Recording stopped"); 98 | } 99 | 100 | void RecorderImpl::record() 101 | { 102 | std::lock_guard state_lock(start_stop_transition_mutex_); 103 | if (in_recording_.exchange(true)) { 104 | RCLCPP_WARN_STREAM( 105 | node->get_logger(), 106 | "Called Recorder::record() while already in recording, dismissing request."); 107 | return; 108 | } 109 | paused_ = false; 110 | topic_qos_profile_overrides_ = record_options_.topic_qos_profile_overrides; 111 | if (record_options_.rmw_serialization_format.empty()) { 112 | throw std::runtime_error("No serialization format specified!"); 113 | } 114 | 115 | writer_->open( 116 | storage_options_, 117 | {rmw_get_serialization_format(), record_options_.rmw_serialization_format}); 118 | 119 | files_.push_back(storage_options_.uri); // Add the first file to the list 120 | split_event_pub_ = 121 | node->create_publisher("events/write_split", 1); 122 | 123 | // Start the thread that will publish events 124 | event_publisher_thread_ = std::thread(&RecorderImpl::event_publisher_thread_main, this); 125 | 126 | rosbag2_cpp::bag_events::WriterEventCallbacks callbacks; 127 | callbacks.write_split_callback = 128 | [this](rosbag2_cpp::bag_events::BagSplitInfo & info) { 129 | { 130 | std::lock_guard lock(event_publisher_thread_mutex_); 131 | bag_split_info_ = info; 132 | write_split_has_occurred_ = true; 133 | } 134 | event_publisher_thread_wake_cv_.notify_all(); 135 | }; 136 | writer_->add_event_callbacks(callbacks); 137 | 138 | serialization_format_ = record_options_.rmw_serialization_format; 139 | RCLCPP_INFO(node->get_logger(), "Listening for topics..."); 140 | if (!record_options_.use_sim_time) { 141 | subscribe_topics(get_requested_or_available_topics()); 142 | } 143 | 144 | // Insert all requested topics into topics_info_ to ensure they are tracked 145 | auto unknown_topics = get_unknown_topics(); 146 | for (const auto & t : unknown_topics) { 147 | topics_info_[t]; 148 | } 149 | if (!record_options_.is_discovery_disabled) { 150 | start_discovery(); 151 | } 152 | } 153 | 154 | void RecorderImpl::split() 155 | { 156 | writer_->split_bagfile(); 157 | } 158 | 159 | void RecorderImpl::event_publisher_thread_main() 160 | { 161 | RCLCPP_INFO(node->get_logger(), "Event publisher thread: Starting"); 162 | while (!event_publisher_thread_should_exit_.load()) { 163 | std::unique_lock lock(event_publisher_thread_mutex_); 164 | event_publisher_thread_wake_cv_.wait( 165 | lock, 166 | [this] {return event_publisher_thread_should_wake();}); 167 | 168 | if (write_split_has_occurred_) { 169 | write_split_has_occurred_ = false; 170 | 171 | auto message = rosbag2_interfaces::msg::WriteSplitEvent(); 172 | message.closed_file = bag_split_info_.closed_file; 173 | message.opened_file = bag_split_info_.opened_file; 174 | message.node_name = node->get_fully_qualified_name(); 175 | files_.push_back(message.opened_file); 176 | try { 177 | split_event_pub_->publish(message); 178 | } catch (const std::exception & e) { 179 | RCLCPP_ERROR_STREAM( 180 | node->get_logger(), 181 | "Failed to publish message on '/events/write_split' topic. \nError: " << e.what()); 182 | } catch (...) { 183 | RCLCPP_ERROR_STREAM( 184 | node->get_logger(), 185 | "Failed to publish message on '/events/write_split' topic."); 186 | } 187 | } 188 | } 189 | RCLCPP_INFO(node->get_logger(), "Event publisher thread: Exiting"); 190 | } 191 | 192 | bool RecorderImpl::event_publisher_thread_should_wake() 193 | { 194 | return write_split_has_occurred_ || event_publisher_thread_should_exit_; 195 | } 196 | 197 | const rosbag2_cpp::Writer & RecorderImpl::get_writer_handle() 198 | { 199 | return *writer_; 200 | } 201 | 202 | void RecorderImpl::pause() 203 | { 204 | if (paused_.exchange(true)) { 205 | RCLCPP_DEBUG(node->get_logger(), "Recorder is already in pause state."); 206 | } else { 207 | RCLCPP_INFO_STREAM(node->get_logger(), "Pausing recording."); 208 | } 209 | } 210 | 211 | void RecorderImpl::start_discovery() 212 | { 213 | std::lock_guard state_lock(discovery_mutex_); 214 | if (stop_discovery_.exchange(false)) { 215 | RCLCPP_DEBUG(node->get_logger(), "Recorder topic discovery is already running."); 216 | } else { 217 | discovery_future_ = 218 | std::async(std::launch::async, std::bind(&RecorderImpl::topics_discovery, this)); 219 | } 220 | } 221 | 222 | void RecorderImpl::stop_discovery() 223 | { 224 | std::lock_guard state_lock(discovery_mutex_); 225 | if (stop_discovery_.exchange(true)) { 226 | RCLCPP_DEBUG( 227 | node->get_logger(), "Recorder topic discovery has already been stopped or not running."); 228 | } else { 229 | if (discovery_future_.valid()) { 230 | auto status = discovery_future_.wait_for(2 * record_options_.topic_polling_interval); 231 | if (status != std::future_status::ready) { 232 | RCLCPP_ERROR_STREAM( 233 | node->get_logger(), 234 | "discovery_future_.wait_for(" << record_options_.topic_polling_interval.count() << 235 | ") return status: " << 236 | (status == std::future_status::timeout ? "timeout" : "deferred")); 237 | } 238 | } 239 | } 240 | } 241 | 242 | 243 | void RecorderImpl::topics_discovery() 244 | { 245 | // If using sim time - wait until /clock topic received before even creating subscriptions 246 | if (record_options_.use_sim_time) { 247 | RCLCPP_INFO( 248 | node->get_logger(), 249 | "use_sim_time set, waiting for /clock before starting recording..."); 250 | while (rclcpp::ok() && stop_discovery_ == false) { 251 | if (node->get_clock()->wait_until_started(record_options_.topic_polling_interval)) { 252 | break; 253 | } 254 | } 255 | if (node->get_clock()->started()) { 256 | RCLCPP_INFO(node->get_logger(), "Sim time /clock found, starting recording."); 257 | } 258 | } 259 | while (rclcpp::ok() && stop_discovery_ == false) { 260 | try { 261 | auto topics_to_subscribe = get_requested_or_available_topics(); 262 | for (const auto & topic_and_type : topics_to_subscribe) { 263 | warn_if_new_qos_for_subscribed_topic(topic_and_type.first); 264 | } 265 | auto missing_topics = get_missing_topics(topics_to_subscribe); 266 | subscribe_topics(missing_topics); 267 | 268 | // Update topic publisher info 269 | update_topic_publisher_info(); 270 | 271 | if (!record_options_.topics.empty() && 272 | subscriptions_.size() == record_options_.topics.size()) 273 | { 274 | RCLCPP_INFO( 275 | node->get_logger(), 276 | "All requested topics are subscribed. Stopping discovery..."); 277 | return; 278 | } 279 | } catch (const std::exception & e) { 280 | RCLCPP_ERROR_STREAM(node->get_logger(), "Failure in topics discovery.\nError: " << e.what()); 281 | } catch (...) { 282 | RCLCPP_ERROR_STREAM(node->get_logger(), "Failure in topics discovery."); 283 | } 284 | std::this_thread::sleep_for(record_options_.topic_polling_interval); 285 | } 286 | } 287 | 288 | std::unordered_map 289 | RecorderImpl::get_requested_or_available_topics() 290 | { 291 | auto all_topics_and_types = node->get_topic_names_and_types(); 292 | return topic_filter_->filter_topics(all_topics_and_types); 293 | } 294 | 295 | std::vector RecorderImpl::get_unknown_topics() const 296 | { 297 | std::vector unknown_topics; 298 | for (const auto & topic : record_options_.topics) { 299 | if (subscriptions_.find(topic) == subscriptions_.end()) { 300 | unknown_topics.push_back(topic); 301 | } 302 | } 303 | return unknown_topics; 304 | } 305 | 306 | std::unordered_map 307 | RecorderImpl::get_missing_topics(const std::unordered_map & all_topics) 308 | { 309 | std::unordered_map missing_topics; 310 | for (const auto & i : all_topics) { 311 | if (subscriptions_.find(i.first) == subscriptions_.end()) { 312 | missing_topics.emplace(i.first, i.second); 313 | } 314 | } 315 | return missing_topics; 316 | } 317 | 318 | 319 | void RecorderImpl::subscribe_topics( 320 | const std::unordered_map & topics_and_types) 321 | { 322 | for (const auto & topic_with_type : topics_and_types) { 323 | auto endpoint_infos = node->get_publishers_info_by_topic(topic_with_type.first); 324 | subscribe_topic( 325 | { 326 | 0u, 327 | topic_with_type.first, 328 | topic_with_type.second, 329 | serialization_format_, 330 | offered_qos_profiles_for_topic(endpoint_infos), 331 | type_description_hash_for_topic(endpoint_infos), 332 | }); 333 | } 334 | } 335 | 336 | void RecorderImpl::subscribe_topic(const rosbag2_storage::TopicMetadata & topic) 337 | { 338 | // Need to create topic in writer before we are trying to create subscription. Since in 339 | // callback for subscription we are calling writer_->write(bag_message); and it could happened 340 | // that callback called before we reached out the line: writer_->create_topic(topic) 341 | writer_->create_topic(topic); 342 | 343 | rosbag2_storage::Rosbag2QoS subscription_qos{subscription_qos_for_topic(topic.name)}; 344 | 345 | auto subscription = create_subscription(topic.name, topic.type, subscription_qos); 346 | if (subscription) { 347 | subscriptions_.insert({topic.name, subscription}); 348 | topics_info_[topic.name]; 349 | RCLCPP_INFO_STREAM( 350 | node->get_logger(), 351 | "Subscribed to topic '" << topic.name << "'"); 352 | } else { 353 | writer_->remove_topic(topic); 354 | subscriptions_.erase(topic.name); 355 | } 356 | } 357 | 358 | std::shared_ptr 359 | RecorderImpl::create_subscription( 360 | const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos) 361 | { 362 | #ifdef _WIN32 363 | if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") != 364 | std::string::npos) 365 | { 366 | return node->create_generic_subscription( 367 | topic_name, 368 | topic_type, 369 | qos, 370 | [this, topic_name, topic_type](std::shared_ptr message, 371 | const rclcpp::MessageInfo &) { 372 | if (!paused_.load()) { 373 | update_topic_statistics(topic_name, std::chrono::nanoseconds(mi.get_rmw_message_info().received_timestamp), message->size()); 374 | writer_->write( 375 | std::move(message), topic_name, topic_type, node->now().nanoseconds(), 376 | 0); 377 | } 378 | }); 379 | } 380 | #endif 381 | 382 | if (record_options_.use_sim_time) { 383 | return node->create_generic_subscription( 384 | topic_name, 385 | topic_type, 386 | qos, 387 | [this, topic_name, topic_type](std::shared_ptr message, 388 | const rclcpp::MessageInfo & mi) { 389 | if (!paused_.load()) { 390 | update_topic_statistics(topic_name, std::chrono::nanoseconds(mi.get_rmw_message_info().received_timestamp), message->size()); 391 | writer_->write( 392 | std::move(message), topic_name, topic_type, node->now().nanoseconds(), 393 | mi.get_rmw_message_info().source_timestamp); 394 | } 395 | }); 396 | } else { 397 | return node->create_generic_subscription( 398 | topic_name, 399 | topic_type, 400 | qos, 401 | [this, topic_name, topic_type](std::shared_ptr message, 402 | const rclcpp::MessageInfo & mi) { 403 | if (!paused_.load()) { 404 | update_topic_statistics(topic_name, std::chrono::nanoseconds(mi.get_rmw_message_info().received_timestamp), message->size()); 405 | writer_->write( 406 | std::move(message), topic_name, topic_type, 407 | mi.get_rmw_message_info().received_timestamp, 408 | mi.get_rmw_message_info().source_timestamp); 409 | } 410 | }); 411 | } 412 | } 413 | 414 | void RecorderImpl::update_topic_statistics( 415 | const std::string & topic_name, std::chrono::nanoseconds stamp, int size) 416 | { 417 | if (!first_msg_received_) { 418 | first_stamp_ = node->now(); 419 | first_msg_received_ = true; 420 | } 421 | topics_info_[topic_name].update_statistics(stamp, size); 422 | } 423 | 424 | void RecorderImpl::update_topic_publisher_info() { 425 | for (auto &topic_info : topics_info_) { 426 | const auto &topic_name = topic_info.first; 427 | auto &info = topic_info.second; 428 | auto endpoint_infos = node->get_publishers_info_by_topic(topic_name); 429 | if (!endpoint_infos.empty()) { 430 | std::string topic_type = endpoint_infos[0].topic_type(); 431 | int publisher_count = endpoint_infos.size(); 432 | std::string qos = reliability_to_string( 433 | endpoint_infos[0].qos_profile().reliability()); 434 | info.update_publisher_info(topic_type, publisher_count, qos); 435 | } else { 436 | info.update_publisher_info("Unknown", 0, "Unknown"); 437 | } 438 | } 439 | } 440 | 441 | std::vector RecorderImpl::offered_qos_profiles_for_topic( 442 | const std::vector & topics_endpoint_info) const 443 | { 444 | std::vector offered_qos_profiles; 445 | for (const auto & info : topics_endpoint_info) { 446 | offered_qos_profiles.push_back(info.qos_profile()); 447 | } 448 | return offered_qos_profiles; 449 | } 450 | 451 | const std::unordered_map& RecorderImpl::get_topics_info() { 452 | if (record_options_.is_discovery_disabled) { 453 | update_topic_publisher_info(); 454 | } 455 | return topics_info_; 456 | } 457 | 458 | const rclcpp::Duration RecorderImpl::get_bagfile_duration() const 459 | { 460 | if (!first_msg_received_) { 461 | return rclcpp::Duration(0, 0); 462 | } 463 | return rclcpp::Duration(node->now() - first_stamp_); 464 | } 465 | 466 | const uint64_t RecorderImpl::get_bagfile_size() const 467 | { 468 | size_t total_size = 0; 469 | for (const auto & topic_info : topics_info_) { 470 | total_size += topic_info.second.size(); 471 | } 472 | return total_size; 473 | } 474 | 475 | const std::vector & RecorderImpl::get_files() const 476 | { 477 | return files_; 478 | } 479 | 480 | std::string type_hash_to_string(const rosidl_type_hash_t & type_hash) 481 | { 482 | if (type_hash.version == 0) { 483 | // version is unset, this is an empty type hash. 484 | return ""; 485 | } 486 | if (type_hash.version > 1) { 487 | // this is a version we don't know how to serialize 488 | ROSBAG2_TRANSPORT_LOG_WARN_STREAM( 489 | "attempted to stringify type hash with unknown version " << type_hash.version); 490 | return ""; 491 | } 492 | rcutils_allocator_t allocator = rcutils_get_default_allocator(); 493 | char * stringified_type_hash = nullptr; 494 | rcutils_ret_t status = rosidl_stringify_type_hash(&type_hash, allocator, &stringified_type_hash); 495 | std::string result = ""; 496 | if (status == RCUTILS_RET_OK) { 497 | result = stringified_type_hash; 498 | } 499 | if (stringified_type_hash != nullptr) { 500 | allocator.deallocate(stringified_type_hash, allocator.state); 501 | } 502 | return result; 503 | } 504 | 505 | std::string type_description_hash_for_topic( 506 | const std::vector & topics_endpoint_info) 507 | { 508 | rosidl_type_hash_t result_hash = rosidl_get_zero_initialized_type_hash(); 509 | for (const auto & info : topics_endpoint_info) { 510 | // If all endpoint infos provide the same type hash, return it. Otherwise return an empty 511 | // string to signal that the type description hash for this topic cannot be determined. 512 | rosidl_type_hash_t endpoint_hash = info.topic_type_hash(); 513 | if (endpoint_hash.version == 0) { 514 | continue; 515 | } 516 | if (result_hash.version == 0) { 517 | result_hash = endpoint_hash; 518 | continue; 519 | } 520 | bool difference_detected = (endpoint_hash.version != result_hash.version); 521 | difference_detected |= ( 522 | 0 != memcmp(endpoint_hash.value, result_hash.value, ROSIDL_TYPE_HASH_SIZE)); 523 | if (difference_detected) { 524 | std::string result_string = type_hash_to_string(result_hash); 525 | std::string endpoint_string = type_hash_to_string(endpoint_hash); 526 | ROSBAG2_TRANSPORT_LOG_WARN_STREAM( 527 | "type description hashes for topic type '" << info.topic_type() << "' conflict: '" << 528 | result_string << "' != '" << endpoint_string << "'"); 529 | return ""; 530 | } 531 | } 532 | return type_hash_to_string(result_hash); 533 | } 534 | 535 | std::string reliability_to_string( 536 | const rclcpp::ReliabilityPolicy & reliability) 537 | { 538 | switch (reliability) { 539 | case rclcpp::ReliabilityPolicy::BestEffort: 540 | return "Best E."; 541 | case rclcpp::ReliabilityPolicy::Reliable: 542 | return "Rel."; 543 | case rclcpp::ReliabilityPolicy::SystemDefault: 544 | return "Sys. Default"; 545 | default: 546 | return "Unknown"; 547 | } 548 | } 549 | 550 | rclcpp::QoS RecorderImpl::subscription_qos_for_topic(const std::string & topic_name) const 551 | { 552 | if (topic_qos_profile_overrides_.count(topic_name)) { 553 | RCLCPP_INFO_STREAM( 554 | node->get_logger(), 555 | "Overriding subscription profile for " << topic_name); 556 | return topic_qos_profile_overrides_.at(topic_name); 557 | } 558 | return rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( 559 | topic_name, node->get_publishers_info_by_topic(topic_name)); 560 | } 561 | 562 | void RecorderImpl::warn_if_new_qos_for_subscribed_topic(const std::string & topic_name) 563 | { 564 | auto existing_subscription = subscriptions_.find(topic_name); 565 | if (existing_subscription == subscriptions_.end()) { 566 | // Not subscribed yet 567 | return; 568 | } 569 | if (topics_warned_about_incompatibility_.count(topic_name) > 0) { 570 | // Already warned about this topic 571 | return; 572 | } 573 | const auto actual_qos = existing_subscription->second->get_actual_qos(); 574 | const auto & used_profile = actual_qos.get_rmw_qos_profile(); 575 | auto publishers_info = node->get_publishers_info_by_topic(topic_name); 576 | for (const auto & info : publishers_info) { 577 | auto new_profile = info.qos_profile().get_rmw_qos_profile(); 578 | bool incompatible_reliability = 579 | new_profile.reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT && 580 | used_profile.reliability != RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; 581 | bool incompatible_durability = 582 | new_profile.durability == RMW_QOS_POLICY_DURABILITY_VOLATILE && 583 | used_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE; 584 | 585 | if (incompatible_reliability) { 586 | RCLCPP_WARN_STREAM( 587 | node->get_logger(), 588 | "A new publisher for subscribed topic " << topic_name << " " 589 | "was found offering RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, " 590 | "but rosbag already subscribed requesting RMW_QOS_POLICY_RELIABILITY_RELIABLE. " 591 | "Messages from this new publisher will not be recorded."); 592 | topics_warned_about_incompatibility_.insert(topic_name); 593 | } else if (incompatible_durability) { 594 | RCLCPP_WARN_STREAM( 595 | node->get_logger(), 596 | "A new publisher for subscribed topic " << topic_name << " " 597 | "was found offering RMW_QOS_POLICY_DURABILITY_VOLATILE, " 598 | "but rosbag2 already subscribed requesting RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL. " 599 | "Messages from this new publisher will not be recorded."); 600 | topics_warned_about_incompatibility_.insert(topic_name); 601 | } 602 | } 603 | } 604 | 605 | } // namespace hector_recorder 606 | 607 | --------------------------------------------------------------------------------