diff --git a/realsense2_camera/examples/launch_from_rosbag/README.md b/realsense2_camera/examples/launch_from_rosbag/README.md index 5bfbb017f5..4981c594c8 100644 --- a/realsense2_camera/examples/launch_from_rosbag/README.md +++ b/realsense2_camera/examples/launch_from_rosbag/README.md @@ -14,4 +14,9 @@ or ros2 launch realsense2_camera rs_launch.py rosbag_filename:="/full/path/to/rosbag/file" ``` +Additionally, the 'rosbag_loop' cmd line argument enables the looped playback of the rosbag file: +``` +ros2 launch realsense2_camera rs_launch_from_rosbag.py rosbag_filename:="/full/path/to/rosbag/file" rosbag_loop:="true" +``` + Check-out [sample-recordings](https://github.com/IntelRealSense/librealsense/blob/master/doc/sample-data.md) for a few recorded samples. \ No newline at end of file diff --git a/realsense2_camera/examples/launch_from_rosbag/rs_launch_from_rosbag.py b/realsense2_camera/examples/launch_from_rosbag/rs_launch_from_rosbag.py index a0d9620cad..e3ef9227ff 100644 --- a/realsense2_camera/examples/launch_from_rosbag/rs_launch_from_rosbag.py +++ b/realsense2_camera/examples/launch_from_rosbag/rs_launch_from_rosbag.py @@ -38,6 +38,7 @@ {'name': 'enable_gyro', 'default': 'true', 'description': "'enable gyro stream'"}, {'name': 'enable_accel', 'default': 'true', 'description': "'enable accel stream'"}, {'name': 'rosbag_filename', 'default': [ThisLaunchFileDir(), "/rosbag/D435i_Depth_and_IMU_Stands_still.bag"], 'description': 'A realsense bagfile to run from as a device'}, + {'name': 'rosbag_loop', 'default': 'false', 'description': 'enable realsense bagfile loop playback'}, ] def set_configurable_parameters(local_params): @@ -48,7 +49,7 @@ def generate_launch_description(): params = rs_launch.configurable_parameters return LaunchDescription( rs_launch.declare_configurable_parameters(local_parameters) + - rs_launch.declare_configurable_parameters(params) + + rs_launch.declare_configurable_parameters(params) + [ OpaqueFunction(function=rs_launch.launch_setup, kwargs = {'params' : set_configurable_parameters(params)} diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 14f3139c4b..fdc2a05f10 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -31,6 +31,7 @@ {'name': 'initial_reset', 'default': 'false', 'description': "''"}, {'name': 'accelerate_gpu_with_glsl', 'default': "false", 'description': 'enable GPU acceleration with GLSL'}, {'name': 'rosbag_filename', 'default': "''", 'description': 'A realsense bagfile to run from as a device'}, + {'name': 'rosbag_loop', 'default': 'false', 'description': 'Enable loop playback when playing a bagfile'}, {'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'}, {'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'}, {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index f80ed002e4..791e3a9529 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -38,7 +38,7 @@ RealSenseNodeFactory::RealSenseNodeFactory(const rclcpp::NodeOptions & node_opti } RealSenseNodeFactory::RealSenseNodeFactory(const std::string & node_name, const std::string & ns, - const rclcpp::NodeOptions & node_options) : + const rclcpp::NodeOptions & node_options) : Node(node_name, ns, node_options), _logger(this->get_logger()) { @@ -200,7 +200,7 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) ROS_INFO("Resetting device..."); _device.hardware_reset(); _device = rs2::device(); - + } catch(const std::exception& ex) { @@ -294,7 +294,39 @@ void RealSenseNodeFactory::init() } if (_device) { + bool rosbag_loop(declare_parameter("rosbag_loop", rclcpp::ParameterValue(false)).get()); startDevice(); + + if (rosbag_loop) + { + auto playback = _device.as(); // Object to check the playback status periodically. + bool is_playing = true; // Flag to indicate if the playback is active + + while (rclcpp::ok()) + { + // Check the current status only if the playback is not active + auto status = playback.current_status(); + if (!is_playing && status == RS2_PLAYBACK_STATUS_STOPPED) + { + RCLCPP_INFO(this->get_logger(), "Bag file playback has completed and it is going to be replayed."); + startDevice(); // Re-start bag file execution + is_playing = true; // Set the flag to true as playback has been restarted + } + else if (status != RS2_PLAYBACK_STATUS_STOPPED) + { + // If the playback status is not stopped, it means the playback is active + is_playing = true; + } + else + { + // If the playback status is stopped, set the flag to false + is_playing = false; + } + + // Add a small delay to prevent busy-waiting + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } } } else @@ -410,7 +442,7 @@ void RealSenseNodeFactory::startDevice() std::cerr << "Failed to start device: " << e.what() << '\n'; _device.hardware_reset(); _device = rs2::device(); - } + } } void RealSenseNodeFactory::tryGetLogSeverity(rs2_log_severity& severity) const