diff --git a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/snapshot_capture.hpp b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/snapshot_capture.hpp index 08438abe..d02226fb 100644 --- a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/snapshot_capture.hpp +++ b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/snapshot_capture.hpp @@ -51,8 +51,8 @@ struct RosbagConfig { /// If false (default), ring buffer runs continuously from startup bool lazy_start{false}; - /// Storage format: "sqlite3" or "mcap" - std::string format{"sqlite3"}; + /// Storage format: "mcap" or "sqlite3" + std::string format{"mcap"}; /// Path to store bag files (empty = system temp directory) std::string storage_path; diff --git a/src/ros2_medkit_fault_manager/package.xml b/src/ros2_medkit_fault_manager/package.xml index e348aade..1acfa8c4 100644 --- a/src/ros2_medkit_fault_manager/package.xml +++ b/src/ros2_medkit_fault_manager/package.xml @@ -17,6 +17,7 @@ nlohmann-json-dev rosbag2_cpp rosbag2_storage + rosbag2_storage_mcap ament_lint_auto ament_lint_common @@ -27,7 +28,6 @@ launch_testing_ros sensor_msgs std_msgs - rosbag2_storage_mcap ament_cmake diff --git a/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp b/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp index e9c2620c..f492e91c 100644 --- a/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp +++ b/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp @@ -643,7 +643,7 @@ SnapshotConfig FaultManagerNode::create_snapshot_config() { declare_parameter>("snapshots.rosbag.exclude_topics", std::vector{}); config.rosbag.lazy_start = declare_parameter("snapshots.rosbag.lazy_start", false); - config.rosbag.format = declare_parameter("snapshots.rosbag.format", "sqlite3"); + config.rosbag.format = declare_parameter("snapshots.rosbag.format", "mcap"); config.rosbag.storage_path = declare_parameter("snapshots.rosbag.storage_path", ""); int64_t max_bag_size = declare_parameter("snapshots.rosbag.max_bag_size_mb", 50); diff --git a/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp b/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp index 3d42768f..95083d90 100644 --- a/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp +++ b/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp @@ -679,42 +679,34 @@ void RosbagCapture::validate_storage_format() const { "Valid options: 'sqlite3', 'mcap'"); } - // sqlite3 is always available (built into rosbag2) - if (config_.format == "sqlite3") { - return; - } - - // For MCAP, verify the plugin is available by trying to create a test bag - if (config_.format == "mcap") { - std::string test_path = - std::filesystem::temp_directory_path().string() + "/.rosbag_mcap_test_" + std::to_string(getpid()); - - try { - rosbag2_cpp::Writer writer; - rosbag2_storage::StorageOptions opts; - opts.uri = test_path; - opts.storage_id = "mcap"; - writer.open(opts); - // Success - plugin is available, clean up test file - } catch (const std::exception & e) { - // Clean up any partial test files - std::error_code ec; - std::filesystem::remove_all(test_path, ec); - - throw std::runtime_error( - "MCAP storage format requested but rosbag2_storage_mcap plugin is not available. " - "Install with: sudo apt install ros-${ROS_DISTRO}-rosbag2-storage-mcap " - "Or use format: 'sqlite3' (default). " - "Error: " + - std::string(e.what())); - } + // Verify the selected storage plugin is available by trying to create a test bag + std::string test_path = + std::filesystem::temp_directory_path().string() + "/.rosbag_format_test_" + std::to_string(getpid()); - // Clean up test file + try { + rosbag2_cpp::Writer writer; + rosbag2_storage::StorageOptions opts; + opts.uri = test_path; + opts.storage_id = config_.format; + writer.open(opts); + // Success - plugin is available + } catch (const std::exception & e) { + // Clean up any partial test files std::error_code ec; std::filesystem::remove_all(test_path, ec); - RCLCPP_INFO(node_->get_logger(), "MCAP storage format validated successfully"); + throw std::runtime_error("Rosbag storage format '" + config_.format + + "' is not available. " + "Install the plugin or use a different format. " + "Error: " + + std::string(e.what())); } + + // Clean up test file + std::error_code ec; + std::filesystem::remove_all(test_path, ec); + + RCLCPP_INFO(node_->get_logger(), "%s storage format validated successfully", config_.format.c_str()); } } // namespace ros2_medkit_fault_manager diff --git a/src/ros2_medkit_fault_manager/test/test_rosbag_capture.cpp b/src/ros2_medkit_fault_manager/test/test_rosbag_capture.cpp index b1117cc1..3327bf9e 100644 --- a/src/ros2_medkit_fault_manager/test/test_rosbag_capture.cpp +++ b/src/ros2_medkit_fault_manager/test/test_rosbag_capture.cpp @@ -58,7 +58,7 @@ class RosbagCaptureTest : public ::testing::Test { config.duration_sec = 2.0; config.duration_after_sec = 0.5; config.topics = "all"; - config.format = "sqlite3"; + config.format = "mcap"; config.storage_path = temp_dir_.string(); config.max_bag_size_mb = 10; config.max_total_storage_mb = 50; @@ -117,9 +117,9 @@ TEST_F(RosbagCaptureTest, ConstructorThrowsOnInvalidFormat) { } // @verifies REQ_INTEROP_088 -TEST_F(RosbagCaptureTest, ConstructorAcceptsSqlite3Format) { +TEST_F(RosbagCaptureTest, ConstructorAcceptsMcapFormat) { auto rosbag_config = create_rosbag_config(); - rosbag_config.format = "sqlite3"; + rosbag_config.format = "mcap"; auto snapshot_config = create_snapshot_config(); EXPECT_NO_THROW(RosbagCapture(node_.get(), storage_.get(), rosbag_config, snapshot_config)); } @@ -293,19 +293,10 @@ TEST_F(RosbagCaptureTest, CustomStoragePathAccepted) { // Format tests -TEST_F(RosbagCaptureTest, Sqlite3FormatAccepted) { - auto rosbag_config = create_rosbag_config(); - rosbag_config.format = "sqlite3"; - auto snapshot_config = create_snapshot_config(); - EXPECT_NO_THROW(RosbagCapture(node_.get(), storage_.get(), rosbag_config, snapshot_config)); -} - TEST_F(RosbagCaptureTest, McapFormatAccepted) { auto rosbag_config = create_rosbag_config(); rosbag_config.format = "mcap"; auto snapshot_config = create_snapshot_config(); - // Note: mcap might not be available in all ROS installations - // The constructor should not throw, but actual writing might fail EXPECT_NO_THROW(RosbagCapture(node_.get(), storage_.get(), rosbag_config, snapshot_config)); } diff --git a/src/ros2_medkit_fault_manager/test/test_rosbag_integration.test.py b/src/ros2_medkit_fault_manager/test/test_rosbag_integration.test.py index eb46ec81..a6bb8122 100644 --- a/src/ros2_medkit_fault_manager/test/test_rosbag_integration.test.py +++ b/src/ros2_medkit_fault_manager/test/test_rosbag_integration.test.py @@ -160,7 +160,7 @@ def main(): 'snapshots.rosbag.duration_after_sec': 0.5, # 0.5 second after confirm # Use explicit topics - faster than discovery, more reliable for tests 'snapshots.rosbag.topics': '/test/temperature,/test/status', - 'snapshots.rosbag.format': 'sqlite3', + 'snapshots.rosbag.format': 'mcap', 'snapshots.rosbag.storage_path': ROSBAG_STORAGE_PATH, 'snapshots.rosbag.max_bag_size_mb': 10, 'snapshots.rosbag.max_total_storage_mb': 50, @@ -271,7 +271,7 @@ def test_01_rosbag_created_on_fault_confirmation(self): self.assertTrue(rosbag_response.success, f'GetRosbag failed: {rosbag_response.error_message}') self.assertGreater(len(rosbag_response.file_path), 0) - self.assertEqual(rosbag_response.format, 'sqlite3') + self.assertEqual(rosbag_response.format, 'mcap') self.assertGreater(rosbag_response.duration_sec, 0) self.assertGreater(rosbag_response.size_bytes, 0)