PR #3214 from acornaglia: Add ROS bag loop option

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.

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)}

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'},

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<rclcpp::PARAMETER_BOOL>());
                 startDevice();
+
+                if (rosbag_loop)
+                {
+                    auto playback = _device.as<rs2::playback>(); // 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