From 9d8987743a313a5f59b4e3d68288dbaee6e7beef Mon Sep 17 00:00:00 2001 From: "Cornaglia, Alessandro" Date: Wed, 25 Sep 2024 10:10:28 +0200 Subject: [PATCH 1/6] Added ROS bag loop option Signed-off-by: Cornaglia, Alessandro --- realsense2_camera/launch/rs_launch.py | 1 + realsense2_camera/src/realsense_node_factory.cpp | 14 +++++++++----- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index a79df0f84b..b239bf94c8 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 f7ea18bca7..788b27b00f 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()) { @@ -123,7 +123,7 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) } else { - ROS_INFO_STREAM("Device with port number " << port_id << " was found."); + ROS_INFO_STREAM("Device with port number " << port_id << " was found."); } bool found_device_type(true); if (!_device_type.empty()) @@ -193,7 +193,7 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) ROS_INFO("Resetting device..."); _device.hardware_reset(); _device = rs2::device(); - + } catch(const std::exception& ex) { @@ -287,7 +287,11 @@ void RealSenseNodeFactory::init() } if (_device) { - startDevice(); + bool rosbag_loop(declare_parameter("rosbag_loop", rclcpp::ParameterValue(false)).getrclcpp::PARAMETER_BOOL()); + do + { + startDevice(); + } while (rosbag_loop); // Terminate loop only after CTRL-C } } else @@ -390,7 +394,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 From c6e006ae9a9bd762c9d44830b31e4d41793a2f8b Mon Sep 17 00:00:00 2001 From: "Cornaglia, Alessandro" Date: Wed, 25 Sep 2024 15:22:49 +0200 Subject: [PATCH 2/6] Fixed compilation error Signed-off-by: Cornaglia, Alessandro --- realsense2_camera/src/realsense_node_factory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 788b27b00f..8ad11e8955 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -287,7 +287,7 @@ void RealSenseNodeFactory::init() } if (_device) { - bool rosbag_loop(declare_parameter("rosbag_loop", rclcpp::ParameterValue(false)).getrclcpp::PARAMETER_BOOL()); + bool rosbag_loop(declare_parameter("rosbag_loop", rclcpp::ParameterValue(false)).get()); do { startDevice(); From 1b498c1f703a4e38fa9d3294a99576ea9dc4d6d4 Mon Sep 17 00:00:00 2001 From: "Cornaglia, Alessandro" Date: Thu, 26 Sep 2024 17:25:46 +0200 Subject: [PATCH 3/6] Fixed bag looping mechanism Signed-off-by: Cornaglia, Alessandro --- .../src/realsense_node_factory.cpp | 33 +++++++++++++++++-- 1 file changed, 30 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 8ad11e8955..baad584816 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -288,10 +288,37 @@ void RealSenseNodeFactory::init() if (_device) { bool rosbag_loop(declare_parameter("rosbag_loop", rclcpp::ParameterValue(false)).get()); - do + startDevice(); + + if (rosbag_loop) { - startDevice(); - } while (rosbag_loop); // Terminate loop only after CTRL-C + 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 + if (!is_playing && playback.current_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 (playback.current_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 From 1797686830bc273f5cc773f3c07d0ecc9b8be449 Mon Sep 17 00:00:00 2001 From: "Cornaglia, Alessandro" Date: Fri, 27 Sep 2024 15:06:31 +0200 Subject: [PATCH 4/6] Added loop back argument description to launch_from_rosbag README Signed-off-by: Cornaglia, Alessandro --- realsense2_camera/examples/launch_from_rosbag/README.md | 5 +++++ 1 file changed, 5 insertions(+) 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 From d47eca549a520dc1d4839f5ff1230ee43bf5e816 Mon Sep 17 00:00:00 2001 From: "Cornaglia, Alessandro" Date: Fri, 27 Sep 2024 15:11:41 +0200 Subject: [PATCH 5/6] Added rosbag_loop parameter to launch_from_rosbag example Signed-off-by: Cornaglia, Alessandro --- .../examples/launch_from_rosbag/rs_launch_from_rosbag.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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)} From 171d50581e58dd506da0c70275c45fc9f5fedea6 Mon Sep 17 00:00:00 2001 From: "Cornaglia, Alessandro" Date: Thu, 17 Oct 2024 15:02:54 +0200 Subject: [PATCH 6/6] Using variable for avoiding double function call Signed-off-by: Cornaglia, Alessandro --- realsense2_camera/src/realsense_node_factory.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index baad584816..502ac2471a 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -298,13 +298,14 @@ void RealSenseNodeFactory::init() while (rclcpp::ok()) { // Check the current status only if the playback is not active - if (!is_playing && playback.current_status() == RS2_PLAYBACK_STATUS_STOPPED) + 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 (playback.current_status() != RS2_PLAYBACK_STATUS_STOPPED) + else if (status != RS2_PLAYBACK_STATUS_STOPPED) { // If the playback status is not stopped, it means the playback is active is_playing = true;