From b1e3963aa4f45a9d2f49403e376567be98b11c50 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Tue, 10 Oct 2023 03:58:57 +0300 Subject: [PATCH 001/111] Update README.md --- README.md | 71 +++++++++++++++---------------------------------------- 1 file changed, 19 insertions(+), 52 deletions(-) diff --git a/README.md b/README.md index 0311e9ed2b..dfe404158b 100644 --- a/README.md +++ b/README.md @@ -23,11 +23,11 @@
## Table of contents - * [ROS1 and ROS2 legacy](#legacy) + * [ROS1 and ROS2 legacy](#ros1-and-ros2-legacy) * [Installation](#installation) * [Usage](#usage) - * [Starting the camera node](#start-camera-node) - * [Camera name and namespace](#camera-name-and-namespace) + * [Starting the camera node](#start-the-camera-node) + * [Camera name and namespace](#camera-name-and-camera-namespace) * [Parameters](#parameters) * [ROS2-vs-Optical Coordination Systems](#coordination) * [TF from coordinate A to coordinate B](#tfs) @@ -43,9 +43,7 @@
-

- Legacy -

+# ROS1 and ROS2 Legacy
@@ -80,9 +78,7 @@
-

- Installation -

+# Installation
@@ -165,13 +161,9 @@
-

- Usage -

+# Usage -

- Start the camera node -

+## Start the camera node #### with ros2 run: ros2 run realsense2_camera realsense2_camera_node @@ -184,9 +176,7 @@
-

- Camera Name And Camera Namespace -

+## Camera Name And Camera Namespace User can set the camera name and camera namespace, to distinguish between cameras and platforms, which helps identifying the right nodes and topics to work with. @@ -247,13 +237,9 @@ User can set the camera name and camera namespace, to distinguish between camera /camera/camera/device_info ``` -
- -

- Parameters -

+## Parameters ### Available Parameters: - For the entire list of parameters type `ros2 param list`. @@ -371,9 +357,7 @@ The `/diagnostics` topic includes information regarding the device temperatures
-

- ROS2(Robot) vs Optical(Camera) Coordination Systems: -

+## ROS2(Robot) vs Optical(Camera) Coordination Systems: - Point Of View: - Imagine we are standing behind of the camera, and looking forward. @@ -389,9 +373,7 @@ The `/diagnostics` topic includes information regarding the device temperatures
-

- TF from coordinate A to coordinate B: -

+## TF from coordinate A to coordinate B: - TF msg expresses a transform from coordinate frame "header.frame_id" (source) to the coordinate frame child_frame_id (destination) [Reference](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Transform.html) - In RealSense cameras, the origin point (0,0,0) is taken from the left IR (infra1) position and named as "camera_link" frame @@ -403,10 +385,7 @@ The `/diagnostics` topic includes information regarding the device temperatures
-

- Extrinsics from sensor A to sensor B: -

- +## Extrinsics from sensor A to sensor B: - Extrinsic from sensor A to sensor B means the position and orientation of sensor A relative to sensor B. - Imagine that B is the origin (0,0,0), then the Extrensics(A->B) describes where is sensor A relative to sensor B. @@ -441,9 +420,7 @@ translation:
-

- Published Topics -

+## Published Topics The published topics differ according to the device and parameters. After running the above command with D435i attached, the following list of topics will be available (This is a partial list. For full one type `ros2 topic list`): @@ -485,9 +462,7 @@ Enabling stream adds matching topics. For instance, enabling the gyro and accel
-

- RGBD Topic -

+## RGBD Topic RGBD new topic, publishing [RGB + Depth] in the same message (see RGBD.msg for reference). For now, works only with depth aligned to color images, as color and depth images are synchronized by frame time tag. @@ -507,9 +482,7 @@ ros2 launch realsense2_camera rs_launch.py enable_rgbd:=true enable_sync:=true a
-

- Metadata topic -

+## Metadata topic The metadata messages store the camera's available metadata in a *json* format. To learn more, a dedicated script for echoing a metadata topic in runtime is attached. For instance, use the following command to echo the camera/depth/metadata topic: ``` @@ -518,10 +491,8 @@ python3 src/realsense-ros/realsense2_camera/scripts/echo_metadada.py /camera/cam
-

- Post-Processing Filters -

- +## Post-Processing Filters + The following post processing filters are available: - ```align_depth```: If enabled, will publish the depth image aligned to the color image on the topic `/camera/camera/aligned_depth_to_color/image_raw`. - The pointcloud, if created, will be based on the aligned depth image. @@ -551,17 +522,13 @@ Each of the above filters have it's own parameters, following the naming convent
-

- Available services -

+## Available services - device_info : retrieve information about the device - serial_number, firmware_version etc. Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. Call example: `ros2 service call /camera/camera/device_info realsense2_camera_msgs/srv/DeviceInfo`
-

- Efficient intra-process communication: -

+## Efficient intra-process communication: Our ROS2 Wrapper node supports zero-copy communications if loaded in the same process as a subscriber node. This can reduce copy times on image/pointcloud topics, especially with big frame resolutions and high FPS. From 88818df2e6f03982f96a78884b0fcea08d6e573b Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Tue, 10 Oct 2023 04:01:26 +0300 Subject: [PATCH 002/111] Update README.md --- README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index dfe404158b..522ec969a6 100644 --- a/README.md +++ b/README.md @@ -29,15 +29,15 @@ * [Starting the camera node](#start-the-camera-node) * [Camera name and namespace](#camera-name-and-camera-namespace) * [Parameters](#parameters) - * [ROS2-vs-Optical Coordination Systems](#coordination) - * [TF from coordinate A to coordinate B](#tfs) - * [Extrinsics from sensor A to sensor B](#extrinsics) - * [Topics](#topics) - * [RGBD Topic](#rgbd) - * [Metadata Topic](#metadata) - * [Post-Processing Filters](#filters) - * [Available Services](#services) - * [Efficient intra-process communication](#intra-process) + * [ROS2-vs-Optical Coordination Systems](#ros2robot-vs-opticalcamera-coordination-systems) + * [TF from coordinate A to coordinate B](#tf-from-coordinate-a-to-coordinate-b) + * [Extrinsics from sensor A to sensor B](#extrinsics-from-sensor-a-to-sensor-b) + * [Topics](#published-topics) + * [RGBD Topic](#rgbd-topic) + * [Metadata Topic](#metadata-topic) + * [Post-Processing Filters](#post-processing-filters) + * [Available Services](#available-services) + * [Efficient intra-process communication](#efficient-intra-process-communication) * [Contributing](CONTRIBUTING.md) * [License](LICENSE) From 9ef9638020b1debe3f48660304270f39b2927153 Mon Sep 17 00:00:00 2001 From: Arun Prasad <127019120+Arun-Prasad-V@users.noreply.github.com> Date: Mon, 4 Dec 2023 16:05:14 +0530 Subject: [PATCH 003/111] Update main.yml Using setup-ros v0.7 --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 55ed6e9468..ecd144d630 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -49,7 +49,7 @@ jobs: ./pr_check.sh - name: build ROS2 - uses: ros-tooling/setup-ros@v0.6 + uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} From e09414dc09fca43341c3c317e8df3e22084dd9d0 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 22 Nov 2023 06:04:50 +0530 Subject: [PATCH 004/111] Disabling hdr while updating exposure & gain values --- README.md | 3 +++ realsense2_camera/src/ros_sensor.cpp | 19 ++++++++++++++++++- 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 0311e9ed2b..6c0bbf9615 100644 --- a/README.md +++ b/README.md @@ -531,7 +531,10 @@ The following post processing filters are available: * The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting `pointcloud.allow_no_texture_points` to true. * pointcloud is of an unordered format by default. This can be changed by setting `pointcloud.ordered_pc` to true. - ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. + - `depth_module.hdr_enabled`: to enable/disable HDR - The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. + - From FW versions 5.14.x.x and above, if HDR is enabled, the preset configs (like exposure, gain, etc.,) cannot be updated. + - The user should disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. - To initialize these parameters in start time use the following parameters: - `depth_module.exposure.1` diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 635424d8c5..73b9cac11b 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -102,6 +102,24 @@ void RosSensor::UpdateSequenceIdCallback() if (!supports(RS2_OPTION_SEQUENCE_ID)) return; + bool is_hdr_enabled = static_cast(get_option(RS2_OPTION_HDR_ENABLED)); + + // Deleter to revert back the RS2_OPTION_HDR_ENABLED value at the end. + auto deleter_to_revert_hdr = std::unique_ptr>(&is_hdr_enabled, + [&](bool* enable_back_hdr) { + if (enable_back_hdr && *enable_back_hdr) + { + set_option(RS2_OPTION_HDR_ENABLED, true); + } + }); + + // From FW version 5.14.x.x, if HDR is enabled, updating UVC controls like exposure, gain , etc are restricted. + // So, disable it before updating. + if (is_hdr_enabled) + { + set_option(RS2_OPTION_HDR_ENABLED, false); + } + int original_seq_id = static_cast(get_option(RS2_OPTION_SEQUENCE_ID)); // To Set back to default. std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); @@ -146,7 +164,6 @@ void RosSensor::UpdateSequenceIdCallback() ROS_WARN_STREAM("Setting alternative callback: Failed to set parameter:" << option_name << " : " << e.what()); return; } - } void RosSensor::set_sensor_parameter_to_ros(rs2_option option) From 573d6a76fd4c36d4539bec4d67225a8a6de525f1 Mon Sep 17 00:00:00 2001 From: gwen2018 Date: Wed, 13 Dec 2023 18:05:01 -0800 Subject: [PATCH 005/111] add error handling to avoid crash when asic temperature monitor occasionally fails --- realsense2_camera/src/base_realsense_node.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9870d3ac59..a3ebd0e675 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1140,10 +1140,18 @@ void BaseRealSenseNode::startDiagnosticsUpdater() { for (rs2_option option : _monitor_options) { - if (sensor->supports(option)) + try { - status.add(rs2_option_to_string(option), sensor->get_option(option)); - got_temperature = true; + if (sensor->supports(option)) + { + status.add(rs2_option_to_string(option), sensor->get_option(option)); + got_temperature = true; + } + } + catch(const std::exception& ex) + { + got_temperature = false; + ROS_WARN_STREAM("An error has occurred during monitoring: " << ex.what()); } } if (got_temperature) break; From 8384c891b22a74ccd8f0e1ea32a2aca11c3d3d21 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 14 Dec 2023 21:44:48 +0530 Subject: [PATCH 006/111] Return failure in SetParam callback in case of error --- README.md | 2 +- realsense2_camera/src/dynamic_params.cpp | 12 +++++++----- realsense2_camera/src/sensor_params.cpp | 9 +-------- 3 files changed, 9 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index ee99a27473..a77621972e 100644 --- a/README.md +++ b/README.md @@ -505,7 +505,7 @@ The following post processing filters are available: - `depth_module.hdr_enabled`: to enable/disable HDR - The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. - From FW versions 5.14.x.x and above, if HDR is enabled, the preset configs (like exposure, gain, etc.,) cannot be updated. - - The user should disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. + - Disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. - To initialize these parameters in start time use the following parameters: - `depth_module.exposure.1` diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index acce9038ca..e51dfb1818 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -25,6 +25,8 @@ namespace realsense2_camera _params_backend.add_on_set_parameters_callback( [this](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; for (const auto & parameter : parameters) { try @@ -43,15 +45,15 @@ namespace realsense2_camera } } } - catch(const std::out_of_range& e) - {} catch(const std::exception& e) { - std::cerr << e.what() << ":" << parameter.get_name() << '\n'; + result.successful = false; + result.reason = e.what(); + ROS_WARN_STREAM("Set parameter {" << parameter.get_name() + << "} failed: " << e.what()); } } - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; + return result; }); monitor_update_functions(); // Start parameters update thread diff --git a/realsense2_camera/src/sensor_params.cpp b/realsense2_camera/src/sensor_params.cpp index ff0cf151e9..c347a1d979 100644 --- a/realsense2_camera/src/sensor_params.cpp +++ b/realsense2_camera/src/sensor_params.cpp @@ -70,14 +70,7 @@ std::map get_enum_method(rs2::options sensor, rs2_option optio template void param_set_option(rs2::options sensor, rs2_option option, const rclcpp::Parameter& parameter) { - try - { - sensor.set_option(option, parameter.get_value()); - } - catch(const std::exception& e) - { - std::cout << "Failed to set value: " << e.what() << std::endl; - } + sensor.set_option(option, parameter.get_value()); } void SensorParams::clearParameters() From c5e5d3b47687099976506d4658cdab6daf843f21 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 30 Nov 2023 02:43:18 +0530 Subject: [PATCH 007/111] Fixing the data_type of ROS Params exposure & gain --- realsense2_camera/include/ros_sensor.h | 1 + realsense2_camera/src/ros_sensor.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/include/ros_sensor.h b/realsense2_camera/include/ros_sensor.h index c4430bfd4f..57d224300e 100644 --- a/realsense2_camera/include/ros_sensor.h +++ b/realsense2_camera/include/ros_sensor.h @@ -105,6 +105,7 @@ namespace realsense2_camera void set_sensor_auto_exposure_roi(); void registerAutoExposureROIOptions(); void UpdateSequenceIdCallback(); + template void set_sensor_parameter_to_ros(rs2_option option); private: diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 73b9cac11b..2eae865995 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -154,8 +154,8 @@ void RosSensor::UpdateSequenceIdCallback() { set_option(RS2_OPTION_SEQUENCE_ID, parameter.get_value()); std::vector > funcs; - funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_GAIN);}); - funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_EXPOSURE);}); + funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_GAIN);}); + funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_EXPOSURE);}); _params.getParameters()->pushUpdateFunctions(funcs); }); } @@ -166,11 +166,12 @@ void RosSensor::UpdateSequenceIdCallback() } } +template void RosSensor::set_sensor_parameter_to_ros(rs2_option option) { std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); const std::string option_name(module_name + "." + create_graph_resource_name(rs2_option_to_string(option))); - float value = get_option(option); + auto value = static_cast(get_option(option)); _params.getParameters()->setRosParamValue(option_name, &value); } From 25ceeaec89f9528ba7c163845c061ba16a456348 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Sat, 16 Dec 2023 01:21:05 +0530 Subject: [PATCH 008/111] Added live camera test for dynamic seq_id update --- .../test/live_camera/test_d455_basic_tests.py | 53 +++++++++++++++++++ .../test/utils/pytest_rs_utils.py | 23 ++++++++ 2 files changed, 76 insertions(+) diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index 7848ff4986..fb2f58cab6 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -97,3 +97,56 @@ def test_D455_Change_Resolution(self,launch_descr_with_parameters): self.shutdown() +test_params_seq_id_update = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'exposure_1' : 2000, + 'gain_1' : 20, + 'exposure_2' : 3000, + 'gain_2' : 30, + } +''' +This test sets the sequence ID param and validates the corresponding Exposure & Gain values. +''' +@pytest.mark.d455 +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_seq_id_update],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class Test_D455_Seq_ID_Update(pytest_rs_utils.RsTestBaseClass): + def test_D455_Seq_ID_update(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + return + + try: + ''' + initialize, run and check the data + ''' + print("Starting camera test...") + self.init_test("RsTest"+params['camera_name']) + self.wait_for_node(params['camera_name']) + self.create_param_ifs(get_node_heirarchy(params)) + + assert self.set_bool_param('depth_module.hdr_enabled', False) + + assert self.set_integer_param('depth_module.sequence_id', 1) + assert self.set_integer_param('depth_module.exposure', params['exposure_1']) + assert self.set_integer_param('depth_module.gain', params['gain_1']) + + assert self.set_integer_param('depth_module.sequence_id', 2) + assert self.set_integer_param('depth_module.exposure', params['exposure_2']) + assert self.set_integer_param('depth_module.gain', params['gain_2']) + + assert self.set_integer_param('depth_module.sequence_id', 1) + assert self.get_integer_param('depth_module.exposure') == params['exposure_1'] + assert self.get_integer_param('depth_module.gain') == params['gain_1'] + + assert self.set_integer_param('depth_module.sequence_id', 2) + assert self.get_integer_param('depth_module.exposure') == params['exposure_2'] + assert self.get_integer_param('depth_module.gain') == params['gain_2'] + + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index db9c09c3c0..0a0069115f 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -808,6 +808,20 @@ def send_param(self, req): pass return False + def get_param(self, req): + future = self.get_param_if.call_async(req) + while rclpy.ok(): + rclpy.spin_once(self.node) + if future.done(): + try: + response = future.result() + return response.values[0] + except Exception as e: + print("exception raised:") + print(e) + pass + return None + def set_string_param(self, param_name, param_value): req = SetParameters.Request() new_param_value = ParameterValue(type=ParameterType.PARAMETER_STRING, string_value=param_value) @@ -826,6 +840,15 @@ def set_integer_param(self, param_name, param_value): req.parameters = [Parameter(name=param_name, value=new_param_value)] return self.send_param(req) + def get_integer_param(self, param_name): + req = GetParameters.Request() + req.names = [param_name] + value = self.get_param(req) + if (value == None) or (value.type == ParameterType.PARAMETER_NOT_SET): + return None + else: + return value.integer_value + def spin_for_data(self,themes, timeout=5.0): ''' timeout value varies depending upon the system, it needs to be more if From 46108438e348fd91824282978fae209c4448bff5 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Mon, 18 Dec 2023 21:30:34 +0530 Subject: [PATCH 009/111] Disabling HDR during INIT --- realsense2_camera/src/dynamic_params.cpp | 9 ++++++++- realsense2_camera/src/ros_sensor.cpp | 8 ++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index e51dfb1818..45db52d90a 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -151,7 +151,14 @@ namespace realsense2_camera }; if (result_value != initial_value && func) { - func(rclcpp::Parameter(param_name, result_value)); + try + { + func(rclcpp::Parameter(param_name, result_value)); + } + catch(const std::exception& e) + { + ROS_WARN_STREAM("Set parameter {" << param_name << "} failed: " << e.what()); + } } return result_value; } diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 73b9cac11b..b32fa7ca2b 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -85,6 +85,14 @@ RosSensor::~RosSensor() void RosSensor::setParameters(bool is_rosbag_file) { std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); + + // From FW version 5.14.x.x, if HDR is enabled, updating UVC controls like exposure, gain , etc are restricted. + // So, during init of the node, forcefully disabling the HDR upfront and update it with new values. + if((!is_rosbag_file) && supports(RS2_OPTION_HDR_ENABLED)) + { + set_option(RS2_OPTION_HDR_ENABLED, false); + } + _params.registerDynamicOptions(*this, module_name); // for rosbag files, don't set hdr(sequence_id) / gain / exposure options From 0ab7e4fb64b18073835b4e13e9ec5bbac9ee0ca7 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 22 Dec 2023 19:55:27 +0530 Subject: [PATCH 010/111] Added urdf & mesh files for D405 model --- .../pointcloud/rs_d405_pointcloud_launch.py | 92 ++++++++++ realsense2_description/meshes/d405.stl | Bin 0 -> 194684 bytes realsense2_description/urdf/_d405.urdf.xacro | 165 ++++++++++++++++++ .../urdf/test_d405_camera.urdf.xacro | 10 ++ 4 files changed, 267 insertions(+) create mode 100644 realsense2_camera/examples/pointcloud/rs_d405_pointcloud_launch.py create mode 100644 realsense2_description/meshes/d405.stl create mode 100644 realsense2_description/urdf/_d405.urdf.xacro create mode 100644 realsense2_description/urdf/test_d405_camera.urdf.xacro diff --git a/realsense2_camera/examples/pointcloud/rs_d405_pointcloud_launch.py b/realsense2_camera/examples/pointcloud/rs_d405_pointcloud_launch.py new file mode 100644 index 0000000000..e4db15f0bc --- /dev/null +++ b/realsense2_camera/examples/pointcloud/rs_d405_pointcloud_launch.py @@ -0,0 +1,92 @@ +# Copyright 2023 Intel Corporation. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# DESCRIPTION # +# ----------- # +# Use this launch file to launch a device and visualize pointcloud along with the camera model D405. +# The Parameters available for definition in the command line for the camera are described in rs_launch.configurable_parameters +# command line example: +# ros2 launch realsense2_camera rs_d405_pointcloud_launch.py + +"""Launch realsense2_camera node.""" +from launch import LaunchDescription +import launch_ros.actions +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir +import os +import sys +import pathlib +import xacro +import tempfile +from ament_index_python.packages import get_package_share_directory +sys.path.append(str(pathlib.Path(__file__).parent.absolute())) +sys.path.append(os.path.join(get_package_share_directory('realsense2_camera'), 'launch')) +import rs_launch + +local_parameters = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'}, + {'name': 'camera_namespace', 'default': 'camera', 'description': 'camera namespace'}, + {'name': 'device_type', 'default': "d405", 'description': 'choose device by type'}, + {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, + {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, + {'name': 'pointcloud.enable', 'default': 'true', 'description': 'enable pointcloud'}, + ] + +def to_urdf(xacro_path, parameters=None): + """Convert the given xacro file to URDF file. + * xacro_path -- the path to the xacro file + * parameters -- to be used when xacro file is parsed. + """ + urdf_path = tempfile.mktemp(prefix="%s_" % os.path.basename(xacro_path)) + + # open and process file + doc = xacro.process_file(xacro_path, mappings=parameters) + # open the output file + out = xacro.open_output(urdf_path) + out.write(doc.toprettyxml(indent=' ')) + + return urdf_path + +def set_configurable_parameters(local_params): + return dict([(param['name'], LaunchConfiguration(param['name'])) for param in local_params]) + + +def generate_launch_description(): + params = rs_launch.configurable_parameters + xacro_path = os.path.join(get_package_share_directory('realsense2_description'), 'urdf', 'test_d405_camera.urdf.xacro') + urdf = to_urdf(xacro_path, {'use_nominal_extrinsics': 'true', 'add_plug': 'true'}) + return LaunchDescription( + rs_launch.declare_configurable_parameters(local_parameters) + + rs_launch.declare_configurable_parameters(params) + + [ + OpaqueFunction(function=rs_launch.launch_setup, + kwargs = {'params' : set_configurable_parameters(params)} + ), + launch_ros.actions.Node( + package='rviz2', + namespace='', + executable='rviz2', + name='rviz2', + arguments=['-d', [ThisLaunchFileDir(), '/rviz/urdf_pointcloud.rviz']], + output='screen', + parameters=[{'use_sim_time': False}] + ), + launch_ros.actions.Node( + name='model_node', + package='robot_state_publisher', + executable='robot_state_publisher', + namespace='', + output='screen', + arguments=[urdf] + ) + ]) diff --git a/realsense2_description/meshes/d405.stl b/realsense2_description/meshes/d405.stl new file mode 100644 index 0000000000000000000000000000000000000000..413d260f6d2294e19401298bf698be7e4cfa502f GIT binary patch literal 194684 zcmb@v2fQ6s^*uf!O+W~}O0P=qki0t?q$3cTK+353uBxkBhI^d1PLfgp&03Gclb z25bm|6%hC;3K66Tf&wb#zt%ow*17LK@b~%r-{(U-&OK|dea_52Wz9bK?!3(&+pn|p z|8M`yxA8(*w)VKaSIn>te^DJD? zvC*EeJF{-egCn$7C&%%RtyLX1DK`$ab&9odDHDOGJr7U2 ze$td@))zgrfkc#Wv>T3@wIZ79gL9}|Efaw_&qJIm`xeJ-6vvXL+<|B}h?#X$tCItx zPaejrrWS2&jQ`HCv8yTbx**yOf}>i4Xs&yNjENf;VN67=q8Ssz^}!LBvKtBb?g8vs8pJu_=>7$HWF< z@evWtbs}_3R9lFmB8-Vmd2ocqc|@qCbc9n|7$u4@CN^bq=$P0bZ2X9b<~k8NCaNur zY7xNC6ZMm(Z1Z`Hf`p^paM+9-IhyN(bEsV{6QN@w)+Lc0 zYo9nWH02INyFu8BEFzlgM92uo^_8#1PZlVeF)@@o5bXwGE4_$lt`o7|ejDc_Hn^|n zlaU)^eJ}&Hh7`};c@jREQLMGp0!8aulUyOrvgE*z6r!EAk-6<713B) z42W}&dAhakd5arIV{1ln!8&slmy~0TW3hARDds!mba1Q-L@yBknXtHV1iR!=L}O_s zAP)Itp8VJEzgx29jjb8Qlq=^bPPqSc&3(m&tDio3DPvLYHw zs{!%IA6LxJdTEAnG`40G*Ysz_@U_n}j#cNKv-rhvXM^KwK+FQ-P9W4S%Zg|$eI1Bv zk6b^W{l+@R(b$?%EPmr>!xtalHjX8C`f_o^Lv3&naUBqU148Yxtcb=E`)ZBvZ({32 z_ElrcA?_V}PUJXn_&L5Ew}9i2H`i$eg!?2#kmLWVbAceoYMw(q31#JQ?clRJ5gZr3 zu?}`Eja`=6S9cyXA@;80SP+O8(YuGfG{d}V*xenx$(e!gHK7%Hs^z5As|iy;t%h?YaGEYIr!`b zL}O`tAhyTTKIeIh=i%9Pwp>I_eDFf!;M0D~Y8QZmQCkA>@r1>37aO}QGgbnk@#^0r zH_q37q22oFpVl&t#+DlyRypb-<6zX@^8<2h1&-Ozw_6Vbp>|p3=o1i)B}UYf7=4z& z|FRv8tuS^u4vu5n|Ct~6LPi7Q~ z*kH(_b@wr``2kF5uyN`*%E;$s@Sn3=T8(Vl1w_2eNW8zC6E?ajW z6N6oHD59~%QFJm!hJW}mv9UFyIAx<1>o6w%?ZM@0HZq@wyxNh5#`G9N+O{V)IFgAcq^TltVoUW#wqb#OuMap&t|JJ8Ol$3S(l! z(K#kIpSU|1MmY9v_c4)uMZ4tSy$OiMQs80V^+WM>uuBe%i4oCQI?|1~6~^7_$eCZletbrK(~axZ@2qx6#qu1xZcvY3_2t#FYznS# zhC90_5b&&83jgURikNT0RzOdw$1l1C5a(dUNBcj=s!u;z?B*AY?X`;ivqC*)`@ECm=BkIlTevpH8^eo zg6&uZ2(_ySwUlKCVrE_zh}W=AR9k>10Hb>5NcNuYAMS&9b{?t}L{CeEz$nmf9 zMu9^Sp%y&}Wkooze&D^DfDtZm&{ptp!{Os&UwwGYINuI>g?6;mt|HV@mT?8X3=j`v z)fZYwTcM}#`|Ek}o_~BhSFeu(!KeLxAk?lR)KcOnicm{g_VZ@E%2Qi>(l~Zq1-ty? zcy&3-`vb88I2OR1t9D&2hZtz-;Fmt=`8wXUa4pK!B-evsUFueP6XSJfDIl_}`J@xk z>}!c>;~mTPc-KNi_=JdNXF5^Jicq_qh+xTiH3{!nR>Zp&as<0XG&|FYQYI!K)NUst zSaKYr@s8y{ylWvxuuDX6Q!&OwcCjZmRO&E?i~v`g57{{YeRS4l@$?cadj8U zig2qwBCbR`&KPnEc94xNBIt=*O}h5EvLe)OCxYHKULA@%c+Wd;8%MB9M6)w>ch;>F z!O{K}o{|gj<|N#6e$t6(_MG%8aHuDph``PISr$077R!xY_k_BYB@zBz|Hzu7C5vP%Z*((Vz`wh5q|VBuR<*% zn*FjHmE1Z}5o)&+5iGHOJ;44;sr*_lq1 zvLe)zPDHT8`thDu^eWgTqS={Fl(Hh!ZYLsGGKx4}kt5h8qS={Flrk|oCWcy_2=1fY zPM8QC69?l6*IB>eP`fNQb|*@i2ptncEh3sd?nEgoLhW`Uf+g1HUo$2Ky8#hKxMqi} zh)^qxiA}9euXIcdwStFXOr%%c$Hc)9;kxrFj90wp!4^51ovFL$-N(dGs}sRH*o;?! zLw)Z=1a9Y*j)}_A*mX~6!=Ynhs6~!uXX@gn``t_isNv8tG1MX-M4x>2e7m^c_BTt|kA&%y{U!5|)>ZuL)s9hOxiE=kY+t?MMmeLVUZQW2W zWgHK_v0v@VqiUABA)F_QP)k`h)sKm5VoX$9l$h}xA$)hu^Iw2fU6YAM5C zGs3B@fcVkwCq@pIBcjbxsMUDVt`#xBt5)D(t5S_O_ky};{OwZwTWW?H0a3D zX3LR>ScgnWX!*WEjW(wfmu;Oyu)q2!C&&X-Z!iNBT*F@OLmDh)6$a5dOL9f=EAU5dL}Vf=EAU z5dP`uf=EAU5dK-{f=EAU5dO|~L126;IG(UiFwzwqr&z`qR&Xp!`pTX}j(ZsY>>hAz z1j5Q38!-Y|xw(IgD7A}P1;;7cg{6WcX~JQ5P`y%wT8bRzm9lfpMVrhMj*S@QQm+ib zQ5r3@PZXhTN;&isMJRVfpwO-`&K4X=*+O_yaJ*$%+m|Br6Gdn*L`0h$+Cq-&;Pb6* z?th9<-xZ;jA_9dtCVIOg&<1KT@q|j3=PG^T8iXG)x**a|8iXHLQiQc!efP7>;4Sy< z;Ml=_;h4cTkY^_6!+4+4mZzUI2)#EEVP5Gt*yJ}4G7ko?=#6>Ofq;j>ThC$~$hwX* zry`Ww>Z5iR5M8pa^?a0!qK0VBd1==5APCY_>Q$3Noo0NaV*G1Y>O1a#%%OK>=MgZBvR+yOIJ|$MG1HpUVWzC?TV5W}eCUlf&801Mb4u1+_0AGsm@zD*z=gO|92=xRjju^p% zd!-ycx(4v29DdBOS{(?EFl*lg6vmTjfx@T#a!JAy1+&4~F1sm8%OPQnVtp7T3C5H~#MkuG6`qBtp@~(8j;` zuU&DK&ow%0ah1>2H_M7Pg#V{qsVzltoy{Eu(J4Z0DMBqd#2bg5U9G#z?0)8rO@C^I z`Yy|Rn*n<(*1vq};(GU)P1z9bZV0W#a$^aY_Ca@yu2x;QDK%D?S6f?aZi>yD!vqSM2Gb{uR2J!7pKPFuD<{HV*V9lS(dl78JB}rGpWXiwo-NkOvZe4WIgVhL9O1g- z=!WR@FrXcR(d0_{jJonh8|CrYWqELf^JK+Z5q$C;;@+KS_rH1A*?I8f(j%Gw?R?*8 zqs=_WQb6-M|GFVUYXgGqaF&Shy=xro#W_E9c0(xa83&*0oi^PvKD&W0Tpt`kj^N2auYA-R2cz@mM}`_l zuuDX^?l`(3$f1Z(b_m7>?g{2=HGK-w z62URJd3L)YIz0?%$Kn2M=g!trMR@ucckX#@p5qkZyh;#;_SZd{7{*3u2iObOC+0g4 zF$S&dIJzNxYa^nuL=5gn9_|Nvvol-rr=1Xc3BSB2S;>zn0m!$icbn_;d#ZA5y!;H7p^;wZir3~Q?D?8 zz&`&5KCiO&k!QCXBA^w~%paK5dcr(NcFwh-O<_)CJ6LwUbVIO5L#@!e&Jr>HepIx} zUJPr7z!&ZfuRD%z2%;4c$`0Y5E92nP5Y|4*7p@PE==3n49RjPRJgl$yREITOu*-IY z>w_c6p@>j+2p{Lx4n}8Kiw3*o2-gQkkV6rn>=0PJ=3%|dQHE=3waaq2J~)CL!4pM0 z1XkgBShr)I!1LqQ>cK7%;rieRawsB{9RmNh-5RTT`iQ`j7}s;591#Rnjy*ip4ccEf zG!bF_mbLi_o`$Y#AKXD(uOx_!i0}!^@ZDx)mT^C9^)V0;KIufDr)@2fAj~Vofvweu zU~9uj6u1+_0I$Lvu6zR#YF811bFfyp=kz^1$Ls~Shm+nlue28Jh6rYlFw(ktrK@7S z=ZPzmm3kF=WMDhO zC!Jn}_C-V+CG(0SS+}{Cqi9&^n^zGLJ|PF}#?=5kwApSTht}eV6V@n$BN)$N^@?7w znT&jBUtBGvUWHFw%Ns=C8|YQ|q!Tf?SK$+faHE7TTXaPZ7W^G-;*RL>l%PCtEC~qf z#o$$lm4J>MZItW|YX7jT2)A19jHqCX9K8RX^KQanUWM_s6A}0#f(74>#uMcT+!4_Z z2;RXk8t~qPSu}7*1PcSa;xiUThQJpQ?SN3bick*L&0TkO-M=YfmqRZpww`g_0CYsO$)UEEKmEqy&Cf0{gmOm&3hi8L zT{p*##i8H-B7}Of=QZq3GyUo9$4q{=Ug= z#Xr7%f+5tyh%gSVwdnUI6(i^RB7}PK;3tO_pFDos;9ePrB37GmSh2xo4x#AiRh#Wl zud?j+g4#HrWA!j1j6>}@&H8pjbmi!Zm@>+FrRbDHJwfgN!7JJlyS0>ob{=+eP?Zm# zj8H_~TyHSkOEvxDy(-vO>S0r#z3UNL%gSmgMc^J6>`B_i-7DDJEUTpyp`R#1J?um@ zt>sgQdtEyY<~4*KX)tU@^NAw()Z+>AXhk6A3Pum@BAN<5nJjDjn)>W3{X`MkrWB!` za8y9-nJ0sLrJpE*qYxsxOFJ+I797p!J4UyHBL&OazSJxIL=oDi6rt=Kr7;$oCmiE3 zig)o!eOH89N;xnm6`Y&sJ7$}LBe#{MeJMg)t_W>Yico(!A7XAY-#G_j{_4^W^<5Ea zDdo_108w!M>ul5D2zXL(CM81ADMCHrT#kPGf_8BJMvq!8MX03#9MN~Qw%}|{uh1g} z=R%fKg!-yIGcRZm5=fZ*QP~R1y zmQoIF2coOSxH6zkDT4DiMjWf92(^?V)L)De1?NTjjxnZK6tnffcBt=)P)jKXXJd@# z<`w61jPq7jZ&QSRq6lqMir~D*(c1ZYdn_w=iby|c5XzS#(mt_S)c23g!M@yJ+PU_R zpY^R)YDp2k9p09CW#zPeT@bajT+#lzBGeN{$@;mK0-a=*vQ$8h^}5)I~2hYl=C7HijF>6 zJJc)Ade!C~RIfOfbEY9e zJ&XwBP`irg%3%nt#WezFx&NI*5o(EEMUAZ8GoOu#1jIX|Q_>cOCxav2P>ZOi9Y(TAma2#jPb9VK}tvmOReCf5Gd`GOh zr+vq4g<7<$ma^=MGsac#-MMg?pMFpOr=xDId#@gA9Ewg6-?{Dd>fD9*FDJioaL<44 zyQSW01AZ&kkXjLHDa)Sy%jjy$rSB}i{;iw)ADeztee7lD8i%4&#HX(vQ(f`o&&s97 zzTWfMtQ+dz9>~*QLuy5+r7S!4@dK*k-~4a+jh%kbKmEY#>JOK?#5fe4B3}LMTUGDe zOH?l$@k!5{qpqsY`4=X_cS!-}vQ;^#X@qWgLo55yMYh zty+1TqS|e%S9_k>_u_ibzg%Y=icm{g_Qd^5RA+v9=jyqC{ki|1(=VtWzxxK`P;`pe z_Rp`EPi$~#_3h1G?OEylc0Fv?&Bmb!wUlLlzwwFk;3bZ&CLj7}|A}Xgs~?(ji*YDA zMa=*4f#vk)POnZJ@{M7C`^IMMj1Ob1 z6NeZt7;jxTgc~v5!&qmWBgPBHp7+FRv7A~$jJFu;jB~_z!FVHwqEiH7yv0~&oFm2y z#vbh|LM`FjjhDW&#aL&YBgPBH8#xr6A`s&(#yaC1F>|I^!HM zUNGLsq39HW7|$8&jB~_z!Pp~*BGeMTVf^Sda>hF295G%n-pHZo6oDAe8S9L5#CXBj zBZnf?QkEgcbH+O395G%n-pHZo6oDAe8S9L5#CX9-B8MW>625C`iDPrdI^!HMUNGLs zq39HW7|$8&jB~_z!Pp~*q7mZ-ql%WY3^ATF)*0uDP!2_B8DczVtTWCL;{{`n9Ewgk z5aT&xopFx%D;RHNS9FR%jOUDXMpQtMLlNP+YX>40DwO?kJ|{=PPePgKfMA(5BO-7p z!rBxNjsr2CGuDYkj2DczK{)JsmLbM-#=4)+3&x&!Vr8{t^Lfr#_w#wdcq0c!h=@)R zi1D1U?&tG@u}2O?V6=#qY(CE!>wZ2j7;ogj2ocdK0x_O5*8O~5F!soy2#gldlFjEi zW8Kf^1>=nz7$G7$MIgp=#=4)+3&tKf6oJtqTC({(XRQ1AykNYM10zI4rwGJ&&RF;J zdBNBthaxaqL`ycG=ZtkfpBIcba$tmr=oEn%Z!y;Wd|oh;$e{>~7SWQ;=Pky%pU(@% z8#ypSM0AQkjJFu;em*Z4d*r|f5z&b8f>A|FHlMc`>wZ3u2<1?;&F4Lgbw8gMj6HH- zOo`}}12NviSoiaJ!RR47Mu><`5s2|&jCDp7Bi6;6&F2vjuDf<{JuqN2dt2lp@A$e0#n3!)~3ZdrtK> zMd&ArQ0^3={_4)_gTJ3q-?{75F6~g?6`__=4sD0-&(*^e(YM`{dg59a;UA&J?BOnP4B{? zb`_zPQVy-<=O}M?1hRv=acE!pNFC5`DTlK&!fGi(Epzm+)UlQ|vgglYhaZWwq! zwB#zbjrc`GQ9^5NM;K_&@kfKu#^+Z?tgL|b2 z&eDilYlos!4y~m-mad34qZZLwaAsp`5y1s#OqR7xDTjWdh{0Q~UU4OWQPkR@9!3uH z3S(bXqcA=eoMY*gdK(car0r0IdYB^A6V5)E1qS!ZI64uS0Suw&$kC=(&Ze84U2Rv^ z_C zpILGY?v;L`2-Sd64)u!T2xm-ksD~*+6+uO)r4*r_a4zSFK#sw^(oYnDu{dgSDTjK+ zah)?JIn=`xp{k=I)KZF2PdI~e79hvqUg;-_P&G2;P_H=8a>gWwdYB^At|A8K&`%Vh z+>T?|;b-S}ZpQCU7*YT6Khq234AhJ+oRKPyJkph$>%BaI(Eo2;x-4R7|{c{kBhYgyDpFh4&{_+O=?gZpT{dRis^CKKbV|PT+T>l(I z;$ed(;_@$VlJ7e=zdK<>{pthLi#w+|j>hhYqPhM#h{VGNO~lJM`+B~8cfyEzp-ZP1 zqn>jdjolGNbDfBuW$$XOYu|*BBAQwwik}`lz3BZHE%9#8fiSa#3C5Zt}VfB4hTq4hoqLTmB95aI6Z^^4!t+G6_& z!?b1J9o%@G**%B+ZH}J%w>mpPXsvXPdib`hS}VVJ(6C<*KP5qEt#mdx zVu9iL&)%+vZn^bZ)uF$bUYs#|WHIuPh3vTvE!Q5&vagKE^C!N3S@zfT4M)@0uBdiuBB?!!pNWqt)+JpPvL{(^Fh z%bvV@_x$oT&+Iw+#vuuZ_FIa+ZS_6!F}M6;=)HX@LOoG5PB<-cM{BdAt{k?=lP6VY z>@>YNaie4HNz~p=@8GXbKO{eBhaG#qzREvJMW`o=&a%_)yrwmE*K>O&9Y3Zz{rqXg z#~+O`4z-)kwXcjGm%sVQ(yfa|U)isSsf$i4#vgg6-9hEcvOmr7+tw-@jqbViv7-|X z^-%e;>~B9hEx&Q~1zSC*Y+Wjc+EsLxJ^$L|)~Khh?fLgACsm&vFts@QC->=jJfBOYQ+h67Poye!8p`zI-lPUg4B8IMgM{(927g}E_UzWX4Ki#@w==`le-*8mIp&lw9erM~I6Y$&Qx3}K< z&4#6Ns9i;8+1ULiwPrlGQ0uLqj;fy8XL2#}#}`>U6rHx?k-bOc^W^=l1FxE0Dni?# z=q!8tr2|@jIboSr>&>xM&*(`-tGwJe)UK{kvTU_I_Q~HoWA6N-A3W8gh!+>UrMT?0 ztE{h-4{v22p4obG^4_ggZa6yOP!E+a%icKr+xhpl`%mlnHO?rNL+vU$%jW*-X8G1{ z&7Ci@#+&6HAKg^+jJev{p#ozr zR%f4ZeX;(2*Bgh{($!#=z4rDN`EDPrm(P3ealMMrnM^&*vd#Z>OKa91m$w%C>M;q2 zdZ>I^_Wc=~<#S%VdOmag@uhO8T}2}o`lUtk-%Z~&KmWes~%#^ zKmXj;10T(5ZM@A9l_IpYiq5hf|GPx~`pIkLAB}sjRD{}9Z?mk}fA7|FFPzYNY5tQF z4y~npS@!s`Rw(r zk5@RknzqIGV(ea1tsRO^IUas|V(X)^d*z$(jITW2`^X&6j&)A)R)mr!ZqpOn_ zJEhof!ZhPhbjoqk2Zy(gKl+sX^5Zrx6`>r8&aw%8oAmr;mFrqZojE4qP`kQs&$3TG zJg9ZWS5MEcx%sAk<(TmC3B`M@>DE`umu2TZyG73&^WD+fVB$&D?46D+hVD4s+M(!_ zO*DIhw~xAU=;Qb%o%ZC>0}##XUeSNqWWQnm+-7#q zN(bV%w#2K{6Wv+Ny7_{kc{wCMboD)|?S4D5*ks!DVuiaGsyoq*S91>Cx#xnThU7o^ zboT_IJ({A&-Sy*PKbiDG>!72Ls^ny0yPF->bH}yg@>~0sD;1&l zL(%v}=$Du8*>a<^^V8;^-LD9?yxGg@&ify?+&yf=YewgbFMe0TK}54NRrEt!FEMOP zy-$A3`tv63Afnl4D*9`ePaL`|_Bpe$$LaK{=}|?WzT|d2H%u9yU;pxqzD`8g_iHcU zn;kDXuV?9*Yv!Y$$rBFkQN6SH{m;Xv51pQ`mH%SM@C1?8(%m*y3GO1DmZ9r&E7#OA z{63*}y9ufiL{OQ~9fa2M`-IxD>~(% zDgkw(qB21p18aqvqYqUH%(3?DQkB5U#>#lgp?p9ADXD%aLf=}YZ)K=sK+mYCWl+b! z-p@Gv$l#woizxq;P{)9tQBlk2M4*R#EgReYJ*SQVJ)@+S(TPBR`C2x%`#VS-1A0b3 zwTw;#`pehSH}2Fipl6iSGCC3HRl93c>amOTcaS;;^o)vH26YVVtZQl++Va#Abqwek z6}1dSXf5g(*n|4}LB#;dK}FSoItH|cnp#F$%f@z(pz;9)q@wzv2zrI_#MiR1-H#-` z?B0!osswh!*1L+vni|Rn6p)hYha%JyzB$2;+do&-F`#FZ)G`#I9x58Y!3!M&dPYSp zLlNqUqOqfgDgkw(qB21p1H1PD?NCejJuE06P(VtmABxagX*;M&K%JMWO7C#Hq;fUEEN?NMQAPM0|!(Is1qfX3F;V73#=WANY@Nh zKA?bk>udd!wOaK+hW3oKZkjDZRRZcnNo9gM1{4BohjOT;ETi%P1*D(qha$9A z%0V3idPYCB4C)xzy&H$3Qv@{zXbmNG21QU~z>dpWt{!6afsO$^qn}!aBGhi0UqY<} znn_8$L=kFNJ%mmTRRZcnNo9gM26nyH4)sbcWf_$ZC?Ne*KNO+0Qm?2=K%MBNGC>^! zyZ4$}hOX|@-2_z$s1v}=l zN^+OR_D{VP7251fa)ETH_V2^J1L-)0Q$7)YZ zmQiDX*3d_tK@qyoOwrUBpf&VSXHbOh8Wo*o)EJ;O^ipR~g!-;%{LUye251fa)EN|^ zzUwoVWz=J!%k)!|QH0jg=Pk>q`9S;Wqwb>!t)=%S%cx^O&*-O?p$M&|_X%??bPVVj zz0@)kp|!LZxZAWWi^}I^P#ub^d)XUcZ58$j0JNb_bVEoj%kc1)h=A5m`MeD3Nnt-g zoh0xv8^T|wSA!tbLqU@RT0`aYGN?d>eWuv8d<}n{9D^Vd4->E2&>AYAm%;pturuY` z>d4nzC&wU&#KQ&+KpR>^-~UIx<~k8j4oaVwksz8{mP_IHxb$iej-*Wu zA32aS(eLvznBx)l%gmXG<*+LzL+X{(N<3_?cXBkgbQiDr`&1>MPM9)59RuqF8-4Iz zC7$!aTL(G8L*)Ysh^Zfnpz?uv+{P#>A6U)!@rpVI^o(Agmr-NyXB@m^*gxAnXFstV z^D-*v8NEI)BO(dY9rB0_7ipNNQg8U4^ROv|9U61>t{uK!#nXqHjO zfS%Fk^D-hrYf;C*E)qOcqo`v*&*=4e88vp_L6+ zQk8%@(dY9rYV7Jw%iw6k)_Nt?$}umaf;!RX^D-iW;|LL~#fW7&=4Dh+CwhHeMnq^W zZIk6?R8U#^d|pOGq`o&ET3$v4b)w(rWkdw^pU`h9n#uqVfR+r1W_i z5kchxJ8tVM<+Hqu5_*Pd8PrMw2ep~dCgrodj0$>2>GLuoLhUNr@-j-O6Q)d1$G}eB z+M#@DJE(j>0V#c6Mnq^k6m5AKCDaL1Ca7az_ih|&7kL2j)TZTSR8T-n{ZIsz5A2_< zuawX7GD@furc5Y8+oXJ!mr+3hDSci>^h)h2+VV0gs1v44P{+XT-P)mEDYxZilu$rS z{ZNF~N?T4<0_ub*6Vx%Vdp8cPr8Au6WmHf=O#M)V&SdJLWm!~ECrp`8gw|4TEt8{! z0%GciBD9uzXn7eGlmk-@sAE7muy*LYs~%#^hbjSe!juU`XloU1Sr#QU6Vpo+p&lyQ zGC3+}CZ?AtLOoHm|6cAHCsC)#3)=JkU)G?rER6Z}G z#!h|!hg!0{j1qc=X&F>L0zzx098@KsPLw_`BOGLuYM6Yfa}6Sb+DvFu+77BKP+2OU$q^B(rF-XQe@=A;Dof=vITA#(msK>?6{swg&*VrD zT#x$m4YH4>x&oD@@|he7qPYjU+ork#m8J5T90?-$uKi}29Q{y0Dxb*_5qb~OexoYE zji=Acuswh~S7K}RLUt2H%*!x@&&x0bKk>T>tL66zMa;{H9J=S|vSaZa)EMZM&&x0l z+ETlI)nCNCjOdl_Qj|~8PGca4&&x0l+ETlI|5?PmjOdl_Ih0S)PB9>d&&x0l+ETlI z$6Ca^4CA0zzLxEgl~2)@0fCb{F)zb7XiM$-J#G>6GK|CLWkiJXDcW)-Z~`gjWf+3C z)UMwZ7cnm*dZjyTZ`XG5icUFnhpp(8gE|J!nfNS=8f%4uc^P;s6VF%U9YQ?4o@Gm1 zw_Ek%#6|L#7TAQD91$_*ncM3hZgYt}?cZMSruu8uHCE2Df1EN$|7e`(coP{A>%9Y2 z+h?9}EP-dfNA;X%->2vH-HKBqt0T*z6M@$I`>g0K_IkN@At>luU_9wW;GX!WPfrg` z-Q$|x#i7{#5o&EG0#AyM2|We%`-cwd+x~*9T4!v1$^Zm<)$ST~4D2HP9UQ%I-8&NJ zOtwZ|#-Gsg?QqV-=VdTPj3ebfZ{N%M$DW+$3nMQ>5nAh`zZ_9N{3DlhnPqDoHM941 zoB`Pb84$<4JF@;g&Y8F~Cdo;($cL58CCl`^gmWerBg>)_fmrwBhobk~eo7zDvOJ4S zj!pzdAb+3rblrMu{kHF{N5<##ZogcDNPVYD6wtT*?9Bf8aL(jA$g)rl?S&M*((EUD zufjQ#A0d-N5qz#f?QPOA29dI!I`)K2|VzI#Z% z)|dZLDndO`be3(s^tt`7A8}3V__1TE!t zH1_9zysm%JC3m#`G}lSh!$Zc`YwzcBofVz7W9?D9lp~HkFn{IJ*GffbI~1K|htK?T z|6V;4TAw~}N_EjZ=hf@C@3hfJ?NZ0U{@M4{-N)WjPTPG{zTenudlYf&Ll@SctbB*{ zmGWiT%?Hn4?t9A9txIPdm2jws%7-l01t*t3yy}Gf8$aK?R1USPXzXhr=r1=vc0@k$ zomb29mlxF^?lIZgp}6joYd-_I;v8H)=FFc;_zk4r*Ar-_2|1}tFQj- z@_O`flZ-=aArByauLWjTQl#O-dbXD#Z^!idhwW}Ut1+eXn<(KALr}c*; zMpc*ZGPyqKd3Od&Inq_e{d?_Dy}RQptp{Fuu~dYvG8CO<>wj=a_1%4+Zf$zosm#l$ zv3s}YN)c)aE15Oxa+kx`Z*B9QR!>7$-M-^?k9~IA;_}q_(P~Wu|EH9(tITN3k5fR#>RCB_ewY-dq z=S+NFMnq^W^$@e#B99LH^Qn1wAMh55AIkqzvAZM)(*85eldiIP<9;M5L!$59O6IQ?vm%X zENL9U7HjG6qlD}K8=@19yzqY>oBw9VCr!^#yWzKFhzMoJ(G8)s267zPcX~cmAlQa*<`W%~I1ThHxjN=|BvdEh+Pz&}?W96`IG7VD>$kiwGlD*oD` zma{d0qjuK?0g%=5pA1CMEY5SjJw9iwyZ`49c3pnYE4B1Rh=FLtc+SYxPguq;D8=7C zOMji~ix8b?yn|fuq@0nfpRlaI`lftejOavX8DczV#xZv-xnh~(OCQ3JUnOQ zc776mbCB0V+1>MQ2(8s=2`hkGmdqLJT8p;uD^2$MjJzJoj-wkwYjs+}+wdKqY%y}R z7H#1dt?ZX6c|DXJM>mAl>a>()i18L9S8LH0e*MdS#go@V*>QA3Xsu35cw_S1o;{59 z&RTx%v*=1Wz6e1}igy3MiCm5yh(pY^ZX5}}UuttO#+K$Uj1KyVL%Y_x>#mF#?_=aT zbi;x9)qaPU^KQ!54MAIq2xZ5C7%v&QTP!4ZB15D{w0=JS%VP7bw& zdDnj5nH-_)I4~*q}*>QA3Xf5S)2*h~B$R&r`!u)E# zu}wrM5021U%I6S>@rsd44z-1Ow_tn|5z2!jw3hNY1Y*2meVcxah!X_e=2S;cv z<#Pzcc*V#ihuXqyX}?oVL?{oA&|1pp5Qy=LkxP!$78OTE68~Iza0Km!TCAU1LX20q z*mnP!ts=Yq(y1O{zZ0iu)N=?H@HfDd1mV2mnw^o0>g`~c9Q@TqM$%W0@9p3dMBp&H ziU?)L!8JQ$ogCp4+T|}IGA3CoTpxhQ!Y7KrXqTXEcCHw?R?SP=&P>c1$b%)@U zCUVtQ!htcx{f?@uF9`W$AOhSNW87a{0l@DW_}z@B?7J(MlI`Whw%`2^iOwjm^ zX;5olAD{2?^h?8L@3zktilE=@Ve%;&y694)^Y_=dr?tc4*FSRV%TtTTva_s~+C>Is z{HC!i>zg$$U$a=Hwb?D--BJ_Mx4Z2t`Af47-%=6Er|2v@Wv)f@?_9N8zVS*6_a1)g)x|#FyxBO^E;8ui zcS0h=bm>j=kN>_={?SW|4poHmDH<83`1YhNa8B{{&5wU%&qppUwpwXtK1JiL%*xyKoc+M~{G@F+d1U%;zEj+@`*h<-?J^$2o9cDO z?%8_&J7?q%{B+GN6`_17diXE4=((g%SK%%?7462Az=2UA55XQ{!sisl`0&?j7n_0r zu!+>PmR+x%CB`^n-Nr=E?rm9Xt(KnK&oah1V!dLVGsY1~hR|AidL6xs7)PvEjB`aO zpQ4fXiWo<%SB!JUI3me7)UKWr$H;&fN32(jb44hhqJcn+Bi1X%Ib$4=WE^T&PfKSR zV;r$wG0qjCe2RwRg&0SySB!JUI3me7)NXounK6!7uNdcwP(DRx8Dku=UNO!Y-~&##yDcnI8wVBAB=ItddWCf zgz}|m#yDc#)Ip6qMThGy2L0@u#Gw1c#h{CAo1M*DKPJTeK@+Kns2a9?EmwAy7~_a_ zzc+B_hn(H!ED`2fIUmIsN355AKCcl;W>;(J$qt*(E5v%ApU)$L9?>)MDca`q3b9`L z`MgFXSuM4TnKPbHvH840te1X1j|h%J!9zvcd|n~eEzTL^h$Q1syO=rS$qt*(E5v%~ z=kti*C=@(Yw9V%gV%_4LF^)(w4z;UU{?M-w&F3Xzz4G&U zjYu*MwaXYsRCx}@IAXo>^La!lpQ3F(??NF9%*B#^=Y{{jyVcj0e6Dn<36!U9QHVu+)5S$}8iKzp?)Qk=I!Hy4NSxANBC}Ak)8g z4&A4Q0%f0E@YUPv#XjK~&J?Yf6rmi-og$#g#J4@_$zHgA;`Niec%=xntLT&?yba^k zt9G4IS3`Ji7vBvcZQ}nAf9+fmT1$H*)aL)TbLc+tjgopecsmrKwH6q6M*a5w{FTb| zubo3rv^SN!zT0in>UEbq-fC%^6q6#fmbNcNKtqaet<;m&rAO4e+wVbYeYRW?+9pLS zrkz83t)T*&@7fM+UzXkS@&41cUaWU@a`-%=y1nU|(-onXvaI#mmeViW`cmjp+^sfm zkrkaH?#QoM{pqX!I?W+C3ljxPNo)Pdu=Aclz2=V`4MKY%MelL^B`fDU{w;76#^K(p z@t2M2%Sq3q260RK5}L8DnSGf$=#smK701G#a>)NTMiUfgS1Z_@sg*fX)n0G za&3uyr3mY3?L%lX;F#~StF0Y;63|LT7>7sL^(?#WOQ%iy%aE6#QMU8&{w%!VGjGsZ zJi`U=sPkpk85gbE@_p5bkbdJgDA$?w)tCqSSH0}rlUF4N)Y+OEDBpjmhpAT&P5bI< z^X+=qot+5A9G+S8Jw@;H-#70(WAXy45fR?@kwbmQm{KspTs!W5__jL_`ed#F2yK(1 zabo?0Iaiy}`;K`PvPHB0 z3U3t2p>0ar@wWpHn{xYUb6Ptv`WGAv*+NC9h?NfgVAW;T-OId!>R(fHCx?2NdbQjU zAFTS}->x8}WveW|f-9P1(=bkW*@Sc+hu>a^Z>R^jNQi11dIo+ujM3Hb1B zs~r5vT;>(m2pC_j9qK#hU%mr%L?%&lI7Svh12`_nR{EySJ^*mizgx$hN(CTI##a!F($R%P+6K z?&>G}c7lypiqKw2(U{d%$E;=?m{putm@U=A)T@!dTYZg(7hT6%?h!?M)*912Lf@rZ z`)uZ1{jllF+dc1suv#hi`#(PNj#b`&HMHFQV!(p*N?VJ0-TINem}O_IB7K!0%r^Qh zMemDt+;PrVtgrYaaBkA~@_b*PdRuOE^|Y0}SDgLXFOJ*)rh)TNygK%uo36Icc9+|8 z)rkncbH?UV9H`r(u-NB?X9LTl-pUwl)@d%u}F;p6+wD~=zpOYpz849R?sYi$KwM$MVMmrvC0p z=M`svj^pIeHl<#@bl34ypZ@H2<4}aw;*8BX%e7<6SAH<{_K9a0g7YlrJaVX4+P*B? z{@AOh-o2#bP=xZO9B&?S{nVddJYfKb)>1z5XYF;}Fa6mF$MeS@@49L)B2^=}Xs#17 z2%?ra7c@B*UGA>dSMK1iFSs+u-y6O%^mq50AAyf2nca1A41%x|(gBfp)&Av%^X8k_ zuP?Y$%Ehb)jH9vZem%lnC&wTNo^$t}xZg2}UbW|%bCdj)x%fK=?o92!Hd}_d2K>Ih zBJlC7t-DT+K@dD!5)n-qfcAK_VK20W95_``-08_b(8gp=ftZLwdm(Z<{Mvp0F)*}H5t%kF;s z5GTpEd3*U`C*g@#gw|qvh;Y5IEKZVFr%f65({Ie~QAAqHy}_oUhgJ9s==?0)EM$%v(gE`o1f;;z=Gi=8v9=cdsK zLTfP=iEuIaPuzzc=X!PMi}*HL<C5v{9&G3hmJ(lDne^%o3iY;-?^bR z<<{+ot#Qzp1fjE~wkgY2Jt5E6XkXs5`B&CT5Zcpe?>;i$@cfnio*4SCZBm4KqG){6 z)cc3$&)mFi&#Mo;(z^EB(~Ga}aZK^#TkrH~n^KM|e|%qSld(4pTjq#SxgxY3ipKXR z{%BnOTE0qaw;Mm}z2=H(#W$}TV;pK%GkI{@>79#P7p#0t&z&Efk}E=IWaYy*c`P(0 zAO85lvFXLT7#D%d(Z{JuV-=_2kw^hmCD1LOoPI ze81?qzi55`pYydoy7=gXL+vU$%f9*J+gh7GwrFcNJSFR_G`ZM+y^D+8-g~D{y;ANh z`@tK#=HK7p(boFcz1mWQ)=FEx=Dpv}U;V#%^VN2Gw*OD-O)7rA;uXfBwfO!BZ)tpA zeRkRYt&jIyw)O9Sj?ERJ>mc|>x8~h^-`0Zf zADwWhT}9(NGrxC9>wwF5Yn?cKRQ}=jZz`&%t}gnvez#A(Qf_<`(3P9!hrBg!zS$hF zw-lkZ(w2`JvPpjN-!{yzdG`Cx<84jz|*_*bWy=~jpWABd56`?bk zdWiGRU)dy|aMPOkUiX~aQiOV_d|CGCQn$3uKlsYlIkS#VIMlA9v0j*SOzV$B?r+^b z0lmB9RmCfRy}7vI?RWZg-c=9r?H7A}H6Qi&oPSg6=uJoEJDzw^@vRAyi-WMr z&^1Hq)w7?y-1_q_JLC)Q^2e4Uv{t$TT2{Tw~ku+n0$pRCloJ!|IPs%YAMU! zz5d6o<90bdfAq^U`xK#e`3{e{Id08x;*k&cJpGk(T0gpMY{H>?HEk1AgO#6XP5Ahb z{KogrZ7GL(qI_9)!h)Bz=3RI1{NUgIy|w>3XBX$6F}2tV_gvAbSGSIRy65H7Z*8r& z`ANAV)GI|}mGQfgtuJ46az142VXcq18B=`g`f0|Y=#=Br)n@hlYJsO)hi!69t_bB& zbe28aw@2%ct547OcxOhBBGfM5cQTL3_3k>wMm=wSbZzT5=Z;A@bVsjzI3fMozk7DS z{LuXEA%ATx^uuF{H+Gv|e1iF0`BIMSKijP5rKhL0_J3@2t_bB&G=3TAqAhy%JZyY^ z(Bpgcy!gz>;yQfO6gg76RB^)U{Gy_F*x4&Q*ZR@WqjNSG3Q_hrGE{!lA8A`)&5r^Lk!*c+I@{Rh}<}-p%pr z4sTcA>(jnUy?r~IKJ=j0TKU@x4^I$jE!|BHIb*+Jf5Wd-EPvqH`G}`S7N=b`z4-A0 z#|_v!+j`X0uOo(CFm%k!A^8Dc-y=b&hwO#seoWf!#-TUiH}Cetp0JZccf!qXyM5+P zLnmX0`Yv`yoruP+?wwcMW9Oc`4;hkAnj`f}`zrPD_8T4^di~~yw7&7k=>s@IYZdKO z3GO2C?fALwApBar<~vA#y>|N#>KLDcFjZC2PL&{nT88d9)ULl?yB!O4jL$*XPDs&C zl^}vzhVD|-uD@QpJq~q@&q3JUNzqP~Ac9(k?m5)1zh1itrjGGB2-|Hb+NlymP|MKW zuiEw3Yqxi%j`2AN8%q@JR0$%eW$1oc?fUDr+ig?F_#A}o;uY;w2_l@9(d@8OyLP>H zvjKIC&q3I}KSfiOfI3l9nV^n=wL(oTL*K-tZWXVDA-m4ComZwTw;#Mrx~tOt+X@k9RCkC7@1JR3@lnV4qx5%g}bDmZ*F{0ja2d zC_-zc9Nhmy2PmllP{+VN+3rXB&RlaQsC+;H>8JXk2zrGv#Age*-_UTQ50wunASKn0 zP6S2-KSsG<+jL_Il@BN&71fVU1mfF|I5xKXk%7tw6p)JQM<)W&>T9{*gLH9DRRZcn zMP;HBf!MRBMEfwysC+;Hsi=M^LTjbHOC1AxMnx@yItF&{HMI;yrwHm8&@)PE8H&*R zp=jh*L6v|yQBs+pj)C2~aqv9|cEdH}Q_(nU3FQL{NJaHS5$Xx@A>#>*ETfJAJ)@$Q zp$PR*(a2kXjsZQRq?Vxw^+eHGMpXjpL`h|WItF&{HMIuKq{&qiqKkV zJE%%PohYeHP{+Vd-Z=QK1-oJEE9JxYctQDq0#Z@^P=wm0@_`+eGZ6?F{g85Ok*MW~00&NAv4&@)PE z8H!L(6paiBs1i^oDk>AyF|d2DsbwfS^@_>|6p)hYha$A)ipD+)ssz-DlF9^i4D76p zLuWEY<3tsd4=5lN)el9eCpyF7JK~{ZK+mYCWhg>DR5ZRh89D~^jFMW0BGeN_g991^ zw1$d019c3j6E(FAMW6(Gc2NaN!>W3n9U7~1wiy(9i=ouxo4C)xzy&H!j z)KZpFm4G_YPh~<8n!TauETi%P1*D|DQ8ZQ=P$i&F z^i!FjjsbKM>7N@^L@F|d0#4n?O3Y7EdC`l&N0 zLNi3vL&Q0B4CompwG2h5UFsOv+xotuRszkWpL&TR)UJA%WmF}gPV`fmppJn(b4{H= zy;4i~6(uMiP(b>rekej~rCw2EfY#7Uoq;+A_RNmMy@|7RNxJ`_#sICMk2-@QbakKZ zCa5t$Yv`lSpa@;7D;nQ<28{t)LqBx}Mff*y)}xBXHyc4?fY#7Yok0=W3u()#;6Tag zr;4M9w3hBasNg`!>7$CH2)!T5hhI2?jsZQRpIQcW4D7{gY8i@9OIc<*MmD{dT81LD zR(e0EV?fX7qn1G(1Ij^7Ekk#|>28~<1k{OMDiey(y{w|~?XB2rLY?TPGNB0dMA2DB zjR9IiFLee*s3+PB$e@9`0+pqg3X3AN7nCo{sC+;H>8JXk2(_zdtVJ)mcx9*){G~ta zB$FKkvn)c+wtZW^+f8si3$3B_Sr*LL2s;R=<$nJo<})ypBXS5rcPxS?2egLDXIa=U z2s&G8*ZuxSlVzb^=`Kb1?3V=^ub?$lKFfkSRoJC4XCjuvUW7iSUP-OQ!{#~>&>AYA zWkH=X?0%Uu5zEaUmk9MrY9$^v*NK4EQ28v2qOoOJ9bxZmzj5LA&P1qJf=E1Ut`h;R zq4HT4h3v4!u4P~Nv7HF@N)U;M&Gk->uqWqJ+0;@F>KM>7Ov|8-f%Spyo$+=do~Xh5 z7CG}n9RqrXX&H*3j)C=$`A!`JGqi69bqweky*@9a#!lX}4E8@fvpdLsVmanzRM0c} zd|pOGXf3vfh?tjw-;SQr=kqcmBCX|496H^HItKKNKA)En5$rSCWi7nhj;C|8j5-GN zj9#B*QDe_+T86fRI!$PwWpY%|Gx~idM?`3?l!H13^o%~A&roBZZ1)52&*KRm8&lvR z&$n16M?dt8KA*`E5n7AU%QnSKjuLuCpU>oo2(87~C&I-!bqweky*`s8BD5AGmWY_i zQ9;k>^_d(Ip|!N%vWz+g^o(AgWf2iti|SG6b<5>&fS%Fk^D-hr?}wr-%c6vy(eJY?YV72VgE|Iw!|0>*JeFm0RM0a@ zpUDvs>IpIj;u&ts%c!7dls+#bBB;%THYwWjGD_$f{XQ=vBD5Wf&NAv4&@=jdmPL)- zyXg$t4(c?aeU`~lLC+|CCPzeQt+XA~F`#Gk`z(tZJ9*=vj)C2<^_B8jCPxK5qx6{^ z5utXeV_?VaM<41K&@)P(mk|-vWd-Mi@wicYW3nP!3GX&~~UL%d)7TfSCHB2(6X2gE|KE4AU|cp|y0L zw``FTdWLBkiqKl>A+#mv7|=5+pJh>FCvQ4~BGNSjbqwekre#q12nb!5C>pvCbPVVj zre#paz)s#c6rq-~j5-GNjM8UWLWQM!yU;P9XOuq6qQ>ssbO!ZG+h>^^CG?ESXL3Y@)=IsijsZQR@_89GcJjtS z-6Zg(2<6J^K+hKFj3jhn`XSOpb`qTB%pmF`#EuK4+rFp4m9K*5TU3 z=S;|chdKuIjLPRsB#34|plIqC&@(EZGm#*4-QJ+7V?fWSe9lCIkX66zEU05Z&!~LP zL_|>k3H_F~oH_>djLK)EL_}IkcN5expl6gmBPAmAekdQlxdl1~^o+{qWz^VNo0g#n zsz*()BQK*DdPe2*G9n_arTbdy7|=8NeO^Y5UA<`;yo0*0ZT9NaF`#FZKFcCO$gWt> z)G?rER6ffhK{R_=-FZ{TfSytMoQVVxJkegjdmQK(&@(EZEfNvbe?q^pO${G)4Con^ z&*X>*wVR@;F>rTR>HF^ zKKCO*;LiDHNzWTm$KW{=pO=v!sBW_zc(*Ozctjn8r$&5UM$PjJJ}-l_nsHFaXxc{| zgC|pbUPeS{t(1d02G3{syo{RX7kEGL{yd&C=6Hqi#7^qsggq1ko&oV05E0==H6N*Z znv1Fg&zblviv)pa_2Y-0F{ARqvn)Q7BSByc@ne*pe51O;^D;hLBtbO3yI=Hl3JY}% zo*MCa83`ivE_IBi*QsOhWQxzrhzPBf_AYe{o-^@z88uH;_`HmWP7%~Gc+SM zP{-h@5ucY)WA|4&%9mwSC3w!nXIa$P$rsGa&~{L#3GK5?j*90@d?rUkXsxsz)G>H! z#OGzy*vT7*&Sc7Gc^MT?rue*!h)}ya!&zQN#d9V;FC!wT&4e~7+VV0=o-^@z84;nL zC_2liWAH?W&$6hY9M~BXZ3lIl&_2uLsCa6`XL3Y@)=Jw!9fK!Rd|pOGXf3|sfO23n zk>zESJpJPHG9p52sfU)AQSsD>&&#N>lP`Q;Mzou*8K`6MWQxzrhzR6K#C3`CSzboT zb0$77qsC6&I21ueByd|^M#)nnJ})C8kOvUIr%2J3mr)@vqvi85B0_zqj)DEUk0|OG zJZIwbG9p4fQMBb{lsq-!^D=6l-0*oB(JSgSp?#K@QSoGo&&!Ait(AI39fRjgd|pP) zQx)W(juH4$1a%Ca0r7bm5rH?n@q4PY-z+bq;yDwamk|+ampTS^#=ftpWAH4C&&!Ai zwW}UlUPj4NBR(&q=2-@xml3_9juF~tc^MT?rue*!h|pT8SJW|h2E^xO)I7^z9PUk= zt@_ga2Xzdd0r7bm5uvO5bT>gAgJ(c|UPeUdT3ykWmr?Nyh|kN22>&L|dQ{Ptmr?Ny zh|kN2259K70;RYyo`v@TIv0uj=^&#J};x@i4LEa!9A6&Ueo`F`wJHDHH_@(g&-2)MkE#zCH=(|vUg}Ynm;TFX^_*kow(^&L*+&1lKZls|{Pl*| zdHW0MIS*gX%A3t|QLUCzg!_M^>XmN0h=`mVL?}8%Xf5y=qLVL0j9+)ux(>sE%v*e9HK4v z!&=Mx0lUN5Cfe0rFph`-ht=YpQUvZtw3H&y4ny!hD?%+90{;wrN&qi?Gr_)w<$tD z=|GrQY*UI*c10+6mc9Ps2d2HQy|O!zgeOMElD3T{&aN9{x*>G*aR~ITdBu?k>%_|+ zDrsZ015V8K6W6=#nxjwXX_mVooLxnzC4AG=@e5bvSmnOmbG7AK-nKC~^aA6*8^U>_ z2(^SgWu-hI2Jw%pxp+x!vk z?9v!xx*?n=icm}VrjRR6GmbSEI;EwyT%5OUObjva#@&Gk=ZPZJ67NL(Hug+y1q5S0 zTxYo(LOp5Q>=O_Jyeh*d%~;Y7W6AI^u0Z#3z1v>@-M7o&uOD$x%OOHvF`ARU@*K{S z(7Qw{cjU+#4z=5^X*cDNr=-$P!js6cBs?Wqb!-89bt0`_Vb){pcL$lhLPATW-{D+YF#1 zcsGQzs|dBkCy{^d`CYD>`%!wb+ol)#naoAk2V1UT*FV1fTzhu2c0iy{5M43OtrkZE zwUqfk$Fbo5^%>$9_g})^wKB)S!4dk2B9uGjc>%38} z9e_Enw1s%ut=7^Qanuqjqk2H#iF=eSXKNLqmQdgR9LIbUwzAevf9WzmBfD{4dn848 zbbZr}mnR6dlp@p??z8c6UebO`5$Zc)`%$)uBhKJ`rCx=yUy(5){ltw#Zf{<_;<)NC!cBkx(Q+fP@|{JxG8M>4Y92^j>q%%pJOb zfPnO-RFx)(pikkgwcD(H&b`5WpO1NW_WJ*Sm)Wyt?|-eM2(^^@&`2#vZOYb9U7uMx}QKLU0~SeS|r~_lKQ**n=8jiU?09M`DBpWo!7;;v)*x$OsV-!3{mmgm|Ofxe_nhE>m5aeI&oDykPjETBGmUl zK3pWu?iBl0OMQedmVNQs;N)c#wI-%**~QA8MToP(S1^TvmZL=o!S zm?{2Ccs+axt1WjurKa8RY`XPTcT_HRMX05Le5lLE2R)mEqnXP;0HUMKbN2xnIjYAO8^R$HMzI5UKvb?YlXN_x?QmEKWA z=t0gB1NoRzGhRig?_`vCA5&_2cXp@Lw43^nC#upr;rZa47@jm zsjVh^OTyU)RHmxHhX=Q+H!9)&Rsw5c&l|IoLxnzB{$pMyxF_0 z5BC~ZU3B3kI7hp+|Kz{zU5^}jO}yscUw-KR{!Nb_iF)u~Kxipmhf>{$wHLlF+xg1T z)<-Bs#7Dy)>fdLXeNmT7;2_yLHPd2^bQeh(LYW-%|;cj zosWP}yPb&O&msJ{%IPE6B7!61h{sO1KEid1P`jOoV96mC!t?QaD9h1Dutfw%_z`EG zVSR+_6rpxI5y6r}tcK_OOFSd=5o{5`Cw1<3&a^(lb&62Corqw`A^hxPeFR%XaK>5e z-m|QaaGfI5ZYLsGatJ?*S|7m{5uAhZntnlC4OI}yQ> zL*O~D@cyuE73>m$C$&fB5__E@)NUstSaJx=J{8^{cD@RBiNK80!&!|z7_L)<+U-OH zOAf&`NtkzoEg~?V_sC3buTzBD?L-7i4iVPwS}I&0bIS>~9>HIVU=72afe=JH#H{c9 zteUjX(iKZBzv3TzMyz(UwL5q9QqNy^J=U(Z!gY$EkHHY(?+!8H;XSJPcKUlsAHf#U zd+hsaPxjhz)Aoo|KEpI zGhKd;jVgGjKShM=6rr(qB7!A{2vKP%UbAo52VGJ1evb7U|HnI_6^WQVV6+!Y-Ff?^JzP`M$M?3H(zD|lBW+aSIz_P5V2JQ{hgcge zI~!V`sMIXTy5zpLybI;>D>}!35 z>lC4OI}yQ>L)?UC#lMh z>@5)y?hvuYoU_%FmvcUvYelHtPDHTeqPi42QcGetiavrZBIY}Bmb&$`Q*1q&YhnUI z?RFx9CFkSk$jvwvdrR~YY!Q(k{+;^5#ZI<9nrmVLLhW`Uf+go;Ph=0=_{m35Yn~YG z60z)%nd{dpo&!&mYhupE{?bwNjIoGNsuQu`rZd&M_Jp}T_|Ql?5otYw4=u&(#_mM9 zb~OpRMa`Xn*!wQ#B)Y%5Yej@oj5k~>qN&G)sQH_7k3rr~V~Ys3!sEXl7u(8R)5n%* z-A_LG$Ra|iP6TVd&8_&w?=N%$A zGG^Q3NL!EQno$LW+U-O%{#X$|+O3aZiwKVJpI?5s_0e1t(TiQ-jdwZ7BEp?c1fRiG z#*g)F6?|wUorvI@9vUB7ir0-@H)BK}#?Mzas!)mu&Rzdpo{_lu%3ae%K&ahLM6hIo z3O>S|ScExo^?BwkWKOhyPZ8exfQSZ7jLwPS4x@VNv-yiKCpOoLV5z|n;qNZ0FvBsb zV2kJvj~QBoIkCAWCLlDDPDHTee1sW}K7uVG>hTK~VNPtWi3td`+ldI4oR2VH(MPaF z#A{zJQiM6Nxh5veS44z6oru5OxoFYMCBcVA(uoMZT~xt`mg03|ccNT7Ofx5jQUMWW zIR3qvOE7#f(v1nF!kpNY>huxjE7m-e3K51mku5UtoEYwOB3Sb{UnO(mU_Qd%UA=Wq zR3D99*R#<_GAD*ojEW;8%!$o4yd-ntV2JQ{7gd8z30f3P7k`MTGMfcQU%`e`l!{0pk77Kh45v4{g0Xv9LaF zdGF+SZsg7&1L7SZ)UG1bk}>zav1GL{eBAxo)J#i-Q!<_?hO;T>qaTR*fw;8;ap+%n zvQe$kbDTekbLWP}+%a-Bi#YkOEBjMRcW3ou_OpmT95dRV54ux8V{Z3|6M#^=iqHs+ zS@Y(#@%_J+o#Dc{BTo(k;y+vL6@9ofNJWHFj3itu!kt0h=A%04uPY}fiXp;_Cu|pM z?#>`t4@QN0lxkNIAzBxe^hX(LNL!(A4*F)}*dOi;lKsJPh5jhjt|HWuF?(TLu?Ii> z{L?b@Ep3Gnv+iD59J}t+@|Qqxlzaw++Es*Ff`)}3?LRnq{^Ue4jQpp!TPHq)?hNu$ zAlO#R0ikvk5yq~I>Lfh1^8)esn=AM;NOz(bp5Pg`SlypNx>G=7#`&4ycR;9JMTBS2 zMYSzvA4WA4|I7sfOhz)^IyEZ~WG?tEpj~}mBXBXECSs#t9sl^QUJ=4GF z=5D`)Ck;H0#M=Of<$$;w2(`;=MKqR92V&ShCSPG6V|_HXrWOZ0|5E=Sb~?be6;Dif z9%;V@)Q>(zGWuCX<>_`i?d=>PRPqpc5~{O~*y8W2GI z7Kouhs9jzwqOrst+<4?l`NQuWWPLQYrWU*3^XLAHFB@xpV7JzuM?$3lh@Sv42?({z zYeh7cIIb{4@*&q=Y4_DhpUMr%p^dio>=E0el z!M|Y3og6lZfOdZ$2*K0PP>R>7CAJmo@k5OGP>Qx#iji=?eP9Hi5k>HtZS}>~4iV6< zbu+Z?y%_Oomk9RY89V;Q`e5tw48e`75kO1^LhW`U8Xu>j4rgGLtcnp&AB`eE6u^p&qzn z5x!16q2~NM(Z;akJb1?WU$$;@$F@~OpmqJJrGI~vcz;+#bI0~zPv}9ku0Ml33$}mo z{z2ukzCX|h)~>DjOpiuHW9bMWzJnR`CO@tkTXrP&gpr6*;?E%W1>(zf>+&H#t}vFY zUHZV-jflq51RyrUn_vO|d^EP~6V(%*D2#T02FclH1Q6@{=Ofsq4?Ker(OB~Dj|%S( z`|D|#bBRBL^zV-f?+^Q&2fOrv*(V|zOaA>);r$Wj5^u}Sa6K}^+4IQw{;2T& z2y=w6|r+{wFz*P)aGisODcz;AhW68e_ zDm)|jmzL^0!AjmA38MR)tFdIv>*uVM&wuV()eD=PY2SmvZtxe-%}Lz*K%6*btWyz{{-W#ls#@5tg zo~{4X(|_eS>!Ue?d<}?G5Y_UxZ(XWgUMr%p^dS&8!p9HbLrd{``|rKc^Z5BkclcmE zj{fN!)%okcXQOJoPc3e~_2r&fK03l8n)AVrf!Ga*TYylzyjDbG=~c{y--ApX|EHyR zecpNZ^!(qO?(DAd(TUJfdP>$HSdTtLv_B$a4~9~mKAKaqr+_#Z?KBPTMMQIFYB6_l zd(XAExD|49cJ~J$Cd0=W@S%2jt%%0bb3m+w-a8w;NFR-@sl~D@-Pki??qAu~ZO$Nx zI0jMOj;PcwuNBc)dKQRBHa@ex3`Qe;G`6M|cTT*z=iRy7*F@`k%`Sn8IjSoKOe8T5O{s-^y?G^GYn^N{|=f8bA5n77Z;R}hD z-1YadL-fKLx3u|RM#6pk7q8-4ZECT?&^hX5_BqY&$%V5?V`fCj*YSn7JpSwVtZMGi zcOtl_?{@1qMaj?b6?r6fiil|L;BBz~kb2^sKK>Pw<^*{K> zry1+f*qT~QIeW(X^ux*NSjatq8=q zi0WPJ6a^n{@7~qC=SRoJwsQOJ0r3G4YM0k+tAJ>t!hU7`09yAH>=e;QIJG2#J$T@) zZqK?o?OP6rJAjxBgxc*yG?v(d`(u>+89POx&)rVE8;PfvV{JW}v%4jL*cyn>fl#}= z=GYC0CaN`o*n|5^*eRlq#+G}c_CDb->m!^rLMsf2KLRl)5Nem#d{zRYv9u}>KgLsg zKX!`fqp{`Yl5DX)1XKJAc?&%LaCpLF563<<* z2yA{tAbb7Es_YH{SV3m0KdY)<<+=fq%_J`~Yd>YNiBTT_dj4_vGW zb0SZE+<9c@oEYrVhawtFopWMi3nOuZC5kX7hLgtbIWgF!4@ESV=D{eLin(NzpA#Ef zQ;RWcE?I;*k>_e|_Tkt)ABZ*loEYrVhawtF*gwv>gC6F@#+E~b8IFH<^Oz#kZn##& zz;j|_%hf#0aN!it7@QO3*PtGg{hY|yX*aZ0Ks1&*=fuXA>%lN5Hm7}^b0XV{cIktC z6A+E1&N;EMfMGeelKn$y0{IkCBuA~0Y16UELsF+{?sI7Lc3-?I&WX*bC5J8k z_2w7OG_juJSNv_4`Rp%q@fSUK?Ug+3wJYoXZ*P9>uIJTX4n5u8@k)+e=)nS(n!3rs{Q-m2(_!|6!E~d7uEZpaiX>5H5%=;)@g8rmeLkU5eFT8 zOnujs$=0scY4Ca|LQ6g1^pFRr`PTtlEUiOYgrO~EQVT5DW;jggGXMZ7tzd6BcwUi=Svt1Su ziXI$6`$W;M)@ks1BnYRcZqfWZebQ1I748(lM_Ovj;(tR7jBdR2=Dy~I^WL8;M3Z zcvOn;zxvgMPJQ6{D>zOWiN;P{T+S`s^-zS?Nzthf&LepCY$O_$Mw=r1uW)rC)KZGT zd{}VaV7waP;Pv3I9&j!sLeYaGIIm+av{7k*0SPtILP_P7(Yi z2fmx=L;VfzgKtf|X{-rQsQ6ay@}a&{1hvii&Z7_YH@FYJ%h9^l z2jBj|-{3w_!-8+KP9N$!jS8*E-}>zGp}tcD-^>_^7NO|D5mdnE8l_L1&;rq4|og(l?D>#p{ltwssREp42 zd}H&y*j*34_xP6N9gRw(O`}qT`upE}XesrF->-dqz4qyS$Ki?PzwkRzc4cX2ZDTIy zDl^XD10%E)uho(8be09`O)`!{(b2|~?+O^d{2rZ@l9OCLJ50$s>a)9-rw!-_I zh;ThPLQAPXhuC_vxvG_)+Sd9|TVW+bM7SOtp{3NHL;PT&b*hCvS=0JZTNshtC&A2p zIe$~eUJs7YQtHnkPIztmYQ$x;S|4f)Pp=*EM1*UHnCIhzs_Rd`9cMVk=$&v)gnlO& zGmsCp%WL(G-J=0~s9n8wQ9XFyG1bb&GBzspj<?H z=BX|FU2FRTZv&2a|C|qmpxsbP{kfNXwV+f8+oSNkx- zaC$zp6nkySJH8E(mKuuIoelzBLhsGUXr4R!_ri@B=?{xQ>lQLz3pc`vSP}@1m`(&* zw~%KdA{tAb2()hDo=)375z$!cM4)wx@N8>68cUrBv~CfeZACPeIuU5yB0P7BXe@Oi z(7L`A(|RD-HgAx(02S;cr_2&>+hgMvV(udl@>fWwFi3rz& zBeazIbBL9n+P30)ls?oJz7y;kl!$OWI6_OQKZn3NwBmY{KGYW0p?2*?M7SOtp{3NH zLtq_Raji%nYKyyTTwM|ot_MeGDfQjYNd&!4X!&U9 zE5Ssw*W^J4AI=gG_!m~NO(fuaA=0Te8K7#(6%Pot3r1Z(5o9&~Z$7{hvp&dVRez}u zwF_pT^+ERG|K=kg$WaW4?zRf0Jgd?AP(*iI1q4fl2+3;f@`1{@*40v>-N;cK9HFH` zACPf4I6_N>7A1dhaD>R>dJ>vqN6WdYwO1T=Sa6LFeOQ}DHz}hP38jC*E7S>kw1e1txJvc&3sXvFn z+A8N7i$2sAPc^s}A|hN5j?hx-&mpk3%DKj(54DB0l|A_*B3uuS&{FEpA+WZ}xyGW8 z)RsGyBLYuW!KZC-gqBi&4uN?jr+xR&W*AzI0qr95&~gES4`-Q)q14D(S2CsSG2@}!OX?loJp7l#agYHCV?139TU{t+~FTK;S$@%xd zf?+9Jk6vD-cTz;)w6N54YA&4NrcRv*jh$Tmz{2nH(MxZPs=33l)F1*2hNWUuy+mj! zUI*U(fC%fOx#Q4^aL-Do47pOL2~(X3g{wTB_l2POs)#N*SO7wiQ~T;m-3K{cKr6 zyvA6teD|cSEJ91+nYWyu=_AnRkw=daW7$rrKWSa-L)ra`Xe@OiG!o2RMOZBjUXSLE zo#!J0b7j;Ek8^uhR2UhS8OC}v5q3s}QDPMqSSmqyRfO)=ZM<9NWKXjqd&g>3Cx~9! zCBi9*r-;C*PZ0Qj(%Rx(lK61%#Oc)o&tv4rHFXk%jjHkPq6+V;z`{?WvIvcY*8$O3 z>h#gra&-zEqrlrA*hjF-Q4$c1rA{AOiet>p=Z!W%w~xlVUGGFhVET6>)GnVc_m*k2 z0V1N8SFX>SKESh$G2p%|$m}6sk)_i0RddJA9}$7KoqrxX5zUl*dtgW{7XyWO&w>ru9~xWFA>^n zoDJN{Vt^iuh-Ngn`6D2LZi6k@vG!^D8S8{NUo{a*J#17O37=HALTVHwq7RgI^=Q1i z`6?iSRzpPa0{_ys!dk>KC;9g#!p^8P64wWAJ@>!+2v2b1uhR$bgi;)dVV%gRy4F{{ zA>J?>D1xoqwT82Z=8kKH;De*2Yn^Bj&7Bm%C#`EOY7xzy6v3ytYrSg`&7Bm{v~*`% zHFr|Pfc;^8=(DYD)wFu2kH%K&Bj|G&{EP2e>!Z1oB7)jRH=?=Ifrv^Nojy8NNO6>) zPTea@IJ9;#@5TM3IE!-KMV+b`73#o$POZb;aZv?yGgo47H|LIRK!g^>-mcAJCL&zB zF(!y!AC-+HlnOqAC7jVBkCS`6^btzYR>;65B3wHkg6IthE!7dzsV`N=(A`cPZg+qIeNM1<=u zL~lT7DfQ zJhS2v=M#&;*8QJ_fc)I>Nv zL{n-a`X>19L^P@js=(f8uCql$U%1w7^dy{0clafNM$2M)2)0kq({Fr#8^Wo$IAwmP znl6Z5_PIvYs0O&-3ve3ffDSz;qr27(ztb={f_)HF%-QEbTYYdu zQ_B7pjMs9XUQ`^fLGhja-_=%KHE&9}F&32T2agI`I9@~FJyAh{e{cj8dc01*{e6@8 zy8weD_|%495@?>_>D9a`rO(Rr5UfWSiOrKXJ%m2JSNh_+PlO z^NRnioX)5auh#^#4+0pKQiKr8S_NhaQHi}8Mt>Ul5r2T>MT8H@xb&5V3-<>`% zUSm{Rrv^PeL{rK>QQmhag0Y9+%3z(GT1)f0Io(e!MtF=}Bc!sC`-PHDABv#zQ|Oz< z_qQR!d__xL&tNZSANA3ca=-aB@O-6+MwKc3U75586~UUzd{qxTUnzq9ALc9e`M~p) zBAQZeBz}86Fz?1+?P33SwUwQ()JIdw{?f)EV^rLcaCnV`nZ zy6y_C<429Ie)GlD^6JUIsJGhxn);^`X>ZH=jLWXBAKUNx4uAXqyl!v$#P-!8=g(4g zB7XbemGyo9ytxCt>d?PWTmDyLsxO{7yURzYM_Ruf&i;7#KPQi^)}A=NoN=8U>+|ow zrk-;7wYB<75ht9mVEdzq`e_8FJ;X_v#QZ0PP4qb@H_PYpua#siXwHf+{?epHU z)lpCXq&n-hk@agmSJY2$eQVw6uko?K#@n`EpJS(L$7g3s5bX0%N?RI|t#>}s{^_o} zRhKXSMX5ftl(w5O=f3#a@WW@hu3YNW(bd6o|FVATHy6}r|N3ql`%2rNi}8Dpy>9BX z%cS1E+y3)XIr)Wsx)7mMTBiy39y|P#)m|Mh)W7UfulU)8^=rSst5$z0;>G&S z_7}&^Sxq=@n*^byw0_3?b-}9d<&FBv_J2lI@Ad6cKXLl`^&$1$i4XN{%=`bY*S{1` z?5lWk*EwsK`sD@Atsg(g)fR6c)cW3zR#;)#8>T(K{&VdMu1FD&ZM=Vd<#zo2V25bX z+r076aI@TL<;iD{O?+t0HCkid?w>ks)#WFZOYJZwL9ow5e<&K-(W{+O7Ki860!uDh zo%r7q>UqDJTp#h(t@ZVjj;ZIo`cixS)&?ik|2pZ4`lh}c5asuNKcaoh$oZ;$*V(fA z@f~C8i9fi=j&^OIv~?dg$F^TMZGq~M!!}P4TI$nd4y%v5=Cb-=_%`OvzinKuw*B1I z+uu(S8kI(C%sn$KP@eJd0#$qLril-=t7v@pKX7+Bbi%Oeo1N#YZkcm@ec728)rZ5o z)^@K^6Y7sAPOfjXHUEC^o#n;b4y*RtX8r`Bkto`jtH+;O9(dgx)kANNs75VyVm)G= z^Xru`GbloRLq+$r5#^UR&sF{8z|9kcwzQ&+Ir)b#mEXU7rE0=ji&htGH=#c6wexK~ z)NWdj9X6P;`m}H9YO%@lRogy0vHtGQ&antZr-FI~NNON!7^`EKXdJMMR` zZ>wV_UtW%z*6jb9Aebm?l34`!@dr_5LN?K-*Mrf-666ZMz+SbOiKtHO;x@q)9_2$KRi%@imn18GFt5?3dznt-+`4WVd zI&Ih$^`Wz$=zDOv1x)p){EBku5t~;>Y&NO>>#V2S9#kLd8^7oE^gqgdXMLc&`uPzF zLhGSuV;=d-AIl4GeXbm_$QB7gTUXJ>JpcT9)zI;GmXlUqup08!)yJ>{RY;yPo02#xu{%5zu$ z>D#n?Wv0y&gqG6!LD6{Fan;xZ)@xsTMsNGb-L9=?-eecMvc$f4WVpHYBKHC9%5u+f z)!GlgGHutNzndVm6xVX@eZqC=h?##@?fL13ydd+SjkqH}`92B?zsPqOmTyb8Iz! z^EHM)d%^ewvHES-)K6r)*j1*YjT!#psOr*x4QbDCSzm&XwXyDq4v2%sY)gQ0j3UJD;J5>LA;@RO7emEpSXl)e@ zA6WBzf9JFNmRk1ZQhjK`$sC|GMa~Z?;@6@u86{|KH2%lP(!q&wRmch;BKo z#J269Y52nnr#`gi>d%;Gj(ev)YS$auryX}}HN(Z{*AK1q%lbg9mbk)TZQZJmYaos* zu4+aOTc&*Bw50AKW*_owkLK<5XbCye!c%& zd)8y%FRjPHOTW|p=A9c_qdKBeyU)CEPQB5|d)B9o&_lt-*9MxP`haxK6%58)ttSnw6}O;rv#zxqyBI{wED;8zi)rAz04+i zBnY*uXv{u$JltM(l`++EeZMKE{NTX);#aS(hp#xb)^Doxt7yhMmM(98{phNGbZYyn zc@M9beEaHp&-1R1yhP48I7x||NMk0vG;2BG<)f>Qulp=PsNMAIZux6WIc=H4tAEeF zf2lq+LPZ<%%@#|PPmepKy7BO{%GHJ(R$ssA)%BYPPOTN8mW)~Z+Ih2AwEOhCyy#KGe4{n?5p2yPmv9_1=zuPZ0VIsQr)ql7&BRpSMm?-M{g2mHNgqFf-X5=Lsv;NY%mcM#_!>ZhC!K!-s(0ZE# zuB!Lj^v2|iCH=-Z;K2pUyGLwT4c&LK1fl(*{bbB{em-+~`;l8#JH9keg3wawD4F}6 znaTrq-@TgszrRu5e8Nx8w!bj#7V^6X(0G zocPGD)rqT3OAuO0ePeDnUzN-3wP|H0Oe>e%;`I8^FD|n+S0C!zn8n_FvwZHXy{eU; zpFKgS-LxJ{zx7J_z|p%_o4q|QL8x6t8?zo*VI#r3KX#az_QlL+)`vZFseL}wF7g8+ z)8ClMqdzPkdVcrnM_Yc~R)p3`(Z<|)#X{BF8;+E)Q6(6npwzH|Gs9EYQ6v5lOVKp6>ZFIE6-HD|J;Vvx|61rx2`s+ zKIFoSy82vw8}sB(=B)Odyjr!^2cIMe{c2Qy*e}_9@#>nzHmpYc-|Xe{&z@6%v)qNY z=4v;s`P+{#Ta8W2ajNRwD z`&&OczwYTh-}bpig1nQcAYshx+b&Zr@tX~*r62vetq84?qA_1>e_P+kufA;WHrKf7 zmfnNvA%8fhUVFoPYTZ>!_viMw=fL(1hfXVh^uVYDp*v~1mu1XKM;_gt`c}J~uwIJL zQo285%-w7Jq3?{1$CszwG&b>}k*Gg>U(HZdhb^~UwcXxdx8MKZg1U|UM84Iu71Fj^ zWw+(4caL15T71TPOGRjZXuBD+|N5&|SMI)AHDuPq5`>mgf5!ZI$ZhR=m$|1LG3$ZV zuxG~9kI#8dy)If;TRN@zkG{WXd+4}(%Ib*&s}uioRDIIc=hpk(e0Qzr6tTiq^Oi@Q z{dD>Bh4)DiT1t0xjA^ZXUf)^wPHsQ8)^XLmSC6iLH(^qJ%q{oSTYYB?P6MdUb8piB zSTo%5m-aSqz0!Mx|)H4R+bEz2C6S$_I}-DnV$?6>ZG4ueNNRdCB7CWs_1L zIuaF)FRw?ptuEVmu4?X!PwabVt4r&n9y`-MAKK5z{fa6s$bs8&wQ6iWS2b$8kIOT6 zzo_2w@JV(q(On$ve`Bt@WW{QR-F{HrJjb*Ip`~>67&Fs~8&((pYUXP5FYhlmI)8F~ z>X{SmNK_x{8@rs@M%6M~FIN5WZ!;wbt%ueRXJoJcv>N*U64jY&&z>MO5=9&H%5FW? zX2qh_wMB1&&`1=GGp{W-C~trJf^z8SgR1Z2J7ewVCfTv8qcR;=uV>4)XZXRV?Weyx zErAk%tL>Ba z`SO=;R2}=;jMe1%?@JKcAKFjYq3^-Y{K-SAH?AF(AheYFGiKc{W^OTIsmfxH6plZs-C)ZzGaCW^v<|~ab^|8{{qsmL(?<=zdMkWX?rTdp) z!f$^-b?#*kVI=O`_lq5`s@GZdWc!?JKO>JcDneoBZpl5WQ-1%K^0PN)E&J!ZvL3$j zsdkj;h*325!bj~{ZFj-D0 zGiK3OcB+=!=AH7R#g9)AT1uleX2VN%sp>;+Ef;+6z67DAbXV1w<)6a(?W8Bl8Rz{d zL1-x*`NsU>Wg>pwc|>6fwea=N{-sOn)~fL%7ea{mLRm0wwp2k7_ocx`Np@E8{U0Gg3wah3dX!R z@z>RQN1s*x;j7aVgqG4e8T0*L{Iq@3Zw_l;ksnvhH|~`Bg*Q&ObK*kJo>ITM<0*Dd z)Va%;4^Eu5ecee5luvyB*aV@abc7nS+Ol($D=czS`@Kj~cgt;e|6zIfmPaKBwX0}jF8ba^~uRPYJ{ne{t1_se|~Rr**j`vg3wZW7Glh>4GyhFJhDT1;P>_~^QEq-Z+z)c zJ9f1Xbi5g}#Q3ADoiCWV{PhDv5`>mY$L{QZ-?w^Th7HOk{?eBqbR_Bs#TWgXzpge~ zee3cM|2H*3Xeo^l+5DT_)js9O+uNfs`>ejlg!-nh#&`9gwj1*1r(RT6*a3NM?t>D9 zwshK7FTXsZ?7efN^6d}CBnY*uXk%8Ge025emA-B-G^&skNok zwt8W`vDK2V9MN9*it!0TPf61Ak2jt?rh4Y-JKAp!`EG(xyNbqocdk?0V|SXhe0Ygt zs{=NeP(Ss}$z4yBwxKcGUv)(LH#6t$yYUPvV)QRhub&*#Yo9@VR*bo3fpf|`XBb)T zj^|_e!e`YBAAYj!54D@N?z(5LP+ocCOy&7o9F-u{pQ5oAJ#uKd)Ri-|*O>X(1ffwW z+L*nrJ)xZEUq_dJ+wGugx!2CF-+T2W`$TDkX;e!c&@Rt8XTNgwGe;%}Eu|-o#;kJ9 z%k5`JpV{v)aw&lfyr5 zZ}X=E6NHvZqdMl}xvI(4#%0g0n^%)Rxv1X#^+W3mC*N6XDLpI1{?#%IRWDqzaJkPB zSf2<&OX*3YG5_45YMWO-Xdk}LvDKMlaH9Y9v36W(&9z1F1$pUN?Y~X^ci(#$SBltg z%QNfywxu4!y|to^dEw3L%Z+bcxLjhb1FN~``fdI2vq#(3Rl8azoIzgwlX9!YZfl>u z`B4c%{VCd*?@m~}{OyA$4qNdr$0P`iO3}uAcE+vcf1jDVeCqsB)e^tBpk8a_q4lhv z-&JdbX;h>4|8u$Xhx3=4ow{d&&{Ep!#{BG&7t7WxE3~)!>ps=P|GBtc@BRJjM@HXS z>&ckD4& zyupYBp>`E*%=I%2sa|;G|604A_lpFfcC}8%{Ai=^mY?_E-FMA>$5tQy`uZNaH}#=*6^$d753enM zdiS2~YY!Nm_)xoPJ@T`kYuBR}Z_jz~xCEi+oLVQmKaM=5Jngshv_E)hY=Tg`ipKl= zvwf@e{%^_h$KxI<-}&^a`mb9bSYN&JjkUgO_3eXq?T-6Zd*8K4IqtFt6NHx1cNdihkE)xS4=uzg$K`N~)LJTi*X}ZQ-|EZdziE#spG*+yPtnLp-Dl&f_2_=nZrWstN_}X}6>ZF2 z-x*ciH1)RjDkG2wDN$)8niGJX?cbq4{xMto$CDOMd}yXZ;?GRFZ;z^eceeIN?|+*3 zP`ipY<`(2w;{2!gBjj276Y!!lzd%pMBf<=VC^)xk-hnaW7QT*3Z@pVwIlA&^@kQrJ zetJgl5oQpCjWCUB=$b<+`k48_9V&koUoa~~?WSk+9$^MS*a#JE%;z)PU+wMXe|;WF z(V2yk=9kbmqoOZGXsI;6#QU(m6rpuW>!Cipt%7+yicWp7RT&jq+2{2X%!1caX}*s~ z*q$5!Vf`uE*wH~BZhYBHc#Mq8k-MVVO)f*8e~$=1BD)Ya!W0erwlqf~vqLx{LPm6x z<+~;PGFOJ9aykepwIzH7OGL1pTnjK`DrS#MsV&h0;qTKyXoP|$f+NKBBr}_0PP^FM z5@Rj=eL9FF!Za${ccZ?^P)EDrIf;A&jQYSi9HVNN<0|}pItV-BJ;y|% zA|nONl=aCm0%poEvQiYGrP650NC7jYkE|3jQjmR8 zkd>n76hTG`m?Ox*AR`6LlonYjiqKM8KV!&90W)PdSt(?s;KaBfD@A>%C1c1)0W;-(vQo%M!FhAx zSSftNa7MT{X$9m{gOLJe$}qB06hTG`PA)CNua^8Z87W|<^pTaK2#rdkHHM57FjIz+ zm7)mk4@G159E=n&Q!=tr$VdU>p&%=Tj1*)E*=sUVkOfmXRtgy@V5YRlN+BZ!r^j}* zYx|_FOGXNqDJ`;66rrWaNI@o2;aDkTq=1={k(HtdjY^}%moOM9V5Vebr6@w}DjH8M z7%5<;WMrj~kplig!EAM{Eg31u|0o&9;f6Qy*lcfSHn$l|n`em>V`KMW`h(U%*HKGbJZ0g^Uz%87xB4DT0g? zFjI1}QWT-3$VkCSknceY_Z5|=xUwW8<;Xq9N+BZ!j0d|Gr4OtXy<85P2hwmz``p8HDGbJZ0r4xa5wXdzB$u9xVBqztD6A|o^UxK}#!f{N< zF9FXaC&xq)S`V!s-XCDSfc=t_`J#xll=cT1DPX3w$Vwq21zd)LtQ7VjRvLEI=SIxL z!hLO%kuq}FG8tJZod~RXY$-BQus7-vWTb$Zl9QF92r^QzH(HRDlGdDz6fjdPD@74% zmy8tbrWTHsLPiRhDH&NQiqKN(&loaNz)Z==N+BZ!%#?zx6xJ3iSii>ARv;q<%#@t0 zluktG!4yqK3YaMwSt*@}(7GC}F=V8GnUa&0qKLG8>~4Y|iDaaJnUax}q6oE{qRB`B zGo?jV3K=P2KNMu8=oeD@RYXP#m?;@qDP*J|i@zW%MZeGZD_zLKN1t={AtMFMl#Hwt zMW|gxBNGse6fjdVvQiYG5h~gkGE%@y$;e6}BLzHzf~*ups3n|7fnNfiNk)zd87atw zFUU$!bh_#zhXq`gj64=as1HTs8wdOn@Jw3dn2?czjQ)bG6h){dWaEOv0xnC7JQhW0 ze<&I?2O|Z{ls>Xj$VdUtpdc$n5o*a8@=L%oX^~^12whX_8Xj*oa9F@)X_3dG2>s4f ze>gV)BL&QqHd!f(P`ipYhKv+2Q!=tr$VdTGpkV&4_F6iw$VdS*B_k_E5!!2Me~^&^ zW=cj@iXya>`oozp7%5<;WMrj~kpgByK~{>Q)2PTu0W&2dD@74n3ctD%f4LdIB?CqZ zm?;@qDP*L815%KcqV-6>amX(L&m<$qL=oE3X`hqB0xnBN9*ZKh&lQbt957PAOv%Vf zQH0t}$1WKuV5Vebr6@vUSAY1W9xzhCOv%VfQH0vnR=^Gm7%5<;WMrj~kphl{9VOZy z+UmxTkpgB)MplX<)UKlO#R5hOm?;@qDT+|LX%CW-0%l4^Rtgy@U~v>=rRaB*`op^s zj1(|aGO|(>p{3NfF=V8GnUax}LPiQW61L{*LoFFYMhciI8CfZcP`hb8$VdS*B_k_E z5o%Y_;H83*0%l51Rtgy@;Ck5SL+wI4)9Plrw*eU`V5Vebr6@xEDH@s_V5ESVl9QD} zMhaLJw&sdXeUOm?W=c+0iXzm9qM-)>MhciIIaw)+(AHHn^h>}<0W&2hD}{^{FgLpT zTrFWu4Mqx>DLGjwiqNk{^#_FwutmTc$;lWYBLzGITXRLIC1c1)0W&2hD@74nDvgS) z7qDM)GG7#-rP9_VBL&QqoU9ZwQowkyeXgbO`&d?w)%7_UDPX4LWThxV{V5vb3XBvm zQ-+h3LPiSm{tL2FbXQGx!i*s!1_&l+ z0%po^vQiYGk*Ggo$VdS*B_}I|j1*+p+aAhh4rsQO$ zC_+oAKdjxsNC7jYMOF$KDaf`j$V$;Vr8OtN1U!?B91}89kdt4Km7=9|4+vlM;IM$p z(jt#V5$aFT#*mQ$X3B7~QpiXFGo>Iag^Uzr#oI3k?SEubfsq1cN{g%%MW_!&8$(74 zm?yCM!h|8i}HjNe)H|m?=3~DP*L8DPW%u?PvT(oz+Bk&j%SP zV5a0`rI3*VPKKRJbQeeaAEOB0z%yx)V?ss> z*a&v)YCoqvNDd3QEE#z$iqKJ+j$JZRz)We8m7)l>t7v1$76EG{Cu4++6lC<<*3~+t ztxHAgxQnWE-y@36allh_uwVRGBGE%@yX_1veMhaLQ1z9O%q#&o-K2h3k z@Bu~&m?_i9N>PNC(iSm>j1(|aT4be=k%EkV+aKyfEg3^b3YaMwSt*K8yJ>%rkpgB) zi>wqys9i-HLw*T(CK)*_|Pc6B$>7;;#^WoePeq6qb;Xk=4?kpgB)PF4yT zDc~8{=Un>$zjtZ%)!lPWehGLcIXNa|q#)bgjuJ(vC1c280hc8wk3|vM|B6QIg8Kmu zNKXC-87W|<*ttY^IdzPICkze?xGXt&EQ-)l>JQ(8V5ESVl9QF92#rwvA#)k57qDM) zGG7#-b`=d34=_@|OlgsoLPiQQ?7PO5_6>Mfi=_ny11Rn6&WdDretKL zC_+o=SqMZtz(@fzB_}I|j1=Ue+OeyBpyLhSIN+CnXOfd+q6i&V+Ox)xUjm*Q?NuLigQou}Uk(EM53iu1Q2em)6-Jom)MhciI8CfZc&~{7P zii{L6Q!=tr6rpw%Z44PHV5a0`rI3+=jMajy6s=QQ4>D4~Ov%YgQG}k7q~{-Gq=1={ zla-lonYjiXbBetO5HBYD+_z0*n+e zQ!=tr$VdUh!uE&SOA0W+mVRtgy@U_98GYgFnFD;Y3Sz)Z==N>PNC zQs2q5c$Y3>hh4retKLC_` zA|nONl#HwtMQAB)b?kV7`vDF}PW}fODPX1)WTj}Sv_HsU0hc8sk3|uBmZrXqA%_KA zmW(_WMQADYhx|lvSioh;$zxH3Mv|Ujk&yysN={aaBD9o7h*bs{DPX2#WTlXi0%l4< zR*Ie|YTtm*2}TN-DH&NQiqQVhI-v)_NC7h?BP&G_YFE+7>jNWY;SaCP$VyR!+D+?0 zMoND6b1kw`6rtywS|?-3NC7h?BP&G_YFE)%UxASVW=c+03K=QjWE5nj=-XM}KE{xd z0%l51R*E9Dl)hJtAtMFMl$@*-MQER^Kja64kpgB)PF9K{w9gfda~v>Iz)Z=>N+BZ! z%#DJq6h){dJRe}BfSHn$m7)kOmDYod6fjeAvQo%M0SBZYD@9AC?^-faz)Z=>N>POR zQ#5#YV5ESVl9QF92(5>rvEBtE1~WEe}d$d1mb+wZfZ3u^QE?C6M25y+0tsoU?fqYLUW`t0avH?28xopY-2 z`&{RO8iVwqc5!|lE#Xx=er!&qf1ghs5jdlb5h~i|I_K2w_qommwc(i^&Flx(oVhXD zZrD#;VMtEhexFYr5zMe4!sk;*w9R$Ssj2UCoeOFjGCLZ%&atgDLYwQHQ|aI5I!6R@ zgrmPSDr8{iRQmTB*b$+n)Su0c&Zyh(v!e^@!TaoJ8@tbrj&-ux(HV97eRgz2Xw5Z3 zn;o4|x8G++7gWmf+0hZ5B9I-OQ@7t|M@NK~!s&agpUsZWsN3(eqYLV3`RwTE17{)8 zlFfB)QH9^rIcx2W{*^Qj}kWe2%Z%zh!lF?*2f zoKc0}=Q>A(mO`#`j1cR@NLje=$yL!K07)haDE=6 zLXL2Z)@DcN)b01#(Gj6`6^&wId-)PBZN9WY-_u0`Af%BnQC-rBuqjT!^`|Rj~8uzS+ z+D+?$T<4rB{65#YpyoUgS_-Ep1$Dh#&5=)?Q|aI5Q%3~ykfT3E<0S51UuV?R_u0_} z)jL>o>>I~AX@oXAI-}CR&yJ1=Ev5c!c63Iif1e#45n2kF%Q0G;9i3CR-)Bb`)PrZu zwNzSjWJhPz?f2Qy5rKW5=ugo$J36Oszt4^?s4&Dh}BW=z>b`L?HJ#qEiI2qciIE`|Rk5&{EiqEU1&?`W(5=8CCdwu5&@P z2=<^xrT&aTK6OT=f1ghs5n2k_ztOkNj?Spt@3W&LLR(k;;i(;eXGYzApB-ILv7R+o zyJnXGcebmclB$AV1u-6|$o<>h}BW=zE}pX*#uLB;;tS*spN~RvNqlZ(s-)Bch1oqxygm`NT8oACbs_^?<=ZL^6 zydXc^=Q_J*SSR% zexK_c5oxbAy@~Jh+aGLEh2Q5oM})RQibe)@AAcLbXJAJJV-G#3Xk(Baom02pXGa&* zhWFXg@e3*ajzV^HPThW=9bHi4+-FC}FH>ZK7St$r^9r(~bL#f{?C6M4yXn^*vZHhA z_WSJUh(KO&j8M@wJ36Oszt4^?sKV~Eqay-&uhEjtj?SsV@3W%|>V*63=!i~NeaMc^ zsr2u&qa#A2LY{3z+wAC^D*Qe>x}e^<&yJ2!ArmxOvf0r&mHvHpbVMNcIQED7v)R!( zb^CpGbU_t%pB)_$$Y_n0Z02%KO?{ua91*&1Pro^l9i3C@-)BchgqG3>Z3cEurGKA+ z9TB>2SARI|+iS0!D*QgzxuA|Zv!k^S($+;jbxx&!pHCeT+Ry1oM0Rve-F}}P9T93* z(Kb6er*6N`jxMO%?z5vKI`x6<=$yL!K07)hv=p*G3+j@)=L6Z%Id%Jec6337e4ia1 zzgW_59Aro5RN?p8(Gh|C=Gf9{pCdaur_#UAj*bZJ4@KMT=$yL!K07)h)NVR`ao83w1hSF9-HP=`uF+N z5usmOiniI&Id%Jec631ndDdJJ$YPF`Y<6@`-F}}P9T6HyS`TDL=hW@@+0hZ9b`@<5 za-DOk@cUfnf=c*&K9ETq(WwvQQ|DCr_xaQjp{0;1TTsK@Js-%9&Z*n)v!e@Y&a>vo zZ;t2`f$Zp)wx{{B0D;#ZokitjtJ!MMt_R7xy~6? z_j=}a_aW` z?C6M4yNbq2e3^SP>h}BW=z?10K07+rDQzp{I%m}F_qomm70a0&jeOCFP7%nb&Zyh( z^Qj|3OX;qW&5mwSx8G++7u4l_hk7rD-{6>L6rMx}qBPaP3jO8wdF=$yL!K0CUgjyj(Y?KNaH7S!E#&j)gybE@$B zT<3y{^__DGa)e{gA|oZAE2q-G&!>(E-S^QkX0xMns_^^l=z>b`9Elng@@!+Y*pqpG ziJU6@K07)hG!jMIT<4rB{65z?B9QAGBUH4_r_T9n0zRKQBD5Zgw%O4ws_^^l=z?19 z9J|`jX@4L)I-}CR&yJ1=`E*GnX^!_WR7`f_mp{U2V~{ z&yi1^Q-$B>Q%3|cqGKy4+8E?h=T!Rl`P31iZKY^r%Kq}q7Ipi5c633Fd!HR$P;r`1 zl(w7Aj?Spt@3W&LLQ5eJIkt$+bYcMcv{c%<$fs^m>EGv5M+CABV^qj{ zj=qtR^5-I>ZokitjtDKK{%m%1MiqXa9bHg)-DgL~eQMo}wAs-amHvHpbVMLiHu_8J zf$Zpvy8S*ox}XX>pL6YJWSADzK6lSKvZFJq@cZoOf?DJpC5k}SY_w#vqcbY~`|Rk5 zKvr?=e?{Bu=!^>aK0CUgZa?P|-R0B~W3!_(D*gNH=!no#>d$6JXVmTY+0hY!EY=vI z`m@>58GjqVXGceb+Euj8j&4!6-)Bb`)F0=#(ov#)W3!_(>h}BW=!igmbL`Eu&ygLS zQMcb`M@NL(RkY2H&Zyh(v!e@Y!?Oo9_Ou6)9i36P-)BchgqBK239_Rz>h}BW=z?nE z^nr}uh)xm6j?Spt@3W%|>YZ~WYN>Ru7unGnb^CpGbVMMlH2PDt&5q8f+wZfZ3+kP7 zT1apJcE(>5@HyKNp^+#WqMvtelu=XPXD%01cV`c3TOo5fwg_hGBX4g}>EGv5 zM}+pEqHR8PMx}qBPaP3zH*H;HF1M(u?=zPR>iIG|T5Fr0W*|GdMcsa%9UT!`O3y-U z26l_T4d64dBLdlNvE9_4&2?^3h2Q5o7u2=q6NNm~f(q$;qO=ukK6OT=f1ghs5n2kF z%dtglc63JFexDs(P=B8Np{3HcLauX*D*QgzIU+PFwy&5oW%-F}}P9T8d&MceG?jJo|k zJG!85yU&h}=rk&1M`zUS_u0`Ap{0h}BW=vXT44`fHTsPyl% zqa#Ak(vZs=ecN2;7Ipi5u5(1F-Sh+t`P41`Hh|BkjtI4z)&tqmZR+;>?C6M4yBeX* zjvh|kexDs(P`BS_N5_*&?HijN-J)*4&yJ1=8JJ31otoKsIpZFY2vy8S*oIwCX@McZ8Gj4J#-*SVl# zJ+q_rZGimU_?EEw)ESljeLi(W==)sJHaj|_ZokitjtCtkiZ;xSZc(@2XGceb_PL^M zc65un{XRRopsu~oj*e%|$a{{KY<6^uy8S*oIwG`GT61Jax2W6iv!e?th}BW=!igmbM%+`Kz8&n>h}BW=!npoE81p9x2W6iv!f$IBhj1yn;qSvZokit zjtJy8$1DQ%hmrX1Y%S{c`|Rk5P`iqz*0j|L|{ziUiTa$BPc|wU5>Hv_vs*#2%BhOyX8^*y$SdT znZ249Y_D;Tc{1OEu%)~u{}gcC(g(*bM}ljkyX^CTYg@|y-H+*SLDYP@QXd?J^ue*q zex(opdlS62cKzS|nEn<-&8I7kildM|I1)AA++W*K!k(l*HxfD0iQs5gghrxh_iRiD zVe6!5_iPZssJwT?ZGUgVwt}!Ad{i8_L~!gX^Fd?xfA?ehTM*WtqTRTq4>xujW=Lw+ z{vAz*{~^^=)GRtoj{$#@94gUtwMj#qd-sMk+s3Rx+gh){}p z{mkHTQBkj-JQH$EsMk+!LC6GT&xL$A*536w_4>(WA&;dK(Uh{8fWCF9*H2~&St*@} zP>(cP>h+VELRJbGDFt&^m?Nm@6hXayGE>M(pt;UQfbYp*H4}aIVRNWXHE=# zs6R#HH>9E0Pc932EQ-)l>f0FV^^<2pjtTYp$$ns-zI%e=8%}0z>h(jnKPQi+6T#=g zX}s#JO}&1yM#vbUUOzK+$V#D)(1Y4;*vo)kKXm(Z@>mq1rL+}{p@@rNr1DL}w~V#|_4=XP-y)Ai5!CDFS!X~rXzKNoXF`sNA~Y(E7OHj7>nE3mJQhW0 zB#JhMdi~Jt&&f)mUOyQRWTH^7pE*Cgrd~fYUmPoidi~Jt&&f)mUO!KdIoh>-($=M3 zKba|Hr6@v6QLmqQMUIt1y?*HS=VYZQLZi}XjiFvYbo+C%QWT+f6%7pt==GDCLRJd( z`pJG^KDE}Cdi~4=aoNVy>xXWCPF9K{G!jMQH`bxoPi6{PDb(xdX%(~86`_`lpp{JK@=VAvpPORQ#2SO(Cde8zt!xgUO&(5*d~fj zeNeBT%oMUxsMk*x2cuGiTEdqw^!lOOpOKY9y?$~Th){Hjpk6`C z2dURjo(VZ7)a&PdD0@(Ss3oumpw~|>3wbPx(0VA^80z&yx8G{^D?(dW(O?5Yub<2m zvQntmPfiAFu92kmp!z;}Cghkh(jne;9czig2q+zS-<=t>M3Xv++BTdi`Xkkd;Ece(v$G zbv2TX)`ea_nJHwYP_Lhi2d+hRO`Wd7slHFv2pJ=ap!z=dOt_v)qXi=+Co_et6h%;d zpF1FAr6}4M{w4s~FJ!(bLhWjV;2S`FpUf1pQmEI@-4u==jXnJ;qQXC!DP*N6LQADz zMby$KGli@aMQDWT&loEFlbJ$RiXzl575=$f z|Daw!`3q#Fq~Ee!198M~9Yno;GE>M(QG{FTxKh;X=iaD8P_LiN6tYqjLA`$Njgpm; z)`NQeWTudnq6oE1y?*YdI#vqx`k~uDoU9Z@Xesq)4Au9^Od%@;jFbX;_g1rCTQ~jM zroumz{@Y}wC_+o6?MC%|GE>M(QH0i3{o%Klp}tRM3Rx+N&^jp^nH=~{05Vg^N>PMH zqG-%M(Ca5Ng{&0n^^@^HR*HThrC&wV>nAgXtQ6|?GmD?B6#YJPN`RWy_kpw~}k3Rx+N&n1A==DRlKPM}Ndi~7kCo4q}Y6&YD==DRlKPM|i5!xS$ z#!L;pe(3h+WTjBApWFhnQWT+PMeNOd#sPO0Hm?%O^r6Z9F|Kzfe$D#-wCF&2WCFu2& zXF`sNA~beIqjjOzPc932EQ-)bv=xk@UO#mEbFxyX*H4ZFM~U`_wmSBGpw|!G{+z57 zMW|gxnDqY8C&`trT*|cCeZ6AmxVkQMQADY zjkf{x`k~vOla)ffesUyObM>K?jG>IqoQ6v*)L?iC_+o6txLUr zGE>M(pnAgXtQ6|?GuxiI zI9jK)=G5ya&x9Nk>h&`xpR5!urF%feP_Lg{7V=mWq5c#NJu>L^L$|+0Rtoj{p^$Gi z`>EH@tayGwX#ZnuL9ZXW{Z_MI5$Z$HcpE^kAG-b7gV5|(gw|Zq$o_<0KXm&u@>mq1 zktiA&*wE{TZok#+r(Qp~1$;iVpTS5em`(1U59;-kXF`q%_4>)f;9R1+INJZnl!abD zxh&+dC_+o=SiqiTwowkn9t5V^{k*?Lq4GlbJ$RiXwDW zrel|S{bZ()m7)l>t7v1W*H4}aIVRNWXSO|CSL>9vF7^7!Ga<)B5!zOY#^{G$KXm&u zGG7#-cGGc1y?!!N$V#DJKUo~i>!V&jbE^47X}h7%q1O-H{*0^?MQAB)5o4&=Po4=m zCe-U^MnC&QeW)d4sMinOeyiE92(_E`2le{NOd%^p5o%Y_nA@S(58ZyN*$+laL2d!_ zMb)nEMxy4>>xXWCMplX<)Sse_p(7AuEM? z{mi!KC{cu3!d@Tr`k~vOk(Htd?SDn%+ZlTO(CyF2N&zFKfI_~VOLS*b$CxqH>xXWC zMplX`E*4E6e<+ix}dsn^eJ`_6Ht zeFGl-smEo|?YEl!iqPIn`<#0HWTudnq6oFCXsEqGuOGVo8CfaR>nHnxJ*cs#JxINN zGE>M(QG}LCM+x=%$xI~)rwHoxlbJ$R3ibM#Vb77MrPAGD>h+VELRN|* z)Sse_p(7AuB}@T1wl^80z&yw?88*MG;y`TfrFW z^+UHmBP&G_T1x8#wKwSXL$}{*_EWE)To2BP)az$fJm*B6yNsb;KXm&uvQiYGrF8V* z><)VU(CxRH{nYCx8-Zh2OQj=`di`Xkkd>kcjY@qRL%n|J_FK(h+VELRN|*w3MEO7(=~&azM!cpk6=oPdRqA4|Kd4L%n`7Q^-nD zgpMoiS$v;EuOGVo8JRDN&{FCTI}_0Bhi-pH9*ZJ0lJxnYUO#yzs# zPi6{PDT>fi>4^z{69B&r&>|~E5!!BPTT!ncy8RhBCe-UEi=*?2()t-gy?*HSTg`q& zP_Lg10zQM<($HssUO#mEt!6*<`pK|hf2iHGb*a}+o(VZ7ico)w##$75{p7Nc$D#<0 zO3}tpuOGVoRxXW?)$CVu z1A6_??YEl!)az&7KU-JrYMqc91-*VUQ^-nDg!)soG1TjaZvQm0QWT+4DcTt7^+UJc zYW7pFpIipAQWTv=MZJD9Q^-nDgqG4)$GC!CKXm(BWTjBAAA0vzvtLW4{XxBcGE>M( zQG}kQsc)!wK(C)X6LL%xp{3NHG1TjaZhwnB7DZ?z>6sVx`pHZoD@74nN+ZP11oZl$ z+drJF6zcUuz20i}>xrWFjWN{ghi-q1tQ19Pe`uYM^#Z+q@=VAvQH0u6G!A8;*ALzP z7I`d+P`hb8z({G4nL<{IBJ`Y7>tqb|`k~w3A}d7^YFE*CpF^)7y8RhBCe-UECxfgM zeLL%$0nZ22_o2d{kz=9=?LkH3eGY|xDE((-z9>TN>YE6nC%AtQ1A4C9L|O*ALzP7Fj8Z&{Ans)axfRg{&0n^+UbhYW8cX^j%B6 zelkPORQ?xPE>nAgXtQ19PJrr#W_4=XP4+dIa;zJ|RoB-@>L$4pY{VlRm)Q4sf zB>oKb`pHZoD@74%SJBL*?zQ>Uyt?SZON!^;ulvVbJhtZb5zjr>f9{S4+Uq~A@N)mF z7ame~BW~TZzyIx*#&jWqrRgJ%p7vz_c~9-1pc6zdBe`sj2m22>>2O;r*yY~`kLvD; zW&g1ox~SAgh&J(IYrf|L5B9G#{)nWu^fA{_ul2vX-Qj7?ZB&ZT2nUbqr>FJxPyX9s z*2hP~AL`#{nSJebh_E{83M_RiY;ehq9HCIF^MIRSk_3d!8dk@}o-x25IyBg}_N(p*F zgI8fPK`#)zB25DAePrDw-V*IoYd=!YA6a*q6H4PMJa+iyc|X$CB~jOacC%Hu2ba9- za@Sy7^aG>hiJS3>i1llw1T?5ex=CR4EqyX*FfKHZ!RI`LzmHzmP(t-cHwon3Ge>;X z`=RuB4dmKxYXhNlE8$lcBmA7Ju5=BhTM55|N+2hW-x%~rXZ>&ujHT_Ky~;I|V5E$2 z4J9!8*6ngdMo%U%^7q|05TrB0TZNo>&Fs}4R|#rogtrQLXTRUoAQ-7h+Y4KSjmur*tzUVreD9_I;P&kw`cb*pq03vw)uJmOF2A(#V)s&d$8-O)eCneY zPa!t$_OtTn*+Hh)Y?W$gtEkr`PPqDx@@MY|l|kuGzNai7`-`@$RYF@ux~Z|`owp?o zB^aqmeDk{hnH#=4%(Z8}ZbQl02Q(`9Wa7MyFO-}UNlzxeaK*1n&h4Zp6Z@_HUCFtf zbd$K@wvWxdcw3ljm0@oZ2e(Z!)17OgbaH zmRz&_rR7I<53S-oz-%ynTq!{>63E9)!>f-H+H+o!$%N(y=_c`wneWYTm7zW7HJnW7 zx`cF-*!*QT_o%V`_ILKWyM(pqV|U!z`{uEywe(yGdf{5VVRlXL^_#vgC;WaWK`#(5 zf5%O||Nx`(ya-Z~ZA3wOn1**s%D4-oKw4=BTef_CRlo4Q=_M#N$iv?_IfFAV^Pc6cj5)!bJZK)s->qr)$9J(MNZHQ8n()`p5xOI>+3`%RF8C%cwy_Od+QGg>jb68 zYuL)Wx9qmC%20YTAuD}vJNtpva@Hyz?a&~hdgC>09o3T+S&wTtt1CUMyD|+QiMBrP zVb`E7fo<#S9(Dj3;jL0aTZmRQ`r#V39_(@TYU{StDz-M$Xb`sk>~ZxQJz zjQ0E5N4pZb7t#Fa3~qeig7Wu2e4DLjSM{~lvY!9oyk+S)`qAl-&@AVRz-LV0UwG*(!+Qe_9uKV|p=UrVK|5%iZhiRi z+(UElVts|pIRlY+JZO;6Gj)P=AYNhjViUV3Ab17@=T{u39f*f)gzRsBZ$i)137u;@ z18QhX>IAiO4fTS}xX`8@2;>Gvyq*EUdw}D#ofweNxOF1eusOX13op(rv@z>LgU3Uw zNa&e1f@=o*{Lh2e&%NxF3k!H@W#F8FNIV`iNaz{RO3)6(uCHD*+}l2t1O(53U~R{7 z+BujWbO;`8?TD7W2ztZOh92-aO_&<;e*4<~r0PH-;CHMAvlg4(%;dckH~w2F4r zxc}JlEB0wOdIkh%862mb6B@TpUencBAUKEPIPIW8LgUtnyj4H3&wp5c z)cP5X1%l%h$7u%*5*oKoK42oJOje6s6QP>B-%Nlaq9#l1>*CT8yr`R z1;R##KOKe!Z6^knANU+Ej>Pl~nO-0)Z~RGduAwcdYfw87)KD+jjN77NxxqZA7YNHc ze})W#c5GE7G;W<>q(E4`>at!j76_|2{tOu!v~xn^)(J)mgjLZQ)Lo4Qf};}0Y3GE- ztrK~xkQ-p(Ww~}A-!8$%IEDs~2MrQ>hO`pg<8(T=+GoA|)N-9I;WHVu1aHjP4;&8~ zB=ihuC1?lYLaSBFtY(4W84$Rlcvpzyv~xn^)(J)mgv}oqSFXWWATToUBo!L80};or z6FgHVkSlm{n`>xG>IAiO4fTS}xM&saKs;-C{&t%S^$ZB)9iBSlIPIL!xOF1eIL7ML z(XU$LD>B9cfr^7?;n1KRG)QRNI>AVRurXEGNSV`EAUIxeoOVuV+&YoBDsTh8R(#S3 zf;KdegVoqIVqpLHXr~tldNqgvas&NIC1C*(YaPAATM4L*EH9 zu0{&|Xd~1MHX{XMspZE4t5;|V-+-zYY#tB&Xd^%~uF^r{CL8T<-7M_S`92d~YU{hy z&H$ff`6?TYh^t;~iPE7}ZA86Kq?@gxdR>pl1970W=;xLn@ZvjDw1i%;c|5eLjR36# z?Vz!<-3we{c=1iH#)Zw}L8FbRw=iqfVBeRN$8kN*=ILqOm~m-9L`g z&IyfMC-PP;|IKd2Y47N{24jKXJ%9%7pg}_8)(J*(VqkyTzhTwK=eQtv51_&0PQ*V# zW#E}QK^o50?{Xp*cP6m8AArqWMw8%57+$&-Wu(ajn8pce>Tw~r_21wvQh z;|aDM1Y6kb2jVV$@^asEpoQF}LZk8Su7gG=`#0?pPoTg80_yi{*Ip=(^F=SOh21wGPPtNaeuSH)OIPwt1V zSd|{%DkbOz4UV3yRX$3%=7g8hhcgK;OfitkL}#dn*q^&Moc!8eEM1)H;7vmZ*R z-gphGOa2=oh|BkNu=V|5-YO;N1w!AejVI^@Lf_rxgpG#uu7JG-!TZ$gxe~lnlM&u3CA1%;Ga}T)_?sutu(gQ)9uaKP8Q~f>>-+Djs26N% zXM}6m%EEus1zt8I_qYzCUPicv67&L1?Tl~@>*0Nvx8cR!!sh-n5d&CyrcO*8iAjTT zLF-Pqk%Nh#7YJ5x`zEs&?^*P-=P&a2=YMwd$^E_e|EJOUJM1~cXRqF5-*~L{(X%I- z-!uYVj1-8Khwaea=g13OL*w#z(3nO*Luvce?6v>fJ^VymgHGqDlRn;GeDg}z(6~GvG^P>IP}=^s>`A*7 z`+cY98fV^lP5=7EpKuM0%i}>~8UYQZ?b}G+epIpSx*c6(i_=1Wv-!dc|2%LBcP$QeGk@uoLgLR^WVD0D{ood zKW?)#T|?vYc+i+eKtpN!cKJ0I6|Y-z^c?zO`CxTviEC(F9uFGR2xusc$oL7*kePQO zR+fXKDjz2h;5D7VMnR_otJw_L~l7$Cl(;$%O2Mt4KE&V5FMUD zToP&r0y%D(>l(;$%O2Mt4KE&V5FMUDToP&r0y%D(>l(;$%O2Mt4KE&V5FMUDToP&r z0y%D(>l(-h<{IhXr5PoJ-NYsVjhv1-J}`IG669SUUYd8baU6cE3Y|d<(MPLDgcc&l zE!PXM$Z^Xg_lhlaJ=)`mpzeKgB9DX|w_JA(IGXP`FuXKTz3uRxMh-S&})8#dcoF6KA)GC>#l(uw@h*kdd-heFW4H% z=kwBX-8GQomPxKbulW(`1zRKed|q0vcQLMzIGXP`FvhluDb?u+_J|t z=run=yJ@2czy{$q%~#rCU^XDxCZ zD{-*MamzQ>60A`&JLKRr0$z+12;?}{?$AJvTkd&W_2TjQ5x7=L+c&>kj$m_V~c+{@lO=uoeu~Ud0Rw z)xc;m!sDu5<&UeYUKmpr*9nX%({?*2R6_}kloQ%sqx%>GXuI9}KCW=HAJ*PJMjg&5 zp;sZ-u&da|_^lHfDJL|RYG`Y70wd2x$|$ydWNmj_uSibl8H{uidfjqDuZijPF*2c- zkFkpJd!DYS5*n%U$34$u`Dk|y)Ga&beaZ=q8@;Bq%DhI5h5zFJ(GR_b4UKelmC);! z6M1`GkNrs6t%Q2f9@F#K5BG|#A~9Ygol!#j)Myn5kBjkzs~zt-u3OZr7|c9XZ!xBD zuR$nX`Qx6e7pf3O{QL=21>^+?rE?8EgX)Id1A*#>{Dhqo8W*(+IXQm~B~;Hf20>|1 zpo*cABCZmu-RXS${}z_FbT7I0B`+>4Y?Ydxk?CEt+1bZ&n9t97bfEsLL=GU`5Q%f z%=ycQw_ozdg{@N4Gcu)fV%a0xl#f1h)^On~jx4n5E1{7(oqe9zr~Jew>xKvHx>I56 z+VqS}>72OswZ9r}amyXUW#2g}n{kxTNS)5nJHMuUde1G(8$Y;1VQbvloG3s1 z=a7U#I-S1$&kYuGylnRcd-hrgTnh*lN}A+AFpyZ$9hL zLg#HIG?G1sS-WPq>$=6|hqu_KuvKb$My7O5TyWTt!vzoRR9?9L;A}ouLL+rLUp{7! z;emr*dHU|V6t=ES&&YK4$%%IzaQEC*`@Xh(?hl6)T9K8|NcKC(e?7SO%`Yx2kF)$h z^+7JgRdK3z8p+<0e0s0BbN}Vt7t&Xmgvqz35{fb`+k#kUDOlQ670>9)6h^lC#;TiQBP1yte&S`krEoo z?%%D5j`K&-(@6HsCl~Blpq`+XU~i6`hKABP zVRfWHJwYweotF|C$-YN%-QoiE1hoWvbL2EMl+FpOBL(USY6(`|$ZzyR35{gmwKeD! zs3)i;*qb9mp`mn6SRE-)Pf$y+T19?CLkW%4=~x{pP)|@xEbscrb!gZ~cN?@u>U6A* z6sRYtB}%AQu7SDSayDMx{ue zgG0-~BIck!3x|fToN@wlx#eIt=3rX4DPb9vB6$v$mV-siL4Otw4PBAt1m<$f!EVgK zw3<}HGAc##94svdi=4t8S>rj@r6mQg8^=U{0$*o`^p&%&Xh>+77rTy8ly6LT=_Ae6BDNRd1TOUuDU zF$dE;s54oa-X4# zxMuKYeYjS6Zfnj@BBU~4-i@t79l@L)_Xg>CyS80h*y*5-U`~&FgY=X`30oDUNWM2H zFsH}8L3-YfogHRq)T^989l@L)_Xg>SkP@~kNRfPRP+(4vdxP}69XmVB(9p;U)Dg_- zac_{GMk!&df)vU31_kEyxHm}8+pz<~3=NH(KpnxH9`^?6DVGwqDoBxhZ%|-Pk9z}u z-i~{&Pjqqubp&&I+#954OG??j}o>jNRfPR(8Zh{_Xg>CJNEyWp`noz zs3Vxup;82It7WsJ${tUsm$neZ_O~5f+;zswOK51U8#aG=|E2rB(={p^#2NR$u{e2k z;e?*4?Ag)s8+YiRzV>3H$7{@vlu+$)8X8w^XdLwO4c(cwCwZ$>{fwXN)L(r4KYBkZ z8^kW#eYpF@tIu&l&s6s8==&>Q(?9-+)v+J#8nYuMR2$b6KjHT#G*)HLj&^bWlUa{s|{jc}Ds+>4ewNPh?ydO%aS6+83W>6}jRjsOalUk)-RkfQ$sF&W^ zs!g~b+N$!ZHe0BH8if;vFB+GXE~A7t?s6NRQ7nnSFefCNcoCPV_DwCd*iF{bf3l% z>ZN*Um9F8cxGS(XNaL!qLFgJz&s4VWcVe!L*YN#E(opS8qthO@uAwV3n^Tjz&9$kP zR5n`GPWb*T5gI99k>#?(7mVPLqUa`VWA=E2Y z>XtP_Rcmogsut$8z3R!JR6@P-x;u{0s#ev<`u$K1^{T4fBtpIPK2>eP9aIgy&*Mj; z61kU-_DUt!u$4s|?f$d{*)`>UL=E?fEo`~x+A8&mBXJ6$Ua|U2A=E35{3(Qb#rrXZ kP%q8-bZrOqJE&Dc$8jRe$nS^7&FPBC2^~v1Vmh7w2MAp!MgRZ+ literal 0 HcmV?d00001 diff --git a/realsense2_description/urdf/_d405.urdf.xacro b/realsense2_description/urdf/_d405.urdf.xacro new file mode 100644 index 0000000000..5ca17f2116 --- /dev/null +++ b/realsense2_description/urdf/_d405.urdf.xacro @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense2_description/urdf/test_d405_camera.urdf.xacro b/realsense2_description/urdf/test_d405_camera.urdf.xacro new file mode 100644 index 0000000000..5a2b313f5d --- /dev/null +++ b/realsense2_description/urdf/test_d405_camera.urdf.xacro @@ -0,0 +1,10 @@ + + + + + + + + + + From d27a3273157f0fe1776ca2b4b2d295b42adebfc4 Mon Sep 17 00:00:00 2001 From: totoro Date: Mon, 1 Jan 2024 13:03:34 +0800 Subject: [PATCH 011/111] to_urdf fun retrun a str, not a BufferedRandom --- realsense2_description/launch/launch_utils.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/realsense2_description/launch/launch_utils.py b/realsense2_description/launch/launch_utils.py index 2f65eeb220..10e43fb064 100644 --- a/realsense2_description/launch/launch_utils.py +++ b/realsense2_description/launch/launch_utils.py @@ -21,12 +21,13 @@ def to_urdf(xacro_path, parameters=None): * xacro_path -- the path to the xacro file * parameters -- to be used when xacro file is parsed. """ - urdf_path = tempfile.TemporaryFile(prefix="%s_" % os.path.basename(xacro_path)) + with tempfile.NamedTemporaryFile(prefix="%s_" % os.path.basename(xacro_path), delete=False) as xacro_file: + urdf_path = xacro_file.name # open and process file doc = xacro.process_file(xacro_path, mappings=parameters) # open the output file - out = xacro.open_output(urdf_path) - out.write(doc.toprettyxml(indent=' ')) + with open(urdf_path, 'w') as urdf_file: + urdf_file.write(doc.toprettyxml(indent=' ')) return urdf_path From da05adda690de8059f89d813d0c2552f9b87c05b Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 29 Dec 2023 23:18:02 +0530 Subject: [PATCH 012/111] Extending LibRS's GL support to RS ROS2 --- realsense2_camera/CMakeLists.txt | 31 ++++++- .../include/base_realsense_node.h | 16 ++++ realsense2_camera/include/gl_window.h | 83 +++++++++++++++++++ realsense2_camera/launch/rs_launch.py | 1 + realsense2_camera/src/base_realsense_node.cpp | 18 +++- realsense2_camera/src/gl_gpu_processing.cpp | 44 ++++++++++ realsense2_camera/src/parameters.cpp | 6 ++ realsense2_camera/src/rs_node_setup.cpp | 4 + 8 files changed, 198 insertions(+), 5 deletions(-) create mode 100644 realsense2_camera/include/gl_window.h create mode 100644 realsense2_camera/src/gl_gpu_processing.cpp diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 67361a948c..1d3ad2ac81 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -117,6 +117,9 @@ find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(realsense2 2.54.1) +if (BUILD_ACCELERATE_WITH_GPU) + find_package(realsense2-gl 2.54.1) +endif() if(NOT realsense2_FOUND) message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n") endif() @@ -141,6 +144,10 @@ set(SOURCES src/tfs.cpp ) +if (BUILD_ACCELERATE_WITH_GPU) + list(APPEND SOURCES src/gl_gpu_processing.cpp) +endif() + if(NOT DEFINED ENV{ROS_DISTRO}) message(FATAL_ERROR "ROS_DISTRO is not defined." ) endif() @@ -171,6 +178,10 @@ if(${rclcpp_VERSION} VERSION_GREATER_EQUAL "17.0") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DRCLCPP_HAS_OnSetParametersCallbackType") endif() +if (BUILD_ACCELERATE_WITH_GPU) + add_definitions(-DACCELERATE_WITH_GPU) +endif() + set(INCLUDES include/constants.h include/realsense_node_factory.h @@ -184,6 +195,9 @@ set(INCLUDES include/profile_manager.h include/image_publisher.h) +if (BUILD_ACCELERATE_WITH_GPU) + list(APPEND INCLUDES include/gl_window.h) +endif() if (BUILD_TOOLS) @@ -200,9 +214,15 @@ add_library(${PROJECT_NAME} SHARED ${SOURCES} ) +if (BUILD_ACCELERATE_WITH_GPU) + set(link_libraries ${realsense2-gl_LIBRARY}) +else() + set(link_libraries ${realsense2_LIBRARY}) +endif() + target_link_libraries(${PROJECT_NAME} - ${realsense2_LIBRARY} - ) + ${link_libraries} +) set(dependencies cv_bridge @@ -214,11 +234,16 @@ set(dependencies sensor_msgs nav_msgs tf2 - realsense2 tf2_ros diagnostic_updater ) +if (BUILD_ACCELERATE_WITH_GPU) + list(APPEND dependencies realsense2-gl) +else() + list(APPEND dependencies realsense2) +endif() + ament_target_dependencies(${PROJECT_NAME} ${dependencies} ) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 34c5e8ebae..7aa736c2c6 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -49,6 +49,10 @@ #include #include +#if defined (ACCELERATE_WITH_GPU) +#include +#endif + #include #include #include @@ -115,6 +119,7 @@ namespace realsense2_camera public: enum class imu_sync_method{NONE, COPY, LINEAR_INTERPOLATION}; + enum class accelerate_with_gpu {NO_GPU, GL_GPU}; protected: class float3 @@ -261,6 +266,12 @@ namespace realsense2_camera void startRGBDPublisherIfNeeded(); void stopPublishers(const std::vector& profiles); +#if defined (ACCELERATE_WITH_GPU) + void initOpenGLProcessing(bool use_gpu_processing); + void shutdownOpenGLProcessing(); + void glfwPollEventCallback(); +#endif + rs2::device _dev; std::map _sensors; std::map> _sensors_callback; @@ -342,6 +353,11 @@ namespace realsense2_camera std::shared_ptr _diagnostics_updater; rs2::stream_profile _base_profile; +#if defined (ACCELERATE_WITH_GPU) + GLwindow _app; + rclcpp::TimerBase::SharedPtr _timer; + accelerate_with_gpu _accelerate_with_gpu; +#endif };//end class } diff --git a/realsense2_camera/include/gl_window.h b/realsense2_camera/include/gl_window.h new file mode 100644 index 0000000000..aa7555cf7c --- /dev/null +++ b/realsense2_camera/include/gl_window.h @@ -0,0 +1,83 @@ +// Copyright 2023 Intel Corporation. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include // Include RealSense Cross Platform API + +#if defined (ACCELERATE_WITH_GPU) + +#define GL_SILENCE_DEPRECATION +#define GLFW_INCLUDE_GLU +#include +#include +#include + +#include // Include GPU-Processing API + + +#ifndef PI +#define PI 3.14159265358979323846 +#define PI_FL 3.141592f +#endif + + +class GLwindow +{ +public: + + GLwindow(int width, int height, const char* title) + : _width(width), _height(height) + { + glfwInit(); + win = glfwCreateWindow(width, height, title, nullptr, nullptr); + if (!win) + throw std::runtime_error("Could not open OpenGL window, please check your graphic drivers or use the textual SDK tools"); + glfwMakeContextCurrent(win); + + glfwSetWindowUserPointer(win, this); + + } + + ~GLwindow() + { + glfwDestroyWindow(win); + glfwTerminate(); + } + + void close() + { + glfwSetWindowShouldClose(win, 1); + } + + float width() const { return float(_width); } + float height() const { return float(_height); } + + operator bool() + { + auto res = !glfwWindowShouldClose(win); + + glfwPollEvents(); + + return res; + } + + operator GLFWwindow* () { return win; } + +private: + GLFWwindow* win; + int _width, _height; +}; + +#endif diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index c6c14db6c4..fa6dfaabf2 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -29,6 +29,7 @@ {'name': 'config_file', 'default': "''", 'description': 'yaml config file'}, {'name': 'json_file_path', 'default': "''", 'description': 'allows advanced configuration'}, {'name': 'initial_reset', 'default': 'false', 'description': "''"}, + {'name': 'accelerate_with_gpu', 'default': "0", 'description': '[0-No_GPU, 1-GL_GPU]'}, {'name': 'rosbag_filename', 'default': "''", 'description': 'A realsense bagfile to run from as a device'}, {'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'}, {'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'}, diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index a3ebd0e675..b95aec4d61 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -115,6 +115,9 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _imu_sync_method(imu_sync_method::NONE), _is_profile_changed(false), _is_align_depth_changed(false) +#if defined (ACCELERATE_WITH_GPU) + ,_app(1280, 720, "RS_GLFW_Window") +#endif { if ( use_intra_process ) { @@ -127,6 +130,10 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, BaseRealSenseNode::~BaseRealSenseNode() { +#if defined (ACCELERATE_WITH_GPU) + shutdownOpenGLProcessing(); +#endif + // Kill dynamic transform thread _is_running = false; _cv_tf.notify_one(); @@ -229,10 +236,17 @@ void BaseRealSenseNode::setupFilters() _cv_mpc.notify_one(); }; - _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); - _filters.push_back(_colorizer_filter); +#if defined (ACCELERATE_WITH_GPU) + _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); + + _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); +#else + _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); +#endif + + _filters.push_back(_colorizer_filter); _filters.push_back(_pc_filter); _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); diff --git a/realsense2_camera/src/gl_gpu_processing.cpp b/realsense2_camera/src/gl_gpu_processing.cpp new file mode 100644 index 0000000000..44c8798826 --- /dev/null +++ b/realsense2_camera/src/gl_gpu_processing.cpp @@ -0,0 +1,44 @@ +// Copyright 2023 Intel Corporation. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../include/base_realsense_node.h" +#include + +using namespace realsense2_camera; +using namespace std::chrono_literals; + +#if defined (ACCELERATE_WITH_GPU) + +void BaseRealSenseNode::initOpenGLProcessing(bool use_gpu_processing) +{ + // Once we have a window, initialize GL module + // Pass our window to enable sharing of textures between processed frames and the window + // The "use_gpu_processing" is going to control if we will use CPU or GPU for data processing + rs2::gl::init_processing(_app, use_gpu_processing); + + _timer = _node.create_wall_timer(1000ms, std::bind(&BaseRealSenseNode::glfwPollEventCallback, this)); +} + +void BaseRealSenseNode::glfwPollEventCallback() +{ + // Must poll the GLFW events perodically, else window will hang or crash + glfwPollEvents(); +} + +void BaseRealSenseNode::shutdownOpenGLProcessing() +{ + rs2::gl::shutdown_processing(); +} + +#endif diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 72523cb801..4ee2caee06 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -82,6 +82,12 @@ void BaseRealSenseNode::getParameters() _base_frame_id = _parameters->setParam(param_name, DEFAULT_BASE_FRAME_ID); _base_frame_id = (static_cast(std::ostringstream() << _camera_name << "_" << _base_frame_id)).str(); _parameters_names.push_back(param_name); + +#if defined (ACCELERATE_WITH_GPU) + param_name = std::string("accelerate_with_gpu"); + _accelerate_with_gpu = accelerate_with_gpu(_parameters->setParam(param_name, int(accelerate_with_gpu::NO_GPU))); + _parameters_names.push_back(param_name); +#endif } void BaseRealSenseNode::setDynamicParams() diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index d98157f6d9..76243425b5 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -22,6 +22,10 @@ using namespace rs2; void BaseRealSenseNode::setup() { +#if defined (ACCELERATE_WITH_GPU) + bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); + initOpenGLProcessing(use_gpu_processing); +#endif setDynamicParams(); startDiagnosticsUpdater(); setAvailableSensors(); From 33c5f477c3100b37af3e4a5ff251283d337f93a6 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 8 Jan 2024 08:24:16 +0200 Subject: [PATCH 013/111] Set top level permission for GHA --- .github/workflows/main.yml | 2 ++ .github/workflows/pre-release.yml | 1 + .github/workflows/static_analysis.yaml | 2 ++ 3 files changed, 5 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index ecd144d630..5da1956304 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -13,6 +13,8 @@ on: # Allows you to run this workflow manually from the Actions tab workflow_dispatch: +permissions: read-all + # A workflow run is made up of one or more jobs that can run sequentially or in parallel # This workflow contains a single job called "build" diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index 4b54702bcc..afebcb6542 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -23,6 +23,7 @@ on: # Allows you to run this workflow manually from the Actions tab workflow_dispatch: +permissions: read-all jobs: build: diff --git a/.github/workflows/static_analysis.yaml b/.github/workflows/static_analysis.yaml index 7672fd6956..26cb9bb20d 100644 --- a/.github/workflows/static_analysis.yaml +++ b/.github/workflows/static_analysis.yaml @@ -6,6 +6,8 @@ on: pull_request: branches: ['**'] +permissions: read-all + jobs: cppcheck: name: cppcheck From 8b298b4d7b006ceaa462e8b30d74e14c84baa835 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 8 Jan 2024 10:14:37 +0200 Subject: [PATCH 014/111] remove-deprecated-nodes-usages --- .github/workflows/main.yml | 4 ++-- .github/workflows/pre-release.yml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 5da1956304..93a0bcb4b0 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -37,10 +37,10 @@ jobs: steps: - name: Setup ROS2 Workspace - run: | + run: | mkdir -p ${{github.workspace}}/ros2/src - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: path: 'ros2/src/realsense-ros' diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index afebcb6542..66d2332deb 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -45,6 +45,6 @@ jobs: BASEDIR: ${{ github.workspace }}/.work steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: industrial_ci uses: ros-industrial/industrial_ci@master From 78b58d17e3023b6b6193f232a9de5457773e23c4 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 11 Jan 2024 03:58:36 +0530 Subject: [PATCH 015/111] updated readme on how to enable GPU acceleration --- README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/README.md b/README.md index a77621972e..731dd42c2c 100644 --- a/README.md +++ b/README.md @@ -354,6 +354,15 @@ User can set the camera name and camera namespace, to distinguish between camera - double, positive values set the period between diagnostics updates on the `/diagnostics` topic. - 0 or negative values mean no diagnostics topic is published. Defaults to 0.
The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams. +- **accelerate_with_gpu**: + - GPU accelerated processing of PointCloud and Colorizer filters. + - integer: + - 0 --> **NO_GPU**: use only CPU for proccessing PointCloud and Colorizer filters + - 1 --> **GL_GPU**: use GLSL to accelerate GPU for processing PointCloud and Colorizer filters + - Note: To enable GPU acceleration, turn ON `BUILD_ACCELERATE_WITH_GPU` during build: + ```bash + colcon build --cmake-args '-DBUILD_ACCELERATE_WITH_GPU=ON' + ```
From 08e5fb42f8f8512b0d4c1944fcb99d862e8fb158 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Thu, 11 Jan 2024 09:39:21 +0200 Subject: [PATCH 016/111] Update issue templates --- .../bug-report---feature-request.md | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 .github/ISSUE_TEMPLATE/bug-report---feature-request.md diff --git a/.github/ISSUE_TEMPLATE/bug-report---feature-request.md b/.github/ISSUE_TEMPLATE/bug-report---feature-request.md new file mode 100644 index 0000000000..ac28286b25 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug-report---feature-request.md @@ -0,0 +1,35 @@ +--- +name: Bug report / Feature Request +about: Create a report to help us improve +title: '' +labels: '' +assignees: '' + +--- + +* Before opening a new issue, we wanted to provide you with some useful suggestions (Click "Preview" above for a better view): + + * Consider checking our ROS RealSense Wrapper documentation [README](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/README.md). + * Have you looked in our [Discussions](https://github.com/IntelRealSense/realsense-ros/discussions)? + * Try [searching our GitHub Issues](https://github.com/IntelRealSense/realsense-ros/issues) (open and closed) for a similar issue. + +* All users are welcomed to report bugs, ask questions, suggest or request enhancements and generally feel free to open new issue, even if they haven't followed any of the suggestions above :) + +---------------------------------------------------------------------------------------------------- + +| Required Info | | +|---------------------------------|------------------------------------------- | +| Camera Model | { R200 / F200 / SR300 / ZR300 / D400 } | +| Firmware Version | (Open RealSense Viewer --> Click info) | +| Operating System & Version | {Win (8.1/10/11) / Linux (Ubuntu 18/20/22) / MacOS | +| Kernel Version (Linux Only) | (e.g. 5.14) | +| Platform | PC/Raspberry Pi/ NVIDIA Jetson / etc.. | +| Librealsense SDK Version | { legacy / 2.. } | +| Language | {C/C#/labview/nodejs/opencv/pcl/python/unity } | +| Segment | {Robot/Smartphone/VR/AR/others } | +| ROS Distro | {Iron/Humble/Rolling/etc.. } | +| ROS Version | {2.4.0, 2.5.0, 2.5.1, etc.. } | + + +### Issue Description + From 8d0474782ef64969c53e4b9a396617c542293063 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Thu, 11 Jan 2024 09:41:32 +0200 Subject: [PATCH 017/111] Update issue templates --- .../ISSUE_TEMPLATE/bug-report---feature-request.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/bug-report---feature-request.md b/.github/ISSUE_TEMPLATE/bug-report---feature-request.md index ac28286b25..e4ec20d4a1 100644 --- a/.github/ISSUE_TEMPLATE/bug-report---feature-request.md +++ b/.github/ISSUE_TEMPLATE/bug-report---feature-request.md @@ -17,18 +17,18 @@ assignees: '' ---------------------------------------------------------------------------------------------------- -| Required Info | | -|---------------------------------|------------------------------------------- | -| Camera Model | { R200 / F200 / SR300 / ZR300 / D400 } | -| Firmware Version | (Open RealSense Viewer --> Click info) | +| Required Info | | +|---------------------------------|------------------------------------------------------------ | +| Camera Model | { R200 / F200 / SR300 / ZR300 / D400 } | +| Firmware Version | (Open RealSense Viewer --> Click info) | | Operating System & Version | {Win (8.1/10/11) / Linux (Ubuntu 18/20/22) / MacOS | | Kernel Version (Linux Only) | (e.g. 5.14) | | Platform | PC/Raspberry Pi/ NVIDIA Jetson / etc.. | | Librealsense SDK Version | { legacy / 2.. } | | Language | {C/C#/labview/nodejs/opencv/pcl/python/unity } | | Segment | {Robot/Smartphone/VR/AR/others } | -| ROS Distro | {Iron/Humble/Rolling/etc.. } | -| ROS Version | {2.4.0, 2.5.0, 2.5.1, etc.. } | +| ROS Distro | {Iron/Humble/Rolling/etc.. } | +| ROS Version | {2.4.0, 2.5.0, 2.5.1, etc.. } | ### Issue Description From 15cf3965c7768c45b0911c870315a950fdc14bed Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Thu, 11 Jan 2024 09:49:34 +0200 Subject: [PATCH 018/111] Update bug-report---feature-request.md --- .github/ISSUE_TEMPLATE/bug-report---feature-request.md | 9 --------- 1 file changed, 9 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/bug-report---feature-request.md b/.github/ISSUE_TEMPLATE/bug-report---feature-request.md index e4ec20d4a1..f1f391aa1d 100644 --- a/.github/ISSUE_TEMPLATE/bug-report---feature-request.md +++ b/.github/ISSUE_TEMPLATE/bug-report---feature-request.md @@ -1,12 +1,3 @@ ---- -name: Bug report / Feature Request -about: Create a report to help us improve -title: '' -labels: '' -assignees: '' - ---- - * Before opening a new issue, we wanted to provide you with some useful suggestions (Click "Preview" above for a better view): * Consider checking our ROS RealSense Wrapper documentation [README](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/README.md). From a3a04ebb3d29b5f100f4d69734448a812dd0f6e0 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Thu, 11 Jan 2024 09:53:34 +0200 Subject: [PATCH 019/111] Rename bug-report---feature-request.md to ISSUE_TEMPLATE.md --- .../{bug-report---feature-request.md => ISSUE_TEMPLATE.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename .github/ISSUE_TEMPLATE/{bug-report---feature-request.md => ISSUE_TEMPLATE.md} (100%) diff --git a/.github/ISSUE_TEMPLATE/bug-report---feature-request.md b/.github/ISSUE_TEMPLATE/ISSUE_TEMPLATE.md similarity index 100% rename from .github/ISSUE_TEMPLATE/bug-report---feature-request.md rename to .github/ISSUE_TEMPLATE/ISSUE_TEMPLATE.md From 6b54ac7a2a7f50f203991d8c647fc2387f74499e Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Thu, 11 Jan 2024 09:58:26 +0200 Subject: [PATCH 020/111] Update Issue Template --- .github/{ISSUE_TEMPLATE => }/ISSUE_TEMPLATE.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename .github/{ISSUE_TEMPLATE => }/ISSUE_TEMPLATE.md (100%) diff --git a/.github/ISSUE_TEMPLATE/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md similarity index 100% rename from .github/ISSUE_TEMPLATE/ISSUE_TEMPLATE.md rename to .github/ISSUE_TEMPLATE.md From febbfa4f279bb718c30b7eba667871c4e87cf462 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Thu, 11 Jan 2024 10:00:34 +0200 Subject: [PATCH 021/111] Update Issue Template --- .github/ISSUE_TEMPLATE.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md index f1f391aa1d..0a947ad519 100644 --- a/.github/ISSUE_TEMPLATE.md +++ b/.github/ISSUE_TEMPLATE.md @@ -19,7 +19,7 @@ | Language | {C/C#/labview/nodejs/opencv/pcl/python/unity } | | Segment | {Robot/Smartphone/VR/AR/others } | | ROS Distro | {Iron/Humble/Rolling/etc.. } | -| ROS Version | {2.4.0, 2.5.0, 2.5.1, etc.. } | +| RealSense ROS Wrapper Version | {2.4.0, 2.5.0, 2.5.1, etc.. } | ### Issue Description From 7bd5c600bb16c649b7eb01f6e7e8b4905db8896d Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Fri, 12 Jan 2024 13:52:09 +0200 Subject: [PATCH 022/111] Update Issue Template --- .github/ISSUE_TEMPLATE.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md index 0a947ad519..921b769f2e 100644 --- a/.github/ISSUE_TEMPLATE.md +++ b/.github/ISSUE_TEMPLATE.md @@ -10,16 +10,16 @@ | Required Info | | |---------------------------------|------------------------------------------------------------ | -| Camera Model | { R200 / F200 / SR300 / ZR300 / D400 } | +| Camera Model | { D405 / D415 / D435 / D435i / D455 / D457 } | | Firmware Version | (Open RealSense Viewer --> Click info) | -| Operating System & Version | {Win (8.1/10/11) / Linux (Ubuntu 18/20/22) / MacOS | -| Kernel Version (Linux Only) | (e.g. 5.14) | +| Operating System & Version | { Win (10/11) / Linux (Ubuntu 18/20/22) / MacOS } | +| Kernel Version (Linux Only) | (e.g. 5.4) | | Platform | PC/Raspberry Pi/ NVIDIA Jetson / etc.. | -| Librealsense SDK Version | { legacy / 2.. } | -| Language | {C/C#/labview/nodejs/opencv/pcl/python/unity } | +| Librealsense SDK Version | { 2.. } | +| Language | {C/C#/labview/opencv/pcl/python/unity } | | Segment | {Robot/Smartphone/VR/AR/others } | | ROS Distro | {Iron/Humble/Rolling/etc.. } | -| RealSense ROS Wrapper Version | {2.4.0, 2.5.0, 2.5.1, etc.. } | +| RealSense ROS Wrapper Version | {4.51.1, 4.54.1, etc..} | ### Issue Description From d6417d9192ccd6a61e3475fb6a6e7634f62f9d6c Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Sat, 13 Jan 2024 01:16:10 +0530 Subject: [PATCH 023/111] Added GL support for Align-Depth filter --- README.md | 6 +++--- realsense2_camera/src/base_realsense_node.cpp | 6 ++---- realsense2_camera/src/gl_gpu_processing.cpp | 3 ++- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 731dd42c2c..cfcf5e2735 100644 --- a/README.md +++ b/README.md @@ -355,10 +355,10 @@ User can set the camera name and camera namespace, to distinguish between camera - 0 or negative values mean no diagnostics topic is published. Defaults to 0.
The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams. - **accelerate_with_gpu**: - - GPU accelerated processing of PointCloud and Colorizer filters. + - GPU accelerated processing of PointCloud, Align-Depth and Colorizer filters. - integer: - - 0 --> **NO_GPU**: use only CPU for proccessing PointCloud and Colorizer filters - - 1 --> **GL_GPU**: use GLSL to accelerate GPU for processing PointCloud and Colorizer filters + - 0 --> **NO_GPU**: use only CPU for proccessing PointCloud, Align-Depth and Colorizer filters + - 1 --> **GL_GPU**: use GLSL to accelerate GPU for processing PointCloud, Align-Depth and Colorizer filters - Note: To enable GPU acceleration, turn ON `BUILD_ACCELERATE_WITH_GPU` during build: ```bash colcon build --cmake-args '-DBUILD_ACCELERATE_WITH_GPU=ON' diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index b95aec4d61..9a1ca8b3b1 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -238,18 +238,16 @@ void BaseRealSenseNode::setupFilters() #if defined (ACCELERATE_WITH_GPU) _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); - _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); + _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); #else _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); - _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); + _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); #endif _filters.push_back(_colorizer_filter); _filters.push_back(_pc_filter); - - _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); _filters.push_back(_align_depth_filter); } diff --git a/realsense2_camera/src/gl_gpu_processing.cpp b/realsense2_camera/src/gl_gpu_processing.cpp index 44c8798826..26717294a4 100644 --- a/realsense2_camera/src/gl_gpu_processing.cpp +++ b/realsense2_camera/src/gl_gpu_processing.cpp @@ -26,6 +26,7 @@ void BaseRealSenseNode::initOpenGLProcessing(bool use_gpu_processing) // Pass our window to enable sharing of textures between processed frames and the window // The "use_gpu_processing" is going to control if we will use CPU or GPU for data processing rs2::gl::init_processing(_app, use_gpu_processing); + rs2::gl::init_rendering(); _timer = _node.create_wall_timer(1000ms, std::bind(&BaseRealSenseNode::glfwPollEventCallback, this)); } @@ -38,7 +39,7 @@ void BaseRealSenseNode::glfwPollEventCallback() void BaseRealSenseNode::shutdownOpenGLProcessing() { - rs2::gl::shutdown_processing(); + } #endif From 73df46b22e25a8171407ffa9f564014e1d950152 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Sat, 20 Jan 2024 01:30:36 +0530 Subject: [PATCH 024/111] Configured GLFW to create a hidden window --- realsense2_camera/include/gl_window.h | 1 + 1 file changed, 1 insertion(+) diff --git a/realsense2_camera/include/gl_window.h b/realsense2_camera/include/gl_window.h index aa7555cf7c..a1cebb7af8 100644 --- a/realsense2_camera/include/gl_window.h +++ b/realsense2_camera/include/gl_window.h @@ -41,6 +41,7 @@ class GLwindow : _width(width), _height(height) { glfwInit(); + glfwWindowHint(GLFW_VISIBLE, 0); win = glfwCreateWindow(width, height, title, nullptr, nullptr); if (!win) throw std::runtime_error("Could not open OpenGL window, please check your graphic drivers or use the textual SDK tools"); From f77ac0ecd75f9cd79f22c2ecf7fefd3b3ff1c8dd Mon Sep 17 00:00:00 2001 From: deep0294 Date: Thu, 25 Jan 2024 12:33:56 +0200 Subject: [PATCH 025/111] Fix All Profiles Test * Use rs-enumerate-devices with verbose option to fetch necessary data * Fix the indices for parsing the Resolution and Frequency * Tested with D455 Camera --- .../test/utils/pytest_live_camera_utils.py | 30 ++++++++++++------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py index 9b5bfeac70..8a6bbe1e67 100644 --- a/realsense2_camera/test/utils/pytest_live_camera_utils.py +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -76,16 +76,23 @@ def get_depth_profiles(long_data, start_index, end_index): if len(depth_profile) == 5: profile = depth_profile[0] value = depth_profile[1]+"x"+depth_profile[3] - format = depth_profile[4] + img_format = depth_profile[4] elif len(depth_profile) == 6: profile = depth_profile[0]+depth_profile[1] value = depth_profile[2]+"x"+depth_profile[4] - format = depth_profile[5] + img_format = depth_profile[5] + elif len(depth_profile) == 7: + profile = depth_profile[1] + value = depth_profile[2]+"x"+depth_profile[5] + img_format = depth_profile[3] + elif len(depth_profile) == 8: + profile = depth_profile[1]+depth_profile[2] + value = depth_profile[3]+"x"+depth_profile[6] + img_format = depth_profile[4] else: assert false, "Seems that the depth profile info format printed by rs-enumerate-devices changed" - value = value[:-2] - debug_print("depth profile added: " + profile, value, format) - cap.append([profile, value, format]) + debug_print("depth profile added: " + profile, value, img_format) + cap.append([profile, value, img_format]) debug_print(cap) return cap @@ -100,12 +107,15 @@ def get_color_profiles(long_data, start_index, end_index): if len(color_profile) == 5: profile = color_profile[0] value = color_profile[1]+"x"+color_profile[3] - format = color_profile[4] + img_format = color_profile[4] + elif len(color_profile) == 7: + profile = color_profile[1] + value = color_profile[2]+"x"+color_profile[5] + img_format = color_profile[3] else: assert false, "Seems that the color profile info format printed by rs-enumerate-devices changed" - value = value[:-2] - debug_print("color profile added: " + profile, value, format) - cap.append([profile, value, format]) + debug_print("color profile added: " + profile, value, img_format) + cap.append([profile, value, img_format]) debug_print(cap) return cap @@ -147,7 +157,7 @@ def parse_device_info(long_data, start_index, end_index, device_type, serial_no) return capability def get_camera_capabilities(device_type, serial_no=None): - long_data = os.popen("rs-enumerate-devices").read().splitlines() + long_data = os.popen("rs-enumerate-devices -v").read().splitlines() debug_print(serial_no) index = 0 while index < len(long_data): From 447f1e92b1fcdbe9b6d6fa5dc4205fccbe9eabf9 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 26 Jan 2024 04:34:47 +0530 Subject: [PATCH 026/111] Reverting the GL support of Align-Depth filter --- realsense2_camera/src/base_realsense_node.cpp | 4 ++-- realsense2_camera/src/gl_gpu_processing.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9a1ca8b3b1..62493d8896 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -239,15 +239,15 @@ void BaseRealSenseNode::setupFilters() #if defined (ACCELERATE_WITH_GPU) _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); - _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); #else _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); - _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); #endif _filters.push_back(_colorizer_filter); _filters.push_back(_pc_filter); + + _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); _filters.push_back(_align_depth_filter); } diff --git a/realsense2_camera/src/gl_gpu_processing.cpp b/realsense2_camera/src/gl_gpu_processing.cpp index 26717294a4..a43f333a9b 100644 --- a/realsense2_camera/src/gl_gpu_processing.cpp +++ b/realsense2_camera/src/gl_gpu_processing.cpp @@ -26,7 +26,8 @@ void BaseRealSenseNode::initOpenGLProcessing(bool use_gpu_processing) // Pass our window to enable sharing of textures between processed frames and the window // The "use_gpu_processing" is going to control if we will use CPU or GPU for data processing rs2::gl::init_processing(_app, use_gpu_processing); - rs2::gl::init_rendering(); + if (use_gpu_processing) + rs2::gl::init_rendering(); _timer = _node.create_wall_timer(1000ms, std::bind(&BaseRealSenseNode::glfwPollEventCallback, this)); } From 4943275f4d8c144a0babd8ee6ca3f2e67ac06a71 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 26 Jan 2024 04:37:37 +0530 Subject: [PATCH 027/111] updated readme --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index cfcf5e2735..731dd42c2c 100644 --- a/README.md +++ b/README.md @@ -355,10 +355,10 @@ User can set the camera name and camera namespace, to distinguish between camera - 0 or negative values mean no diagnostics topic is published. Defaults to 0.
The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams. - **accelerate_with_gpu**: - - GPU accelerated processing of PointCloud, Align-Depth and Colorizer filters. + - GPU accelerated processing of PointCloud and Colorizer filters. - integer: - - 0 --> **NO_GPU**: use only CPU for proccessing PointCloud, Align-Depth and Colorizer filters - - 1 --> **GL_GPU**: use GLSL to accelerate GPU for processing PointCloud, Align-Depth and Colorizer filters + - 0 --> **NO_GPU**: use only CPU for proccessing PointCloud and Colorizer filters + - 1 --> **GL_GPU**: use GLSL to accelerate GPU for processing PointCloud and Colorizer filters - Note: To enable GPU acceleration, turn ON `BUILD_ACCELERATE_WITH_GPU` during build: ```bash colcon build --cmake-args '-DBUILD_ACCELERATE_WITH_GPU=ON' From 2cf6cc61b7aacecf6a86c156c71d40eddec9e914 Mon Sep 17 00:00:00 2001 From: deep0294 Date: Tue, 30 Jan 2024 08:51:42 +0200 Subject: [PATCH 028/111] Used indices to address the enumerated devices output --- .../test/utils/pytest_live_camera_utils.py | 62 ++++++++++--------- 1 file changed, 32 insertions(+), 30 deletions(-) diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py index 8a6bbe1e67..64ff6eeae6 100644 --- a/realsense2_camera/test/utils/pytest_live_camera_utils.py +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -72,27 +72,29 @@ def get_depth_profiles(long_data, start_index, end_index): if len(long_data[line_no]) == 0: break debug_print("depth profile processing:" + long_data[line_no]) - depth_profile = long_data[line_no].split() - if len(depth_profile) == 5: - profile = depth_profile[0] - value = depth_profile[1]+"x"+depth_profile[3] - img_format = depth_profile[4] - elif len(depth_profile) == 6: - profile = depth_profile[0]+depth_profile[1] - value = depth_profile[2]+"x"+depth_profile[4] - img_format = depth_profile[5] - elif len(depth_profile) == 7: - profile = depth_profile[1] - value = depth_profile[2]+"x"+depth_profile[5] - img_format = depth_profile[3] - elif len(depth_profile) == 8: - profile = depth_profile[1]+depth_profile[2] - value = depth_profile[3]+"x"+depth_profile[6] - img_format = depth_profile[4] + enumerate_devices_line_splitted = long_data[line_no].split() + if len(enumerate_devices_line_splitted) == 7: + stream0_idx = 1 + stream1_idx = 0 + resolution_idx = 2 + frequency_idx = 5 + format_idx = 3 + elif len(enumerate_devices_line_splitted) == 8: + stream0_idx = 1 + stream1_idx = 2 + resolution_idx = 3 + frequency_idx = 6 + format_idx = 4 else: assert false, "Seems that the depth profile info format printed by rs-enumerate-devices changed" - debug_print("depth profile added: " + profile, value, img_format) - cap.append([profile, value, img_format]) + if stream1_idx != 0: + depth_camera_stream = enumerate_devices_line_splitted[stream0_idx]+enumerate_devices_line_splitted[stream1_idx] + else: + depth_camera_stream = enumerate_devices_line_splitted[stream0_idx] + depth_profile_param = enumerate_devices_line_splitted[resolution_idx]+"x"+enumerate_devices_line_splitted[frequency_idx] + depth_format_param = enumerate_devices_line_splitted[format_idx] + debug_print("depth profile added: " + depth_camera_stream, depth_profile_param, depth_format_param) + cap.append([depth_camera_stream, depth_profile_param, depth_format_param]) debug_print(cap) return cap @@ -103,19 +105,19 @@ def get_color_profiles(long_data, start_index, end_index): if len(long_data[line_no]) == 0: break debug_print("color profile processing:" + long_data[line_no]) - color_profile = long_data[line_no].split() - if len(color_profile) == 5: - profile = color_profile[0] - value = color_profile[1]+"x"+color_profile[3] - img_format = color_profile[4] - elif len(color_profile) == 7: - profile = color_profile[1] - value = color_profile[2]+"x"+color_profile[5] - img_format = color_profile[3] + enumerate_devices_line_splitted = long_data[line_no].split() + if len(enumerate_devices_line_splitted) == 7: + stream_idx = 1 + resolution_idx = 2 + frequency_idx = 5 + format_idx = 3 else: assert false, "Seems that the color profile info format printed by rs-enumerate-devices changed" - debug_print("color profile added: " + profile, value, img_format) - cap.append([profile, value, img_format]) + color_camera_stream = enumerate_devices_line_splitted[stream_idx] + color_profile_param = enumerate_devices_line_splitted[resolution_idx]+"x"+enumerate_devices_line_splitted[frequency_idx] + color_format_param = enumerate_devices_line_splitted[format_idx] + debug_print("color profile added: " + color_camera_stream, color_profile_param, color_format_param) + cap.append([color_camera_stream, color_profile_param, color_format_param]) debug_print(cap) return cap From 005ec5d15cba28be533a59707ca1b9f57184747d Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 31 Jan 2024 13:47:04 +0200 Subject: [PATCH 029/111] remove d465 sku from ros2 wrapper --- realsense2_camera/include/constants.h | 1 - realsense2_camera/src/realsense_node_factory.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 0f99fee585..8304dbea4a 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -60,7 +60,6 @@ namespace realsense2_camera const uint16_t RS460_PID = 0x0b03; // DS5U const uint16_t RS435_RGB_PID = 0x0b07; // AWGC const uint16_t RS435i_RGB_PID = 0x0B3A; // AWGC_MM - const uint16_t RS465_PID = 0x0b4d; // D465 const uint16_t RS416_RGB_PID = 0x0B52; // F416 RGB const uint16_t RS430i_PID = 0x0b4b; // D430i const uint16_t RS405_PID = 0x0B5B; // DS5U diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index e53b459551..c19bae6e0f 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -373,7 +373,6 @@ void RealSenseNodeFactory::startDevice() case RS435i_RGB_PID: case RS455_PID: case RS457_PID: - case RS465_PID: case RS_USB2_PID: _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); break; From 6036e6020729365f356f670ee098d5a3b660f24a Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 2 Feb 2024 04:53:06 +0530 Subject: [PATCH 030/111] Added support for dynamically switching b/w CPU / GPU processing --- .../include/base_realsense_node.h | 3 + realsense2_camera/src/base_realsense_node.cpp | 8 +- realsense2_camera/src/gl_gpu_processing.cpp | 3 +- realsense2_camera/src/parameters.cpp | 14 +- realsense2_camera/src/rs_node_setup.cpp | 130 +++++++++++++++++- 5 files changed, 150 insertions(+), 8 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 7aa736c2c6..d54cecd7dc 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -261,6 +261,8 @@ namespace realsense2_camera void setAvailableSensors(); void setCallbackFunctions(); void updateSensors(); + void startVideoSensors(); + void stopVideoSensors(); void publishServices(); void startPublishers(const std::vector& profiles, const RosSensor& sensor); void startRGBDPublisherIfNeeded(); @@ -357,6 +359,7 @@ namespace realsense2_camera GLwindow _app; rclcpp::TimerBase::SharedPtr _timer; accelerate_with_gpu _accelerate_with_gpu; + bool _is_accelerate_with_gpu_changed; #endif };//end class diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 62493d8896..6c610b7171 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -116,7 +116,9 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_profile_changed(false), _is_align_depth_changed(false) #if defined (ACCELERATE_WITH_GPU) - ,_app(1280, 720, "RS_GLFW_Window") + ,_app(1280, 720, "RS_GLFW_Window"), + _accelerate_with_gpu(accelerate_with_gpu::NO_GPU), + _is_accelerate_with_gpu_changed(false) #endif { if ( use_intra_process ) @@ -130,10 +132,6 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, BaseRealSenseNode::~BaseRealSenseNode() { -#if defined (ACCELERATE_WITH_GPU) - shutdownOpenGLProcessing(); -#endif - // Kill dynamic transform thread _is_running = false; _cv_tf.notify_one(); diff --git a/realsense2_camera/src/gl_gpu_processing.cpp b/realsense2_camera/src/gl_gpu_processing.cpp index a43f333a9b..8bbf8c4b44 100644 --- a/realsense2_camera/src/gl_gpu_processing.cpp +++ b/realsense2_camera/src/gl_gpu_processing.cpp @@ -40,7 +40,8 @@ void BaseRealSenseNode::glfwPollEventCallback() void BaseRealSenseNode::shutdownOpenGLProcessing() { - + rs2::gl::shutdown_rendering(); + rs2::gl::shutdown_processing(); } #endif diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 4ee2caee06..0747c49c42 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -85,9 +85,21 @@ void BaseRealSenseNode::getParameters() #if defined (ACCELERATE_WITH_GPU) param_name = std::string("accelerate_with_gpu"); - _accelerate_with_gpu = accelerate_with_gpu(_parameters->setParam(param_name, int(accelerate_with_gpu::NO_GPU))); + _parameters->setParam(param_name, int(accelerate_with_gpu::NO_GPU), + [this](const rclcpp::Parameter& parameter) + { + accelerate_with_gpu temp_value = accelerate_with_gpu(parameter.get_value()); + if (_accelerate_with_gpu != temp_value) + { + _accelerate_with_gpu = temp_value; + std::lock_guard lock_guard(_profile_changes_mutex); + _is_accelerate_with_gpu_changed = true; + } + _cv_mpc.notify_one(); + }); _parameters_names.push_back(param_name); #endif + } void BaseRealSenseNode::setDynamicParams() diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 76243425b5..198384bebc 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -25,6 +25,7 @@ void BaseRealSenseNode::setup() #if defined (ACCELERATE_WITH_GPU) bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); initOpenGLProcessing(use_gpu_processing); + _is_accelerate_with_gpu_changed = false; #endif setDynamicParams(); startDiagnosticsUpdater(); @@ -43,7 +44,11 @@ void BaseRealSenseNode::monitoringProfileChanges() std::function func = [this, time_interval](){ std::unique_lock lock(_profile_changes_mutex); while(_is_running) { - _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return (!_is_running || _is_profile_changed || _is_align_depth_changed);}); + _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return (!_is_running || _is_profile_changed || _is_align_depth_changed +#if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed +#endif + );}); if (_is_running && (_is_profile_changed || _is_align_depth_changed)) { ROS_DEBUG("Profile has changed"); @@ -58,6 +63,27 @@ void BaseRealSenseNode::monitoringProfileChanges() _is_profile_changed = false; _is_align_depth_changed = false; } +#if defined (ACCELERATE_WITH_GPU) + if (_is_running && _is_accelerate_with_gpu_changed) + { + ROS_DEBUG("Accelerate_with_gpu changed"); + try + { + stopVideoSensors(); + shutdownOpenGLProcessing(); + + bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); + initOpenGLProcessing(use_gpu_processing); + + startVideoSensors(); + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("Error updating the sensors: " << e.what()); + } + _is_accelerate_with_gpu_changed = false; + } +#endif } }; _monitoring_pc = std::make_shared(func); @@ -406,6 +432,108 @@ void BaseRealSenseNode::updateSensors() } } +void BaseRealSenseNode::stopVideoSensors() +{ + std::lock_guard lock_guard(_update_sensor_mutex); + try{ + for(auto&& sensor : _available_ros_sensors) + { + std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); + + bool is_video_sensor = (sensor->is() || sensor->is()); + + // do all updates if profile has been changed, or if the align depth filter status has been changed + // and we are on a video sensor. TODO: explore better options to monitor and update changes + // without resetting the whole sensors and topics. + if (is_video_sensor) + { + std::vector active_profiles = sensor->get_active_streams(); + + // Start/stop sensors only if profile was changed + // No need to start/stop sensors if align_depth was changed + ROS_INFO_STREAM("Stopping Sensor: " << module_name); + sensor->stop(); + + stopPublishers(active_profiles); + } + } + } + catch(const std::exception& ex) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "An exception has been thrown: " << ex.what()); + throw; + } + catch(...) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "Unknown exception has occured!"); + throw; + } +} + +void BaseRealSenseNode::startVideoSensors() +{ + std::lock_guard lock_guard(_update_sensor_mutex); + try{ + for(auto&& sensor : _available_ros_sensors) + { + std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); + // if active_profiles != wanted_profiles: stop sensor. + std::vector wanted_profiles; + + sensor->getUpdatedProfiles(wanted_profiles); + bool is_video_sensor = (sensor->is() || sensor->is()); + + // do all updates if profile has been changed, or if the align depth filter status has been changed + // and we are on a video sensor. TODO: explore better options to monitor and update changes + // without resetting the whole sensors and topics. + if (is_video_sensor) + { + std::vector active_profiles = sensor->get_active_streams(); + + if (!wanted_profiles.empty()) + { + startPublishers(wanted_profiles, *sensor); + updateProfilesStreamCalibData(wanted_profiles); + if (_publish_tf) + { + std::lock_guard lock_guard(_publish_tf_mutex); + for (auto &profile : wanted_profiles) + { + calcAndAppendTransformMsgs(profile, _base_profile); + } + } + + // Start/stop sensors only if profile was changed + // No need to start/stop sensors if align_depth was changed + ROS_INFO_STREAM("Starting Sensor: " << module_name); + sensor->start(wanted_profiles); + + if (sensor->rs2::sensor::is()) + { + _depth_scale_meters = sensor->as().get_depth_scale(); + } + } + } + } + if (_publish_tf) + { + std::lock_guard lock_guard(_publish_tf_mutex); + publishStaticTransforms(); + } + startRGBDPublisherIfNeeded(); + } + catch(const std::exception& ex) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "An exception has been thrown: " << ex.what()); + throw; + } + catch(...) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "Unknown exception has occured!"); + throw; + } +} + void BaseRealSenseNode::publishServices() { // adding "~/" to the service name will add node namespace and node name to the service From 5337ee5010f4d54295bbe2e145815f2777d811c1 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 7 Feb 2024 11:43:24 +0000 Subject: [PATCH 031/111] Use zeroed intrinsic for profiles that does not have defined intrinsic --- realsense2_camera/include/base_realsense_node.h | 1 - realsense2_camera/src/base_realsense_node.cpp | 15 +++++++++++++-- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 7aa736c2c6..67156240c7 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -284,7 +284,6 @@ namespace realsense2_camera double _angular_velocity_cov; bool _hold_back_imu_for_frames; - std::map _stream_intrinsics; std::map _enable; bool _publish_tf; double _tf_publish_rate, _diagnostics_period; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 62493d8896..ef689f1a70 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -710,8 +710,19 @@ void BaseRealSenseNode::updateProfilesStreamCalibData(const std::vector Date: Wed, 7 Feb 2024 19:07:06 -0800 Subject: [PATCH 032/111] add pointcloud QoS description --- README.md | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/README.md b/README.md index 731dd42c2c..3a7bdc7998 100644 --- a/README.md +++ b/README.md @@ -281,6 +281,7 @@ User can set the camera name and camera namespace, to distinguish between camera - can be any of *infra, infra1, infra2, color, depth, gyro, accel*. - Available values are the following strings: `SYSTEM_DEFAULT`, `DEFAULT`, `PARAMETER_EVENTS`, `SERVICES_DEFAULT`, `PARAMETERS`, `SENSOR_DATA`. - For example: ```depth_qos:=SENSOR_DATA``` + - Pointcloud QoS is controlled with the `pointcloud.pointcloud_qos` parameter in the pointcloud filter, refer to the Post-Processing Filters section for details. - Reference: [ROS2 QoS profiles formal documentation](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-profiles) - **Notice:** ****_info_qos** refers to both camera_info topics and metadata topics. - **tf_publish_rate**: @@ -510,6 +511,16 @@ The following post processing filters are available: * The texture of the pointcloud can be modified using the `pointcloud.stream_filter` parameter.
* The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting `pointcloud.allow_no_texture_points` to true. * pointcloud is of an unordered format by default. This can be changed by setting `pointcloud.ordered_pc` to true. + * The QoS of the pointcloud topic is independent from depth and color streams and can be controlled with the `pointcloud.pointcloud_qos` parameter. + - The same set of QoS values are supported as other streams, refer to _qos in the Parameters section of this page. + - The launch file should include the parameter with initial QoS value, for example,`{'name': 'pointcloud.pointcloud_qos', 'default': 'SENSOR_DATA', 'description': 'pointcloud qos'}` + - The QoS value can also be overridden at launch with command option, for example, `pointcloud.pointcloud_qos:=SENSOR_DATA` + - At runtime, the QoS can be changed dynamically but require the filter re-enable for the change to take effect, for example, + ```bash + ros2 param set /camera/camera pointcloud.pointcloud_qos SENSOR_DATA + ros2 param set /camera/camera pointcloud.enable false + ros2 param set /camera/camera pointcloud.enable true + ``` - ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. - `depth_module.hdr_enabled`: to enable/disable HDR - The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. From 57f819ab68f77f34dde9d965b63930d0ea7cf215 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 9 Feb 2024 04:18:53 +0530 Subject: [PATCH 033/111] Updated updateSensors() function --- .../include/base_realsense_node.h | 4 +- realsense2_camera/src/rs_node_setup.cpp | 168 +++++++----------- 2 files changed, 67 insertions(+), 105 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index d54cecd7dc..903d70f805 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -261,8 +261,8 @@ namespace realsense2_camera void setAvailableSensors(); void setCallbackFunctions(); void updateSensors(); - void startVideoSensors(); - void stopVideoSensors(); + void startUpdatedSensors(); + void stopRequiredSensors(); void publishServices(); void startPublishers(const std::vector& profiles, const RosSensor& sensor); void startRGBDPublisherIfNeeded(); diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 198384bebc..ad4f8abe8e 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -44,12 +44,20 @@ void BaseRealSenseNode::monitoringProfileChanges() std::function func = [this, time_interval](){ std::unique_lock lock(_profile_changes_mutex); while(_is_running) { - _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return (!_is_running || _is_profile_changed || _is_align_depth_changed -#if defined (ACCELERATE_WITH_GPU) - || _is_accelerate_with_gpu_changed -#endif - );}); - if (_is_running && (_is_profile_changed || _is_align_depth_changed)) + _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), + [&]{return (!_is_running || _is_profile_changed + || _is_align_depth_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + );}); + + if (_is_running && (_is_profile_changed + || _is_align_depth_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + )) { ROS_DEBUG("Profile has changed"); try @@ -62,28 +70,11 @@ void BaseRealSenseNode::monitoringProfileChanges() } _is_profile_changed = false; _is_align_depth_changed = false; - } -#if defined (ACCELERATE_WITH_GPU) - if (_is_running && _is_accelerate_with_gpu_changed) - { - ROS_DEBUG("Accelerate_with_gpu changed"); - try - { - stopVideoSensors(); - shutdownOpenGLProcessing(); - - bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); - initOpenGLProcessing(use_gpu_processing); - startVideoSensors(); - } - catch(const std::exception& e) - { - ROS_ERROR_STREAM("Error updating the sensors: " << e.what()); - } - _is_accelerate_with_gpu_changed = false; + #if defined (ACCELERATE_WITH_GPU) + _is_accelerate_with_gpu_changed = false; + #endif } -#endif } }; _monitoring_pc = std::make_shared(func); @@ -361,64 +352,19 @@ void BaseRealSenseNode::updateSensors() { std::lock_guard lock_guard(_update_sensor_mutex); try{ - for(auto&& sensor : _available_ros_sensors) - { - std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); - // if active_profiles != wanted_profiles: stop sensor. - std::vector wanted_profiles; - - bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles)); - bool is_video_sensor = (sensor->is() || sensor->is()); + stopRequiredSensors(); - // do all updates if profile has been changed, or if the align depth filter status has been changed - // and we are on a video sensor. TODO: explore better options to monitor and update changes - // without resetting the whole sensors and topics. - if (is_profile_changed || (_is_align_depth_changed && is_video_sensor)) + #if defined (ACCELERATE_WITH_GPU) + if (_is_accelerate_with_gpu_changed) { - std::vector active_profiles = sensor->get_active_streams(); - if(is_profile_changed) - { - // Start/stop sensors only if profile was changed - // No need to start/stop sensors if align_depth was changed - ROS_INFO_STREAM("Stopping Sensor: " << module_name); - sensor->stop(); - } - stopPublishers(active_profiles); - - if (!wanted_profiles.empty()) - { - startPublishers(wanted_profiles, *sensor); - updateProfilesStreamCalibData(wanted_profiles); - if (_publish_tf) - { - std::lock_guard lock_guard(_publish_tf_mutex); - for (auto &profile : wanted_profiles) - { - calcAndAppendTransformMsgs(profile, _base_profile); - } - } + shutdownOpenGLProcessing(); - if(is_profile_changed) - { - // Start/stop sensors only if profile was changed - // No need to start/stop sensors if align_depth was changed - ROS_INFO_STREAM("Starting Sensor: " << module_name); - sensor->start(wanted_profiles); - } - - if (sensor->rs2::sensor::is()) - { - _depth_scale_meters = sensor->as().get_depth_scale(); - } - } + bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); + initOpenGLProcessing(use_gpu_processing); } - } - if (_publish_tf) - { - std::lock_guard lock_guard(_publish_tf_mutex); - publishStaticTransforms(); - } - startRGBDPublisherIfNeeded(); + #endif + + startUpdatedSensors(); } catch(const std::exception& ex) { @@ -432,28 +378,39 @@ void BaseRealSenseNode::updateSensors() } } -void BaseRealSenseNode::stopVideoSensors() +void BaseRealSenseNode::stopRequiredSensors() { - std::lock_guard lock_guard(_update_sensor_mutex); try{ for(auto&& sensor : _available_ros_sensors) { std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); + // if active_profiles != wanted_profiles: stop sensor. + std::vector wanted_profiles; + bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles)); bool is_video_sensor = (sensor->is() || sensor->is()); - // do all updates if profile has been changed, or if the align depth filter status has been changed + // do all updates if profile has been changed, or if the align depth filter or gpu acceleration status has been changed // and we are on a video sensor. TODO: explore better options to monitor and update changes // without resetting the whole sensors and topics. - if (is_video_sensor) + if (is_profile_changed || (is_video_sensor && (_is_align_depth_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + ))) { std::vector active_profiles = sensor->get_active_streams(); - - // Start/stop sensors only if profile was changed - // No need to start/stop sensors if align_depth was changed - ROS_INFO_STREAM("Stopping Sensor: " << module_name); - sensor->stop(); - + if (is_profile_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + ) + { + // Start/stop sensors only if profile or gpu acceleration status was changed + // No need to start/stop sensors if align_depth was changed + ROS_INFO_STREAM("Stopping Sensor: " << module_name); + sensor->stop(); + } stopPublishers(active_profiles); } } @@ -470,9 +427,8 @@ void BaseRealSenseNode::stopVideoSensors() } } -void BaseRealSenseNode::startVideoSensors() +void BaseRealSenseNode::startUpdatedSensors() { - std::lock_guard lock_guard(_update_sensor_mutex); try{ for(auto&& sensor : _available_ros_sensors) { @@ -480,16 +436,15 @@ void BaseRealSenseNode::startVideoSensors() // if active_profiles != wanted_profiles: stop sensor. std::vector wanted_profiles; - sensor->getUpdatedProfiles(wanted_profiles); + bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles)); bool is_video_sensor = (sensor->is() || sensor->is()); - // do all updates if profile has been changed, or if the align depth filter status has been changed - // and we are on a video sensor. TODO: explore better options to monitor and update changes - // without resetting the whole sensors and topics. - if (is_video_sensor) + if (is_profile_changed || (is_video_sensor && (_is_align_depth_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + ))) { - std::vector active_profiles = sensor->get_active_streams(); - if (!wanted_profiles.empty()) { startPublishers(wanted_profiles, *sensor); @@ -503,10 +458,17 @@ void BaseRealSenseNode::startVideoSensors() } } - // Start/stop sensors only if profile was changed - // No need to start/stop sensors if align_depth was changed - ROS_INFO_STREAM("Starting Sensor: " << module_name); - sensor->start(wanted_profiles); + if (is_profile_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + ) + { + // Start/stop sensors only if profile or gpu acceleration was changed + // No need to start/stop sensors if align_depth was changed + ROS_INFO_STREAM("Starting Sensor: " << module_name); + sensor->start(wanted_profiles); + } if (sensor->rs2::sensor::is()) { From d155f823610987dbe0cd8060f7b0e16d9dbcf657 Mon Sep 17 00:00:00 2001 From: deep0294 Date: Fri, 9 Feb 2024 16:40:30 +0530 Subject: [PATCH 034/111] Update ReadMe to run ROS2 Unit Test --- realsense2_camera/test/README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index ea3a043a92..15b882a2f8 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -126,6 +126,15 @@ The xml files mentioned by the command can be directly opened also. ### Running pytests directly +Note : +1. All the commands for test execution has to be executed from realsense-ros folder. For example: If the ROS2 workspace was created based on Step 3 [Option2] of [this](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/README.md#installation). +Then, the path to execute the tests would be ~/ros2_ws/src/realsense-ros. + + cd ~/ros2_ws/src/realsense-ros + +2. Please setup below required environment variables. If not, Please prefix them for every test execution. + + PYTHONPATH=$PYTHONPATH:$PWD/realsense2_camera/test/utils:$PWD/realsense2_camera//launch:$PWD/realsense2_camera//scripts User can run all the tests in a pytest file directly using the below command: From c86312d370d0663212c83ef25b404695ca8b5cd3 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 14 Feb 2024 22:06:18 +0530 Subject: [PATCH 035/111] updated readme --- README.md | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 731dd42c2c..a29107bfc7 100644 --- a/README.md +++ b/README.md @@ -305,6 +305,20 @@ User can set the camera name and camera namespace, to distinguish between camera - 1 -> **copy**: Every gyro message will be attached by the last accel message. - 2 -> **linear_interpolation**: Every gyro message will be attached by an accel message which is interpolated to gyro's timestamp. - Note: When the param *unite_imu_method* is dynamically updated, re-enable either gyro or accel stream for the change to take effect. +- **accelerate_with_gpu**: + - GPU accelerated processing of PointCloud and Colorizer filters. + - integer: + - 0 --> **NO_GPU**: use only CPU for proccessing PointCloud and Colorizer filters + - 1 --> **GL_GPU**: use GLSL to accelerate GPU for processing PointCloud and Colorizer filters + - Note: + - To have smooth transition between the processing blocks when this parameter is updated dynamically, the node will: + - Stop the video sensors + - Do necessary GLSL configuration + - And then, start the video sensors + - To enable GPU acceleration, turn ON `BUILD_ACCELERATE_WITH_GPU` during build: + ```bash + colcon build --cmake-args '-DBUILD_ACCELERATE_WITH_GPU=ON' + ``` #### Parameters that cannot be changed in runtime: - **serial_no**: @@ -354,15 +368,6 @@ User can set the camera name and camera namespace, to distinguish between camera - double, positive values set the period between diagnostics updates on the `/diagnostics` topic. - 0 or negative values mean no diagnostics topic is published. Defaults to 0.
The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams. -- **accelerate_with_gpu**: - - GPU accelerated processing of PointCloud and Colorizer filters. - - integer: - - 0 --> **NO_GPU**: use only CPU for proccessing PointCloud and Colorizer filters - - 1 --> **GL_GPU**: use GLSL to accelerate GPU for processing PointCloud and Colorizer filters - - Note: To enable GPU acceleration, turn ON `BUILD_ACCELERATE_WITH_GPU` during build: - ```bash - colcon build --cmake-args '-DBUILD_ACCELERATE_WITH_GPU=ON' - ```
From 5e835f79ede885a08f75abdafba48045da247f86 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Sat, 17 Feb 2024 01:21:47 +0530 Subject: [PATCH 036/111] renamed GL GPU enable param --- README.md | 11 ++--- realsense2_camera/CMakeLists.txt | 14 +++---- .../include/base_realsense_node.h | 11 +++-- realsense2_camera/include/gl_window.h | 2 +- realsense2_camera/launch/rs_launch.py | 2 +- realsense2_camera/src/base_realsense_node.cpp | 8 ++-- realsense2_camera/src/gl_gpu_processing.cpp | 2 +- realsense2_camera/src/parameters.cpp | 14 +++---- realsense2_camera/src/rs_node_setup.cpp | 42 +++++++++---------- 9 files changed, 50 insertions(+), 56 deletions(-) diff --git a/README.md b/README.md index 52af27c039..c925020397 100644 --- a/README.md +++ b/README.md @@ -306,19 +306,16 @@ User can set the camera name and camera namespace, to distinguish between camera - 1 -> **copy**: Every gyro message will be attached by the last accel message. - 2 -> **linear_interpolation**: Every gyro message will be attached by an accel message which is interpolated to gyro's timestamp. - Note: When the param *unite_imu_method* is dynamically updated, re-enable either gyro or accel stream for the change to take effect. -- **accelerate_with_gpu**: - - GPU accelerated processing of PointCloud and Colorizer filters. - - integer: - - 0 --> **NO_GPU**: use only CPU for proccessing PointCloud and Colorizer filters - - 1 --> **GL_GPU**: use GLSL to accelerate GPU for processing PointCloud and Colorizer filters +- **accelerate_gpu_with_glsl**: + - Boolean: GPU accelerated with GLSL for processing PointCloud and Colorizer filters. - Note: - To have smooth transition between the processing blocks when this parameter is updated dynamically, the node will: - Stop the video sensors - Do necessary GLSL configuration - And then, start the video sensors - - To enable GPU acceleration, turn ON `BUILD_ACCELERATE_WITH_GPU` during build: + - To enable GPU acceleration, turn ON `BUILD_ACCELERATE_GPU_WITH_GLSL` during build: ```bash - colcon build --cmake-args '-DBUILD_ACCELERATE_WITH_GPU=ON' + colcon build --cmake-args '-DBUILD_ACCELERATE_GPU_WITH_GLSL=ON' ``` #### Parameters that cannot be changed in runtime: diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 1d3ad2ac81..380a9707f4 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -117,7 +117,7 @@ find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(realsense2 2.54.1) -if (BUILD_ACCELERATE_WITH_GPU) +if (BUILD_ACCELERATE_GPU_WITH_GLSL) find_package(realsense2-gl 2.54.1) endif() if(NOT realsense2_FOUND) @@ -144,7 +144,7 @@ set(SOURCES src/tfs.cpp ) -if (BUILD_ACCELERATE_WITH_GPU) +if (BUILD_ACCELERATE_GPU_WITH_GLSL) list(APPEND SOURCES src/gl_gpu_processing.cpp) endif() @@ -178,8 +178,8 @@ if(${rclcpp_VERSION} VERSION_GREATER_EQUAL "17.0") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DRCLCPP_HAS_OnSetParametersCallbackType") endif() -if (BUILD_ACCELERATE_WITH_GPU) - add_definitions(-DACCELERATE_WITH_GPU) +if (BUILD_ACCELERATE_GPU_WITH_GLSL) + add_definitions(-DACCELERATE_GPU_WITH_GLSL) endif() set(INCLUDES @@ -195,7 +195,7 @@ set(INCLUDES include/profile_manager.h include/image_publisher.h) -if (BUILD_ACCELERATE_WITH_GPU) +if (BUILD_ACCELERATE_GPU_WITH_GLSL) list(APPEND INCLUDES include/gl_window.h) endif() @@ -214,7 +214,7 @@ add_library(${PROJECT_NAME} SHARED ${SOURCES} ) -if (BUILD_ACCELERATE_WITH_GPU) +if (BUILD_ACCELERATE_GPU_WITH_GLSL) set(link_libraries ${realsense2-gl_LIBRARY}) else() set(link_libraries ${realsense2_LIBRARY}) @@ -238,7 +238,7 @@ set(dependencies diagnostic_updater ) -if (BUILD_ACCELERATE_WITH_GPU) +if (BUILD_ACCELERATE_GPU_WITH_GLSL) list(APPEND dependencies realsense2-gl) else() list(APPEND dependencies realsense2) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 3d808f5e1c..4792f5456c 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -49,7 +49,7 @@ #include #include -#if defined (ACCELERATE_WITH_GPU) +#if defined (ACCELERATE_GPU_WITH_GLSL) #include #endif @@ -119,7 +119,6 @@ namespace realsense2_camera public: enum class imu_sync_method{NONE, COPY, LINEAR_INTERPOLATION}; - enum class accelerate_with_gpu {NO_GPU, GL_GPU}; protected: class float3 @@ -268,7 +267,7 @@ namespace realsense2_camera void startRGBDPublisherIfNeeded(); void stopPublishers(const std::vector& profiles); -#if defined (ACCELERATE_WITH_GPU) +#if defined (ACCELERATE_GPU_WITH_GLSL) void initOpenGLProcessing(bool use_gpu_processing); void shutdownOpenGLProcessing(); void glfwPollEventCallback(); @@ -354,11 +353,11 @@ namespace realsense2_camera std::shared_ptr _diagnostics_updater; rs2::stream_profile _base_profile; -#if defined (ACCELERATE_WITH_GPU) +#if defined (ACCELERATE_GPU_WITH_GLSL) GLwindow _app; rclcpp::TimerBase::SharedPtr _timer; - accelerate_with_gpu _accelerate_with_gpu; - bool _is_accelerate_with_gpu_changed; + bool _accelerate_gpu_with_glsl; + bool _is_accelerate_gpu_with_glsl_changed; #endif };//end class diff --git a/realsense2_camera/include/gl_window.h b/realsense2_camera/include/gl_window.h index a1cebb7af8..2bca9d4eb5 100644 --- a/realsense2_camera/include/gl_window.h +++ b/realsense2_camera/include/gl_window.h @@ -16,7 +16,7 @@ #include // Include RealSense Cross Platform API -#if defined (ACCELERATE_WITH_GPU) +#if defined (ACCELERATE_GPU_WITH_GLSL) #define GL_SILENCE_DEPRECATION #define GLFW_INCLUDE_GLU diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index fa6dfaabf2..4e7aee856c 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -29,7 +29,7 @@ {'name': 'config_file', 'default': "''", 'description': 'yaml config file'}, {'name': 'json_file_path', 'default': "''", 'description': 'allows advanced configuration'}, {'name': 'initial_reset', 'default': 'false', 'description': "''"}, - {'name': 'accelerate_with_gpu', 'default': "0", 'description': '[0-No_GPU, 1-GL_GPU]'}, + {'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': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'}, {'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'}, diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index d35112a968..0202e4e70b 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -115,10 +115,10 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _imu_sync_method(imu_sync_method::NONE), _is_profile_changed(false), _is_align_depth_changed(false) -#if defined (ACCELERATE_WITH_GPU) +#if defined (ACCELERATE_GPU_WITH_GLSL) ,_app(1280, 720, "RS_GLFW_Window"), - _accelerate_with_gpu(accelerate_with_gpu::NO_GPU), - _is_accelerate_with_gpu_changed(false) + _accelerate_gpu_with_glsl(false), + _is_accelerate_gpu_with_glsl_changed(false) #endif { if ( use_intra_process ) @@ -234,7 +234,7 @@ void BaseRealSenseNode::setupFilters() _cv_mpc.notify_one(); }; -#if defined (ACCELERATE_WITH_GPU) +#if defined (ACCELERATE_GPU_WITH_GLSL) _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); #else diff --git a/realsense2_camera/src/gl_gpu_processing.cpp b/realsense2_camera/src/gl_gpu_processing.cpp index 8bbf8c4b44..d6b8f883e2 100644 --- a/realsense2_camera/src/gl_gpu_processing.cpp +++ b/realsense2_camera/src/gl_gpu_processing.cpp @@ -18,7 +18,7 @@ using namespace realsense2_camera; using namespace std::chrono_literals; -#if defined (ACCELERATE_WITH_GPU) +#if defined (ACCELERATE_GPU_WITH_GLSL) void BaseRealSenseNode::initOpenGLProcessing(bool use_gpu_processing) { diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 0747c49c42..dda0b6133e 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -83,17 +83,17 @@ void BaseRealSenseNode::getParameters() _base_frame_id = (static_cast(std::ostringstream() << _camera_name << "_" << _base_frame_id)).str(); _parameters_names.push_back(param_name); -#if defined (ACCELERATE_WITH_GPU) - param_name = std::string("accelerate_with_gpu"); - _parameters->setParam(param_name, int(accelerate_with_gpu::NO_GPU), +#if defined (ACCELERATE_GPU_WITH_GLSL) + param_name = std::string("accelerate_gpu_with_glsl"); + _parameters->setParam(param_name, false, [this](const rclcpp::Parameter& parameter) { - accelerate_with_gpu temp_value = accelerate_with_gpu(parameter.get_value()); - if (_accelerate_with_gpu != temp_value) + bool temp_value = parameter.get_value(); + if (_accelerate_gpu_with_glsl != temp_value) { - _accelerate_with_gpu = temp_value; + _accelerate_gpu_with_glsl = temp_value; std::lock_guard lock_guard(_profile_changes_mutex); - _is_accelerate_with_gpu_changed = true; + _is_accelerate_gpu_with_glsl_changed = true; } _cv_mpc.notify_one(); }); diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index ad4f8abe8e..191644b83e 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -22,10 +22,9 @@ using namespace rs2; void BaseRealSenseNode::setup() { -#if defined (ACCELERATE_WITH_GPU) - bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); - initOpenGLProcessing(use_gpu_processing); - _is_accelerate_with_gpu_changed = false; +#if defined (ACCELERATE_GPU_WITH_GLSL) + initOpenGLProcessing(_accelerate_gpu_with_glsl); + _is_accelerate_gpu_with_glsl_changed = false; #endif setDynamicParams(); startDiagnosticsUpdater(); @@ -47,15 +46,15 @@ void BaseRealSenseNode::monitoringProfileChanges() _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return (!_is_running || _is_profile_changed || _is_align_depth_changed - #if defined (ACCELERATE_WITH_GPU) - || _is_accelerate_with_gpu_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed #endif );}); if (_is_running && (_is_profile_changed || _is_align_depth_changed - #if defined (ACCELERATE_WITH_GPU) - || _is_accelerate_with_gpu_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed #endif )) { @@ -71,8 +70,8 @@ void BaseRealSenseNode::monitoringProfileChanges() _is_profile_changed = false; _is_align_depth_changed = false; - #if defined (ACCELERATE_WITH_GPU) - _is_accelerate_with_gpu_changed = false; + #if defined (ACCELERATE_GPU_WITH_GLSL) + _is_accelerate_gpu_with_glsl_changed = false; #endif } } @@ -354,13 +353,12 @@ void BaseRealSenseNode::updateSensors() try{ stopRequiredSensors(); - #if defined (ACCELERATE_WITH_GPU) - if (_is_accelerate_with_gpu_changed) + #if defined (ACCELERATE_GPU_WITH_GLSL) + if (_is_accelerate_gpu_with_glsl_changed) { shutdownOpenGLProcessing(); - bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); - initOpenGLProcessing(use_gpu_processing); + initOpenGLProcessing(_accelerate_gpu_with_glsl); } #endif @@ -394,15 +392,15 @@ void BaseRealSenseNode::stopRequiredSensors() // and we are on a video sensor. TODO: explore better options to monitor and update changes // without resetting the whole sensors and topics. if (is_profile_changed || (is_video_sensor && (_is_align_depth_changed - #if defined (ACCELERATE_WITH_GPU) - || _is_accelerate_with_gpu_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed #endif ))) { std::vector active_profiles = sensor->get_active_streams(); if (is_profile_changed - #if defined (ACCELERATE_WITH_GPU) - || _is_accelerate_with_gpu_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed #endif ) { @@ -440,8 +438,8 @@ void BaseRealSenseNode::startUpdatedSensors() bool is_video_sensor = (sensor->is() || sensor->is()); if (is_profile_changed || (is_video_sensor && (_is_align_depth_changed - #if defined (ACCELERATE_WITH_GPU) - || _is_accelerate_with_gpu_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed #endif ))) { @@ -459,8 +457,8 @@ void BaseRealSenseNode::startUpdatedSensors() } if (is_profile_changed - #if defined (ACCELERATE_WITH_GPU) - || _is_accelerate_with_gpu_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed #endif ) { From 41200eaff56c7bcd3df298969ecc5808144b8c26 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 16 Feb 2024 19:34:19 +0530 Subject: [PATCH 037/111] skip updating exp 1,2 & gain 1,2 params when HDR is disabled --- README.md | 26 +++++---- realsense2_camera/include/dynamic_params.h | 5 +- realsense2_camera/src/dynamic_params.cpp | 8 +++ realsense2_camera/src/ros_sensor.cpp | 68 +++++++++++++++------- 4 files changed, 74 insertions(+), 33 deletions(-) diff --git a/README.md b/README.md index 52af27c039..08fcfc99fe 100644 --- a/README.md +++ b/README.md @@ -527,17 +527,21 @@ The following post processing filters are available: ros2 param set /camera/camera pointcloud.enable true ``` - ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. - - `depth_module.hdr_enabled`: to enable/disable HDR - - The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. - - From FW versions 5.14.x.x and above, if HDR is enabled, the preset configs (like exposure, gain, etc.,) cannot be updated. - - Disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. - - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. - - To initialize these parameters in start time use the following parameters: - - `depth_module.exposure.1` - - `depth_module.gain.1` - - `depth_module.exposure.2` - - `depth_module.gain.2` - - For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). + - `depth_module.hdr_enabled`: to enable/disable HDR. The way to set exposure and gain values for each sequence: + - during Runtime: + - is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. + - From FW versions 5.14.x.x and above, if HDR is enabled, the preset configs (like exposure, gain, etc.,) cannot be updated. + - Disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. + - during Launch time of the node: + - is by setting below parameters + - `depth_module.exposure.1` + - `depth_module.gain.1` + - `depth_module.exposure.2` + - `depth_module.gain.2` + - Make sure to set `depth_module.hdr_enabled` to true, otherwise these parameters won't be considered. + - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. + - For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). + - **Note**: Auto exposure functionality is not supported when HDR is enabled. i.e., Auto exposure will be auto-disabled if HDR is enabled. - The following filters have detailed descriptions in : https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md - ```disparity_filter``` - convert depth to disparity before applying other filters and back. diff --git a/realsense2_camera/include/dynamic_params.h b/realsense2_camera/include/dynamic_params.h index b87caea5c0..d438ad937b 100644 --- a/realsense2_camera/include/dynamic_params.h +++ b/realsense2_camera/include/dynamic_params.h @@ -46,7 +46,10 @@ namespace realsense2_camera template void queueSetRosValue(const std::string& param_name, const T value); - + + template + T getParam(std::string param_name); + private: void monitor_update_functions(); diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index 45db52d90a..52a521240d 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -267,6 +267,12 @@ namespace realsense2_camera _param_functions.erase(param_name); } + template + T Parameters::getParam(std::string param_name) + { + return _node.get_parameter(param_name).get_value(); + } + template void Parameters::setParamT(std::string param_name, bool& param, std::function func, rcl_interfaces::msg::ParameterDescriptor descriptor); template void Parameters::setParamT(std::string param_name, int& param, std::function func, rcl_interfaces::msg::ParameterDescriptor descriptor); template void Parameters::setParamT(std::string param_name, double& param, std::function func, rcl_interfaces::msg::ParameterDescriptor descriptor); @@ -284,4 +290,6 @@ namespace realsense2_camera template void Parameters::queueSetRosValue(const std::string& param_name, const int value); template int Parameters::readAndDeleteParam(std::string param_name, const int& initial_value); + + template bool Parameters::getParam(std::string param_name); } diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index dfd5daed99..35a4ef8ca3 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -110,8 +110,35 @@ void RosSensor::UpdateSequenceIdCallback() if (!supports(RS2_OPTION_SEQUENCE_ID)) return; + std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); + std::string param_name = module_name + "." + create_graph_resource_name(rs2_option_to_string(RS2_OPTION_ENABLE_AUTO_EXPOSURE)); + + bool user_set_enable_ae_value = _params.getParameters()->getParam(param_name); bool is_hdr_enabled = static_cast(get_option(RS2_OPTION_HDR_ENABLED)); + if (is_hdr_enabled && user_set_enable_ae_value) + { + bool is_ae_enabled = static_cast(get_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE)); + + // If AE got auto-disabled, update the Enable_Auto_Exposure ROS paramerter as well accordingly. + if (!is_ae_enabled) + { + ROS_WARN_STREAM("Auto Exposure functionality is not supported when HDR is enabled. " << + "So, disabling the '" << param_name << "' param."); + + try + { + std::vector > funcs; + funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_ENABLE_AUTO_EXPOSURE);}); + _params.getParameters()->pushUpdateFunctions(funcs); + } + catch(const std::exception& e) + { + ROS_WARN_STREAM("Failed to set parameter:" << param_name << " : " << e.what()); + } + } + } + // Deleter to revert back the RS2_OPTION_HDR_ENABLED value at the end. auto deleter_to_revert_hdr = std::unique_ptr>(&is_hdr_enabled, [&](bool* enable_back_hdr) { @@ -121,36 +148,35 @@ void RosSensor::UpdateSequenceIdCallback() } }); - // From FW version 5.14.x.x, if HDR is enabled, updating UVC controls like exposure, gain , etc are restricted. - // So, disable it before updating. if (is_hdr_enabled) { + // From FW version 5.14.x.x, if HDR is enabled, updating UVC controls like exposure, gain , etc are restricted. + // So, disable it before updating. It will be reverted back by the deleter 'deleter_to_revert_hdr'. set_option(RS2_OPTION_HDR_ENABLED, false); - } - int original_seq_id = static_cast(get_option(RS2_OPTION_SEQUENCE_ID)); // To Set back to default. - std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); - - // Read initialization parameters and set to sensor: - std::vector options{RS2_OPTION_EXPOSURE, RS2_OPTION_GAIN}; - unsigned int seq_size = get_option(RS2_OPTION_SEQUENCE_SIZE); - for (unsigned int seq_id = 1; seq_id <= seq_size; seq_id++ ) - { - set_option(RS2_OPTION_SEQUENCE_ID, seq_id); - for (rs2_option& option : options) + int original_seq_id = static_cast(get_option(RS2_OPTION_SEQUENCE_ID)); // To Set back to default. + + // Read initialization parameters and set to sensor: + std::vector options{RS2_OPTION_EXPOSURE, RS2_OPTION_GAIN}; + unsigned int seq_size = get_option(RS2_OPTION_SEQUENCE_SIZE); + for (unsigned int seq_id = 1; seq_id <= seq_size; seq_id++ ) { - std::stringstream param_name_str; - param_name_str << module_name << "." << create_graph_resource_name(rs2_option_to_string(option)) << "." << seq_id; - int option_value = get_option(option); - int user_set_option_value = _params.getParameters()->readAndDeleteParam(param_name_str.str(), option_value); - if (option_value != user_set_option_value) + set_option(RS2_OPTION_SEQUENCE_ID, seq_id); + for (rs2_option& option : options) { - ROS_INFO_STREAM("Set " << rs2_option_to_string(option) << "." << seq_id << " to " << user_set_option_value); - set_option(option, user_set_option_value); + std::stringstream param_name_str; + param_name_str << module_name << "." << create_graph_resource_name(rs2_option_to_string(option)) << "." << seq_id; + int option_value = get_option(option); + int user_set_option_value = _params.getParameters()->readAndDeleteParam(param_name_str.str(), option_value); + if (option_value != user_set_option_value) + { + ROS_INFO_STREAM("Set " << rs2_option_to_string(option) << "." << seq_id << " to " << user_set_option_value); + set_option(option, user_set_option_value); + } } } + set_option(RS2_OPTION_SEQUENCE_ID, original_seq_id); // Set back to default. } - set_option(RS2_OPTION_SEQUENCE_ID, original_seq_id); // Set back to default. // Set callback to update ros parameters to gain and exposure matching the selected sequence_id: const std::string option_name(module_name + "." + create_graph_resource_name(rs2_option_to_string(RS2_OPTION_SEQUENCE_ID))); From 567aaa371919b7826d0fd54460a4c17653b2562d Mon Sep 17 00:00:00 2001 From: Madhukar Reddy Kadireddy Date: Wed, 13 Mar 2024 00:37:46 +0530 Subject: [PATCH 038/111] Assertion failed when the camera device not found --- .../test/live_camera/test_camera_point_cloud_tests.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py index 14def86ca9..3f012f133b 100644 --- a/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py @@ -55,9 +55,9 @@ 'device_type': 'D455', 'pointcloud.enable': 'true' } -test_params_points_cloud_d435 = { - 'camera_name': 'D435', - 'device_type': 'D435', +test_params_points_cloud_d435i = { + 'camera_name': 'D435I', + 'device_type': 'D435I', 'pointcloud.enable': 'true' } @@ -76,7 +76,7 @@ ''' @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_points_cloud_d455, marks=pytest.mark.d455), - pytest.param(test_params_points_cloud_d435, marks=pytest.mark.d435), + pytest.param(test_params_points_cloud_d435i, marks=pytest.mark.d435i), pytest.param(test_params_points_cloud_d415, marks=pytest.mark.d415), ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) @@ -85,6 +85,7 @@ def test_camera_test_point_cloud(self,launch_descr_with_parameters): self.params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: print("Device not found? : " + self.params['device_type']) + assert False return themes = [ { From d318ee0f923b8bf43ed6e92cddc0d533a3906658 Mon Sep 17 00:00:00 2001 From: Madhukar Reddy Kadireddy Date: Wed, 13 Mar 2024 00:37:46 +0530 Subject: [PATCH 039/111] Assertion failed when the camera device not found --- .../live_camera/test_camera_aligned_tests.py | 10 ++++++---- .../test_camera_all_profile_tests.py | 12 ++++++++---- .../test/live_camera/test_camera_fps_tests.py | 1 + .../test/live_camera/test_camera_imu_tests.py | 1 + .../test/live_camera/test_camera_tf_tests.py | 18 ++++++++++-------- .../test/live_camera/test_d415_basic_tests.py | 1 + .../test/live_camera/test_d455_basic_tests.py | 2 ++ 7 files changed, 29 insertions(+), 16 deletions(-) diff --git a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py index 0c17e76e47..f09b8a0de6 100644 --- a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py @@ -79,6 +79,7 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return themes = [ {'topic':get_node_heirarchy(params)+'/color/image_raw', @@ -144,9 +145,9 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): 'rgb_camera.profile':'640x480x30', 'align_depth.enable':'true' } -test_params_all_profiles_d435 = { - 'camera_name': 'D435', - 'device_type': 'D435', +test_params_all_profiles_d435i = { + 'camera_name': 'D435I', + 'device_type': 'D435I', 'enable_color':'true', 'enable_depth':'true', 'depth_module.profile':'848x480x30', @@ -166,7 +167,7 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), - pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] + pytest.param(test_params_all_profiles_d435i, marks=pytest.mark.d435i),] ,indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestCamera_AllAlignDepthColor(pytest_rs_utils.RsTestBaseClass): @@ -178,6 +179,7 @@ def test_camera_all_align_depth_color(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return themes = [ {'topic':get_node_heirarchy(params)+'/color/image_raw', diff --git a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py index b337e6f8d5..ab66cf26be 100644 --- a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py @@ -127,9 +127,9 @@ def check_if_skip_test(profile, format): 'camera_name': 'D415', 'device_type': 'D415', } -test_params_all_profiles_d435 = { - 'camera_name': 'D435', - 'device_type': 'D435', +test_params_all_profiles_d435i = { + 'camera_name': 'D435I', + 'device_type': 'D435I', } @@ -144,7 +144,7 @@ def check_if_skip_test(profile, format): @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), - pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] + pytest.param(test_params_all_profiles_d435i, marks=pytest.mark.d435i),] ,indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestLiveCamera_Change_Resolution(pytest_rs_utils.RsTestBaseClass): @@ -154,6 +154,10 @@ def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): num_passed = 0 num_failed = 0 params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + assert False + return themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1}] config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) try: diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index c4b91354d4..ef67597014 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -74,6 +74,7 @@ def test_camera_test_fps(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return try: ''' diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index 384d249f47..cad8e8033e 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -57,6 +57,7 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return themes = [{'topic':get_node_heirarchy(params)+'/imu', 'msg_type':msg_Imu,'expected_data_chunks':1}, {'topic':get_node_heirarchy(params)+'/gyro/sample', 'msg_type':msg_Imu,'expected_data_chunks':1}, diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py index a31f74e77c..15edc4d6f9 100644 --- a/realsense2_camera/test/live_camera/test_camera_tf_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -58,9 +58,9 @@ 'enable_accel': 'true', 'enable_gyro': 'true', } -test_params_tf_static_change_d435 = { - 'camera_name': 'D435', - 'device_type': 'D435', +test_params_tf_static_change_d435i = { + 'camera_name': 'D435I', + 'device_type': 'D435I', 'enable_infra1': 'false', 'enable_infra2': 'true', 'enable_accel': 'true', @@ -77,7 +77,7 @@ } @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_tf_static_change_d455, marks=pytest.mark.d455), - pytest.param(test_params_tf_static_change_d435, marks=pytest.mark.d435), + pytest.param(test_params_tf_static_change_d435i, marks=pytest.mark.d435i), pytest.param(test_params_tf_static_change_d415, marks=pytest.mark.d415), ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) @@ -86,6 +86,7 @@ def test_camera_test_tf_static_change(self,launch_descr_with_parameters): self.params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: print("Device not found? : " + self.params['device_type']) + assert False return themes = [ {'topic':'/tf_static', @@ -145,9 +146,9 @@ def process_data(self, themes, enable_infra1): 'tf_publish_rate': '1.1', } -test_params_tf_d435 = { - 'camera_name': 'D435', - 'device_type': 'D435', +test_params_tf_d435i = { + 'camera_name': 'D435I', + 'device_type': 'D435I', 'publish_tf': 'true', 'tf_publish_rate': '1.1', } @@ -160,7 +161,7 @@ def process_data(self, themes, enable_infra1): } @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_tf_d455, marks=pytest.mark.d455), - pytest.param(test_params_tf_d435, marks=pytest.mark.d435), + pytest.param(test_params_tf_d435i, marks=pytest.mark.d435i), pytest.param(test_params_tf_d415, marks=pytest.mark.d415), ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) @@ -169,6 +170,7 @@ def test_camera_test_tf_dyn(self,launch_descr_with_parameters): self.params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: print("Device not found? : " + self.params['device_type']) + assert False return themes = [ {'topic':'/tf', diff --git a/realsense2_camera/test/live_camera/test_d415_basic_tests.py b/realsense2_camera/test/live_camera/test_d415_basic_tests.py index 649f84e7bd..7454d75a78 100644 --- a/realsense2_camera/test/live_camera/test_d415_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d415_basic_tests.py @@ -57,6 +57,7 @@ def test_D415_Change_Resolution(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return failed_tests = [] num_passed = 0 diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index fb2f58cab6..3b501e8e6c 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -58,6 +58,7 @@ def test_D455_Change_Resolution(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return themes = [ @@ -116,6 +117,7 @@ def test_D455_Seq_ID_update(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return try: From 2f78ad01a80470ab20449b6d9826adcf43640579 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 19 Mar 2024 09:25:17 +0530 Subject: [PATCH 040/111] Applying Colorizer filter to Aligned-Depth image --- realsense2_camera/src/base_realsense_node.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 0202e4e70b..76d3b2cb06 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -242,11 +242,14 @@ void BaseRealSenseNode::setupFilters() _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); #endif - _filters.push_back(_colorizer_filter); + // Apply PointCloud filter before applying Align-depth as it requires original depth image not aligned-depth image. _filters.push_back(_pc_filter); _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); _filters.push_back(_align_depth_filter); + + // Apply Colorizer filter after applying Align-Depth to get colorized aligned depth image. + _filters.push_back(_colorizer_filter); } cv::Mat& BaseRealSenseNode::fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image) From 0ca251087a5a5879e3bda86de6871eb8b5f5404c Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 20 Mar 2024 11:23:38 +0530 Subject: [PATCH 041/111] Support for selecting profile for each stream_type --- README.md | 8 +- realsense2_camera/include/profile_manager.h | 12 +- realsense2_camera/launch/rs_launch.py | 5 +- realsense2_camera/src/profile_manager.cpp | 208 +++++++++++++------- realsense2_camera/src/ros_sensor.cpp | 28 ++- 5 files changed, 175 insertions(+), 86 deletions(-) diff --git a/README.md b/README.md index bf23475d2b..86d4f0fa67 100644 --- a/README.md +++ b/README.md @@ -172,7 +172,7 @@ #### with ros2 launch: ros2 launch realsense2_camera rs_launch.py - ros2 launch realsense2_camera rs_launch.py depth_module.profile:=1280x720x30 pointcloud.enable:=true + ros2 launch realsense2_camera rs_launch.py depth_module.depth_profile:=1280x720x30 pointcloud.enable:=true
@@ -251,10 +251,10 @@ User can set the camera name and camera namespace, to distinguish between camera #### Parameters that can be modified during runtime: - All of the filters and sensors inner parameters. - Video Sensor Parameters: (```depth_module``` and ```rgb_camera```) - - They have, at least, the **profile** parameter. + - They have, at least, the **_profile** parameter. - The profile parameter is a string of the following format: \X\X\ (The dividing character can be X, x or ",". Spaces are ignored.) - - For example: ```depth_module.profile:=640x480x30 rgb_camera.profile:=1280x720x30``` - - Since infra, infra1, infra2 and depth are all streams of the depth_module, their width, height and fps are defined by the same param **depth_module.profile** + - For example: ```depth_module.depth_profile:=640x480x30 depth_module.infra_profile:=640x480x30 rgb_camera.color_profile:=1280x720x30``` + - Note: The param **depth_module.infra_profile** is common for all infra streams. i.e., infra 0, 1 & 2. - If the specified combination of parameters is not available by the device, the default or previously set configuration will be used. - Run ```ros2 param describe ``` to get the list of supported profiles. - Note: Should re-enable the stream for the change to take effect. diff --git a/realsense2_camera/include/profile_manager.h b/realsense2_camera/include/profile_manager.h index 3021e95f6c..a63cc7f3b0 100644 --- a/realsense2_camera/include/profile_manager.h +++ b/realsense2_camera/include/profile_manager.h @@ -68,22 +68,22 @@ namespace realsense2_camera VideoProfilesManager(std::shared_ptr parameters, const std::string& module_name, rclcpp::Logger logger, bool force_image_default_qos = false); bool isWantedProfile(const rs2::stream_profile& profile) override; void registerProfileParameters(std::vector all_profiles, std::function update_sensor_func) override; - int getHeight() {return _height;}; - int getWidth() {return _width;}; - int getFPS() {return _fps;}; + int getHeight(rs2_stream stream_type) {return _height[stream_type];}; + int getWidth(rs2_stream stream_type) {return _width[stream_type];}; + int getFPS(rs2_stream stream_type) {return _fps[stream_type];}; private: bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format); + rs2::stream_profile validateAndGetSuitableProfile(rs2_stream stream_type, rs2::stream_profile given_profile); void registerVideoSensorProfileFormat(stream_index_pair sip); void registerVideoSensorParams(std::set sips); - std::string get_profiles_descriptions(); + std::string get_profiles_descriptions(rs2_stream stream_type); std::string getProfileFormatsDescriptions(stream_index_pair sip); private: std::string _module_name; std::map _formats; - int _fps; - int _width, _height; + std::map _fps, _width, _height; bool _is_profile_exist; bool _force_image_default_qos; }; diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 4e7aee856c..0fa7938ae1 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -34,15 +34,16 @@ {'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'}, - {'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'}, + {'name': 'rgb_camera.color_profile', 'default': '0,0,0', 'description': 'color stream profile'}, {'name': 'rgb_camera.color_format', 'default': 'RGB8', 'description': 'color stream format'}, {'name': 'rgb_camera.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for color image'}, {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, {'name': 'enable_infra', 'default': 'false', 'description': 'enable infra0 stream'}, {'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'}, {'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'}, - {'name': 'depth_module.profile', 'default': '0,0,0', 'description': 'depth module profile'}, + {'name': 'depth_module.depth_profile', 'default': '0,0,0', 'description': 'depth stream profile'}, {'name': 'depth_module.depth_format', 'default': 'Z16', 'description': 'depth stream format'}, + {'name': 'depth_module.infra_profile', 'default': '0,0,0', 'description': 'infra streams (0/1/2) profile'}, {'name': 'depth_module.infra_format', 'default': 'RGB8', 'description': 'infra0 stream format'}, {'name': 'depth_module.infra1_format', 'default': 'Y8', 'description': 'infra1 stream format'}, {'name': 'depth_module.infra2_format', 'default': 'Y8', 'description': 'infra2 stream format'}, diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index 95dadbbad6..be1598a5f8 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -139,6 +139,40 @@ std::map ProfilesManager::getDefaultProf return sip_default_profiles; } +rs2::stream_profile VideoProfilesManager::validateAndGetSuitableProfile(rs2_stream stream_type, rs2::stream_profile given_profile) +{ + rs2::stream_profile suitable_profile = given_profile; + bool is_given_profile_suitable = false; + + for (auto temp_profile : _all_profiles) + { + if (temp_profile.stream_type() == stream_type) + { + auto video_profile = given_profile.as(); + + if (isSameProfileValues(temp_profile, video_profile.width(), video_profile.height(), video_profile.fps(), RS2_FORMAT_ANY)) + { + is_given_profile_suitable = true; + break; + } + } + } + + // If given profile is not suitable, choose the first available profile for the given stream. + if (!is_given_profile_suitable) + { + for (auto temp_profile : _all_profiles) + { + if (temp_profile.stream_type() == stream_type) + { + suitable_profile = temp_profile; + } + } + } + + return suitable_profile; +} + void ProfilesManager::addWantedProfiles(std::vector& wanted_profiles) { std::map found_sips; @@ -241,7 +275,7 @@ bool VideoProfilesManager::isSameProfileValues(const rs2::stream_profile& profil bool VideoProfilesManager::isWantedProfile(const rs2::stream_profile& profile) { stream_index_pair sip = {profile.stream_type(), profile.stream_index()}; - return isSameProfileValues(profile, _width, _height, _fps, _formats[sip]); + return isSameProfileValues(profile, _width[sip.first], _height[sip.first], _fps[sip.first], _formats[sip]); } void VideoProfilesManager::registerProfileParameters(std::vector all_profiles, std::function update_sensor_func) @@ -270,15 +304,18 @@ void VideoProfilesManager::registerProfileParameters(std::vector } } -std::string VideoProfilesManager::get_profiles_descriptions() +std::string VideoProfilesManager::get_profiles_descriptions(rs2_stream stream_type) { std::set profiles_str; for (auto& profile : _all_profiles) { - auto video_profile = profile.as(); - std::stringstream crnt_profile_str; - crnt_profile_str << video_profile.width() << "x" << video_profile.height() << "x" << video_profile.fps(); - profiles_str.insert(crnt_profile_str.str()); + if (stream_type == profile.stream_type()) + { + auto video_profile = profile.as(); + std::stringstream crnt_profile_str; + crnt_profile_str << video_profile.width() << "x" << video_profile.height() << "x" << video_profile.fps(); + profiles_str.insert(crnt_profile_str.str()); + } } std::stringstream descriptors_strm; for (auto& profile_str : profiles_str) @@ -333,25 +370,52 @@ void VideoProfilesManager::registerVideoSensorProfileFormat(stream_index_pair si void VideoProfilesManager::registerVideoSensorParams(std::set sips) { - // Set default values: - std::map sip_default_profiles = getDefaultProfiles(); - - rs2::stream_profile default_profile = sip_default_profiles.begin()->second; + std::set available_streams; - if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end()) - { - default_profile = sip_default_profiles[DEPTH]; - } - else if (sip_default_profiles.find(COLOR) != sip_default_profiles.end()) + for (auto sip : sips) { - default_profile = sip_default_profiles[COLOR]; + available_streams.insert(sip.first); } - auto video_profile = default_profile.as(); + // Set default values: + std::map sip_default_profiles = getDefaultProfiles(); - _width = video_profile.width(); - _height = video_profile.height(); - _fps = video_profile.fps(); + for (auto stream_type : available_streams) + { + rs2::stream_profile default_profile = sip_default_profiles.begin()->second; + switch (stream_type) + { + case RS2_STREAM_COLOR: + if (sip_default_profiles.find(COLOR) != sip_default_profiles.end()) + { + default_profile = sip_default_profiles[COLOR]; + } + break; + case RS2_STREAM_DEPTH: + if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end()) + { + default_profile = sip_default_profiles[DEPTH]; + } + break; + case RS2_STREAM_INFRARED: + if (sip_default_profiles.find(INFRA1) != sip_default_profiles.end()) + { + default_profile = sip_default_profiles[INFRA1]; + } + else if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end()) + { + default_profile = validateAndGetSuitableProfile(stream_type, sip_default_profiles[DEPTH]); + } + break; + default: + default_profile = validateAndGetSuitableProfile(stream_type, sip_default_profiles.begin()->second); + } + + auto video_profile = default_profile.as(); + _width[stream_type] = video_profile.width(); + _height[stream_type] = video_profile.height(); + _fps[stream_type] = video_profile.fps(); + } // Set the stream formats for (auto sip : sips) @@ -364,72 +428,76 @@ void VideoProfilesManager::registerVideoSensorParams(std::set { stream_index_pair sip = sip_default_profile.first; - default_profile = sip_default_profile.second; - video_profile = default_profile.as(); + auto default_profile = sip_default_profile.second; + auto video_profile = default_profile.as(); - if (isSameProfileValues(default_profile, _width, _height, _fps, video_profile.format())) + if (isSameProfileValues(default_profile, _width[sip.first], _height[sip.first], _fps[sip.first], video_profile.format())) { _formats[sip] = video_profile.format(); } } - // Register ROS parameter: - std::string param_name(_module_name + ".profile"); - rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; - crnt_descriptor.description = "Available options are:\n" + get_profiles_descriptions(); - std::stringstream crnt_profile_str; - crnt_profile_str << _width << "x" << _height << "x" << _fps; - _params.getParameters()->setParam(param_name, crnt_profile_str.str(), [this](const rclcpp::Parameter& parameter) - { - std::regex self_regex("\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*", std::regex_constants::ECMAScript); - std::smatch match; - std::string profile_str(parameter.get_value()); - bool found = std::regex_match(profile_str, match, self_regex); - bool request_default(false); - if (found) + for (auto stream_type : available_streams) + { + // Register ROS parameter: + std::stringstream param_name_str; + param_name_str << _module_name << "." << create_graph_resource_name(ros_stream_to_string(stream_type)) << "_profile"; + rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; + crnt_descriptor.description = "Available options are:\n" + get_profiles_descriptions(stream_type); + std::stringstream crnt_profile_str; + crnt_profile_str << _width[stream_type] << "x" << _height[stream_type] << "x" << _fps[stream_type]; + _params.getParameters()->setParam(param_name_str.str(), crnt_profile_str.str(), [this, stream_type](const rclcpp::Parameter& parameter) { - int temp_width(std::stoi(match[1])), temp_height(std::stoi(match[2])), temp_fps(std::stoi(match[3])); - if (temp_width <= 0 || temp_height <= 0 || temp_fps <= 0) - { - found = false; - request_default = true; - } - else + std::regex self_regex("\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*", std::regex_constants::ECMAScript); + std::smatch match; + std::string profile_str(parameter.get_value()); + bool found = std::regex_match(profile_str, match, self_regex); + bool request_default(false); + if (found) { - for (const auto& profile : _all_profiles) + int temp_width(std::stoi(match[1])), temp_height(std::stoi(match[2])), temp_fps(std::stoi(match[3])); + if (temp_width <= 0 || temp_height <= 0 || temp_fps <= 0) { found = false; - if (isSameProfileValues(profile, temp_width, temp_height, temp_fps, RS2_FORMAT_ANY)) + request_default = true; + } + else + { + for (const auto& profile : _all_profiles) { - _width = temp_width; - _height = temp_height; - _fps = temp_fps; - found = true; - ROS_WARN_STREAM("re-enable the stream for the change to take effect."); - break; + found = false; + if (isSameProfileValues(profile, temp_width, temp_height, temp_fps, RS2_FORMAT_ANY)) + { + _width[stream_type] = temp_width; + _height[stream_type] = temp_height; + _fps[stream_type] = temp_fps; + found = true; + ROS_WARN_STREAM("re-enable the stream for the change to take effect."); + break; + } } } } - } - if (!found) - { - std::stringstream crnt_profile_str; - crnt_profile_str << _width << "x" << _height << "x" << _fps; - if (request_default) - { - ROS_INFO_STREAM("Set ROS param " << parameter.get_name() << " to default: " << crnt_profile_str.str()); - } - else + if (!found) { - ROS_ERROR_STREAM("Given value, " << parameter.get_value() << " is invalid. " - << "Run 'ros2 param describe " << parameter.get_name() - << "' to get the list of supported profiles. " - << "Setting ROS param back to: " << crnt_profile_str.str()); + std::stringstream crnt_profile_str; + crnt_profile_str << _width[stream_type] << "x" << _height[stream_type] << "x" << _fps[stream_type]; + if (request_default) + { + ROS_INFO_STREAM("Set ROS param " << parameter.get_name() << " to default: " << crnt_profile_str.str()); + } + else + { + ROS_ERROR_STREAM("Given value, " << parameter.get_value() << " is invalid. " + << "Run 'ros2 param describe " << parameter.get_name() + << "' to get the list of supported profiles. " + << "Setting ROS param back to: " << crnt_profile_str.str()); + } + _params.getParameters()->queueSetRosValue(parameter.get_name(), crnt_profile_str.str()); } - _params.getParameters()->queueSetRosValue(parameter.get_name(), crnt_profile_str.str()); - } - }, crnt_descriptor); - _parameters_names.push_back(param_name); + }, crnt_descriptor); + _parameters_names.push_back(param_name_str.str()); + } for (auto sip : sips) { diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 35a4ef8ca3..b6598079f0 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -405,8 +405,18 @@ void RosSensor::set_sensor_auto_exposure_roi() { try { - int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(); - int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(); + rs2_stream stream_type; + if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_DEPTH; + } + else if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_COLOR; + } + + int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(stream_type); + int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(stream_type); bool update_roi_range(false); if (_auto_exposure_roi.max_x > width) @@ -437,8 +447,18 @@ void RosSensor::registerAutoExposureROIOptions() if (this->rs2::sensor::is()) { - int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(); - int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(); + rs2_stream stream_type; + if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_DEPTH; + } + else if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_COLOR; + } + + int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(stream_type); + int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(stream_type); int max_x(width-1); int max_y(height-1); From c38066f01dd228c1ddaaa51412a56d4dd39de761 Mon Sep 17 00:00:00 2001 From: Nir Date: Wed, 20 Mar 2024 11:57:02 +0200 Subject: [PATCH 042/111] fix static analysis issue 1 --- realsense2_camera/src/base_realsense_node.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 0202e4e70b..6928ed9d04 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -34,7 +34,11 @@ SyncedImuPublisher::SyncedImuPublisher(rclcpp::Publisher: SyncedImuPublisher::~SyncedImuPublisher() { - PublishPendingMessages(); + try + { + PublishPendingMessages(); + } + catch(...){} // Not allowed to throw from Dtor } void SyncedImuPublisher::Publish(sensor_msgs::msg::Imu imu_msg) From 9b8cb5e66eea5d5049d8aa1175864ae552c2f522 Mon Sep 17 00:00:00 2001 From: Nir Date: Wed, 20 Mar 2024 12:07:24 +0200 Subject: [PATCH 043/111] fix static analysis issue 2 --- realsense2_camera/src/base_realsense_node.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 6928ed9d04..5b5e6f9dac 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -153,10 +153,13 @@ BaseRealSenseNode::~BaseRealSenseNode() _monitoring_pc->join(); } clearParameters(); - for(auto&& sensor : _available_ros_sensors) + try { - sensor->stop(); - } + for(auto&& sensor : _available_ros_sensors) + { + sensor->stop(); + } + catch(...){} // Not allowed to throw from Dtor } void BaseRealSenseNode::hardwareResetRequest() From 49cfc09eba28e3d89ed533f4ee6f940fbc741fd3 Mon Sep 17 00:00:00 2001 From: Nir Date: Wed, 20 Mar 2024 12:19:06 +0200 Subject: [PATCH 044/111] fix static analysis issue 3 --- realsense2_camera/src/ros_sensor.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 35a4ef8ca3..0a26f85937 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -78,8 +78,12 @@ RosSensor::RosSensor(rs2::sensor sensor, RosSensor::~RosSensor() { - clearParameters(); - stop(); + try + { + clearParameters(); + stop(); + } + catch(...){} // Not allowed to throw from Dtor } void RosSensor::setParameters(bool is_rosbag_file) From 99873aaaf3a657ab2c69ec8875cb604d92e9a4f0 Mon Sep 17 00:00:00 2001 From: Nir Date: Wed, 20 Mar 2024 13:52:38 +0200 Subject: [PATCH 045/111] fix resource leak --- realsense2_camera/src/profile_manager.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index 95dadbbad6..25b11d110e 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -39,7 +39,9 @@ std::string applyTemplateName(std::string template_name, stream_index_pair sip) const std::string stream_name(create_graph_resource_name(STREAM_NAME(sip))); char* param_name = new char[template_name.size() + stream_name.size()]; sprintf(param_name, template_name.c_str(), stream_name.c_str()); - return std::string(param_name); + std::string full_name(param_name); + delete [] param_name; + return full_name; } void ProfilesManager::registerSensorQOSParam(std::string template_name, From 6db887411a216be3e30e1f63512bd9ddb6ae6e01 Mon Sep 17 00:00:00 2001 From: Nir Date: Wed, 20 Mar 2024 14:18:56 +0200 Subject: [PATCH 046/111] fix empty warning --- realsense2_camera/src/base_realsense_node.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 5b5e6f9dac..97360905cf 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -650,7 +650,11 @@ void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_met bool BaseRealSenseNode::setBaseTime(double frame_time, rs2_timestamp_domain time_domain) { - ROS_WARN_ONCE(time_domain == RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME ? "Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)" : ""); + if (time_domain == RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME) + { + ROS_WARN_ONCE("Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)"); + } + if (time_domain == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK) { ROS_WARN("frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically."); From f1a421396091032b106b6da5720e7a1722a526db Mon Sep 17 00:00:00 2001 From: Nir Date: Wed, 20 Mar 2024 14:34:54 +0200 Subject: [PATCH 047/111] fixup! fix static analysis issue 2 --- realsense2_camera/src/base_realsense_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 97360905cf..617acce5a9 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -159,6 +159,7 @@ BaseRealSenseNode::~BaseRealSenseNode() { sensor->stop(); } + } catch(...){} // Not allowed to throw from Dtor } @@ -654,7 +655,7 @@ bool BaseRealSenseNode::setBaseTime(double frame_time, rs2_timestamp_domain time { ROS_WARN_ONCE("Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)"); } - + if (time_domain == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK) { ROS_WARN("frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically."); From 117263735792930cd149654d2d87050c772c3a0e Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sun, 24 Mar 2024 21:34:51 +0200 Subject: [PATCH 048/111] support running realsense2 ros node on windows --- README.md | 80 ++++++++++++++++++++++++++++++-- realsense2_camera/CMakeLists.txt | 4 ++ 2 files changed, 80 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index bf23475d2b..e3fcf21c38 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,4 @@ +

Intel® RealSense™

@@ -24,7 +25,8 @@ ## Table of contents * [ROS1 and ROS2 legacy](#ros1-and-ros2-legacy) - * [Installation](#installation) + * [Installation on Ubuntu](#installation-on-ubuntu) + * [Installation on Windows](#installation-on-windows) * [Usage](#usage) * [Starting the camera node](#start-the-camera-node) * [Camera name and namespace](#camera-name-and-camera-namespace) @@ -78,7 +80,7 @@
-# Installation +# Installation on Ubuntu
@@ -88,14 +90,14 @@ - #### Ubuntu 22.04: - [ROS2 Iron](https://docs.ros.org/en/iron/Installation/Ubuntu-Install-Debians.html) - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) -
Step 2: Install latest Intel® RealSense™ SDK 2.0 - + **Please choose only one option from the 3 options below (in order to prevent multiple versions installation and workspace conflicts)** + - #### Option 1: Install librealsense2 debian package from Intel servers - Jetson users - use the [Jetson Installation Guide](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation_jetson.md) - Otherwise, install from [Linux Debian Installation Guide](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) @@ -161,6 +163,76 @@
+# Installation on Windows + **PLEASE PAY ATTENTION: RealSense ROS2 Wrapper is not meant to be supported on Windows by our team, since ROS2 and its packages are still not fully supported over Windows. We are putting these installation steps below in order to try and make it easier for users who already started working with ROS2 on Windows and want to take advantage of the capabilities of our RealSense cameras** + +
+ + Step 1: Install the ROS2 distribution + + +- #### Windows 10/11 + **Please choose only one option from the two options below (in order to prevent multiple versions installation and workspace conflicts)** + - Manual install from ROS2 formal documentation: + - [ROS2 Iron](https://docs.ros.org/en/iron/Installation/Windows-Install-Binary.html) + - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Windows-Install-Binary.html) + - Microsoft IOT binary installation: + - https://ms-iot.github.io/ROSOnWindows/GettingStarted/SetupRos2.html + - Pay attention that the examples of install are for Foxy distro (which is not supported anymore by RealSense ROS2 Wrapper) + - Please replace the word "Foxy" with Humble or Iron, as you choose +
+ +
+ + Step 2: Download RealSense™ ROS2 Wrapper and RealSense™ SDK 2.0 source code from github: + + +- Download the latest [Intel® RealSense™ ROS2 Wrapper 4.54.1](https://github.com/IntelRealSense/realsense-ros/tree/4.54.1) +- Download the latest [Intel® RealSense™ SDK 2.0 2.54.1](https://github.com/IntelRealSense/librealsense/tree/v2.54.1) +- Put the librealsense folder inside the realsense-ros folder, to make the librealsense package set beside realsense2_camera, realsense2_camera_msgs and realsense2_description packages +
+ +
+ + Step 3: Build + + +1. Before starting building of our packages, make sure you have OpenCV for Windows installed on your machine. If you choose the Microsoft IOT way to install it, it will be installed automatically. Later, when colcon build, you might need to expose this installation folder by setting CMAKE_PREFIX_PATH, PATH, or OpenCV_DIR environment variables +2. Run "x64 Native Tools Command Prompt for VS 2019" as administrator +3. Setup ROS2 Environment (Do this for every new terminal/cmd you open): + - If you choose the Microsoft IOT Binary option for installation + ``` + > C:\opt\ros\humble\x64\setup.bat + ``` + + - If you choose the ROS2 formal documentation: + ``` + > call C:\dev\ros2_iron\local_setup.bat + ``` +4. Change directory to realsense-ros folder + ```bash + > cd C:\ros2_ws\realsense-ros + ``` +5. Build librealsense2 package only + ```bash + > colcon build --packages-select librealsense2 --cmake-args -DBUILD_EXAMPLES=OFF -DBUILD_WITH_STATIC_CRT=OFF -DBUILD_GRAPHICAL_EXAMPLES=OFF + ``` + - User can add `--event-handlers console_direct+` parameter to see more debug outputs of the colcon build +6. Build the other packages + ```bash + > colcon build --packages-select realsense2_camera_msgs realsense2_description realsense2_camera + ``` + - User can add `--event-handlers console_direct+` parameter to see more debug outputs of the colcon build + +7. Setup environment with new installed packages (Do this for every new terminal/cmd you open): + ```bash + > call install\setup.bat + ``` +
+ +
+ + # Usage ## Start the camera node diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 380a9707f4..93064e40ca 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -115,6 +115,7 @@ find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) +find_package(OpenCV REQUIRED COMPONENTS core) find_package(realsense2 2.54.1) if (BUILD_ACCELERATE_GPU_WITH_GLSL) @@ -126,6 +127,7 @@ endif() #set(CMAKE_NO_SYSTEM_FROM_IMPORTED true) include_directories(include) +include_directories(${OpenCV_INCLUDE_DIRS}) # not needed for opencv>=4.0 set(node_plugins "") @@ -182,6 +184,7 @@ if (BUILD_ACCELERATE_GPU_WITH_GLSL) add_definitions(-DACCELERATE_GPU_WITH_GLSL) endif() + set(INCLUDES include/constants.h include/realsense_node_factory.h @@ -222,6 +225,7 @@ endif() target_link_libraries(${PROJECT_NAME} ${link_libraries} + ${OpenCV_LIBS} ) set(dependencies From 97721e8976d666f8c0a6315d145efda83f55050f Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 25 Mar 2024 23:17:44 +0200 Subject: [PATCH 049/111] add if win32 for open cv and fix some typos in README.md --- README.md | 6 +++--- realsense2_camera/CMakeLists.txt | 15 ++++++++++++--- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index e3fcf21c38..379c7316da 100644 --- a/README.md +++ b/README.md @@ -164,7 +164,7 @@
# Installation on Windows - **PLEASE PAY ATTENTION: RealSense ROS2 Wrapper is not meant to be supported on Windows by our team, since ROS2 and its packages are still not fully supported over Windows. We are putting these installation steps below in order to try and make it easier for users who already started working with ROS2 on Windows and want to take advantage of the capabilities of our RealSense cameras** + **PLEASE PAY ATTENTION: RealSense ROS2 Wrapper is not meant to be supported on Windows by our team, since ROS2 and its packages are still not fully supported over Windows. We added these installation steps below in order to try and make it easier for users who already started working with ROS2 on Windows and want to take advantage of the capabilities of our RealSense cameras**
@@ -175,7 +175,7 @@ **Please choose only one option from the two options below (in order to prevent multiple versions installation and workspace conflicts)** - Manual install from ROS2 formal documentation: - [ROS2 Iron](https://docs.ros.org/en/iron/Installation/Windows-Install-Binary.html) - - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Windows-Install-Binary.html) + - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Windows-Install-Binary.html) - Microsoft IOT binary installation: - https://ms-iot.github.io/ROSOnWindows/GettingStarted/SetupRos2.html - Pay attention that the examples of install are for Foxy distro (which is not supported anymore by RealSense ROS2 Wrapper) @@ -189,7 +189,7 @@ - Download the latest [Intel® RealSense™ ROS2 Wrapper 4.54.1](https://github.com/IntelRealSense/realsense-ros/tree/4.54.1) - Download the latest [Intel® RealSense™ SDK 2.0 2.54.1](https://github.com/IntelRealSense/librealsense/tree/v2.54.1) -- Put the librealsense folder inside the realsense-ros folder, to make the librealsense package set beside realsense2_camera, realsense2_camera_msgs and realsense2_description packages +- Place the librealsense folder inside the realsense-ros folder, to make the librealsense package set beside realsense2_camera, realsense2_camera_msgs and realsense2_description packages
diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 93064e40ca..348218efe0 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -115,7 +115,10 @@ find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) -find_package(OpenCV REQUIRED COMPONENTS core) + +if(WIN32) + find_package(OpenCV REQUIRED COMPONENTS core) +endif() find_package(realsense2 2.54.1) if (BUILD_ACCELERATE_GPU_WITH_GLSL) @@ -127,7 +130,10 @@ endif() #set(CMAKE_NO_SYSTEM_FROM_IMPORTED true) include_directories(include) -include_directories(${OpenCV_INCLUDE_DIRS}) # not needed for opencv>=4.0 + +if(WIN32) + include_directories(${OpenCV_INCLUDE_DIRS}) +endif() set(node_plugins "") @@ -223,9 +229,12 @@ else() set(link_libraries ${realsense2_LIBRARY}) endif() +if(WIN32) + list(APPEND link_libraries ${OpenCV_LIBS}) +endif() + target_link_libraries(${PROJECT_NAME} ${link_libraries} - ${OpenCV_LIBS} ) set(dependencies From 1d5de35d6bf8b2f807b54f2c4413cc8292dbf33e Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 26 Mar 2024 10:30:18 +0200 Subject: [PATCH 050/111] fixes of readme and cmakelists for windows installation --- README.md | 4 ++-- realsense2_camera/CMakeLists.txt | 14 +++----------- 2 files changed, 5 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index 379c7316da..993068a696 100644 --- a/README.md +++ b/README.md @@ -187,8 +187,8 @@ Step 2: Download RealSense™ ROS2 Wrapper and RealSense™ SDK 2.0 source code from github: -- Download the latest [Intel® RealSense™ ROS2 Wrapper 4.54.1](https://github.com/IntelRealSense/realsense-ros/tree/4.54.1) -- Download the latest [Intel® RealSense™ SDK 2.0 2.54.1](https://github.com/IntelRealSense/librealsense/tree/v2.54.1) +- Download Intel® RealSense™ ROS2 Wrapper source code from [Intel® RealSense™ ROS2 Wrapper Releases](https://github.com/IntelRealSense/realsense-ros/releases) +- Download the corrosponding supported Intel® RealSense™ SDK 2.0 source code from the **"Supported RealSense SDK" section** of the specific release you chose fronm the link above - Place the librealsense folder inside the realsense-ros folder, to make the librealsense package set beside realsense2_camera, realsense2_camera_msgs and realsense2_description packages
diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 348218efe0..cf2331e611 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -115,10 +115,7 @@ find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) - -if(WIN32) - find_package(OpenCV REQUIRED COMPONENTS core) -endif() +find_package(OpenCV REQUIRED COMPONENTS core) find_package(realsense2 2.54.1) if (BUILD_ACCELERATE_GPU_WITH_GLSL) @@ -131,9 +128,7 @@ endif() #set(CMAKE_NO_SYSTEM_FROM_IMPORTED true) include_directories(include) -if(WIN32) - include_directories(${OpenCV_INCLUDE_DIRS}) -endif() +include_directories(${OpenCV_INCLUDE_DIRS}) # add OpenCV includes to the included dirs set(node_plugins "") @@ -190,7 +185,6 @@ if (BUILD_ACCELERATE_GPU_WITH_GLSL) add_definitions(-DACCELERATE_GPU_WITH_GLSL) endif() - set(INCLUDES include/constants.h include/realsense_node_factory.h @@ -229,9 +223,7 @@ else() set(link_libraries ${realsense2_LIBRARY}) endif() -if(WIN32) - list(APPEND link_libraries ${OpenCV_LIBS}) -endif() +list(APPEND link_libraries ${OpenCV_LIBS}) # add OpenCV libs to link_libraries target_link_libraries(${PROJECT_NAME} ${link_libraries} From 2f899c422b7f9bcf24b8403858ae72a0f454e214 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 27 Mar 2024 10:23:53 +0000 Subject: [PATCH 051/111] add security.md as part of SDLe CT256 --- security.md | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 security.md diff --git a/security.md b/security.md new file mode 100644 index 0000000000..d5f1e5eaca --- /dev/null +++ b/security.md @@ -0,0 +1,6 @@ +# Security Policy +Intel is committed to rapidly addressing security vulnerabilities affecting our customers and providing clear guidance on the solution, impact, severity and mitigation. + +## Reporting a Vulnerability +Please report any security vulnerabilities in this project [utilizing the guidelines here](https://www.intel.com/content/www/us/en/security-center/vulnerability-handling-guidelines.html). + From 4cc91e6b694cf1849720579802f23e3295a90ccf Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 27 Mar 2024 23:40:12 +0000 Subject: [PATCH 052/111] update checkout to v4 --- .github/workflows/main.yml | 2 +- .github/workflows/pre-release.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 93a0bcb4b0..90de07232c 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -40,7 +40,7 @@ jobs: run: | mkdir -p ${{github.workspace}}/ros2/src - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: 'ros2/src/realsense-ros' diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index 66d2332deb..29122ffe10 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -45,6 +45,6 @@ jobs: BASEDIR: ${{ github.workspace }}/.work steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: industrial_ci uses: ros-industrial/industrial_ci@master From 1443278695ff559dc964261ca88c7f90014c6c1e Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 27 Mar 2024 23:42:03 +0000 Subject: [PATCH 053/111] update checkout to v4 --- .github/workflows/main.yml | 2 +- .github/workflows/pre-release.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 93a0bcb4b0..90de07232c 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -40,7 +40,7 @@ jobs: run: | mkdir -p ${{github.workspace}}/ros2/src - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: 'ros2/src/realsense-ros' diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index 66d2332deb..29122ffe10 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -45,6 +45,6 @@ jobs: BASEDIR: ${{ github.workspace }}/.work steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: industrial_ci uses: ros-industrial/industrial_ci@master From 314948be490cf78fe55358974eeb7d2ade1819d0 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sun, 31 Mar 2024 06:04:32 +0000 Subject: [PATCH 054/111] revert from installation for foxy distro on ubuntu 20 --- .github/workflows/main.yml | 36 +++++++++++++++---- README.md | 17 ++++++--- realsense2_camera/CMakeLists.txt | 6 +++- realsense2_camera/package.xml | 22 ++++++------ realsense2_camera/src/base_realsense_node.cpp | 7 ++++ realsense2_camera/src/dynamic_params.cpp | 6 +++- 6 files changed, 70 insertions(+), 24 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 90de07232c..db9b619f72 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -25,7 +25,7 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [rolling, iron, humble] + ros_distro: [rolling, iron, humble, foxy] include: - ros_distro: 'rolling' os: ubuntu-22.04 @@ -33,6 +33,8 @@ jobs: os: ubuntu-22.04 - ros_distro: 'humble' os: ubuntu-22.04 + - ros_distro: 'foxy' + os: ubuntu-20.04 steps: @@ -50,13 +52,27 @@ jobs: cd ${{github.workspace}}/ros2/src/realsense-ros/scripts ./pr_check.sh - - name: build ROS2 + # setup-ros@v0.6 is the last version supporting foxy (EOL) + # setup-ros@v0.7 is needed to support humble/iron/rolling + # so, seperating steps with if conditions + - name: build ROS2 for foxy + if: ${{ matrix.ros_distro == 'foxy' }} + uses: ros-tooling/setup-ros@v0.6 + with: + required-ros-distributions: ${{ matrix.ros_distro }} + - name: build ROS2 for humble/iron/rolling + if: ${{ matrix.ros_distro != 'foxy' }} uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} - - name: Build RealSense SDK 2.0 from source + - name: Build RealSense SDK 2.0 (development branch) from source run: | + + # libusb-1.0-0-dev is needed for librealsense build in ubuntu 20.04 + # This apt install command will be ignored in ubuntu 22.04 as libusb-1.0-0-dev already installed there + sudo apt install -y libusb-1.0-0-dev + cd ${{github.workspace}} git clone https://github.com/IntelRealSense/librealsense.git -b development cd librealsense @@ -68,13 +84,13 @@ jobs: sudo make -j10 sudo make install - - name: Build RealSense ROS2 Wrapper - run: | + - name: Build RealSense ROS2 Wrapper from source + run: | echo "source /opt/ros/${{ matrix.ros_distro }}/setup.bash" >> ${{github.workspace}}/.bashrc source ${{github.workspace}}/.bashrc cd ${{github.workspace}}/ros2 echo "================= ROSDEP UPDATE =====================" - rosdep update --rosdistro ${{ matrix.ros_distro }} + rosdep update --rosdistro ${{ matrix.ros_distro }} --include-eol-distros echo "================= ROSDEP INSTALL ====================" rosdep install -i --reinstall --from-path src --rosdistro ${{ matrix.ros_distro }} --skip-keys=librealsense2 -y echo "================== COLCON BUILD ======================" @@ -103,10 +119,16 @@ jobs: cd ${{github.workspace}}/ros2 source ${{github.workspace}}/.bashrc . install/local_setup.bash + # the next command might be needed for foxy distro, since this package is not installed + # by default in ubuntu 20.04. For other distro, the apt install command will be ignored. + sudo apt install -y ros-${{matrix.ros_distro}}-sensor-msgs-py python3 src/realsense-ros/realsense2_camera/scripts/rs2_test.py non_existent_file + # don't run integration tests for foxy since some testing dependecies packages like + # tf_ros_py are not avaialble + # TODO: check when we can run integration tests on rolling - name: Run integration tests - if: ${{ matrix.ros_distro != 'rolling'}} + if: ${{ matrix.ros_distro != 'rolling' && matrix.ros_distro != 'foxy' }} run: | cd ${{github.workspace}}/ros2 source ${{github.workspace}}/.bashrc diff --git a/README.md b/README.md index c5b8abf6a4..529ae1fbc0 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,7 @@ [![rolling][rolling-badge]][rolling] [![iron][iron-badge]][iron] [![humble][humble-badge]][humble] +[![foxy][foxy-badge]][foxy] [![ubuntu22][ubuntu22-badge]][ubuntu22] [![ubuntu20][ubuntu20-badge]][ubuntu20] @@ -90,12 +91,15 @@ - #### Ubuntu 22.04: - [ROS2 Iron](https://docs.ros.org/en/iron/Installation/Ubuntu-Install-Debians.html) - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) + #### Ubuntu 20.04 + - [ROS2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html)
Step 2: Install latest Intel® RealSense™ SDK 2.0 + **Please choose only one option from the 3 options below (in order to prevent multiple versions installation and workspace conflicts)** - #### Option 1: Install librealsense2 debian package from Intel servers @@ -103,7 +107,7 @@ - Otherwise, install from [Linux Debian Installation Guide](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) - In this case treat yourself as a developer: make sure to follow the instructions to also install librealsense2-dev and librealsense2-dkms packages -- #### Option 2: Install librealsense2 (without graphical tools and examples) debian package from ROS servers: +- #### Option 2: Install librealsense2 (without graphical tools and examples) debian package from ROS servers (Foxy EOL distro is not supported by this option): - [Configure](http://wiki.ros.org/Installation/Ubuntu/Sources) your Ubuntu repositories - Install all realsense ROS packages by ```sudo apt install ros--librealsense2*``` - For example, for Humble distro: ```sudo apt install ros-humble-librealsense2*``` @@ -119,7 +123,7 @@ Step 3: Install Intel® RealSense™ ROS2 wrapper -#### Option 1: Install debian package from ROS servers +#### Option 1: Install debian package from ROS servers (Foxy EOL distro is not supported by this option): - [Configure](http://wiki.ros.org/Installation/Ubuntu/Sources) your Ubuntu repositories - Install all realsense ROS packages by ```sudo apt install ros--realsense2-*``` - For example, for Humble distro: ```sudo apt install ros-humble-realsense2-*``` @@ -153,7 +157,7 @@ - Source environment ```bash - ROS_DISTRO= # set your ROS_DISTRO: iron, humble + ROS_DISTRO= # set your ROS_DISTRO: iron, humble, foxy source /opt/ros/$ROS_DISTRO/setup.bash cd ~/ros2_ws . install/local_setup.bash @@ -172,14 +176,17 @@ - #### Windows 10/11 + **Please choose only one option from the two options below (in order to prevent multiple versions installation and workspace conflicts)** + - Manual install from ROS2 formal documentation: - [ROS2 Iron](https://docs.ros.org/en/iron/Installation/Windows-Install-Binary.html) - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Windows-Install-Binary.html) + - [ROS2 Foxy](https://docs.ros.org/en/foxy/Installation/Windows-Install-Binary.html) - Microsoft IOT binary installation: - https://ms-iot.github.io/ROSOnWindows/GettingStarted/SetupRos2.html - Pay attention that the examples of install are for Foxy distro (which is not supported anymore by RealSense ROS2 Wrapper) - - Please replace the word "Foxy" with Humble or Iron, as you choose + - Please replace the word "Foxy" with Humble or Iron, depends on the chosen distro.
@@ -676,6 +683,8 @@ ros2 launch realsense2_camera rs_intra_process_demo_launch.py intra_process_comm [rolling-badge]: https://img.shields.io/badge/-ROLLING-orange?style=flat-square&logo=ros [rolling]: https://docs.ros.org/en/rolling/index.html +[foxy-badge]: https://img.shields.io/badge/-foxy-orange?style=flat-square&logo=ros +[foxy]: https://docs.ros.org/en/foxy/index.html [humble-badge]: https://img.shields.io/badge/-HUMBLE-orange?style=flat-square&logo=ros [humble]: https://docs.ros.org/en/humble/index.html [iron-badge]: https://img.shields.io/badge/-IRON-orange?style=flat-square&logo=ros diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index cf2331e611..a838364f8d 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -154,7 +154,11 @@ endif() if(NOT DEFINED ENV{ROS_DISTRO}) message(FATAL_ERROR "ROS_DISTRO is not defined." ) endif() -if("$ENV{ROS_DISTRO}" STREQUAL "humble") +if("$ENV{ROS_DISTRO}" STREQUAL "foxy") + message(STATUS "Build for ROS2 Foxy") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DFOXY") + set(SOURCES "${SOURCES}" src/ros_param_backend.cpp) +elseif("$ENV{ROS_DISTRO}" STREQUAL "humble") message(STATUS "Build for ROS2 Humble") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHUMBLE") set(SOURCES "${SOURCES}" src/ros_param_backend.cpp) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 6db46cff68..9ff26e3d55 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -28,17 +28,17 @@ tf2 tf2_ros diagnostic_updater - ament_cmake_gtest - launch_testing - ament_cmake_pytest - launch_pytest - sensor_msgs_py - python3-numpy - python3-tqdm - sensor_msgs_py - python3-requests - tf2_ros_py - ros2topic + ament_cmake_gtest + launch_testing + ament_cmake_pytest + launch_pytest + sensor_msgs_py + python3-numpy + python3-tqdm + sensor_msgs_py + python3-requests + tf2_ros_py + ros2topic launch_ros ros_environment diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 4e7a508525..4b88ec7df1 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -690,7 +690,14 @@ rclcpp::Time BaseRealSenseNode::frameSystemTimeSec(rs2::frame frame) { double elapsed_camera_ns = millisecondsToNanoseconds(timestamp_ms - _camera_time_base); + /* + Fixing deprecated-declarations compilation error for EOL distro (foxy) + */ +#if defined(FOXY) + auto duration = rclcpp::Duration(elapsed_camera_ns); +#else auto duration = rclcpp::Duration::from_nanoseconds(elapsed_camera_ns); +#endif return rclcpp::Time(_ros_time_base + duration); } diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index 52a521240d..ef7d73cd9e 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -115,7 +115,11 @@ namespace realsense2_camera try { ROS_DEBUG_STREAM("setParam::Setting parameter: " << param_name); - descriptor.dynamic_typing=true; // Without this, undeclare_parameter() throws error. +#if defined(FOXY) + //do nothing for old versions (foxy) +#else + descriptor.dynamic_typing=true; +#endif if (!_node.get_parameter(param_name, result_value)) { result_value = _node.declare_parameter(param_name, initial_value, descriptor); From d80ccf4c95594eb97245663d0add8a47ebdb9458 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sun, 7 Apr 2024 18:35:08 +0000 Subject: [PATCH 055/111] fix compilation warning: stream_type may be used uninitialized --- realsense2_camera/src/ros_sensor.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 0c939f0f2d..f0a5aa5331 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include using namespace realsense2_camera; using namespace rs2; @@ -409,7 +410,7 @@ void RosSensor::set_sensor_auto_exposure_roi() { try { - rs2_stream stream_type; + rs2_stream stream_type = RS2_STREAM_ANY; if (this->rs2::sensor::is()) { stream_type = RS2_STREAM_DEPTH; @@ -418,6 +419,7 @@ void RosSensor::set_sensor_auto_exposure_roi() { stream_type = RS2_STREAM_COLOR; } + assert(stream_type != RS2_STREAM_ANY); int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(stream_type); int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(stream_type); @@ -451,7 +453,7 @@ void RosSensor::registerAutoExposureROIOptions() if (this->rs2::sensor::is()) { - rs2_stream stream_type; + rs2_stream stream_type = RS2_STREAM_ANY; if (this->rs2::sensor::is()) { stream_type = RS2_STREAM_DEPTH; @@ -460,7 +462,8 @@ void RosSensor::registerAutoExposureROIOptions() { stream_type = RS2_STREAM_COLOR; } - + assert(stream_type != RS2_STREAM_ANY); + int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(stream_type); int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(stream_type); From a75c2e2a88b1b00bbc1f091965130cbbdc12d090 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sun, 7 Apr 2024 18:45:46 +0000 Subject: [PATCH 056/111] fetch string from output substitution object for foxy --- realsense2_camera/launch/rs_launch.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 0fa7938ae1..bdbdfd0ac8 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -98,6 +98,14 @@ def yaml_to_dict(path_to_yaml): def launch_setup(context, params, param_name_suffix=''): _config_file = LaunchConfiguration('config_file' + param_name_suffix).perform(context) params_from_file = {} if _config_file == "''" else yaml_to_dict(_config_file) + + _output = LaunchConfiguration('output' + param_name_suffix) + if(os.getenv('ROS_DISTRO') == 'foxy'): + # Foxy doesn't support output as substitution object (LaunchConfiguration object) + # but supports it as string, so we fetch the string from this substitution object + # see related PR that was merged for humble, iron, rolling: https://github.com/ros2/launch/pull/577 + _output = context.perform_substitution(_output) + return [ launch_ros.actions.Node( package='realsense2_camera', @@ -105,7 +113,7 @@ def launch_setup(context, params, param_name_suffix=''): name=LaunchConfiguration('camera_name' + param_name_suffix), executable='realsense2_camera_node', parameters=[params, params_from_file], - output=LaunchConfiguration('output' + param_name_suffix), + output=_output, arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level' + param_name_suffix)], emulate_tty=True, ) From c5f484a36371eca9ffa394f24b7d7f01a35d3a2c Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sun, 7 Apr 2024 19:05:09 +0000 Subject: [PATCH 057/111] fix workflow for rolling --- .github/workflows/main.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 90de07232c..8cad39e4fb 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -74,6 +74,10 @@ jobs: source ${{github.workspace}}/.bashrc cd ${{github.workspace}}/ros2 echo "================= ROSDEP UPDATE =====================" + # temp fix for rolling sources.. TODO: track when we can remove the two commands below + # see https://discourse.ros.org/t/psa-rolling-ci-or-docker-build-fix-from-rosdep-errors-in-24-04-transition/36902 + sudo sed -i "s|ros\/rosdistro\/master|ros\/rosdistro\/rolling\/2024-02-28|" /etc/ros/rosdep/sources.list.d/20-default.list + export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2024-02-28/index-v4.yaml rosdep update --rosdistro ${{ matrix.ros_distro }} echo "================= ROSDEP INSTALL ====================" rosdep install -i --reinstall --from-path src --rosdistro ${{ matrix.ros_distro }} --skip-keys=librealsense2 -y From 72992de0e9c2197d33b50fb0686fa54a95f8d2a2 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 9 Apr 2024 07:59:19 +0300 Subject: [PATCH 058/111] update notice file (TPP) --- NOTICE | 4 - NOTICE.md | 327 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 327 insertions(+), 4 deletions(-) delete mode 100644 NOTICE create mode 100644 NOTICE.md diff --git a/NOTICE b/NOTICE deleted file mode 100644 index f986287012..0000000000 --- a/NOTICE +++ /dev/null @@ -1,4 +0,0 @@ -This project uses code from the following third-party projects, listed here -with the full text of their respective licenses. - -ddynamic_reconfigure (BSD) - https://github.com/awesomebytes/ddynamic_reconfigure diff --git a/NOTICE.md b/NOTICE.md new file mode 100644 index 0000000000..246ae907fa --- /dev/null +++ b/NOTICE.md @@ -0,0 +1,327 @@ +# **Intel® RealSense™ ROS Wrapper Third Party Programs** +## This file specifies all 3rd party SW components used for Intel® RealSense™ ROS Wrapper and the inbound license for each of these 3rd party components. + +# **Apache License 2.0 Third Party Programs** + +|Component|Home Page|License|copyright| +| :- | :- | :- | :- | +|librealsense2|

|Apache License 2.0|None| +|Rclcpp|

 

|Apache License 2.0|None| +|rclcpp\_components|

 

|Apache License 2.0|None| +|builtin\_interfaces|

|Apache License 2.0|None| +|cv\_bridge|

 

|Apache License 2.0|None| +|geometry\_msgs|

 

|Apache License 2.0|None| +|nav\_msgs|

 

|Apache License 2.0|None| +|std\_msgs|

 

|Apache License 2.0|None| +|sensor\_msgs||Apache License 2.0|None| +|ament\_cmake|

|Apache License 2.0|None| +|rosidl\_default\_generators|

|Apache License 2.0|None| +|launch\_pytest|

|Apache License 2.0|None| +|ros\_environment|

|Apache License 2.0|None| +|ros2topic|

|Apache License 2.0|None| +|

rosidl\_default\_runtime

|

|Apache License 2.0|None| +|launch\_ros|

|Apache License 2.0|None| +|ament\_lint\_auto|

|Apache License 2.0|None| +|ament\_lint\_common|

|Apache License 2.0|None| +|ament\_cmake\_gtest|

|Apache License 2.0|None| +|launch\_testing|

|Apache License 2.0|None| +|sensor\_msgs\_py|

|Apache License 2.0|None| +|Open CV|

|Apache License 2.0|

Copyright (C) 2000-2022, Intel Corporation, all rights reserved.

Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.

Copyright (C) 2009-2016, NVIDIA Corporation, all rights reserved.

Copyright (C) 2010-2013, Advanced Micro Devices, Inc., all rights reserved.

Copyright (C) 2015-2023, OpenCV Foundation, all rights reserved.

Copyright (C) 2008-2016, Itseez Inc., all rights reserved.

Copyright (C) 2019-2023, Xperience AI, all rights reserved.

Copyright (C) 2019-2022, Shenzhen Institute of Artificial Intelligence and Robotics for Society, all rights reserved.

Copyright (C) 2022-2023, Southern University of Science And Technology, all rights reserved.

| +## +## **Apache License 2.0** +Apache License +Version 2.0, January 2004 +http://www.apache.org/licenses/ + +TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + +1\. Definitions. + +"License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. + +"Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. + +"Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. + +"You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. + +"Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. + +"Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. + +"Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). + +"Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. + +"Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." + +"Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. + +2\. Grant of Copyright License. + +Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. + +3\. Grant of Patent License. + +Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. + +4\. Redistribution. + +You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: + +You must give any other recipients of the Work or Derivative Works a copy of this License; and +You must cause any modified files to carry prominent notices stating that You changed the files; and +You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and +If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. +You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. + +5\. Submission of Contributions. + +Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. + +6\. Trademarks. + +This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. + +7\. Disclaimer of Warranty. + +Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. + +8\. Limitation of Liability. + +In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. + +9\. Accepting Warranty or Additional Liability. + +While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. + +END OF TERMS AND CONDITIONS + +BSD 3-clause License Third Party Programs +# **BSD 3-cluase License Third Party Programs** + +|Component|Home Page|License|copyright| +| :- | :- | :- | :- | +|xacro|

|BSD 3-clause|Copyright Robert Haschke, Bielefeld University| +|tf2\_ros\_py|

|BSD 3-clause|

Copyright (c) 2008, Willow Garage, Inc.

| +|tf2|

|BSD 3-clause|

Copyright (c) 2008, Willow Garage, Inc.

| +|tf2\_ros|

|BSD 3-clause|

Copyright (c) 2008, Willow Garage, Inc.

| +|

diagnostic\_updater

|

|BSD 3-clause|

Copyright 2008 - 2012, Willow Garage, Inc.

` `2012 - 2022, Open Source Robotics Foundation and contributors

| +## **BSD 3-clause** +Note: This license has also been called the “New BSD License” or “Modified BSD License”. See also the [2-clause BSD License](https://opensource.org/license/bsd-2-clause/). + +Copyright + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1\. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2\. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3\. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# **Mozilla Public License 2.0 Third Party Programs** + +|Component|Home Page|License|copyright| +| :- | :- | :- | :- | +|Eigen|

|Mozilla Public License 2.0|None| +## **Mozilla Public License 2.0** + +Mozilla Public License + +Version 2.0 + +1\. Definitions + +1\.1. “Contributor” + + means each individual or legal entity that creates, contributes to the creation of, or owns Covered Software. + +1\.2. “Contributor Version” + + means the combination of the Contributions of others (if any) used by a Contributor and that particular Contributor’s Contribution. + +1\.3. “Contribution” + + means Covered Software of a particular Contributor. + +1\.4. “Covered Software” + + means Source Code Form to which the initial Contributor has attached the notice in Exhibit A, the Executable Form of such Source Code Form, and Modifications of such Source Code Form, in each case including portions thereof. + +1\.5. “Incompatible With Secondary Licenses” + + means + +` `that the initial Contributor has attached the notice described in Exhibit B to the Covered Software; or + +` `that the Covered Software was made available under the terms of version 1.1 or earlier of the License, but not also under the terms of a Secondary License. + +1\.6. “Executable Form” + + means any form of the work other than Source Code Form. + +1\.7. “Larger Work” + + means a work that combines Covered Software with other material, in a separate file or files, that is not Covered Software. + +1\.8. “License” + + means this document. + +1\.9. “Licensable” + + means having the right to grant, to the maximum extent possible, whether at the time of the initial grant or subsequently, any and all of the rights conveyed by this License. + +1\.10. “Modifications” + + means any of the following: + +` `any file in Source Code Form that results from an addition to, deletion from, or modification of the contents of Covered Software; or + +` `any new file in Source Code Form that contains any Covered Software. + +1\.11. “Patent Claims” of a Contributor + + means any patent claim(s), including without limitation, method, process, and apparatus claims, in any patent Licensable by such Contributor that would be infringed, but for the grant of the License, by the making, using, selling, offering for sale, having made, import, or transfer of either its Contributions or its Contributor Version. + +1\.12. “Secondary License” + + means either the GNU General Public License, Version 2.0, the GNU Lesser General Public License, Version 2.1, the GNU Affero General Public License, Version 3.0, or any later versions of those licenses. + +1\.13. “Source Code Form” + + means the form of the work preferred for making modifications. + +1\.14. “You” (or “Your”) + + means an individual or a legal entity exercising rights under this License. For legal entities, “You” includes any entity that controls, is controlled by, or is under common control with You. For purposes of this definition, “control” means (a) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (b) ownership of more than fifty percent (50%) of the outstanding shares or beneficial ownership of such entity. + +2\. License Grants and Conditions + +2\.1. Grants + +Each Contributor hereby grants You a world-wide, royalty-free, non-exclusive license: + + under intellectual property rights (other than patent or trademark) Licensable by such Contributor to use, reproduce, make available, modify, display, perform, distribute, and otherwise exploit its Contributions, either on an unmodified basis, with Modifications, or as part of a Larger Work; and + + under Patent Claims of such Contributor to make, use, sell, offer for sale, have made, import, and otherwise transfer either its Contributions or its Contributor Version. + +2\.2. Effective Date + +The licenses granted in Section 2.1 with respect to any Contribution become effective for each Contribution on the date the Contributor first distributes such Contribution. + +2\.3. Limitations on Grant Scope + +The licenses granted in this Section 2 are the only rights granted under this License. No additional rights or licenses will be implied from the distribution or licensing of Covered Software under this License. Notwithstanding Section 2.1(b) above, no patent license is granted by a Contributor: + + for any code that a Contributor has removed from Covered Software; or + + for infringements caused by: (i) Your and any other third party’s modifications of Covered Software, or (ii) the combination of its Contributions with other software (except as part of its Contributor Version); or + + under Patent Claims infringed by Covered Software in the absence of its Contributions. + +This License does not grant any rights in the trademarks, service marks, or logos of any Contributor (except as may be necessary to comply with the notice requirements in Section 3.4). + +2\.4. Subsequent Licenses + +No Contributor makes additional grants as a result of Your choice to distribute the Covered Software under a subsequent version of this License (see Section 10.2) or under the terms of a Secondary License (if permitted under the terms of Section 3.3). + +2\.5. Representation + +Each Contributor represents that the Contributor believes its Contributions are its original creation(s) or it has sufficient rights to grant the rights to its Contributions conveyed by this License. + +2\.6. Fair Use + +This License is not intended to limit any rights You have under applicable copyright doctrines of fair use, fair dealing, or other equivalents. + +2\.7. Conditions + +Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted in Section 2.1. + +3\. Responsibilities + +3\.1. Distribution of Source Form + +All distribution of Covered Software in Source Code Form, including any Modifications that You create or to which You contribute, must be under the terms of this License. You must inform recipients that the Source Code Form of the Covered Software is governed by the terms of this License, and how they can obtain a copy of this License. You may not attempt to alter or restrict the recipients’ rights in the Source Code Form. + +3\.2. Distribution of Executable Form + +If You distribute Covered Software in Executable Form then: + + such Covered Software must also be made available in Source Code Form, as described in Section 3.1, and You must inform recipients of the Executable Form how they can obtain a copy of such Source Code Form by reasonable means in a timely manner, at a charge no more than the cost of distribution to the recipient; and + + You may distribute such Executable Form under the terms of this License, or sublicense it under different terms, provided that the license for the Executable Form does not attempt to limit or alter the recipients’ rights in the Source Code Form under this License. + +3\.3. Distribution of a Larger Work + +You may create and distribute a Larger Work under terms of Your choice, provided that You also comply with the requirements of this License for the Covered Software. If the Larger Work is a combination of Covered Software with a work governed by one or more Secondary Licenses, and the Covered Software is not Incompatible With Secondary Licenses, this License permits You to additionally distribute such Covered Software under the terms of such Secondary License(s), so that the recipient of the Larger Work may, at their option, further distribute the Covered Software under the terms of either this License or such Secondary License(s). + +3\.4. Notices + +You may not remove or alter the substance of any license notices (including copyright notices, patent notices, disclaimers of warranty, or limitations of liability) contained within the Source Code Form of the Covered Software, except that You may alter any license notices to the extent required to remedy known factual inaccuracies. + +3\.5. Application of Additional Terms + +You may choose to offer, and to charge a fee for, warranty, support, indemnity or liability obligations to one or more recipients of Covered Software. However, You may do so only on Your own behalf, and not on behalf of any Contributor. You must make it absolutely clear that any such warranty, support, indemnity, or liability obligation is offered by You alone, and You hereby agree to indemnify every Contributor for any liability incurred by such Contributor as a result of warranty, support, indemnity or liability terms You offer. You may include additional disclaimers of warranty and limitations of liability specific to any jurisdiction. + +4\. Inability to Comply Due to Statute or Regulation + +If it is impossible for You to comply with any of the terms of this License with respect to some or all of the Covered Software due to statute, judicial order, or regulation then You must: (a) comply with the terms of this License to the maximum extent possible; and (b) describe the limitations and the code they affect. Such description must be placed in a text file included with all distributions of the Covered Software under this License. Except to the extent prohibited by statute or regulation, such description must be sufficiently detailed for a recipient of ordinary skill to be able to understand it. + +5\. Termination + +5\.1. The rights granted under this License will terminate automatically if You fail to comply with any of its terms. However, if You become compliant, then the rights granted under this License from a particular Contributor are reinstated (a) provisionally, unless and until such Contributor explicitly and finally terminates Your grants, and (b) on an ongoing basis, if such Contributor fails to notify You of the non-compliance by some reasonable means prior to 60 days after You have come back into compliance. Moreover, Your grants from a particular Contributor are reinstated on an ongoing basis if such Contributor notifies You of the non-compliance by some reasonable means, this is the first time You have received notice of non-compliance with this License from such Contributor, and You become compliant prior to 30 days after Your receipt of the notice. + +5\.2. If You initiate litigation against any entity by asserting a patent infringement claim (excluding declaratory judgment actions, counter-claims, and cross-claims) alleging that a Contributor Version directly or indirectly infringes any patent, then the rights granted to You by any and all Contributors for the Covered Software under Section 2.1 of this License shall terminate. + +5\.3. In the event of termination under Sections 5.1 or 5.2 above, all end user license agreements (excluding distributors and resellers) which have been validly granted by You or Your distributors under this License prior to termination shall survive termination. + +6\. Disclaimer of Warranty + +Covered Software is provided under this License on an “as is” basis, without warranty of any kind, either expressed, implied, or statutory, including, without limitation, warranties that the Covered Software is free of defects, merchantable, fit for a particular purpose or non-infringing. The entire risk as to the quality and performance of the Covered Software is with You. Should any Covered Software prove defective in any respect, You (not any Contributor) assume the cost of any necessary servicing, repair, or correction. This disclaimer of warranty constitutes an essential part of this License. No use of any Covered Software is authorized under this License except under this disclaimer. + +7\. Limitation of Liability + +Under no circumstances and under no legal theory, whether tort (including negligence), contract, or otherwise, shall any Contributor, or anyone who distributes Covered Software as permitted above, be liable to You for any direct, indirect, special, incidental, or consequential damages of any character including, without limitation, damages for lost profits, loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses, even if such party shall have been informed of the possibility of such damages. This limitation of liability shall not apply to liability for death or personal injury resulting from such party’s negligence to the extent applicable law prohibits such limitation. Some jurisdictions do not allow the exclusion or limitation of incidental or consequential damages, so this exclusion and limitation may not apply to You. + +8\. Litigation + +Any litigation relating to this License may be brought only in the courts of a jurisdiction where the defendant maintains its principal place of business and such litigation shall be governed by laws of that jurisdiction, without reference to its conflict-of-law provisions. Nothing in this Section shall prevent a party’s ability to bring cross-claims or counter-claims. + +9\. Miscellaneous + +This License represents the complete agreement concerning the subject matter hereof. If any provision of this License is held to be unenforceable, such provision shall be reformed only to the extent necessary to make it enforceable. Any law or regulation which provides that the language of a contract shall be construed against the drafter shall not be used to construe this License against a Contributor. + +10\. Versions of the License + +10\.1. New Versions + +Mozilla Foundation is the license steward. Except as provided in Section 10.3, no one other than the license steward has the right to modify or publish new versions of this License. Each version will be given a distinguishing version number. + +10\.2. Effect of New Versions + +You may distribute the Covered Software under the terms of the version of the License under which You originally received the Covered Software, or under the terms of any subsequent version published by the license steward. + +10\.3. Modified Versions + +If you create software not governed by this License, and you want to create a new license for such software, you may create and use a modified version of this License if you rename the license and remove any references to the name of the license steward (except to note that such modified license differs from this License). + +10\.4. Distributing Source Code Form that is Incompatible With Secondary Licenses + +If You choose to distribute Source Code Form that is Incompatible With Secondary Licenses under the terms of this version of the License, the notice described in Exhibit B of this License must be attached. + +Exhibit A - Source Code Form License Notice + + This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not distributed with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +If it is not possible or desirable to put the notice in a particular file, then You may include the notice in a location (such as a LICENSE file in a relevant directory) where a recipient would be likely to look for such a notice. + +You may add additional accurate notices of copyright ownership. + +Exhibit B - “Incompatible With Secondary Licenses” Notice + + This Source Code Form is “Incompatible With Secondary Licenses”, as defined by the Mozilla Public License, v. 2.0. + From 7041b875360fa73f9ce4c281f32d6d05aeb12039 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 9 Apr 2024 09:35:27 +0300 Subject: [PATCH 059/111] add Intel Copyright --- Intel Copyright | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 Intel Copyright diff --git a/Intel Copyright b/Intel Copyright new file mode 100644 index 0000000000..7cb7ff0ca4 --- /dev/null +++ b/Intel Copyright @@ -0,0 +1,7 @@ +Copyright (2017-2024), Intel Corporation. +This "Software" is furnished under license and may only be used or copied in accordance with the terms of that license. +No license, express or implied, by estoppel or otherwise, to any intellectual property rights is granted by this document. +The Software is subject to change without notice, and should not be construed as a commitment by Intel Corporation to market, license, sell or support any product or technology. +Unless otherwise provided for in the license under which this Software is provided, the Software is provided AS IS, with no warranties of any kind, express or implied. +Except as expressly permitted by the Software license, neither Intel Corporation nor its suppliers assumes any responsibility or liability for any errors or inaccuracies that may appear herein. +Except as expressly permitted by the Software license, no part of the Software may be reproduced, stored in a retrieval system, transmitted in any form, or distributed by any means without the express written consent of Intel Corporation. From 20ec87c47a415fb95a2f63b9308e57d974b06e3b Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Wed, 17 Apr 2024 13:51:39 +0300 Subject: [PATCH 060/111] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 529ae1fbc0..1400b4fab6 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@

- ROS2 packages for using Intel RealSense D400 cameras.
+ ROS Wrapper for Intel(R) RealSense(TM) Cameras
Latest release notes

From 2dac6177f3196f7dc9eb4d34cee76866b902b6c0 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 27 Mar 2024 17:48:58 +0530 Subject: [PATCH 061/111] Updated pr_check.sh script to not verify the year --- scripts/pr_check.sh | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/scripts/pr_check.sh b/scripts/pr_check.sh index 837be9f058..cea49c9bb5 100755 --- a/scripts/pr_check.sh +++ b/scripts/pr_check.sh @@ -1,6 +1,6 @@ #!/bin/bash -# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2024 Intel Corporation. All Rights Reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -29,6 +29,8 @@ fixed=0 license_file=$PWD/../LICENSE +year_format="[[:digit:]][[:digit:]][[:digit:]][[:digit:]]" + function check_folder { for filename in $(find $1 -type f \( -iname \*.cpp -o -iname \*.h -o -iname \*.hpp -o -iname \*.js -o -iname \*.bat -o -iname \*.sh -o -iname \*.txt -o -iname \*.py \)); do @@ -43,7 +45,8 @@ function check_folder { if [[ ! $filename == *"usbhost"* ]]; then # Only check files that are not .gitignore-d if [[ $(git check-ignore $filename | wc -l) -eq 0 ]]; then - if [[ $(grep -oP "Copyright 2023 Intel Corporation. All Rights Reserved" $filename | wc -l) -eq 0 || + if [[ $(grep -oP "Copyright $year_format Intel Corporation. All Rights Reserved" $filename | wc -l) -eq 0 && + $(grep -oP "Copyright $year_format-$year_format Intel Corporation. All Rights Reserved" $filename | wc -l) -eq 0 || $(grep -oP "Licensed under the Apache License, Version 2.0" $filename | wc -l) -eq 0 ]]; then echo "[ERROR] $filename is missing the copyright/license notice" From 4acf08d68e6e014d6783cdc9306a903ef62022ec Mon Sep 17 00:00:00 2001 From: anisotropicity <51104946+anisotropicity@users.noreply.github.com> Date: Sat, 11 May 2024 14:12:14 +1000 Subject: [PATCH 062/111] Update rs_launch.py to add depth_module.color_profile Fix for not being able to set the color profile for D405 cameras. See https://github.com/IntelRealSense/realsense-ros/issues/3090 --- realsense2_camera/launch/rs_launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index bdbdfd0ac8..0048176e4c 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -47,6 +47,7 @@ {'name': 'depth_module.infra_format', 'default': 'RGB8', 'description': 'infra0 stream format'}, {'name': 'depth_module.infra1_format', 'default': 'Y8', 'description': 'infra1 stream format'}, {'name': 'depth_module.infra2_format', 'default': 'Y8', 'description': 'infra2 stream format'}, + {'name': 'depth_module.color_profile', 'default': '0,0,0', 'description': 'Depth module color stream profile'}, {'name': 'depth_module.exposure', 'default': '8500', 'description': 'Depth module manual exposure value'}, {'name': 'depth_module.gain', 'default': '16', 'description': 'Depth module manual gain value'}, {'name': 'depth_module.hdr_enabled', 'default': 'false', 'description': 'Depth module hdr enablement flag. Used for hdr_merge filter'}, From 21fbfd754b1ed347ea1a8323e3cbc32a22abcca6 Mon Sep 17 00:00:00 2001 From: Jiuguang Wang Date: Sat, 11 May 2024 09:21:39 -0400 Subject: [PATCH 063/111] Update CMakeLists.txt --- realsense2_camera_msgs/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/realsense2_camera_msgs/CMakeLists.txt b/realsense2_camera_msgs/CMakeLists.txt index 831cab9243..b5bcdf9ab3 100644 --- a/realsense2_camera_msgs/CMakeLists.txt +++ b/realsense2_camera_msgs/CMakeLists.txt @@ -24,6 +24,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if(POLICY CMP0148) + cmake_policy(SET CMP0148 OLD) +endif() + find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(builtin_interfaces REQUIRED) From d3650f71edd3e6f02dab2ed5e3029954a95f017a Mon Sep 17 00:00:00 2001 From: Madhukar Reddy Kadireddy Date: Thu, 9 May 2024 01:50:48 +0530 Subject: [PATCH 064/111] ROSCI infra for live camera testing --- realsense2_camera/test/live_camera/rosci.py | 189 ++++++++++++++++++++ 1 file changed, 189 insertions(+) create mode 100755 realsense2_camera/test/live_camera/rosci.py diff --git a/realsense2_camera/test/live_camera/rosci.py b/realsense2_camera/test/live_camera/rosci.py new file mode 100755 index 0000000000..5fb98c1554 --- /dev/null +++ b/realsense2_camera/test/live_camera/rosci.py @@ -0,0 +1,189 @@ +# Copyright 2024 Intel Corporation. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys, os, subprocess, re, getopt, time + +start_time = time.time() +running_on_ci = False +if 'WORKSPACE' in os.environ: + #Path for ROS-CI on Jenkins + ws_rosci = os.environ['WORKSPACE'] + sys.path.append( os.path.join( ws_rosci, 'lrs/unit-tests/py' )) + running_on_ci = True +else: + #For running this script locally + #Extract the root where both realsense-ros and librealsense are cloned + ws_local = '/'.join(os.path.abspath( __file__ ).split( os.path.sep )[0:-5]) + #expected to have 'librealsense' repo in parallel to 'realsense-ros' + assert os.path.exists( os.path.join(ws_local, 'librealsense')), f" 'librealsense' doesn't exist at {ws_local} " + sys.path.append( os.path.join( ws_local, 'librealsense/unit-tests/py' )) + +#logs are stored @ ./realsense2_camera/test/logs +logdir = os.path.join( '/'.join(os.path.abspath( __file__ ).split( os.path.sep )[0:-2]), 'logs') +dir_live_tests = os.path.dirname(__file__) + +from rspy import log, file +regex = None +hub_reset = False +handle = None +test_ran = False +device_set = list() + +def usage(): + ourname = os.path.basename( sys.argv[0] ) + print( 'Syntax: ' + ourname + ' [options] ' ) + print( 'Options:' ) + print( ' -h, --help Usage help' ) + print( ' -r, --regex Run all tests whose name matches the following regular expression' ) + print( ' e.g.: --regex test_camera_imu; -r d415_basic') + print( ' --device <> Run only on the specified device(s); list of devices separated by ',', No white spaces' ) + print( ' e.g.: --device=d455,d435i,d585 (or) --device d455,d435i,d585 ') + print( ' Note: if --device option not used, tests run on all connected devices ') + + sys.exit( 2 ) + +def command(dev_name, test=None): + cmd = ['pytest-3'] + cmd += ['-s'] + cmd += ['-m', ''.join(dev_name)] + if test: + cmd += ['-k', f'{test}'] + cmd += [''.join(dir_live_tests)] + cmd += ['--debug'] + cmd += [f'--junit-xml={logdir}/{dev_name.upper()}_pytest.xml'] + return cmd + +def run_test(cmd, test=None, dev_name=None, stdout=None, append =False): + handle = None + try: + if test: + stdout = stdout + os.sep + str(dev_name.upper()) + '_' + test + '.log' + else: + stdout = stdout + os.sep + str(dev_name.upper()) + '_' + 'full.log' + if stdout is None: + sys.stdout.flush() + elif stdout and stdout != subprocess.PIPE: + if append: + handle = open( stdout, "a" ) + handle.write( + "\n----------TEST-SEPARATOR----------\n\n" ) + handle.flush() + else: + handle = open( stdout, "w" ) + + result = subprocess.run( cmd, + stdout=handle, + stderr=subprocess.STDOUT, + universal_newlines=True, + timeout=200, + check=True ) + if not result.returncode: + log.i("---Test Passed---") + except Exception as e: + log.e("---Test Failed---") + log.w( "Error Exception:\n ",e ) + + finally: + if handle: + handle.close() + junit_xml_parsing(f'{dev_name.upper()}_pytest.xml') + +def junit_xml_parsing(xml_file): + ''' + - remove redundant hierarchy from testcase 'classname', and 'name' attributes \ + - running pytest-3 w/ --junit-xml={logdir}/{dev_name}_pytest.xml results in classname w/ \ + too long path wich is redundant. \ + - this helps in better reporting structure of test results in jenkins + ''' + import xml.etree.ElementTree as ET + global logdir + + if not os.path.isfile( os.path.join(logdir, f'{xml_file}' )): + log.e(f'{xml_file} not found, test resutls can\'t be generated') + else: + tree = ET.parse(os.path.join(logdir,xml_file)) + root = tree.getroot() + for testsuite in root.findall('testsuite'): + for testcase in testsuite.findall('testcase'): + testcase.set('classname', testcase.attrib['classname'].split('.')[-2]) + testcase.set('name', re.sub('launch_.*parameters','',testcase.attrib['name'])) + new_xml = xml_file.split('.')[0] + tree.write(f'{logdir}/{new_xml}_refined.xml') + +def find_devices_run_tests(): + global logdir, device_set + + try: + os.makedirs( logdir, exist_ok=True ) + + from rspy import devices + #Update dict '_device_by_sn' from devices module of rspy + devices.query( hub_reset = hub_reset ) + global _device_by_sn + if not devices._device_by_sn: + assert False, 'No Camera device detected!' + else: + connected_devices = [device.name for device in devices._device_by_sn.values()] + log.i('Connected devices:', connected_devices) + + testname = regex if regex else None + + if device_set: + #Loop in for user specified devices and run tests only on them + devices_not_found = [] + for device in device_set: + if device.upper() in connected_devices: + log.i('Running tests on device:', device) + cmd = command(device.lower(), testname) + run_test(cmd, testname, device, stdout=logdir, append =False) + else: + log.e('Skipping test run on device:', device, ', -- NOT found' ) + devices_not_found.append(device) + assert len(devices_not_found) == 0, f'Devices not found:{devices_not_found}' + else: + #Loop in for all connected devices and run all tests + for device in connected_devices: + log.i('Running tests on device:', device) + cmd = command(device.lower(), testname) + run_test(cmd, testname, device, stdout=logdir, append =False) + finally: + if devices.hub and devices.hub.is_connected(): + devices.hub.disable_ports() + devices.wait_until_all_ports_disabled() + devices.hub.disconnect() + if running_on_ci: + log.i("Log path- \"Build Artifacts\":/ros2/realsense_camera/test/logs ") + else: + log.i("log path:", logdir) + run_time = time.time() - start_time + log.d( "server took", run_time, "seconds" ) + +if __name__ == '__main__': + try: + opts, args = getopt.getopt( sys.argv[1:], 'hr:', longopts=['help', 'regex=', 'device=' ] ) + except getopt.GetoptError as err: + log.e( err ) + usage() + + for opt, arg in opts: + if opt in ('-h', '--help'): + usage() + elif opt in ('-r', '--regex'): + regex = arg + elif opt == '--device': + device_set = arg.split(',') + + find_devices_run_tests() + +sys.exit( 0 ) From 21efc80e5dc4f2f9bb6845ce91acf1af26067299 Mon Sep 17 00:00:00 2001 From: Madhukar Reddy Kadireddy Date: Sat, 11 May 2024 19:14:33 +0530 Subject: [PATCH 065/111] ROS live cam test fixes --- .../live_camera/test_camera_aligned_tests.py | 26 ++++++------- .../test/live_camera/test_camera_fps_tests.py | 13 +++++-- .../test/live_camera/test_d415_basic_tests.py | 37 ++++++++++++------- .../test/live_camera/test_d455_basic_tests.py | 10 +++-- .../test/utils/pytest_live_camera_utils.py | 10 ++--- 5 files changed, 59 insertions(+), 37 deletions(-) diff --git a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py index f09b8a0de6..2d6a9839db 100644 --- a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py @@ -46,8 +46,8 @@ 'device_type': 'D455', 'enable_color':'true', 'enable_depth':'true', - 'depth_module.profile':'848x480x30', - 'rgb_camera.profile':'640x480x30', + 'depth_module.depth_profile':'848x480x30', + 'rgb_camera.color_profile':'640x480x30', 'align_depth.enable':'true' } test_params_align_depth_color_d415 = { @@ -55,8 +55,8 @@ 'device_type': 'D415', 'enable_color':'true', 'enable_depth':'true', - 'depth_module.profile':'848x480x30', - 'rgb_camera.profile':'640x480x30', + 'depth_module.depth_profile':'848x480x30', + 'rgb_camera.color_profile':'640x480x30', 'align_depth.enable':'true' } ''' @@ -112,7 +112,7 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): ret = self.run_test(themes) assert ret[0], ret[1] assert self.process_data(themes) - self.set_string_param('rgb_camera.profile', '1280x720x30') + self.set_string_param('rgb_camera.color_profile', '1280x720x30') self.set_bool_param('enable_color', True) themes[0]['width'] = 1280 themes[0]['height'] = 720 @@ -132,8 +132,8 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): 'device_type': 'D455', 'enable_color':'true', 'enable_depth':'true', - 'depth_module.profile':'848x480x30', - 'rgb_camera.profile':'640x480x30', + 'depth_module.depth_profile':'848x480x30', + 'rgb_camera.color_profile':'640x480x30', 'align_depth.enable':'true' } test_params_all_profiles_d415 = { @@ -141,8 +141,8 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): 'device_type': 'D415', 'enable_color':'true', 'enable_depth':'true', - 'depth_module.profile':'848x480x30', - 'rgb_camera.profile':'640x480x30', + 'depth_module.depth_profile':'848x480x30', + 'rgb_camera.color_profile':'640x480x30', 'align_depth.enable':'true' } test_params_all_profiles_d435i = { @@ -150,8 +150,8 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): 'device_type': 'D435I', 'enable_color':'true', 'enable_depth':'true', - 'depth_module.profile':'848x480x30', - 'rgb_camera.profile':'640x480x30', + 'depth_module.depth_profile':'848x480x30', + 'rgb_camera.color_profile':'640x480x30', 'align_depth.enable':'true' } @@ -236,8 +236,8 @@ def test_camera_all_align_depth_color(self,launch_descr_with_parameters): timeout=100.0/fps #for the changes to take effect self.spin_for_time(wait_time=timeout/20) - self.set_string_param('rgb_camera.profile', color_profile) - self.set_string_param('depth_module.profile', depth_profile) + self.set_string_param('rgb_camera.color_profile', color_profile) + self.set_string_param('depth_module.depth_profile', depth_profile) self.set_bool_param('enable_color', True) self.set_bool_param('enable_color', True) self.set_bool_param('align_depth.enable', True) diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index ef67597014..eb66b71a51 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -93,13 +93,17 @@ def test_camera_test_fps(self,launch_descr_with_parameters): } ] profiles = test_profiles[params['camera_name']]['depth_test_profiles'] + self.spin_for_time(wait_time=0.5) assert self.set_bool_param('enable_color', False) + self.spin_for_time(wait_time=0.5) + for profile in profiles: print("Testing profile: ", profile) themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) - assert self.set_string_param('depth_module.profile', profile) + assert self.set_string_param('depth_module.depth_profile', profile) + self.spin_for_time(wait_time=0.5) assert self.set_bool_param('enable_depth', True) - self.spin_for_time(0.5) + self.spin_for_time(wait_time=0.5) ret = self.run_test(themes, timeout=25.0) assert ret[0], ret[1] assert self.process_data(themes) @@ -111,12 +115,15 @@ def test_camera_test_fps(self,launch_descr_with_parameters): } ] assert self.set_bool_param('enable_depth', False) + self.spin_for_time(wait_time=0.5) profiles = test_profiles[params['camera_name']]['color_test_profiles'] for profile in profiles: print("Testing profile: ", profile) themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) - assert self.set_string_param('rgb_camera.profile', profile) + assert self.set_string_param('rgb_camera.color_profile', profile) + self.spin_for_time(wait_time=0.5) assert self.set_bool_param('enable_color', True) + self.spin_for_time(wait_time=0.5) ret = self.run_test(themes, timeout=25.0) assert ret[0], ret[1] assert self.process_data(themes) diff --git a/realsense2_camera/test/live_camera/test_d415_basic_tests.py b/realsense2_camera/test/live_camera/test_d415_basic_tests.py index 7454d75a78..f397b70f9a 100644 --- a/realsense2_camera/test/live_camera/test_d415_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d415_basic_tests.py @@ -77,8 +77,6 @@ def test_D415_Change_Resolution(self,launch_descr_with_parameters): config["Infrared2"]["default_profile2"] = "1280x720x6" cap = [ - #['Infrared1', '1920x1080x25', 'Y8'], - #['Infrared1', '1920x1080x15', 'Y16'], ['Infrared', '848x100x100', 'BGRA8'], ['Infrared', '848x480x60', 'RGBA8'], ['Infrared', '640x480x60', 'RGBA8'], @@ -89,8 +87,14 @@ def test_D415_Change_Resolution(self,launch_descr_with_parameters): ['Infrared', '480x270x60', 'BGRA8'], ['Infrared', '480x270x60', 'RGB8'], ['Infrared', '424x240x60', 'BGRA8'], - + ['Infrared1', '848x100x100', 'Y8'], + ['Infrared1', '848x480x6', 'Y8'], + ['Infrared1', '1920x1080x25', 'Y16'], + ['Infrared2', '848x100x100', 'Y8'], + ['Infrared2', '848x480x6', 'Y8'], + ['Infrared2', '1920x1080x25', 'Y16'], ] + try: ''' initialize, run and check the data @@ -98,7 +102,8 @@ def test_D415_Change_Resolution(self,launch_descr_with_parameters): self.init_test("RsTest"+params['camera_name']) self.spin_for_time(wait_time=1.0) self.create_param_ifs(get_node_heirarchy(params)) - + self.spin_for_time(wait_time=1.0) + for key in cap: profile_type = key[0] profile = key[1] @@ -108,21 +113,22 @@ def test_D415_Change_Resolution(self,launch_descr_with_parameters): themes[0]['width'] = int(profile.split('x')[0]) themes[0]['height'] = int(profile.split('x')[1]) #''' + self.disable_all_streams() if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) else: self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) self.set_bool_param(config[profile_type]["param"], True) - self.disable_all_params() - #self.set_string_param("depth_profile", "640x480x6") - #self.set_bool_param("enable_depth", True) - #''' - self.spin_for_time(wait_time=0.2) + self.spin_for_time(wait_time=1.0) + self.set_string_param(config[profile_type]["profile"], profile) + self.spin_for_time(wait_time=1.0) self.set_string_param(config[profile_type]["format"], format) + self.spin_for_time(wait_time=1.0) self.set_bool_param(config[profile_type]["param"], True) + self.spin_for_time(wait_time=1.0) try: - ret = self.run_test(themes, timeout=5.0) + ret = self.run_test(themes, timeout=10.0) assert ret[0], ret[1] assert self.process_data(themes), " ".join(key) + " failed" num_passed += 1 @@ -144,13 +150,18 @@ def test_D415_Change_Resolution(self,launch_descr_with_parameters): assert False, " Tests failed" - def disable_all_params(self): - ''' + def disable_all_streams(self): + self.set_bool_param('enable_color', False) + self.spin_for_time(wait_time=1.0) self.set_bool_param('enable_depth', False) + self.spin_for_time(wait_time=1.0) self.set_bool_param('enable_infra', False) + self.spin_for_time(wait_time=1.0) self.set_bool_param('enable_infra1', False) + self.spin_for_time(wait_time=1.0) self.set_bool_param('enable_infra2', False) - ''' + self.spin_for_time(wait_time=1.0) + pass diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index 3b501e8e6c..d7e8e2d29d 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -78,13 +78,17 @@ def test_D455_Change_Resolution(self,launch_descr_with_parameters): self.init_test("RsTest"+params['camera_name']) self.wait_for_node(params['camera_name']) self.create_param_ifs(get_node_heirarchy(params)) - #assert self.set_bool_param('enable_color', False) - assert self.set_string_param('rgb_camera.profile', '640x480x30') + self.spin_for_time(0.5) + assert self.set_bool_param('enable_color', False) + self.spin_for_time(0.5) + assert self.set_string_param('rgb_camera.color_profile', '640x480x30') + self.spin_for_time(0.5) assert self.set_bool_param('enable_color', True) + self.spin_for_time(0.5) ret = self.run_test(themes) assert ret[0], ret[1] assert self.process_data(themes) - self.set_string_param('rgb_camera.profile', '1280x800x5') + self.set_string_param('rgb_camera.color_profile', '1280x800x5') self.set_bool_param('enable_color', True) themes[0]['width'] = 1280 themes[0]['height'] = 800 diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py index 64ff6eeae6..90a6a2d55e 100644 --- a/realsense2_camera/test/utils/pytest_live_camera_utils.py +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -24,11 +24,11 @@ def get_profile_config(camera_name): config = { - "Color":{"profile":"rgb_camera.profile", "format":'rgb_camera.color_format', "param":"enable_color", "topic":camera_name+'/color/image_raw',}, - "Depth":{"profile":"depth_module.profile", "format":'depth_module.depth_format', "param":"enable_depth", 'topic':camera_name+'/depth/image_rect_raw'}, - "Infrared":{"profile":"depth_module.profile", "format":'depth_module.infra_format', "param":"enable_infra", 'topic':camera_name+'/infra/image_rect_raw'}, - "Infrared1":{"profile":"depth_module.profile", "format":'depth_module.infra1_format',"param":"enable_infra1", 'topic':camera_name+'/infra/image_rect_raw'}, - "Infrared2":{"profile":"depth_module.profile", "format":'depth_module.infra2_format',"param":"enable_infra2", 'topic':camera_name+'/infra/image_rect_raw'}, + "Color":{"profile":"rgb_camera.color_profile", "format":'rgb_camera.color_format', "param":"enable_color", "topic":camera_name+'/color/image_raw',}, + "Depth":{"profile":"depth_module.depth_profile", "format":'depth_module.depth_format', "param":"enable_depth", 'topic':camera_name+'/depth/image_rect_raw'}, + "Infrared":{"profile":"depth_module.infra_profile", "format":'depth_module.infra_format', "param":"enable_infra", 'topic':camera_name+'/infra/image_rect_raw'}, + "Infrared1":{"profile":"depth_module.infra_profile", "format":'depth_module.infra1_format',"param":"enable_infra1", 'topic':camera_name+'/infra1/image_rect_raw'}, + "Infrared2":{"profile":"depth_module.infra_profile", "format":'depth_module.infra2_format',"param":"enable_infra2", 'topic':camera_name+'/infra2/image_rect_raw'}, } return config From 90294331110c066f5965d3239e9367aaf3539905 Mon Sep 17 00:00:00 2001 From: "Ortiz Cuesta, Fernando" Date: Tue, 14 May 2024 10:14:32 +0200 Subject: [PATCH 066/111] Allow hw synchronization of several realsense devices --- .../include/base_realsense_node.h | 1 + realsense2_camera/include/constants.h | 1 + realsense2_camera/include/ros_sensor.h | 4 +- realsense2_camera/launch/rs_launch.py | 1 + .../launch/rs_multi_camera_launch_sync.py | 82 +++++++++++++++++++ realsense2_camera/src/base_realsense_node.cpp | 1 + realsense2_camera/src/parameters.cpp | 5 ++ realsense2_camera/src/ros_sensor.cpp | 21 ++++- realsense2_camera/src/rs_node_setup.cpp | 2 +- 9 files changed, 115 insertions(+), 3 deletions(-) create mode 100644 realsense2_camera/launch/rs_multi_camera_launch_sync.py diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 4792f5456c..4e3c76ada3 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -318,6 +318,7 @@ namespace realsense2_camera rclcpp::Time _ros_time_base; bool _sync_frames; + int _inter_cam_sync_mode; bool _enable_rgbd; bool _is_color_enabled; bool _is_depth_enabled; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 8304dbea4a..d3688464d4 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -69,6 +69,7 @@ namespace realsense2_camera const bool ALLOW_NO_TEXTURE_POINTS = false; const bool ORDERED_PC = false; const bool SYNC_FRAMES = false; + const int _INTER_CAM_SYNC_MODE = 0; const bool ENABLE_RGBD = false; const bool PUBLISH_TF = true; diff --git a/realsense2_camera/include/ros_sensor.h b/realsense2_camera/include/ros_sensor.h index 57d224300e..b07f39941f 100644 --- a/realsense2_camera/include/ros_sensor.h +++ b/realsense2_camera/include/ros_sensor.h @@ -82,7 +82,8 @@ namespace realsense2_camera std::shared_ptr diagnostics_updater, rclcpp::Logger logger, bool force_image_default_qos = false, - bool is_rosbag_file = false); + bool is_rosbag_file = false, + int inter_cam_sync_mode = 0); ~RosSensor(); void registerSensorParameters(); bool getUpdatedProfiles(std::vector& wanted_profiles); @@ -105,6 +106,7 @@ namespace realsense2_camera void set_sensor_auto_exposure_roi(); void registerAutoExposureROIOptions(); void UpdateSequenceIdCallback(); + void set_inter_cam_sync_mode(int inter_cam_sync_mode = 0); template void set_sensor_parameter_to_ros(rs2_option option); diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index bdbdfd0ac8..d0fcd48dae 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -56,6 +56,7 @@ {'name': 'depth_module.exposure.2', 'default': '1', 'description': 'Depth module second exposure value. Used for hdr_merge filter'}, {'name': 'depth_module.gain.2', 'default': '16', 'description': 'Depth module second gain value. Used for hdr_merge filter'}, {'name': 'enable_sync', 'default': 'false', 'description': "'enable sync mode'"}, + {'name': 'inter_cam_sync_mode', 'default': "0", 'description': '[0-Default, 1-Master, 2-Slave]'}, {'name': 'enable_rgbd', 'default': 'false', 'description': "'enable rgbd topic'"}, {'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"}, {'name': 'enable_accel', 'default': 'false', 'description': "'enable accel stream'"}, diff --git a/realsense2_camera/launch/rs_multi_camera_launch_sync.py b/realsense2_camera/launch/rs_multi_camera_launch_sync.py new file mode 100644 index 0000000000..bb6a22ffe0 --- /dev/null +++ b/realsense2_camera/launch/rs_multi_camera_launch_sync.py @@ -0,0 +1,82 @@ +# Copyright 2023 Intel Corporation. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# DESCRIPTION # +# ----------- # +# Use this launch file to launch 2 devices and enable the hardware synchronization. +# As describe in https://dev.intelrealsense.com/docs/multiple-depth-cameras-configuration both devices +# have to be connected using a sync cable. The devices will by default stream asynchronously. +# Using this launch file one device will operate as master and the other as slave. As a result they will +# capture at exactly the same time and rate. +# command line example: (to be adapted with the serial numbers or the used devices) +# ros2 launch realsense2_camera rs_multi_camera_launch_sync.py camera_name1:=cam_1 camera_name2:=cam_2 camera_namespace1:=camera camera_namespace2:=camera serial_no1:="'_031422250097'" serial_no2:="'_336222301921'" + +"""Launch realsense2_camera node.""" +import copy +from launch import LaunchDescription, LaunchContext +import launch_ros.actions +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir +from launch.launch_description_sources import PythonLaunchDescriptionSource +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.absolute())) +import rs_launch + +local_parameters = [{'name': 'camera_name1', 'default': 'camera1', 'description': 'camera1 unique name'}, + {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera2 unique name'}, + {'name': 'camera_namespace1', 'default': 'camera1', 'description': 'camera1 namespace'}, + {'name': 'camera_namespace2', 'default': 'camera2', 'description': 'camera2 namespace'}, + {'name': 'inter_cam_sync_mode1', 'default': '1', 'description': 'master'}, + {'name': 'inter_cam_sync_mode2', 'default': '2', 'description': 'slave'}, + ] + + +def set_configurable_parameters(local_params): + return dict([(param['original_name'], LaunchConfiguration(param['name'])) for param in local_params]) + +def duplicate_params(general_params, posix): + local_params = copy.deepcopy(general_params) + for param in local_params: + param['original_name'] = param['name'] + param['name'] += posix + return local_params + +def launch_static_transform_publisher_node(context : LaunchContext): + # dummy static transformation from camera1 to camera2 + node = launch_ros.actions.Node( + package = "tf2_ros", + executable = "static_transform_publisher", + arguments = ["0", "0", "0", "0", "0", "0", + context.launch_configurations['camera_name1'] + "_link", + context.launch_configurations['camera_name2'] + "_link"] + ) + return [node] + +def generate_launch_description(): + params1 = duplicate_params(rs_launch.configurable_parameters, '1') + params2 = duplicate_params(rs_launch.configurable_parameters, '2') + return LaunchDescription( + rs_launch.declare_configurable_parameters(local_parameters) + + rs_launch.declare_configurable_parameters(params1) + + rs_launch.declare_configurable_parameters(params2) + + [ + OpaqueFunction(function=rs_launch.launch_setup, + kwargs = {'params' : set_configurable_parameters(params1), + 'param_name_suffix': '1'}), + OpaqueFunction(function=rs_launch.launch_setup, + kwargs = {'params' : set_configurable_parameters(params2), + 'param_name_suffix': '2'}), + OpaqueFunction(function=launch_static_transform_publisher_node) + ]) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 4b88ec7df1..c5a7e7e24f 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -110,6 +110,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_initialized_time_base(false), _camera_time_base(0), _sync_frames(SYNC_FRAMES), + _inter_cam_sync_mode(_INTER_CAM_SYNC_MODE), _enable_rgbd(ENABLE_RGBD), _is_color_enabled(false), _is_depth_enabled(false), diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index dda0b6133e..b91d3531e2 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -47,6 +47,11 @@ void BaseRealSenseNode::getParameters() _parameters->setParamT(param_name, _sync_frames); _parameters_names.push_back(param_name); + param_name = std::string("inter_cam_sync_mode"); + _parameters->setParamT(param_name, _inter_cam_sync_mode); + _parameters_names.push_back(param_name); + + param_name = std::string("enable_rgbd"); _parameters->setParamT(param_name, _enable_rgbd, [this](const rclcpp::Parameter& ) { diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index f0a5aa5331..21e5f174e3 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -46,7 +46,8 @@ RosSensor::RosSensor(rs2::sensor sensor, std::shared_ptr diagnostics_updater, rclcpp::Logger logger, bool force_image_default_qos, - bool is_rosbag_file): + bool is_rosbag_file, + int inter_cam_sync_mode): rs2::sensor(sensor), _logger(logger), _origin_frame_callback(frame_callback), @@ -75,6 +76,7 @@ RosSensor::RosSensor(rs2::sensor sensor, } }; setParameters(is_rosbag_file); + set_inter_cam_sync_mode(inter_cam_sync_mode); } RosSensor::~RosSensor() @@ -108,6 +110,23 @@ void RosSensor::setParameters(bool is_rosbag_file) registerSensorParameters(); } +void RosSensor::set_inter_cam_sync_mode(int inter_cam_sync_mode) +{ + std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); + + // Only depth_module supports the option RS2_OPTION_INTER_CAM_SYNC_MODE, modules rgb_camera and motion_module + // throw an error when trying to set the option RS2_OPTION_INTER_CAM_SYNC_MODE + if (module_name == std::string("depth_module")){ + if ((0 <= inter_cam_sync_mode) && (2 >= inter_cam_sync_mode)) + { + ROS_INFO_STREAM("Set Ros param depth_module.inter_cam_sync_mode to " << inter_cam_sync_mode << " (0=Default, 1=Master and 2=slave)"); + set_option(RS2_OPTION_INTER_CAM_SYNC_MODE, inter_cam_sync_mode); + } else { + ROS_ERROR_STREAM("Ros param depth_module.inter_cam_sync_mode was not set. Given value "<< inter_cam_sync_mode << " is invalid"); + } + } +} + void RosSensor::UpdateSequenceIdCallback() { // Function replaces the trivial parameter callback with one that diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 191644b83e..0f5712ec82 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -159,7 +159,7 @@ void BaseRealSenseNode::setAvailableSensors() sensor.is()) { ROS_DEBUG_STREAM("Set " << module_name << " as VideoSensor."); - rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is()); + rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is(), _inter_cam_sync_mode); } else if (sensor.is()) { From 41d9bab5ede40903fc49d5a0e52fa5420cd9fc00 Mon Sep 17 00:00:00 2001 From: "Ortiz Cuesta, Fernando" Date: Tue, 14 May 2024 16:31:10 +0200 Subject: [PATCH 067/111] Revert changes --- .../include/base_realsense_node.h | 1 - realsense2_camera/include/constants.h | 1 - realsense2_camera/include/ros_sensor.h | 4 +--- realsense2_camera/src/base_realsense_node.cpp | 1 - realsense2_camera/src/parameters.cpp | 5 ----- realsense2_camera/src/ros_sensor.cpp | 21 +------------------ realsense2_camera/src/rs_node_setup.cpp | 2 +- 7 files changed, 3 insertions(+), 32 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 4e3c76ada3..4792f5456c 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -318,7 +318,6 @@ namespace realsense2_camera rclcpp::Time _ros_time_base; bool _sync_frames; - int _inter_cam_sync_mode; bool _enable_rgbd; bool _is_color_enabled; bool _is_depth_enabled; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index d3688464d4..8304dbea4a 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -69,7 +69,6 @@ namespace realsense2_camera const bool ALLOW_NO_TEXTURE_POINTS = false; const bool ORDERED_PC = false; const bool SYNC_FRAMES = false; - const int _INTER_CAM_SYNC_MODE = 0; const bool ENABLE_RGBD = false; const bool PUBLISH_TF = true; diff --git a/realsense2_camera/include/ros_sensor.h b/realsense2_camera/include/ros_sensor.h index b07f39941f..57d224300e 100644 --- a/realsense2_camera/include/ros_sensor.h +++ b/realsense2_camera/include/ros_sensor.h @@ -82,8 +82,7 @@ namespace realsense2_camera std::shared_ptr diagnostics_updater, rclcpp::Logger logger, bool force_image_default_qos = false, - bool is_rosbag_file = false, - int inter_cam_sync_mode = 0); + bool is_rosbag_file = false); ~RosSensor(); void registerSensorParameters(); bool getUpdatedProfiles(std::vector& wanted_profiles); @@ -106,7 +105,6 @@ namespace realsense2_camera void set_sensor_auto_exposure_roi(); void registerAutoExposureROIOptions(); void UpdateSequenceIdCallback(); - void set_inter_cam_sync_mode(int inter_cam_sync_mode = 0); template void set_sensor_parameter_to_ros(rs2_option option); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index c5a7e7e24f..4b88ec7df1 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -110,7 +110,6 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_initialized_time_base(false), _camera_time_base(0), _sync_frames(SYNC_FRAMES), - _inter_cam_sync_mode(_INTER_CAM_SYNC_MODE), _enable_rgbd(ENABLE_RGBD), _is_color_enabled(false), _is_depth_enabled(false), diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index b91d3531e2..dda0b6133e 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -47,11 +47,6 @@ void BaseRealSenseNode::getParameters() _parameters->setParamT(param_name, _sync_frames); _parameters_names.push_back(param_name); - param_name = std::string("inter_cam_sync_mode"); - _parameters->setParamT(param_name, _inter_cam_sync_mode); - _parameters_names.push_back(param_name); - - param_name = std::string("enable_rgbd"); _parameters->setParamT(param_name, _enable_rgbd, [this](const rclcpp::Parameter& ) { diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 21e5f174e3..f0a5aa5331 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -46,8 +46,7 @@ RosSensor::RosSensor(rs2::sensor sensor, std::shared_ptr diagnostics_updater, rclcpp::Logger logger, bool force_image_default_qos, - bool is_rosbag_file, - int inter_cam_sync_mode): + bool is_rosbag_file): rs2::sensor(sensor), _logger(logger), _origin_frame_callback(frame_callback), @@ -76,7 +75,6 @@ RosSensor::RosSensor(rs2::sensor sensor, } }; setParameters(is_rosbag_file); - set_inter_cam_sync_mode(inter_cam_sync_mode); } RosSensor::~RosSensor() @@ -110,23 +108,6 @@ void RosSensor::setParameters(bool is_rosbag_file) registerSensorParameters(); } -void RosSensor::set_inter_cam_sync_mode(int inter_cam_sync_mode) -{ - std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); - - // Only depth_module supports the option RS2_OPTION_INTER_CAM_SYNC_MODE, modules rgb_camera and motion_module - // throw an error when trying to set the option RS2_OPTION_INTER_CAM_SYNC_MODE - if (module_name == std::string("depth_module")){ - if ((0 <= inter_cam_sync_mode) && (2 >= inter_cam_sync_mode)) - { - ROS_INFO_STREAM("Set Ros param depth_module.inter_cam_sync_mode to " << inter_cam_sync_mode << " (0=Default, 1=Master and 2=slave)"); - set_option(RS2_OPTION_INTER_CAM_SYNC_MODE, inter_cam_sync_mode); - } else { - ROS_ERROR_STREAM("Ros param depth_module.inter_cam_sync_mode was not set. Given value "<< inter_cam_sync_mode << " is invalid"); - } - } -} - void RosSensor::UpdateSequenceIdCallback() { // Function replaces the trivial parameter callback with one that diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 0f5712ec82..191644b83e 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -159,7 +159,7 @@ void BaseRealSenseNode::setAvailableSensors() sensor.is()) { ROS_DEBUG_STREAM("Set " << module_name << " as VideoSensor."); - rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is(), _inter_cam_sync_mode); + rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is()); } else if (sensor.is()) { From a6c90ff36e0eed86ba600c1d69030a993df3da09 Mon Sep 17 00:00:00 2001 From: "Ortiz Cuesta, Fernando" Date: Tue, 14 May 2024 16:32:32 +0200 Subject: [PATCH 068/111] Expose depth_module.inter_cam_sync_mode in launch files --- realsense2_camera/launch/rs_launch.py | 2 +- realsense2_camera/launch/rs_multi_camera_launch_sync.py | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index d0fcd48dae..7f5202c811 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -56,7 +56,7 @@ {'name': 'depth_module.exposure.2', 'default': '1', 'description': 'Depth module second exposure value. Used for hdr_merge filter'}, {'name': 'depth_module.gain.2', 'default': '16', 'description': 'Depth module second gain value. Used for hdr_merge filter'}, {'name': 'enable_sync', 'default': 'false', 'description': "'enable sync mode'"}, - {'name': 'inter_cam_sync_mode', 'default': "0", 'description': '[0-Default, 1-Master, 2-Slave]'}, + {'name': 'depth_module.inter_cam_sync_mode', 'default': "0", 'description': '[0-Default, 1-Master, 2-Slave]'}, {'name': 'enable_rgbd', 'default': 'false', 'description': "'enable rgbd topic'"}, {'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"}, {'name': 'enable_accel', 'default': 'false', 'description': "'enable accel stream'"}, diff --git a/realsense2_camera/launch/rs_multi_camera_launch_sync.py b/realsense2_camera/launch/rs_multi_camera_launch_sync.py index bb6a22ffe0..de127d35fb 100644 --- a/realsense2_camera/launch/rs_multi_camera_launch_sync.py +++ b/realsense2_camera/launch/rs_multi_camera_launch_sync.py @@ -18,9 +18,10 @@ # As describe in https://dev.intelrealsense.com/docs/multiple-depth-cameras-configuration both devices # have to be connected using a sync cable. The devices will by default stream asynchronously. # Using this launch file one device will operate as master and the other as slave. As a result they will -# capture at exactly the same time and rate. +# capture at exactly the same time and rate. # command line example: (to be adapted with the serial numbers or the used devices) # ros2 launch realsense2_camera rs_multi_camera_launch_sync.py camera_name1:=cam_1 camera_name2:=cam_2 camera_namespace1:=camera camera_namespace2:=camera serial_no1:="'_031422250097'" serial_no2:="'_336222301921'" +# The value of the param can be checked using ros2 param get /camera/cam_1 depth_module.inter_cam_sync_mode and ros2 param get /camera/cam_2 depth_module.inter_cam_sync_mode """Launch realsense2_camera node.""" import copy @@ -38,8 +39,8 @@ {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera2 unique name'}, {'name': 'camera_namespace1', 'default': 'camera1', 'description': 'camera1 namespace'}, {'name': 'camera_namespace2', 'default': 'camera2', 'description': 'camera2 namespace'}, - {'name': 'inter_cam_sync_mode1', 'default': '1', 'description': 'master'}, - {'name': 'inter_cam_sync_mode2', 'default': '2', 'description': 'slave'}, + {'name': 'depth_module.inter_cam_sync_mode1', 'default': '1', 'description': 'master'}, + {'name': 'depth_module.inter_cam_sync_mode2', 'default': '2', 'description': 'slave'}, ] From ce00324fc71b96d75c95b4a44e917124d268742a Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Tue, 21 May 2024 17:02:12 +0300 Subject: [PATCH 069/111] Update profile_manager.h --- realsense2_camera/include/profile_manager.h | 1 - 1 file changed, 1 deletion(-) diff --git a/realsense2_camera/include/profile_manager.h b/realsense2_camera/include/profile_manager.h index a63cc7f3b0..736c440f39 100644 --- a/realsense2_camera/include/profile_manager.h +++ b/realsense2_camera/include/profile_manager.h @@ -84,7 +84,6 @@ namespace realsense2_camera std::string _module_name; std::map _formats; std::map _fps, _width, _height; - bool _is_profile_exist; bool _force_image_default_qos; }; From 92028df672f183f12695663c35acb1890ec66654 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 28 May 2024 20:58:11 +0000 Subject: [PATCH 070/111] bump version to 4.55.1 --- realsense2_camera/CHANGELOG.rst | 50 +++++++++++++++++++++++++++ realsense2_camera/CMakeLists.txt | 4 +-- realsense2_camera/include/constants.h | 2 +- realsense2_camera_msgs/CHANGELOG.rst | 5 +++ realsense2_description/CHANGELOG.rst | 8 +++++ 5 files changed, 66 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/CHANGELOG.rst b/realsense2_camera/CHANGELOG.rst index a1923a8e60..0e7373f274 100644 --- a/realsense2_camera/CHANGELOG.rst +++ b/realsense2_camera/CHANGELOG.rst @@ -2,6 +2,56 @@ Changelog for package realsense2_camera ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* PR `#3106 `_ from SamerKhshiboun: Remove unused parameter _is_profile_exist +* PR `#3098 `_ from kadiredd: ROS live cam test fixes +* PR `#3094 `_ from kadiredd: ROSCI infra for live camera testing +* PR `#3066 `_ from SamerKhshiboun: Revert Foxy Build Support (From Source) +* PR `#3052 `_ from Arun-Prasad-V: Support for selecting profile for each stream_type +* PR `#3056 `_ from SamerKhshiboun: Add documentation for RealSense ROS2 Wrapper Windows installation +* PR `#3049 `_ from Arun-Prasad-V: Applying Colorizer filter to Aligned-Depth image +* PR `#3053 `_ from Nir-Az: Fix Coverity issues + remove empty warning log +* PR `#3007 `_ from Arun-Prasad-V: Skip updating Exp 1,2 & Gain 1,2 when HDR is disabled +* PR `#3042 `_ from kadiredd: Assert Fail if camera not found +* PR `#3008 `_ from Arun-Prasad-V: Renamed GL GPU enable param +* PR `#2989 `_ from Arun-Prasad-V: Dynamically switching b/w CPU & GPU processing +* PR `#3001 `_ from deep0294: Update ReadMe to run ROS2 Unit Test +* PR `#2998 `_ from SamerKhshiboun: fix calibration intrinsic fail +* PR `#2987 `_ from SamerKhshiboun: Remove D465 SKU +* PR `#2984 `_ from deep0294: Fix All Profiles Test +* PR `#2956 `_ from Arun-Prasad-V: Extending LibRS's GL support to RS ROS2 +* PR `#2953 `_ from Arun-Prasad-V: Added urdf & mesh files for D405 model +* PR `#2940 `_ from Arun-Prasad-V: Fixing the data_type of ROS Params exposure & gain +* PR `#2948 `_ from Arun-Prasad-V: Disabling HDR during INIT +* PR `#2934 `_ from Arun-Prasad-V: Disabling hdr while updating exposure & gain values +* PR `#2946 `_ from gwen2018: fix ros random crash with error hw monitor command for asic temperature failed +* PR `#2865 `_ from PrasRsRos: add live camera tests +* PR `#2891 `_ from Arun-Prasad-V: revert PR2872 +* PR `#2853 `_ from Arun-Prasad-V: Frame latency for the '/topic' provided by user +* PR `#2872 `_ from Arun-Prasad-V: Updating _camera_name with RS node's name +* PR `#2878 `_ from Arun-Prasad-V: Updated ros2 examples and readme +* PR `#2841 `_ from SamerKhshiboun: Remove Dashing, Eloquent, Foxy, L500 and SR300 support +* PR `#2868 `_ from Arun-Prasad-V: Fix Pointcloud topic frame_id +* PR `#2849 `_ from Arun-Prasad-V: Create /imu topic only when motion streams enabled +* PR `#2847 `_ from Arun-Prasad-V: Updated rs_launch param names +* PR `#2839 `_ from Arun-Prasad: Added ros2 examples +* PR `#2861 `_ from SamerKhshiboun: fix readme and nodefactory for ros2 run +* PR `#2859 `_ from PrasRsRos: Fix tests (topic now has camera name) +* PR `#2857 `_ from lge-ros2: Apply camera name in topics +* PR `#2840 `_ from SamerKhshiboun: Support Depth, IR and Color formats in ROS2 +* PR `#2764 `_ from lge-ros2 : support modifiable camera namespace +* PR `#2830 `_ from SamerKhshiboun: Add RGBD + reduce changes between hkr and development +* PR `#2811 `_ from Arun-Prasad-V: Exposing stream formats params to user +* PR `#2825 `_ from SamerKhshiboun: Fix align_depth + add test +* PR `#2822 `_ from Arun-Prasad-V: Updated rs_launch configurations +* PR `#2726 `_ from PrasRsRos: Integration test template +* PR `#2742 `_ from danielhonies:Update rs_launch.py +* PR `#2806 `_ from Arun-Prasad-V: Enabling RGB8 Infrared stream +* PR `#2799 `_ from SamerKhshiboun: Fix overriding frames on same topics/CV-images due to a bug in PR2759 +* PR `#2759 `_ from SamerKhshiboun: Cleanups and name fixes +* Contributors: (=YG=) Hyunseok Yang, Arun Prasad, Arun-Prasad-V, Daniel Honies, Hyunseok, Madhukar Reddy Kadireddy, Nir, Nir Azkiel, PrasRsRos, Samer Khshiboun, SamerKhshiboun, deep0294, gwen2018, nairps + 4.54.1 (2023-06-27) ------------------- * Applying AlignDepth filter after Pointcloud diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index a838364f8d..0d6d14475d 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -117,9 +117,9 @@ find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core) -find_package(realsense2 2.54.1) +find_package(realsense2 2.55.1) if (BUILD_ACCELERATE_GPU_WITH_GLSL) - find_package(realsense2-gl 2.54.1) + find_package(realsense2-gl 2.55.1) endif() if(NOT realsense2_FOUND) message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n") diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 8304dbea4a..80104e46c3 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -18,7 +18,7 @@ #include #define REALSENSE_ROS_MAJOR_VERSION 4 -#define REALSENSE_ROS_MINOR_VERSION 54 +#define REALSENSE_ROS_MINOR_VERSION 55 #define REALSENSE_ROS_PATCH_VERSION 1 #define STRINGIFY(arg) #arg diff --git a/realsense2_camera_msgs/CHANGELOG.rst b/realsense2_camera_msgs/CHANGELOG.rst index 5f1c979598..bee90a6767 100644 --- a/realsense2_camera_msgs/CHANGELOG.rst +++ b/realsense2_camera_msgs/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package realsense2_camera_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* PR `#2830 `_ from SamerKhshiboun: Add RGBD + reduce changes between hkr and development +* Contributors: SamerKhshiboun + 4.54.1 (2023-06-27) ------------------- * add info about extrinsic msg format in Extrinsics.msg and README.md diff --git a/realsense2_description/CHANGELOG.rst b/realsense2_description/CHANGELOG.rst index 6580fcf252..9747693a36 100644 --- a/realsense2_description/CHANGELOG.rst +++ b/realsense2_description/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package realsense2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* PR `#2957 `_ from hellototoro: to_urdf fun retrun a str, not a BufferedRandom +* PR `#2953 `_ from Arun-Prasad-V: Added urdf & mesh files for D405 model +* PR `#2841 `_ from SamerKhshiboun: Remove Dashing, Eloquent, Foxy, L500 and SR300 support +* PR `#2817 `_ from karina-ranadive: Replaced Deprecated function mktemp to TemporaryFile +* Contributors: Arun-Prasad-V, karina-ranadive, SamerKhshiboun, hellototoro + 4.54.1 (2023-06-27) ------------------- * Update mesh path From 8a86cb88a428bdefa204759c899b84adc81606ae Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 28 May 2024 20:58:27 +0000 Subject: [PATCH 071/111] 4.55.1 --- realsense2_camera/CHANGELOG.rst | 4 ++-- realsense2_camera/package.xml | 2 +- realsense2_camera_msgs/CHANGELOG.rst | 4 ++-- realsense2_camera_msgs/package.xml | 2 +- realsense2_description/CHANGELOG.rst | 4 ++-- realsense2_description/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/realsense2_camera/CHANGELOG.rst b/realsense2_camera/CHANGELOG.rst index 0e7373f274..9098646e47 100644 --- a/realsense2_camera/CHANGELOG.rst +++ b/realsense2_camera/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package realsense2_camera ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.55.1 (2024-05-28) +------------------- * PR `#3106 `_ from SamerKhshiboun: Remove unused parameter _is_profile_exist * PR `#3098 `_ from kadiredd: ROS live cam test fixes * PR `#3094 `_ from kadiredd: ROSCI infra for live camera testing diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 9ff26e3d55..d2758d223a 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -2,7 +2,7 @@ realsense2_camera - 4.54.1 + 4.55.1 RealSense camera package allowing access to Intel D400 3D cameras LibRealSense ROS Team Apache License 2.0 diff --git a/realsense2_camera_msgs/CHANGELOG.rst b/realsense2_camera_msgs/CHANGELOG.rst index bee90a6767..8a9d9a5778 100644 --- a/realsense2_camera_msgs/CHANGELOG.rst +++ b/realsense2_camera_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package realsense2_camera_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.55.1 (2024-05-28) +------------------- * PR `#2830 `_ from SamerKhshiboun: Add RGBD + reduce changes between hkr and development * Contributors: SamerKhshiboun diff --git a/realsense2_camera_msgs/package.xml b/realsense2_camera_msgs/package.xml index a9a3daa9e3..acaa572626 100644 --- a/realsense2_camera_msgs/package.xml +++ b/realsense2_camera_msgs/package.xml @@ -2,7 +2,7 @@ realsense2_camera_msgs - 4.54.1 + 4.55.1 RealSense camera_msgs package containing realsense camera messages definitions LibRealSense ROS Team Apache License 2.0 diff --git a/realsense2_description/CHANGELOG.rst b/realsense2_description/CHANGELOG.rst index 9747693a36..40a0ccae03 100644 --- a/realsense2_description/CHANGELOG.rst +++ b/realsense2_description/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package realsense2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.55.1 (2024-05-28) +------------------- * PR `#2957 `_ from hellototoro: to_urdf fun retrun a str, not a BufferedRandom * PR `#2953 `_ from Arun-Prasad-V: Added urdf & mesh files for D405 model * PR `#2841 `_ from SamerKhshiboun: Remove Dashing, Eloquent, Foxy, L500 and SR300 support diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index bc9f573032..6104a506a9 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -2,7 +2,7 @@ realsense2_description - 4.54.1 + 4.55.1 RealSense description package for Intel 3D D400 cameras LibRealSense ROS Team Apache License 2.0 From 8b5207a53be58ff8799bff669d8e8edded4e8a02 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 7 Jun 2024 11:29:55 +0530 Subject: [PATCH 072/111] Ubuntu 24.04 support for Rolling and Jazzy --- .github/workflows/main.yml | 28 ++++++++++++------- realsense2_camera/CMakeLists.txt | 6 +++- .../include/base_realsense_node.h | 2 +- 3 files changed, 24 insertions(+), 12 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 49eeebd637..aa4d904ac9 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -25,16 +25,18 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [rolling, iron, humble, foxy] + ros_distro: [rolling, iron, humble, foxy, jazzy] include: - ros_distro: 'rolling' - os: ubuntu-22.04 + os: ubuntu-24.04 - ros_distro: 'iron' os: ubuntu-22.04 - ros_distro: 'humble' os: ubuntu-22.04 - ros_distro: 'foxy' os: ubuntu-20.04 + - ros_distro: 'jazzy' + os: ubuntu-24.04 steps: @@ -53,14 +55,14 @@ jobs: ./pr_check.sh # setup-ros@v0.6 is the last version supporting foxy (EOL) - # setup-ros@v0.7 is needed to support humble/iron/rolling + # setup-ros@v0.7 is needed to support humble/iron/rolling/jazzy # so, seperating steps with if conditions - name: build ROS2 for foxy if: ${{ matrix.ros_distro == 'foxy' }} uses: ros-tooling/setup-ros@v0.6 with: required-ros-distributions: ${{ matrix.ros_distro }} - - name: build ROS2 for humble/iron/rolling + - name: build ROS2 for humble/iron/rolling/jazzy if: ${{ matrix.ros_distro != 'foxy' }} uses: ros-tooling/setup-ros@v0.7 with: @@ -90,15 +92,14 @@ jobs: source ${{github.workspace}}/.bashrc cd ${{github.workspace}}/ros2 echo "================= ROSDEP UPDATE =====================" - # temp fix for rolling sources.. TODO: track when we can remove the two commands below - # see https://discourse.ros.org/t/psa-rolling-ci-or-docker-build-fix-from-rosdep-errors-in-24-04-transition/36902 - sudo sed -i "s|ros\/rosdistro\/master|ros\/rosdistro\/rolling\/2024-02-28|" /etc/ros/rosdep/sources.list.d/20-default.list - export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2024-02-28/index-v4.yaml rosdep update --rosdistro ${{ matrix.ros_distro }} --include-eol-distros echo "================= ROSDEP INSTALL ====================" rosdep install -i --reinstall --from-path src --rosdistro ${{ matrix.ros_distro }} --skip-keys=librealsense2 -y echo "================== COLCON BUILD ======================" - colcon build --cmake-args '-DBUILD_TOOLS=ON' + # Enable 'BUILD_TOOLS' through cmake arguments. Since, this variable is available only in realsense2_camera package + # and not in realsense2_camera_msgs and realsense2_description packages, to avoid warnings from these packages, + # use '--no-warn-unused-cli'. Ref: https://cmake.org/cmake/help/v3.0/manual/cmake.1.html + colcon build --cmake-args '-DBUILD_TOOLS=ON' --no-warn-unused-cli ## This step is commented out since we don't use rosbag files in "Run Tests" step below. ## Please uncomment when "Run Tests" step is fixed to run all tests. @@ -114,9 +115,15 @@ jobs: - name: Install Packages For Tests run: | + # To avoid mixing of 'apt' provided packages and 'pip' provided packages, one way is to create virtual env + # and manage python packages within it. Ref: https://peps.python.org/pep-0668/ + python3 -m venv .venv + # Activate the virtual env such that following python related commands run within it. + source .venv/bin/activate sudo apt-get install python3-pip pip3 install numpy --upgrade - pip3 install numpy-quaternion tqdm + pip3 install numpy-quaternion tqdm pyyaml + - name: Run Tests run: | @@ -126,6 +133,7 @@ jobs: # the next command might be needed for foxy distro, since this package is not installed # by default in ubuntu 20.04. For other distro, the apt install command will be ignored. sudo apt install -y ros-${{matrix.ros_distro}}-sensor-msgs-py + source ../.venv/bin/activate python3 src/realsense-ros/realsense2_camera/scripts/rs2_test.py non_existent_file # don't run integration tests for foxy since some testing dependecies packages like diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 0d6d14475d..d888a3a8c5 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2024 Intel Corporation. All Rights Reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -170,6 +170,10 @@ elseif("$ENV{ROS_DISTRO}" STREQUAL "rolling") message(STATUS "Build for ROS2 Rolling") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DROLLING") set(SOURCES "${SOURCES}" src/ros_param_backend.cpp) +elseif("$ENV{ROS_DISTRO}" STREQUAL "jazzy") + message(STATUS "Build for ROS2 Jazzy") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DJAZZY") + set(SOURCES "${SOURCES}" src/ros_param_backend.cpp) else() message(FATAL_ERROR "Unsupported ROS Distribution: " "$ENV{ROS_DISTRO}") endif() diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 4792f5456c..24c2ce941f 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -173,7 +173,7 @@ namespace realsense2_camera class CimuData { public: - CimuData() : m_time_ns(-1) {}; + CimuData() : m_data({0,0,0}), m_time_ns(-1) {}; CimuData(const stream_index_pair type, Eigen::Vector3d data, double time): m_type(type), m_data(data), From b76fae6196e7e9ff28234f14f1afa090a620283e Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Tue, 11 Jun 2024 22:30:11 +0300 Subject: [PATCH 073/111] Update README.md --- README.md | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 1400b4fab6..40a4bd2c74 100644 --- a/README.md +++ b/README.md @@ -631,8 +631,12 @@ Each of the above filters have it's own parameters, following the naming convent
## Available services - -- device_info : retrieve information about the device - serial_number, firmware_version etc. Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. Call example: `ros2 service call /camera/camera/device_info realsense2_camera_msgs/srv/DeviceInfo` + +### device_info : + - retrieve information about the device - serial_number, firmware_version etc. + - Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. + - Call example: `ros2 service call /camera/camera/device_info realsense2_camera_msgs/srv/DeviceInfo` +
From d1684b4e53111f5eb3237f76299e83a864ce929d Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Tue, 11 Jun 2024 22:31:08 +0300 Subject: [PATCH 074/111] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 40a4bd2c74..dff65f6105 100644 --- a/README.md +++ b/README.md @@ -632,7 +632,7 @@ Each of the above filters have it's own parameters, following the naming convent ## Available services -### device_info : +### device_info: - retrieve information about the device - serial_number, firmware_version etc. - Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. - Call example: `ros2 service call /camera/camera/device_info realsense2_camera_msgs/srv/DeviceInfo` From 5c298f79d9f2017d8089e4b14d9a6089ba36e5e3 Mon Sep 17 00:00:00 2001 From: Madhukar Reddy Kadireddy Date: Thu, 13 Jun 2024 18:54:31 +0530 Subject: [PATCH 075/111] [ROS][Test Infra] Support testing ROS2 service call device_info --- realsense2_camera/test/README.md | 17 +++-- .../live_camera/test_camera_aligned_tests.py | 4 +- .../test_camera_all_profile_tests.py | 2 +- .../test/live_camera/test_camera_fps_tests.py | 5 +- .../test/live_camera/test_camera_imu_tests.py | 2 +- .../test_camera_point_cloud_tests.py | 2 +- .../live_camera/test_camera_service_call.py | 73 +++++++++++++++++++ .../test/live_camera/test_camera_tf_tests.py | 4 +- .../test/live_camera/test_d415_basic_tests.py | 2 +- .../test/live_camera/test_d455_basic_tests.py | 4 +- .../test/utils/pytest_rs_utils.py | 41 +++++++++-- 11 files changed, 129 insertions(+), 27 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_service_call.py diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index 15b882a2f8..4e59fcb620 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -20,7 +20,7 @@ It is recommended to use the test folder itself for storing all the cpp tests. H ## Test using pytest The default folder for the test py files is realsense2_camera/test. Two test template files test_launch_template.py and test_integration_template.py are available in the same folder for reference. ### Add a new test -To add a new test, the user can create a copy of the test_launch_template.py or test_integration_template.py and start from there. Please name the file in the format test_`testname`.py so that the CMake detects the file and add as a new test. Please be aware that, multiple tests can be added into one file, the test_integration_template.py itself has more than one test.i The marker `@pytest.mark.launch` is used to specify the test entry point. +To add a new test, the user can create a copy of the test_launch_template.py or test_integration_template.py and start from there. Please name the file in the format test_`testname`.py so that the CMake detects the file and add as a new test. Please be aware that, multiple tests can be added into one file, the test_integration_template.py itself has more than one test. The marker `@pytest.mark.launch` is used to specify the test entry point. The test_launch_template.py uses the rs_launch.py to start the camera node, so this template can be used for testing the rs_launch.py together with the rs node. @@ -28,7 +28,7 @@ The test_integration_template.py gives a better control for testing, it uses few The test_integration_template.py has two types of tests, one has a function "test_using_function". If the user wants to have a better control over the launch context for any specific test scenarios, this can be used. Both the function based test and class based tests use a default launch configuration from the utils. It's recommended to modify the camera name to a unique one in the parameters itself so that there are not clashes between tests. -It is expected that the class based test is used as the test format for most of the usecases. The class based test inherits from pytest_rs_utils.RsTestBaseClass and it has three steps, namely: init, run_test and process_data. Unless for the basic tests, the user will have to override the process_data function and check if the data received from the topics are as expected. Also, if the user doesn't want the base class to modify the data, use 'store_raw_data':True in the theme definition. Please see the test_integration_template.py for reference. +It is expected that the class based test is used as the test format for most of the usecases. The class based test inherits from `pytest_rs_utils.RsTestBaseClass` and it has three steps, namely: init, run_test and process_data. Unless for the basic tests, the user will have to override the process_data function and check if the data received from the topics are as expected. Also, if the user doesn't want the base class to modify the data, use 'store_raw_data':True in the theme definition. Please see the test_integration_template.py for reference. An assert command can be used to indicate if the test failed or passed. Please see the template for more info. @@ -47,11 +47,11 @@ new_folder_for_pytest #<-- new folder #but please be aware that the utils functi ``` ### Grouping of tests -The pytests can be grouped using markers. These markers can be used to run a group of tests. However, "colcon test" command doesn't pass a custom marker using (--pytest-args -m `marker_name`) to the pytest internally. This is because, the ament_cmake that works as a bridge between colcon and pytest doesn't pass the pytest arguments to pytest. So till this is fixed, pytest command has to be used directly for running a group of tests. Please see the next session for the commands to run a group py tests. +The pytests can be grouped using markers. These markers can be used to run a group of tests. However, "colcon test" command doesn't pass a custom marker using (--pytest-args -m `marker_name`) to the pytest internally. This is because, the ament_cmake that works as a bridge between colcon and pytest doesn't pass the pytest arguments to pytest. So until this is fixed, pytest command has to be used directly for running a group of tests. Please see the next session for the commands to run a group of py tests. The grouping is specified by adding a marker just before the test declaration. In the test_integration_template.py `rosbag` is specified as a marker specify tests that use rosbag file. This is achieved by adding "@pytest.mark.rosbag" to the begining of the test. So when the pytest parses for test, it detects the marker for the test. If this marker is selected or none of the markers are specified, the test will be added to the list, else will be listed as a deselected test. -It is recommended to use markers such as ds457, rosbag, ds415 etc to differentiate the tests so that it's easier to run a group of tests in a machine that has the required hardware. +It is recommended to use markers such as d457, rosbag, or d435i etc to differentiate the tests so that it's easier to run a group of tests in a machine that has the required hardware. ## Building and running tests @@ -134,7 +134,7 @@ Then, the path to execute the tests would be ~/ros2_ws/src/realsense-ros. 2. Please setup below required environment variables. If not, Please prefix them for every test execution. - PYTHONPATH=$PYTHONPATH:$PWD/realsense2_camera/test/utils:$PWD/realsense2_camera//launch:$PWD/realsense2_camera//scripts + export PYTHONPATH=$PYTHONPATH:$PWD/realsense2_camera/test/utils:$PWD/realsense2_camera//launch:$PWD/realsense2_camera//scripts User can run all the tests in a pytest file directly using the below command: @@ -169,7 +169,7 @@ If a user wants to add a test to this conditional skip, user can add by adding a ## Points to be noted while writing pytests The tests that are in one file are normally run in parallel, there could also be changes in the pytest plugin. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. ### Passing/changing parameters -The parameters passed while creating the node can be initialized individually for each test, please see the test_parameterized_template example for reference. The default values are taken from rs_launch.py and the passed parameters are used for overriding the default values. The parameters that can be dynamically modified can be changed using the param interface provided. However, the function create_param_ifs has to be called to create this interface. Please see the test_d455_basic_tests.py for reference. There are specific functions to change the string, integer and bool parameters, the utils can be extended if any more types are needed. +The parameters passed while creating the node can be initialized individually for each test, please see the test_parameterized_template example for reference. The default values are taken from rs_launch.py and the passed parameters are used for overriding the default values. The parameters that can be dynamically modified can be changed using the param interface provided. However, the function `create_service_client_ifs()` has to be called to create this interface. Please see the test_d455_basic_tests.py for reference. There are specific functions to change the string, integer and bool parameters, the utils can be extended if any more types are needed. ### Difference in setting the bool parameters There is a difference between setting the default values of bool parameters and setting them dynamically. The bool test params are assinged withn quotes as below. @@ -183,4 +183,7 @@ The bool test params are assinged withn quotes as below. However the function that implements the setting of bool parameter dynamically takes the python bool datatype. For example: self.set_bool_param('enable_accel', False) - +### Adding 'service call' client interface +1. Create a client for respective service call in method `create_service_client_ifs()`, refer client creation for service call `device_info` with service type `DeviceInfo`. +2. Create a `get_` or `set_` method for each service call, refer `get_deviceinfo` in pytest_rs_utils.py +3. Use the `get_`, `set_` service calls as used in test_camera_service_call.py diff --git a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py index 2d6a9839db..27fc224aa4 100644 --- a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py @@ -108,7 +108,7 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): print("Starting camera test...") self.init_test("RsTest"+params['camera_name']) self.wait_for_node(params['camera_name']) - self.create_param_ifs(get_node_heirarchy(params)) + self.create_service_client_ifs(get_node_heirarchy(params)) ret = self.run_test(themes) assert ret[0], ret[1] assert self.process_data(themes) @@ -211,7 +211,7 @@ def test_camera_all_align_depth_color(self,launch_descr_with_parameters): if cap == None: debug_print("Device not found? : " + params['device_type']) return - self.create_param_ifs(get_node_heirarchy(params)) + self.create_service_client_ifs(get_node_heirarchy(params)) color_profiles = set([i[1] for i in cap["color_profile"] if i[2] == "RGB8"]) depth_profiles = set([i[1] for i in cap["depth_profile"] if i[0] == "Depth"]) for color_profile in color_profiles: diff --git a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py index ab66cf26be..27cc4a6d32 100644 --- a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py @@ -170,7 +170,7 @@ def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): if cap == None: debug_print("Device not found? : " + params['device_type']) return - self.create_param_ifs(get_node_heirarchy(params)) + self.create_service_client_ifs(get_node_heirarchy(params)) config["Color"]["default_profile1"],config["Color"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["color_profile"], "Color") config["Depth"]["default_profile1"],config["Depth"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Depth") config["Infrared"]["default_profile1"],config["Infrared"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared") diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index eb66b71a51..467a98e0f1 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -83,9 +83,8 @@ def test_camera_test_fps(self,launch_descr_with_parameters): print("Starting camera test...") self.init_test("RsTest"+params['camera_name']) self.wait_for_node(params['camera_name']) - self.create_param_ifs(get_node_heirarchy(params)) - #assert self.set_bool_param('enable_color', False) - + self.create_service_client_ifs(get_node_heirarchy(params)) + themes = [ {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', 'msg_type':msg_Image, diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index cad8e8033e..0dc16f0cf7 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -70,7 +70,7 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): try: #initialize self.init_test("RsTest"+params['camera_name']) - self.create_param_ifs(get_node_heirarchy(params)) + self.create_service_client_ifs(get_node_heirarchy(params)) #run with default params and check the data diff --git a/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py index 3f012f133b..85fc84121c 100644 --- a/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py @@ -100,7 +100,7 @@ def test_camera_test_point_cloud(self,launch_descr_with_parameters): ''' self.init_test("RsTest"+self.params['camera_name']) self.wait_for_node(self.params['camera_name']) - self.create_param_ifs(get_node_heirarchy(self.params)) + self.create_service_client_ifs(get_node_heirarchy(self.params)) ret = self.run_test(themes, timeout=10) assert ret[0], ret[1] ret = self.process_data(themes, False) diff --git a/realsense2_camera/test/live_camera/test_camera_service_call.py b/realsense2_camera/test/live_camera/test_camera_service_call.py new file mode 100644 index 0000000000..9b195064f1 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_service_call.py @@ -0,0 +1,73 @@ +# Copyright 2024 Intel Corporation. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os, sys +import pytest + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +import pytest_live_camera_utils +from pytest_rs_utils import launch_descr_with_parameters +from pytest_rs_utils import get_node_heirarchy + +test_params_test_srv_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +test_params_test_srv_d435i = { + 'camera_name': 'D435i', + 'device_type': 'D435i', + } +test_params_test_srv_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } + +''' +The test checks service call device_info with type DeviceInfo +device info includes - device name, FW version, serial number, etc +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_test_srv_d455, marks=pytest.mark.d455), + pytest.param(test_params_test_srv_d435i, marks=pytest.mark.d435i), + pytest.param(test_params_test_srv_d415, marks=pytest.mark.d415), + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_ServiceCall_DeviceInfo(pytest_rs_utils.RsTestBaseClass): + def test_camera_service_call_device_info(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + assert False + + try: + print("Starting camera test...") + self.init_test("RsTest"+params['camera_name']) + self.wait_for_node(params['camera_name']) + self.create_service_client_ifs(get_node_heirarchy(params)) + #No need to call run_test() as no frame integritiy check required + response = self.get_deviceinfo() + if response is not None: + print(f"device_info service response:\n{response}\n") + assert params['device_type'].casefold() in response.device_name.casefold().split('_') + else: + assert False, "No device_info response received" + except Exception as e: + print(e) + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py index 15edc4d6f9..9b76aae0ad 100644 --- a/realsense2_camera/test/live_camera/test_camera_tf_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -101,7 +101,7 @@ def test_camera_test_tf_static_change(self,launch_descr_with_parameters): ''' self.init_test("RsTest"+self.params['camera_name']) self.wait_for_node(self.params['camera_name']) - self.create_param_ifs(get_node_heirarchy(self.params)) + self.create_service_client_ifs(get_node_heirarchy(self.params)) ret = self.run_test(themes, timeout=10) assert ret[0], ret[1] @@ -192,7 +192,7 @@ def test_camera_test_tf_dyn(self,launch_descr_with_parameters): ''' self.init_test("RsTest"+self.params['camera_name']) self.wait_for_node(self.params['camera_name']) - self.create_param_ifs(get_node_heirarchy(self.params)) + self.create_service_client_ifs(get_node_heirarchy(self.params)) ret = self.run_test(themes, timeout=10) assert ret[0], ret[1] ret = self.process_data(themes, False) diff --git a/realsense2_camera/test/live_camera/test_d415_basic_tests.py b/realsense2_camera/test/live_camera/test_d415_basic_tests.py index f397b70f9a..1b8ca54510 100644 --- a/realsense2_camera/test/live_camera/test_d415_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d415_basic_tests.py @@ -101,7 +101,7 @@ def test_D415_Change_Resolution(self,launch_descr_with_parameters): ''' self.init_test("RsTest"+params['camera_name']) self.spin_for_time(wait_time=1.0) - self.create_param_ifs(get_node_heirarchy(params)) + self.create_service_client_ifs(get_node_heirarchy(params)) self.spin_for_time(wait_time=1.0) for key in cap: diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index d7e8e2d29d..687b4949ec 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -77,7 +77,7 @@ def test_D455_Change_Resolution(self,launch_descr_with_parameters): print("Starting camera test...") self.init_test("RsTest"+params['camera_name']) self.wait_for_node(params['camera_name']) - self.create_param_ifs(get_node_heirarchy(params)) + self.create_service_client_ifs(get_node_heirarchy(params)) self.spin_for_time(0.5) assert self.set_bool_param('enable_color', False) self.spin_for_time(0.5) @@ -131,7 +131,7 @@ def test_D455_Seq_ID_update(self,launch_descr_with_parameters): print("Starting camera test...") self.init_test("RsTest"+params['camera_name']) self.wait_for_node(params['camera_name']) - self.create_param_ifs(get_node_heirarchy(params)) + self.create_service_client_ifs(get_node_heirarchy(params)) assert self.set_bool_param('depth_module.hdr_enabled', False) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 0a0069115f..75cfb78ecd 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -66,6 +66,7 @@ from sensor_msgs.msg import CameraInfo as msg_CameraInfo from realsense2_camera_msgs.msg import Extrinsics as msg_Extrinsics from realsense2_camera_msgs.msg import Metadata as msg_Metadata +from realsense2_camera_msgs.srv import DeviceInfo from sensor_msgs_py import point_cloud2 as pc2 import tf2_ros @@ -785,14 +786,17 @@ def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, - def create_param_ifs(self, camera_name): + def create_service_client_ifs(self, camera_name): self.set_param_if = self.node.create_client(SetParameters, camera_name + '/set_parameters') self.get_param_if = self.node.create_client(GetParameters, camera_name + '/get_parameters') + self.get_device_info = self.node.create_client(DeviceInfo, camera_name + '/device_info') while not self.get_param_if.wait_for_service(timeout_sec=1.0): print('service not available, waiting again...') while not self.set_param_if.wait_for_service(timeout_sec=1.0): print('service not available, waiting again...') - + while not self.get_device_info.wait_for_service(timeout_sec=1.0): + print('service not available, waiting again...') + def send_param(self, req): future = self.set_param_if.call_async(req) while rclpy.ok(): @@ -806,7 +810,7 @@ def send_param(self, req): print("exception raised:") print(e) pass - return False + return False def get_param(self, req): future = self.get_param_if.call_async(req) @@ -820,7 +824,7 @@ def get_param(self, req): print("exception raised:") print(e) pass - return None + return None def set_string_param(self, param_name, param_value): req = SetParameters.Request() @@ -833,7 +837,16 @@ def set_bool_param(self, param_name, param_value): new_param_value = ParameterValue(type=ParameterType.PARAMETER_BOOL, bool_value=param_value) req.parameters = [Parameter(name=param_name, value=new_param_value)] return self.send_param(req) - + + def get_bool_param(self, param_name): + req = GetParameters.Request() + req.names = [param_name] + value = self.get_param(req) + if (value == None) or (value.type != ParameterType.PARAMETER_BOOL): + return None + else: + return value.bool_value + def set_integer_param(self, param_name, param_value): req = SetParameters.Request() new_param_value = ParameterValue(type=ParameterType.PARAMETER_INTEGER, integer_value=param_value) @@ -844,11 +857,25 @@ def get_integer_param(self, param_name): req = GetParameters.Request() req.names = [param_name] value = self.get_param(req) - if (value == None) or (value.type == ParameterType.PARAMETER_NOT_SET): + if (value == None) or (value.type != ParameterType.PARAMETER_INTEGER): return None else: return value.integer_value - + + def get_deviceinfo(self): + self.req = DeviceInfo.Request() + self.future = self.get_device_info.call_async(self.req) + while rclpy.ok(): + rclpy.spin_once(self.node) + if self.future.done(): + try: + response = self.future.result() + return response + except Exception as e: + print("exception raised:") + print(e) + return None + def spin_for_data(self,themes, timeout=5.0): ''' timeout value varies depending upon the system, it needs to be more if From ec8ef109924b9c4a8e2c5f7a44b44d9bb40f6cfa Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 13 Jun 2024 09:45:08 +0000 Subject: [PATCH 076/111] support calib config read/write services --- README.md | 30 +++++++++++++ .../include/base_realsense_node.h | 8 ++++ realsense2_camera/src/rs_node_setup.cpp | 43 +++++++++++++++++++ realsense2_camera_msgs/CMakeLists.txt | 2 + .../srv/CalibConfigRead.srv | 4 ++ .../srv/CalibConfigWrite.srv | 4 ++ 6 files changed, 91 insertions(+) create mode 100644 realsense2_camera_msgs/srv/CalibConfigRead.srv create mode 100644 realsense2_camera_msgs/srv/CalibConfigWrite.srv diff --git a/README.md b/README.md index dff65f6105..abf961ea4e 100644 --- a/README.md +++ b/README.md @@ -637,6 +637,36 @@ Each of the above filters have it's own parameters, following the naming convent - Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. - Call example: `ros2 service call /camera/camera/device_info realsense2_camera_msgs/srv/DeviceInfo` +### calib_config_read (for specific camera modules): + - Read calibration config. + - Note that reading calibration config is applicable only in Safey Service Mode + - Type `ros2 interface show realsense2_camera_msgs/srv/CalibConfigRead` for the full request/response fields. + - Call example: `ros2 service call /camera/camera/calib_config_read realsense2_camera_msgs/srv/CalibConfigRead` +
+ Click to see the full response of the call example + + `response: realsense2_camera_msgs.srv.CalibConfigRead_Response(success=True, error_message='', calib_config='{"calibration_config":{"calib_roi_num_of_segments":0,"camera_position":{"rotation":[[0,0,0],[0,0,0],[0,0,0]],"translation":[0,0,0]},"crypto_signature":[0, 0 ,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],"roi":[[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]]]}}')` + +
+ + - [JSON safety interface config example](realsense2_camera/examples/jsons/calib_config_example.json) + +### calib_config_write (for specific camera modules): + - Write calibration config. + - Note that writing calibration config is applicable only in Safey Service Mode + - Type `ros2 interface show realsense2_camera_msgs/srv/CalibConfigWrite` for the full request/response fields. + - Only for commnad line usage, user should escape all " with \\". Using ros2 services API from rclcpp/rclpy doesn't need escaping. e.g.,: + +
+ Click to see full call example + + `ros2 service call /camera/camera/calib_config_write realsense2_camera_msgs/srv/CalibConfigWrite "{calib_config: '{\"calibration_config\":{\"calib_roi_num_of_segments\":0,\"camera_position\":{\"rotation\":[[0,0,0],[0,0,0],[0,0,0]],\"translation\":[0,0,0]},\"crypto_signature\":[0, 0 ,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\"roi\":[[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]]]}}' }"` + +
+ + - [JSON safety interface config example](realsense2_camera/examples/jsons/calib_config_example.json) + - Result example: `realsense2_camera_msgs.srv.CalibConfigWrite_Response(success=True, error_message='')` +
diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 24c2ce941f..032f8db7ee 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -32,6 +32,8 @@ #include "realsense2_camera_msgs/msg/metadata.hpp" #include "realsense2_camera_msgs/msg/rgbd.hpp" #include "realsense2_camera_msgs/srv/device_info.hpp" +#include "realsense2_camera_msgs/srv/calib_config_read.hpp" +#include "realsense2_camera_msgs/srv/calib_config_write.hpp" #include #include @@ -150,6 +152,8 @@ namespace realsense2_camera std::vector _monitor_options; rclcpp::Logger _logger; rclcpp::Service::SharedPtr _device_info_srv; + rclcpp::Service::SharedPtr _calib_config_read_srv; + rclcpp::Service::SharedPtr _calib_config_write_srv; std::shared_ptr _parameters; std::list _parameters_names; @@ -158,6 +162,10 @@ namespace realsense2_camera void calcAndAppendTransformMsgs(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile); void getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req, realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res); + void CalibConfigReadService(const realsense2_camera_msgs::srv::CalibConfigRead::Request::SharedPtr req, + realsense2_camera_msgs::srv::CalibConfigRead::Response::SharedPtr res); + void CalibConfigWriteService(const realsense2_camera_msgs::srv::CalibConfigWrite::Request::SharedPtr req, + realsense2_camera_msgs::srv::CalibConfigWrite::Response::SharedPtr res); tf2::Quaternion rotationMatrixToQuaternion(const float rotation[9]) const; void append_static_tf_msg(const rclcpp::Time& t, const float3& trans, diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 191644b83e..87dd480229 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -503,6 +503,18 @@ void BaseRealSenseNode::publishServices() [&](const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req, realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res) {getDeviceInfo(req, res);}); + + _calib_config_read_srv = _node.create_service( + "~/calib_config_read", + [&](const realsense2_camera_msgs::srv::CalibConfigRead::Request::SharedPtr req, + realsense2_camera_msgs::srv::CalibConfigRead::Response::SharedPtr res) + {CalibConfigReadService(req, res);}); + + _calib_config_write_srv = _node.create_service( + "~/calib_config_write", + [&](const realsense2_camera_msgs::srv::CalibConfigWrite::Request::SharedPtr req, + realsense2_camera_msgs::srv::CalibConfigWrite::Response::SharedPtr res) + {CalibConfigWriteService(req, res);}); } void BaseRealSenseNode::getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr, @@ -524,3 +536,34 @@ void BaseRealSenseNode::getDeviceInfo(const realsense2_camera_msgs::srv::DeviceI res->sensors = sensors_names.str().substr(0, sensors_names.str().size()-1); res->physical_port = _dev.supports(RS2_CAMERA_INFO_PHYSICAL_PORT) ? _dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT) : ""; } + +void BaseRealSenseNode::CalibConfigReadService(const realsense2_camera_msgs::srv::CalibConfigRead::Request::SharedPtr req, + realsense2_camera_msgs::srv::CalibConfigRead::Response::SharedPtr res){ + try + { + (void)req; // silence unused parameter warning + rs2_calibration_config calib_config = _dev.as().get_calibration_config(); + res->calib_config = _dev.as().calibration_config_to_json_string(calib_config); + res->success = true; + } + catch (const std::exception &e) + { + res->success = false; + res->error_message = std::string("Exception occurred: ") + e.what(); + } +} + +void BaseRealSenseNode::CalibConfigWriteService(const realsense2_camera_msgs::srv::CalibConfigWrite::Request::SharedPtr req, + realsense2_camera_msgs::srv::CalibConfigWrite::Response::SharedPtr res){ + try + { + rs2_calibration_config calib_config = _dev.as().json_string_to_calibration_config(req->calib_config); + _dev.as().set_calibration_config(calib_config); + res->success = true; + } + catch (const std::exception &e) + { + res->success = false; + res->error_message = std::string("Exception occurred: ") + e.what(); + } +} diff --git a/realsense2_camera_msgs/CMakeLists.txt b/realsense2_camera_msgs/CMakeLists.txt index b5bcdf9ab3..3d085d76cc 100644 --- a/realsense2_camera_msgs/CMakeLists.txt +++ b/realsense2_camera_msgs/CMakeLists.txt @@ -43,6 +43,8 @@ set(msg_files rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} "srv/DeviceInfo.srv" + "srv/CalibConfigRead.srv" + "srv/CalibConfigWrite.srv" DEPENDENCIES builtin_interfaces std_msgs sensor_msgs ADD_LINTER_TESTS ) diff --git a/realsense2_camera_msgs/srv/CalibConfigRead.srv b/realsense2_camera_msgs/srv/CalibConfigRead.srv new file mode 100644 index 0000000000..1f2cf5f041 --- /dev/null +++ b/realsense2_camera_msgs/srv/CalibConfigRead.srv @@ -0,0 +1,4 @@ +--- +bool success +string error_message +string calib_config diff --git a/realsense2_camera_msgs/srv/CalibConfigWrite.srv b/realsense2_camera_msgs/srv/CalibConfigWrite.srv new file mode 100644 index 0000000000..b4d0c99f54 --- /dev/null +++ b/realsense2_camera_msgs/srv/CalibConfigWrite.srv @@ -0,0 +1,4 @@ +string calib_config +--- +bool success +string error_message From 732409b6381879bea7af3e4f06d3133577a09e88 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Sun, 16 Jun 2024 21:00:20 +0300 Subject: [PATCH 077/111] Update main.yml --- .github/workflows/main.yml | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index aa4d904ac9..280cc38088 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -113,7 +113,8 @@ jobs: # wget $bag_filename -P "records/" # sudo apt install ros-${{ matrix.ros_distro}}-launch-pytest - - name: Install Packages For Tests + - name: Install Packages For Foxy Tests + if: ${{ matrix.ros_distro == 'foxy' }} run: | # To avoid mixing of 'apt' provided packages and 'pip' provided packages, one way is to create virtual env # and manage python packages within it. Ref: https://peps.python.org/pep-0668/ @@ -121,9 +122,23 @@ jobs: # Activate the virtual env such that following python related commands run within it. source .venv/bin/activate sudo apt-get install python3-pip - pip3 install numpy --upgrade + # numpy-quaternion needs numpy<2.0.0. Chose 1.24.1 as it is the highest version that support foxy. + pip3 install --force-reinstall numpy==1.24.1 pip3 install numpy-quaternion tqdm pyyaml + - name: Install Packages For Humble/Iron/Rolling/Jazzy Tests + if: ${{ matrix.ros_distro != 'foxy' }} + run: | + # To avoid mixing of 'apt' provided packages and 'pip' provided packages, one way is to create virtual env + # and manage python packages within it. Ref: https://peps.python.org/pep-0668/ + python3 -m venv .venv + # Activate the virtual env such that following python related commands run within it. + source .venv/bin/activate + sudo apt-get install python3-pip + # numpy-quaternion needs numpy<2.0.0. Chose 1.26.4 as it is the lowest working version for ubuntu 24.04. + pip3 install --force-reinstall numpy==1.26.4 + pip3 install numpy-quaternion tqdm pyyaml + - name: Run Tests run: | From 8ee70bb07be490a4829e3e1b5e323ae7215bda3d Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 19 Jun 2024 15:09:35 +0300 Subject: [PATCH 078/111] update librealsense2 version to 2.56.0 since it includes new API that need for ros2-development --- realsense2_camera/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index d888a3a8c5..cce916163e 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -117,9 +117,9 @@ find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core) -find_package(realsense2 2.55.1) +find_package(realsense2 2.56.0) if (BUILD_ACCELERATE_GPU_WITH_GLSL) - find_package(realsense2-gl 2.55.1) + find_package(realsense2-gl 2.56.0) endif() if(NOT realsense2_FOUND) message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n") From f41f1652d0e6ed4cd256f73e57897cd2c0abff3a Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Thu, 20 Jun 2024 08:03:10 +0300 Subject: [PATCH 079/111] Update README.md --- README.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index abf961ea4e..dcebde58ab 100644 --- a/README.md +++ b/README.md @@ -50,9 +50,9 @@
- Intel RealSense ROS1 Wrapper + ROS1 Wrapper for Intel® RealSense™ cameras - Intel Realsense ROS1 Wrapper is not supported anymore, since our developers team are focusing on ROS2 distro.
+ ROS1 Wrapper for Intel® RealSense™ cameras is not supported anymore, since our developers team are focusing on ROS2 distro.
For ROS1 wrapper, go to ros1-legacy branch
@@ -120,7 +120,7 @@
- Step 3: Install Intel® RealSense™ ROS2 wrapper + Step 3: Install ROS Wrapper for Intel® RealSense™ cameras #### Option 1: Install debian package from ROS servers (Foxy EOL distro is not supported by this option): @@ -136,7 +136,7 @@ cd ~/ros2_ws/src/ ``` - - Clone the latest ROS2 Intel® RealSense™ wrapper from [here](https://github.com/IntelRealSense/realsense-ros.git) into '~/ros2_ws/src/' + - Clone the latest ROS Wrapper for Intel® RealSense™ cameras from [here](https://github.com/IntelRealSense/realsense-ros.git) into '~/ros2_ws/src/' ```bashrc git clone https://github.com/IntelRealSense/realsense-ros.git -b ros2-development cd ~/ros2_ws @@ -168,7 +168,7 @@
# Installation on Windows - **PLEASE PAY ATTENTION: RealSense ROS2 Wrapper is not meant to be supported on Windows by our team, since ROS2 and its packages are still not fully supported over Windows. We added these installation steps below in order to try and make it easier for users who already started working with ROS2 on Windows and want to take advantage of the capabilities of our RealSense cameras** + **PLEASE PAY ATTENTION: ROS Wrapper for Intel® RealSense™ cameras is not meant to be supported on Windows by our team, since ROS2 and its packages are still not fully supported over Windows. We added these installation steps below in order to try and make it easier for users who already started working with ROS2 on Windows and want to take advantage of the capabilities of our RealSense cameras**
@@ -185,7 +185,7 @@ - [ROS2 Foxy](https://docs.ros.org/en/foxy/Installation/Windows-Install-Binary.html) - Microsoft IOT binary installation: - https://ms-iot.github.io/ROSOnWindows/GettingStarted/SetupRos2.html - - Pay attention that the examples of install are for Foxy distro (which is not supported anymore by RealSense ROS2 Wrapper) + - Pay attention that the examples of install are for Foxy distro (which is not supported anymore by ROS Wrapper for Intel® RealSense™ cameras) - Please replace the word "Foxy" with Humble or Iron, depends on the chosen distro.
@@ -194,7 +194,7 @@ Step 2: Download RealSense™ ROS2 Wrapper and RealSense™ SDK 2.0 source code from github: -- Download Intel® RealSense™ ROS2 Wrapper source code from [Intel® RealSense™ ROS2 Wrapper Releases](https://github.com/IntelRealSense/realsense-ros/releases) +- Download ROS Wrapper for Intel® RealSense™ cameras source code from [ROS Wrapper for Intel® RealSense™ cameras releases](https://github.com/IntelRealSense/realsense-ros/releases) - Download the corrosponding supported Intel® RealSense™ SDK 2.0 source code from the **"Supported RealSense SDK" section** of the specific release you chose fronm the link above - Place the librealsense folder inside the realsense-ros folder, to make the librealsense package set beside realsense2_camera, realsense2_camera_msgs and realsense2_description packages
From 8adf5aebad3e9546acc69b08bd7340755d73572e Mon Sep 17 00:00:00 2001 From: Madhukar Reddy Kadireddy Date: Thu, 20 Jun 2024 19:01:10 +0530 Subject: [PATCH 080/111] Casefolding device name instead os strict case sensitive comparison --- realsense2_camera/test/utils/pytest_live_camera_utils.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py index 90a6a2d55e..ac91620f12 100644 --- a/realsense2_camera/test/utils/pytest_live_camera_utils.py +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -198,11 +198,9 @@ def check_if_camera_connected(device_type, serial_no=None): name_line = long_data[index].split() if name_line[0] != "Intel": continue - if name_line[2] != device_type: + if name_line[2].casefold() != device_type.casefold(): continue - if serial_no == None: - return True - if serial_no == name_line[3]: + if serial_no is None or serial_no == name_line[3]: return True return False From cce5cf2a73569e34122338750affb49db41008f6 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sun, 23 Jun 2024 02:28:26 +0300 Subject: [PATCH 081/111] implement Triggered Calibration action --- README.md | 23 ++++ realsense2_camera/CMakeLists.txt | 2 + .../include/base_realsense_node.h | 13 ++ realsense2_camera/include/ros_utils.h | 2 +- realsense2_camera/src/actions.cpp | 121 ++++++++++++++++++ realsense2_camera/src/ros_utils.cpp | 13 ++ realsense2_camera/src/rs_node_setup.cpp | 18 +++ realsense2_camera_msgs/CMakeLists.txt | 1 + .../action/TriggeredCalibration.action | 9 ++ 9 files changed, 201 insertions(+), 1 deletion(-) create mode 100644 realsense2_camera/src/actions.cpp create mode 100644 realsense2_camera_msgs/action/TriggeredCalibration.action diff --git a/README.md b/README.md index dcebde58ab..2349f9bab2 100644 --- a/README.md +++ b/README.md @@ -40,6 +40,7 @@ * [Metadata Topic](#metadata-topic) * [Post-Processing Filters](#post-processing-filters) * [Available Services](#available-services) + * [Available Actions](#available-actions) * [Efficient intra-process communication](#efficient-intra-process-communication) * [Contributing](CONTRIBUTING.md) * [License](LICENSE) @@ -670,6 +671,28 @@ Each of the above filters have it's own parameters, following the naming convent
+## Available actions + +### triggered_calibration + - Type `ros2 interface show realsense2_camera_msgs/action/TriggeredCalibration` for the full request/result/feedback fields. + ``` + # request + string json "calib run" # default value + --- + # result + string calibration + float32 health + --- + # feedback + float32 progress + + ``` + - To use from command line: `ros2 action send_goal /camera/camera/triggered_calibration realsense2_camera_msgs/action/TriggeredCalibration '{json: "{calib run}"}'` or even with an empty request `ros2 action send_goal /camera/camera/triggered_calibration realsense2_camera_msgs/action/TriggeredCalibration ''` because the default behavior is already calib run. + - The action gives an updated feedback about the progress (%) if the client asks for feedback. + - If succeded, the action writes the new calibration table to the flash. It also returns the new calibration table as json string and the health as float32 + +
+ ## Efficient intra-process communication: Our ROS2 Wrapper node supports zero-copy communications if loaded in the same process as a subscriber node. This can reduce copy times on image/pointcloud topics, especially with big frame resolutions and high FPS. diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index cce916163e..4391297776 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -107,6 +107,7 @@ find_package(builtin_interfaces REQUIRED) find_package(cv_bridge REQUIRED) find_package(image_transport REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(realsense2_camera_msgs REQUIRED) find_package(std_msgs REQUIRED) @@ -145,6 +146,7 @@ set(SOURCES src/profile_manager.cpp src/image_publisher.cpp src/tfs.cpp + src/actions.cpp ) if (BUILD_ACCELERATE_GPU_WITH_GLSL) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 032f8db7ee..c1da386cf2 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -34,6 +34,8 @@ #include "realsense2_camera_msgs/srv/device_info.hpp" #include "realsense2_camera_msgs/srv/calib_config_read.hpp" #include "realsense2_camera_msgs/srv/calib_config_write.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "realsense2_camera_msgs/action/triggered_calibration.hpp" #include #include @@ -120,6 +122,8 @@ namespace realsense2_camera void publishTopics(); public: + using TriggeredCalibration = realsense2_camera_msgs::action::TriggeredCalibration; + using GoalHandleTriggeredCalibration = rclcpp_action::ServerGoalHandle; enum class imu_sync_method{NONE, COPY, LINEAR_INTERPOLATION}; protected: @@ -154,6 +158,8 @@ namespace realsense2_camera rclcpp::Service::SharedPtr _device_info_srv; rclcpp::Service::SharedPtr _calib_config_read_srv; rclcpp::Service::SharedPtr _calib_config_write_srv; + rclcpp_action::Server::SharedPtr _triggered_calibration_action_server; + std::shared_ptr _parameters; std::list _parameters_names; @@ -166,6 +172,12 @@ namespace realsense2_camera realsense2_camera_msgs::srv::CalibConfigRead::Response::SharedPtr res); void CalibConfigWriteService(const realsense2_camera_msgs::srv::CalibConfigWrite::Request::SharedPtr req, realsense2_camera_msgs::srv::CalibConfigWrite::Response::SharedPtr res); + + rclcpp_action::GoalResponse TriggeredCalibrationHandleGoal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + rclcpp_action::CancelResponse TriggeredCalibrationHandleCancel(const std::shared_ptr goal_handle); + void TriggeredCalibrationHandleAccepted(const std::shared_ptr goal_handle); + void TriggeredCalibrationExecute(const std::shared_ptr goal_handle); + tf2::Quaternion rotationMatrixToQuaternion(const float rotation[9]) const; void append_static_tf_msg(const rclcpp::Time& t, const float3& trans, @@ -271,6 +283,7 @@ namespace realsense2_camera void startUpdatedSensors(); void stopRequiredSensors(); void publishServices(); + void publishActions(); void startPublishers(const std::vector& profiles, const RosSensor& sensor); void startRGBDPublisherIfNeeded(); void stopPublishers(const std::vector& profiles); diff --git a/realsense2_camera/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index feccd4647d..d31b0d42bf 100644 --- a/realsense2_camera/include/ros_utils.h +++ b/realsense2_camera/include/ros_utils.h @@ -42,6 +42,6 @@ namespace realsense2_camera const rmw_qos_profile_t qos_string_to_qos(std::string str); const std::string list_available_qos_strings(); rs2_format string_to_rs2_format(std::string str); - + std::string vectorToJsonString(const std::vector& vec); } diff --git a/realsense2_camera/src/actions.cpp b/realsense2_camera/src/actions.cpp new file mode 100644 index 0000000000..740c7391b1 --- /dev/null +++ b/realsense2_camera/src/actions.cpp @@ -0,0 +1,121 @@ +// Copyright 2024 Intel Corporation. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../include/base_realsense_node.h" +using namespace realsense2_camera; +using namespace rs2; + +/*** + * Implementation of ROS2 Actions based on: + * ROS2 Actions Design: https://design.ros2.org/articles/actions.html + * ROS2 Actions Tutorials/Examples: https://docs.ros.org/en/rolling/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html +*/ + +// Triggered Calibration Action Struct (Message) +/* +# request +string json "calib run" +--- +# result +string calibration +float32 health +--- +# feedback +float32 progress +*/ + +/*** + * A callback for handling new goals (requests) for Triggered Calibration + * This implementation just accepts all goals with no restriction on the json input +*/ +rclcpp_action::GoalResponse BaseRealSenseNode::TriggeredCalibrationHandleGoal(const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) +{ + (void)uuid; // unused parameter + ROS_INFO_STREAM("TriggeredCalibrationAction: Received request with json " << goal->json); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +/*** + * A callback for handling cancel events + * This implementation just tells the client that it accepted the cancellation. +*/ +rclcpp_action::CancelResponse BaseRealSenseNode::TriggeredCalibrationHandleCancel(const std::shared_ptr goal_handle) +{ + (void)goal_handle; // unused parameter + ROS_INFO("TriggeredCalibrationAction: Received request to cancel"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +/*** + * A callback that accepts a new goal (request) and starts processing it. + * Since the execution is a long-running operation, we spawn off a + * thread to do the actual work and return from handle_accepted quickly. +*/ +void BaseRealSenseNode::TriggeredCalibrationHandleAccepted(const std::shared_ptr goal_handle) +{ + using namespace std::placeholders; + ROS_INFO("TriggeredCalibrationAction: Request accepted"); + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&BaseRealSenseNode::TriggeredCalibrationExecute, this, _1), goal_handle}.detach(); +} + +/*** + * All processing and updates of Triggered Calibration operation + * are done in this execute method in the new thread called by the + * TriggeredCalibrationHandleAccepted() above. +*/ +void BaseRealSenseNode::TriggeredCalibrationExecute(const std::shared_ptr goal_handle) +{ + ROS_INFO("TriggeredCalibrationAction: Executing..."); + const auto goal = goal_handle->get_goal(); // get the TriggeredCalibration srv struct + auto feedback = std::make_shared(); + float & _progress = feedback->progress; + auto result = std::make_shared(); + + try + { + rs2::auto_calibrated_device ac_dev = _dev.as(); + float health = 0.f; // output health + int timeout_ms = 120000; // 2 minutes timout + auto ans = ac_dev.run_on_chip_calibration(goal->json, + &health, + [&](const float progress) {_progress = progress; }, + timeout_ms); + + // the new calibration is the result without the first 3 bytes + rs2::calibration_table new_calib = std::vector(ans.begin() + 3, ans.end()); + + if (rclcpp::ok() && _progress == 100.0) + { + result->calibration = vectorToJsonString(new_calib); + result->health = health; + goal_handle->succeed(result); + ROS_DEBUG("TriggeredCalibrationExecute: Succeded"); + } + else + { + result->calibration = "{}"; + goal_handle->canceled(result); + ROS_WARN("TriggeredCalibrationExecute: Canceled"); + } + } + catch(...) + { + // exception must have been thrown from run_on_chip_calibration call + result->calibration = "{}"; + goal_handle->abort(result); + ROS_ERROR("TriggeredCalibrationExecute: Aborted"); + } +} diff --git a/realsense2_camera/src/ros_utils.cpp b/realsense2_camera/src/ros_utils.cpp index 0ee2172904..8c4d1528c1 100644 --- a/realsense2_camera/src/ros_utils.cpp +++ b/realsense2_camera/src/ros_utils.cpp @@ -141,4 +141,17 @@ const std::string list_available_qos_strings() return res.str(); } +std::string vectorToJsonString(const std::vector& vec) { + std::ostringstream oss; + oss << "["; + for (size_t i = 0; i < vec.size(); ++i) { + oss << static_cast(vec[i]); + if (i < vec.size() - 1) { + oss << ","; + } + } + oss << "]"; + return oss.str(); +} + } diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 87dd480229..215309df5a 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -35,6 +35,7 @@ void BaseRealSenseNode::setup() monitoringProfileChanges(); updateSensors(); publishServices(); + publishActions(); } void BaseRealSenseNode::monitoringProfileChanges() @@ -515,6 +516,23 @@ void BaseRealSenseNode::publishServices() [&](const realsense2_camera_msgs::srv::CalibConfigWrite::Request::SharedPtr req, realsense2_camera_msgs::srv::CalibConfigWrite::Response::SharedPtr res) {CalibConfigWriteService(req, res);}); + +} + +void BaseRealSenseNode::publishActions() +{ + + using namespace std::placeholders; + _triggered_calibration_action_server = rclcpp_action::create_server( + _node.get_node_base_interface(), + _node.get_node_clock_interface(), + _node.get_node_logging_interface(), + _node.get_node_waitables_interface(), + "~/triggered_calibration", + std::bind(&BaseRealSenseNode::TriggeredCalibrationHandleGoal, this, _1, _2), + std::bind(&BaseRealSenseNode::TriggeredCalibrationHandleCancel, this, _1), + std::bind(&BaseRealSenseNode::TriggeredCalibrationHandleAccepted, this, _1)); + } void BaseRealSenseNode::getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr, diff --git a/realsense2_camera_msgs/CMakeLists.txt b/realsense2_camera_msgs/CMakeLists.txt index 3d085d76cc..fe14467a3b 100644 --- a/realsense2_camera_msgs/CMakeLists.txt +++ b/realsense2_camera_msgs/CMakeLists.txt @@ -45,6 +45,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/DeviceInfo.srv" "srv/CalibConfigRead.srv" "srv/CalibConfigWrite.srv" + "action/TriggeredCalibration.action" DEPENDENCIES builtin_interfaces std_msgs sensor_msgs ADD_LINTER_TESTS ) diff --git a/realsense2_camera_msgs/action/TriggeredCalibration.action b/realsense2_camera_msgs/action/TriggeredCalibration.action new file mode 100644 index 0000000000..4519690805 --- /dev/null +++ b/realsense2_camera_msgs/action/TriggeredCalibration.action @@ -0,0 +1,9 @@ +# request +string json "calib run" +--- +# result +string calibration +float32 health +--- +# feedback +float32 progress From 7415bd00636cbbf184b123cfe2f8e28511547dc4 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 25 Jun 2024 18:15:24 +0000 Subject: [PATCH 082/111] small fixes in main workflow --- .github/workflows/main.yml | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 280cc38088..12f1b7f559 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -47,7 +47,7 @@ jobs: - uses: actions/checkout@v4 with: path: 'ros2/src/realsense-ros' - + - name: Check Copyright & Line-Endings shell: bash run: | @@ -67,7 +67,14 @@ jobs: uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} - + + - name: Checkout librealsense/development + uses: actions/checkout@v4 + with: + repository: IntelRealSense/librealsense + path: librealsense + ref: development + - name: Build RealSense SDK 2.0 (development branch) from source run: | @@ -75,9 +82,7 @@ jobs: # This apt install command will be ignored in ubuntu 22.04 as libusb-1.0-0-dev already installed there sudo apt install -y libusb-1.0-0-dev - cd ${{github.workspace}} - git clone https://github.com/IntelRealSense/librealsense.git -b development - cd librealsense + cd ${{github.workspace}}/librealsense sudo mkdir build cd build sudo cmake ../ -DCMAKE_BUILD_TYPE=Release -DBUILD_EXAMPLES=false -DBUILD_GRAPHICAL_EXAMPLES=false @@ -112,7 +117,7 @@ jobs: # bag_filename="https://librealsense.intel.com/rs-tests/D435i_Depth_and_IMU_Stands_still.bag"; # wget $bag_filename -P "records/" # sudo apt install ros-${{ matrix.ros_distro}}-launch-pytest - + - name: Install Packages For Foxy Tests if: ${{ matrix.ros_distro == 'foxy' }} run: | @@ -138,8 +143,7 @@ jobs: # numpy-quaternion needs numpy<2.0.0. Chose 1.26.4 as it is the lowest working version for ubuntu 24.04. pip3 install --force-reinstall numpy==1.26.4 pip3 install numpy-quaternion tqdm pyyaml - - + - name: Run Tests run: | cd ${{github.workspace}}/ros2 @@ -150,7 +154,7 @@ jobs: sudo apt install -y ros-${{matrix.ros_distro}}-sensor-msgs-py source ../.venv/bin/activate python3 src/realsense-ros/realsense2_camera/scripts/rs2_test.py non_existent_file - + # don't run integration tests for foxy since some testing dependecies packages like # tf_ros_py are not avaialble # TODO: check when we can run integration tests on rolling From b4bc5d1e87e5ef3690cbc091572d24ea09f70987 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 1 Jul 2024 15:22:01 +0300 Subject: [PATCH 083/111] update READMEs and CONTRIBUTING files regarding ros2-master --- .github/ISSUE_TEMPLATE.md | 2 +- CONTRIBUTING.md | 2 +- README.md | 6 +++--- realsense2_camera/test/README.md | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md index 921b769f2e..9cc8abea57 100644 --- a/.github/ISSUE_TEMPLATE.md +++ b/.github/ISSUE_TEMPLATE.md @@ -1,6 +1,6 @@ * Before opening a new issue, we wanted to provide you with some useful suggestions (Click "Preview" above for a better view): - * Consider checking our ROS RealSense Wrapper documentation [README](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/README.md). + * Consider checking our ROS RealSense Wrapper documentation [README](https://github.com/IntelRealSense/realsense-ros/blob/ros2-master/README.md). * Have you looked in our [Discussions](https://github.com/IntelRealSense/realsense-ros/discussions)? * Try [searching our GitHub Issues](https://github.com/IntelRealSense/realsense-ros/issues) (open and closed) for a similar issue. diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 3632ad8983..252bc4283f 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -5,7 +5,7 @@ This project welcomes third-party code via GitHub pull requests. You are welcome to propose and discuss enhancements using project [issues](https://github.com/IntelRealSense/realsense-ros/issues). > **Branching Policy**: -> The `ros2-development` branch is considered stable, at all times. +> The `ros2-master` branch is considered stable, at all times. > If you plan to propose a patch, please commit into the `ros2-development` branch, or its own feature branch. In addition, please run `pr_check.sh` under `scripts` directory. This scripts verify compliance with project's standards: diff --git a/README.md b/README.md index 2349f9bab2..4292d4216c 100644 --- a/README.md +++ b/README.md @@ -59,7 +59,7 @@
- Moving from ros2-legacy to ros2-development + Moving from ros2-legacy to ros2-master * Changed Parameters: @@ -67,7 +67,7 @@ - For video streams: **\.profile** replaces **\_width**, **\_height**, **\_fps** - **ROS2-legacy (Old)**: - ros2 launch realsense2_camera rs_launch.py depth_width:=640 depth_height:=480 depth_fps:=30.0 infra1_width:=640 infra1_height:=480 infra1_fps:=30.0 - - **ROS2-development (New)**: + - **ROS2-master (New)**: - ros2 launch realsense2_camera rs_launch.py depth_module.profile:=640x480x30 - Removed paramets **\_frame_id**, **\_optical_frame_id**. frame_ids are now defined by camera_name - **"filters"** is removed. All filters (or post-processing blocks) are enabled/disabled using **"\.enable"** @@ -139,7 +139,7 @@ - Clone the latest ROS Wrapper for Intel® RealSense™ cameras from [here](https://github.com/IntelRealSense/realsense-ros.git) into '~/ros2_ws/src/' ```bashrc - git clone https://github.com/IntelRealSense/realsense-ros.git -b ros2-development + git clone https://github.com/IntelRealSense/realsense-ros.git -b ros2-master cd ~/ros2_ws ``` diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index 4e59fcb620..581fe6f248 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -127,7 +127,7 @@ The xml files mentioned by the command can be directly opened also. ### Running pytests directly Note : -1. All the commands for test execution has to be executed from realsense-ros folder. For example: If the ROS2 workspace was created based on Step 3 [Option2] of [this](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/README.md#installation). +1. All the commands for test execution has to be executed from realsense-ros folder. For example: If the ROS2 workspace was created based on Step 3 [Option2] of [this](https://github.com/IntelRealSense/realsense-ros/blob/ros2-master/README.md#installation). Then, the path to execute the tests would be ~/ros2_ws/src/realsense-ros. cd ~/ros2_ws/src/realsense-ros From 915f1f61ebec587e93eabea7684293d4a040d3ce Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 1 Jul 2024 15:57:52 +0300 Subject: [PATCH 084/111] update readme and more for jazzy support --- .github/ISSUE_TEMPLATE.md | 2 +- .github/workflows/main.yml | 6 ++++-- README.md | 18 ++++++++++++++---- 3 files changed, 19 insertions(+), 7 deletions(-) diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md index 921b769f2e..2523a66cf7 100644 --- a/.github/ISSUE_TEMPLATE.md +++ b/.github/ISSUE_TEMPLATE.md @@ -18,7 +18,7 @@ | Librealsense SDK Version | { 2.. } | | Language | {C/C#/labview/opencv/pcl/python/unity } | | Segment | {Robot/Smartphone/VR/AR/others } | -| ROS Distro | {Iron/Humble/Rolling/etc.. } | +| ROS Distro | {Iron/Humble/Jazzy/Rolling/etc.. } | | RealSense ROS Wrapper Version | {4.51.1, 4.54.1, etc..} | diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 280cc38088..1d86f01649 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -6,9 +6,11 @@ on: push: branches: - ros2-development + - ros2-master pull_request: branches: - ros2-development + - ros2-master # Allows you to run this workflow manually from the Actions tab workflow_dispatch: @@ -29,14 +31,14 @@ jobs: include: - ros_distro: 'rolling' os: ubuntu-24.04 + - ros_distro: 'jazzy' + os: ubuntu-24.04 - ros_distro: 'iron' os: ubuntu-22.04 - ros_distro: 'humble' os: ubuntu-22.04 - ros_distro: 'foxy' os: ubuntu-20.04 - - ros_distro: 'jazzy' - os: ubuntu-24.04 steps: diff --git a/README.md b/README.md index 2349f9bab2..e1bc86b4f3 100644 --- a/README.md +++ b/README.md @@ -12,9 +12,11 @@ [![rolling][rolling-badge]][rolling] +[![jazzy][jazzy-badge]][jazzy] [![iron][iron-badge]][iron] [![humble][humble-badge]][humble] [![foxy][foxy-badge]][foxy] +[![ubuntu24][ubuntu24-badge]][ubuntu24] [![ubuntu22][ubuntu22-badge]][ubuntu22] [![ubuntu20][ubuntu20-badge]][ubuntu20] @@ -88,7 +90,10 @@ Step 1: Install the ROS2 distribution - + +- #### Ubuntu 24.04: + - [ROS2 Jazzy](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debians.html) + - #### Ubuntu 22.04: - [ROS2 Iron](https://docs.ros.org/en/iron/Installation/Ubuntu-Install-Debians.html) - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) @@ -158,7 +163,7 @@ - Source environment ```bash - ROS_DISTRO= # set your ROS_DISTRO: iron, humble, foxy + ROS_DISTRO= # set your ROS_DISTRO: jazzy, iron, humble, foxy source /opt/ros/$ROS_DISTRO/setup.bash cd ~/ros2_ws . install/local_setup.bash @@ -181,13 +186,14 @@ **Please choose only one option from the two options below (in order to prevent multiple versions installation and workspace conflicts)** - Manual install from ROS2 formal documentation: + - [ROS2 Jazzy](https://docs.ros.org/en/jazzy/Installation/Windows-Install-Binary.html) - [ROS2 Iron](https://docs.ros.org/en/iron/Installation/Windows-Install-Binary.html) - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Windows-Install-Binary.html) - [ROS2 Foxy](https://docs.ros.org/en/foxy/Installation/Windows-Install-Binary.html) - Microsoft IOT binary installation: - https://ms-iot.github.io/ROSOnWindows/GettingStarted/SetupRos2.html - Pay attention that the examples of install are for Foxy distro (which is not supported anymore by ROS Wrapper for Intel® RealSense™ cameras) - - Please replace the word "Foxy" with Humble or Iron, depends on the chosen distro. + - Please replace the word "Foxy" with Humble, Iron or Jazzy, depends on the chosen distro.
@@ -740,12 +746,16 @@ ros2 launch realsense2_camera rs_intra_process_demo_launch.py intra_process_comm [rolling-badge]: https://img.shields.io/badge/-ROLLING-orange?style=flat-square&logo=ros [rolling]: https://docs.ros.org/en/rolling/index.html -[foxy-badge]: https://img.shields.io/badge/-foxy-orange?style=flat-square&logo=ros +[jazzy-badge]: https://img.shields.io/badge/-JAZZY-orange?style=flat-square&logo=ros +[jazzy]: https://docs.ros.org/en/jazzy/index.html +[foxy-badge]: https://img.shields.io/badge/-FOXY-orange?style=flat-square&logo=ros [foxy]: https://docs.ros.org/en/foxy/index.html [humble-badge]: https://img.shields.io/badge/-HUMBLE-orange?style=flat-square&logo=ros [humble]: https://docs.ros.org/en/humble/index.html [iron-badge]: https://img.shields.io/badge/-IRON-orange?style=flat-square&logo=ros [iron]: https://docs.ros.org/en/iron/index.html +[ubuntu24-badge]: https://img.shields.io/badge/-UBUNTU%2024%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white +[ubuntu24]: https://releases.ubuntu.com/noble/ [ubuntu22-badge]: https://img.shields.io/badge/-UBUNTU%2022%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white [ubuntu22]: https://releases.ubuntu.com/jammy/ [ubuntu20-badge]: https://img.shields.io/badge/-UBUNTU%2020%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white From 6c0d1becf8a37a9fe138a1899539f57664f80d93 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 1 Jul 2024 18:42:35 +0300 Subject: [PATCH 085/111] update librealsense ver in readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 4292d4216c..0ed42416f1 100644 --- a/README.md +++ b/README.md @@ -114,7 +114,7 @@ - For example, for Humble distro: ```sudo apt install ros-humble-librealsense2*``` - #### Option 3: Build from source - - Download the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.53.1) + - Download the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.55.1) - Follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md)
From f53762d72c04cf6b6ef90eb157fd519c67de2f8e Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 2 Jul 2024 14:05:41 +0300 Subject: [PATCH 086/111] fix feedback and update readme for TC --- README.md | 14 +++++++++++-- realsense2_camera/src/actions.cpp | 20 +++++++++++++++---- .../action/TriggeredCalibration.action | 2 ++ 3 files changed, 30 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 2349f9bab2..bb8fffd012 100644 --- a/README.md +++ b/README.md @@ -673,13 +673,15 @@ Each of the above filters have it's own parameters, following the naming convent ## Available actions -### triggered_calibration +### triggered_calibration (supported only for D500 devices) - Type `ros2 interface show realsense2_camera_msgs/action/TriggeredCalibration` for the full request/result/feedback fields. ``` # request string json "calib run" # default value --- # result + bool success + string error_msg string calibration float32 health --- @@ -688,8 +690,16 @@ Each of the above filters have it's own parameters, following the naming convent ``` - To use from command line: `ros2 action send_goal /camera/camera/triggered_calibration realsense2_camera_msgs/action/TriggeredCalibration '{json: "{calib run}"}'` or even with an empty request `ros2 action send_goal /camera/camera/triggered_calibration realsense2_camera_msgs/action/TriggeredCalibration ''` because the default behavior is already calib run. - - The action gives an updated feedback about the progress (%) if the client asks for feedback. + - The action gives an updated feedback about the progress (%) if the client asks for feedback. To do that, add `--feedback` to the end of the command. - If succeded, the action writes the new calibration table to the flash. It also returns the new calibration table as json string and the health as float32 + - If failed, it will return the error message inside the result. For example: + ``` + Result: + success: false + error_msg: 'TriggeredCalibrationExecute: Aborted. Error: Calibration completed but algorithm failed' + calibration: '{}' + health: 0.0 + ```
diff --git a/realsense2_camera/src/actions.cpp b/realsense2_camera/src/actions.cpp index 740c7391b1..4d65db97be 100644 --- a/realsense2_camera/src/actions.cpp +++ b/realsense2_camera/src/actions.cpp @@ -89,9 +89,15 @@ void BaseRealSenseNode::TriggeredCalibrationExecute(const std::shared_ptr(); float health = 0.f; // output health int timeout_ms = 120000; // 2 minutes timout + + auto progress_callback = [&](const float progress) { + _progress = progress; + goal_handle->publish_feedback(feedback); + }; + auto ans = ac_dev.run_on_chip_calibration(goal->json, &health, - [&](const float progress) {_progress = progress; }, + progress_callback, timeout_ms); // the new calibration is the result without the first 3 bytes @@ -101,21 +107,27 @@ void BaseRealSenseNode::TriggeredCalibrationExecute(const std::shared_ptrcalibration = vectorToJsonString(new_calib); result->health = health; + result->success = true; goal_handle->succeed(result); - ROS_DEBUG("TriggeredCalibrationExecute: Succeded"); + ROS_INFO("TriggeredCalibrationExecute: Succeded"); } else { result->calibration = "{}"; + result->success = false; + result->error_msg = "Canceled"; goal_handle->canceled(result); ROS_WARN("TriggeredCalibrationExecute: Canceled"); } } - catch(...) + catch(const std::runtime_error& e) { // exception must have been thrown from run_on_chip_calibration call + std::string error_msg = "TriggeredCalibrationExecute: Aborted. Error: " + std::string(e.what()); result->calibration = "{}"; + result->success = false; + result->error_msg = error_msg; goal_handle->abort(result); - ROS_ERROR("TriggeredCalibrationExecute: Aborted"); + ROS_ERROR(error_msg.c_str()); } } diff --git a/realsense2_camera_msgs/action/TriggeredCalibration.action b/realsense2_camera_msgs/action/TriggeredCalibration.action index 4519690805..a4884cf171 100644 --- a/realsense2_camera_msgs/action/TriggeredCalibration.action +++ b/realsense2_camera_msgs/action/TriggeredCalibration.action @@ -2,6 +2,8 @@ string json "calib run" --- # result +bool success +string error_msg string calibration float32 health --- From 3f603ac526336c6c773fdae0c52dd7433ba2ad31 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Wed, 10 Jul 2024 14:58:24 +0300 Subject: [PATCH 087/111] add D421 pid --- realsense2_camera/include/constants.h | 1 + realsense2_camera/src/realsense_node_factory.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 80104e46c3..7f5b28cee1 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -53,6 +53,7 @@ namespace realsense2_camera const uint16_t RS430_MM_PID = 0x0ad5; // AWGT const uint16_t RS_USB2_PID = 0x0ad6; // USB2 const uint16_t RS420_PID = 0x0af6; // PWG + const uint16_t RS421_PID = 0x1155; // D421 const uint16_t RS420_MM_PID = 0x0afe; // PWGT const uint16_t RS410_MM_PID = 0x0aff; // ASR const uint16_t RS400_MM_PID = 0x0b00; // PSR diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index c19bae6e0f..f7ea18bca7 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -364,6 +364,7 @@ void RealSenseNodeFactory::startDevice() case RS460_PID: case RS415_PID: case RS420_PID: + case RS421_PID: case RS420_MM_PID: case RS430_PID: case RS430i_PID: From 7ad5ca4c24c11688ffc9c5da353f52f6cf721a93 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Fri, 19 Jul 2024 14:33:40 +0300 Subject: [PATCH 088/111] update calib config usage to the new API, and update readme --- README.md | 11 ++++- .../d500_tables/calib_config_example.json | 45 +++++++++++++++++++ realsense2_camera/src/rs_node_setup.cpp | 6 +-- 3 files changed, 56 insertions(+), 6 deletions(-) create mode 100644 realsense2_camera/examples/d500_tables/calib_config_example.json diff --git a/README.md b/README.md index b50f43c8ca..d3acb8d674 100644 --- a/README.md +++ b/README.md @@ -652,7 +652,7 @@ Each of the above filters have it's own parameters, following the naming convent
Click to see the full response of the call example - `response: realsense2_camera_msgs.srv.CalibConfigRead_Response(success=True, error_message='', calib_config='{"calibration_config":{"calib_roi_num_of_segments":0,"camera_position":{"rotation":[[0,0,0],[0,0,0],[0,0,0]],"translation":[0,0,0]},"crypto_signature":[0, 0 ,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],"roi":[[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]]]}}')` + `response: realsense2_camera_msgs.srv.CalibConfigRead_Response(success=True, error_message='', calib_config='{"calibration_config":{"camera_position":{"rotation":[[0.0,0.0,1.0],[-1.0,0.0,0.0],[0.0,-1.0,0.0]],"translation":[0.0,0.0,0.0]},"crypto_signature":[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],"roi_0":{"vertex_0":[0,0],"vertex_1":[0,0],"vertex_2":[0,0],"vertex_3":[0,0]},"roi_1":{"vertex_0":[0,0],"vertex_1":[0,0],"vertex_2":[0,0],"vertex_3":[0,0]},"roi_2":{"vertex_0":[0,0],"vertex_1":[0,0],"vertex_2":[0,0],"vertex_3":[0,0]},"roi_3":{"vertex_0":[0,0],"vertex_1":[0,0],"vertex_2":[0,0],"vertex_3":[0,0]},"roi_num_of_segments":0}}')`
@@ -667,7 +667,7 @@ Each of the above filters have it's own parameters, following the naming convent
Click to see full call example - `ros2 service call /camera/camera/calib_config_write realsense2_camera_msgs/srv/CalibConfigWrite "{calib_config: '{\"calibration_config\":{\"calib_roi_num_of_segments\":0,\"camera_position\":{\"rotation\":[[0,0,0],[0,0,0],[0,0,0]],\"translation\":[0,0,0]},\"crypto_signature\":[0, 0 ,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\"roi\":[[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]],[[0,0],[0,0],[0,0],[0,0]]]}}' }"` + `ros2 service call /camera/camera/calib_config_write realsense2_camera_msgs/srv/CalibConfigWrite "{calib_config: '{\"calibration_config\":{\"camera_position\":{\"rotation\":[[0.0,0.0,1.0],[-1.0,0.0,0.0],[0.0,-1.0,0.0]],\"translation\":[0.0,0.0,0.0]},\"crypto_signature\":[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],\"roi_0\":{\"vertex_0\":[0,0],\"vertex_1\":[0,0],\"vertex_2\":[0,0],\"vertex_3\":[0,0]},\"roi_1\":{\"vertex_0\":[0,0],\"vertex_1\":[0,0],\"vertex_2\":[0,0],\"vertex_3\":[0,0]},\"roi_2\":{\"vertex_0\":[0,0],\"vertex_1\":[0,0],\"vertex_2\":[0,0],\"vertex_3\":[0,0]},\"roi_3\":{\"vertex_0\":[0,0],\"vertex_1\":[0,0],\"vertex_2\":[0,0],\"vertex_3\":[0,0]},\"roi_num_of_segments\":0}}' }"`
@@ -695,6 +695,13 @@ Each of the above filters have it's own parameters, following the naming convent float32 progress ``` + - Before calling triggered calibration, user should set the following parameters: + - `depth_module.visual_preset: 1` # switch to visual preset #1 in depth module + - `depth_module.emitter_enabled: true` # enable emitter in depth module + - `depth_module.enable_auto_exposure: true` # enable AE in depth moudle + - `enable_depth: false` # turn off depth stream + - `enable_infra1: false` # turn off infra1 stream + - `enable_infra2: false` # turn off infra2 stream - To use from command line: `ros2 action send_goal /camera/camera/triggered_calibration realsense2_camera_msgs/action/TriggeredCalibration '{json: "{calib run}"}'` or even with an empty request `ros2 action send_goal /camera/camera/triggered_calibration realsense2_camera_msgs/action/TriggeredCalibration ''` because the default behavior is already calib run. - The action gives an updated feedback about the progress (%) if the client asks for feedback. To do that, add `--feedback` to the end of the command. - If succeded, the action writes the new calibration table to the flash. It also returns the new calibration table as json string and the health as float32 diff --git a/realsense2_camera/examples/d500_tables/calib_config_example.json b/realsense2_camera/examples/d500_tables/calib_config_example.json new file mode 100644 index 0000000000..6346ca5498 --- /dev/null +++ b/realsense2_camera/examples/d500_tables/calib_config_example.json @@ -0,0 +1,45 @@ +{ + "calibration_config": + { + "roi_num_of_segments": 2, + "roi_0": + { + "vertex_0": [ 0, 36 ], + "vertex_1": [ 640, 144 ], + "vertex_2": [ 640, 576 ], + "vertex_3": [ 0, 684 ] + }, + "roi_1": + { + "vertex_0": [ 640, 144 ], + "vertex_1": [ 1280, 35 ], + "vertex_2": [ 1280, 684 ], + "vertex_3": [ 640, 576 ] + }, + "roi_2": + { + "vertex_0": [ 0, 0 ], + "vertex_1": [ 0, 0 ], + "vertex_2": [ 0, 0 ], + "vertex_3": [ 0, 0 ] + }, + "roi_3": + { + "vertex_0": [ 0, 0 ], + "vertex_1": [ 0, 0 ], + "vertex_2": [ 0, 0 ], + "vertex_3": [ 0, 0 ] + }, + "camera_position": + { + "rotation": + [ + [ 0.0, 0.0, 1.0], + [-1.0, 0.0, 0.0], + [ 0.0, -1.0, 0.0] + ], + "translation": [0.0, 0.0, 0.27] + }, + "crypto_signature": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + } +} diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 215309df5a..2f9d9bc1dc 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -560,8 +560,7 @@ void BaseRealSenseNode::CalibConfigReadService(const realsense2_camera_msgs::srv try { (void)req; // silence unused parameter warning - rs2_calibration_config calib_config = _dev.as().get_calibration_config(); - res->calib_config = _dev.as().calibration_config_to_json_string(calib_config); + res->calib_config = _dev.as().get_calibration_config(); res->success = true; } catch (const std::exception &e) @@ -575,8 +574,7 @@ void BaseRealSenseNode::CalibConfigWriteService(const realsense2_camera_msgs::sr realsense2_camera_msgs::srv::CalibConfigWrite::Response::SharedPtr res){ try { - rs2_calibration_config calib_config = _dev.as().json_string_to_calibration_config(req->calib_config); - _dev.as().set_calibration_config(calib_config); + _dev.as().set_calibration_config(req->calib_config); res->success = true; } catch (const std::exception &e) From f49627c9ea3f2b7cb3694527cf0f69824e17425d Mon Sep 17 00:00:00 2001 From: administrator Date: Fri, 9 Aug 2024 11:32:02 +0300 Subject: [PATCH 089/111] disabling FPS & TF tests for ROS-CI --- realsense2_camera/test/live_camera/test_camera_fps_tests.py | 1 + realsense2_camera/test/live_camera/test_camera_tf_tests.py | 6 ++++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index 467a98e0f1..e3e66665c8 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -63,6 +63,7 @@ modified to make it work, see py_rs_utils for more details. To check the fps, a value 'expected_fps_in_hz' has to be added to the corresponding theme ''' +@pytest.mark.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="Regression is not enabled, define RS_ROS_REGRESSION") @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_test_fps_d455, marks=pytest.mark.d455), pytest.param(test_params_test_fps_d415, marks=pytest.mark.d415), diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py index 9b76aae0ad..64350ff7fe 100644 --- a/realsense2_camera/test/live_camera/test_camera_tf_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -76,7 +76,8 @@ 'enable_gyro': 'true', } @pytest.mark.parametrize("launch_descr_with_parameters", [ - pytest.param(test_params_tf_static_change_d455, marks=pytest.mark.d455), + #LRS-1181 [ROS2] To debug inconsistent TF (transform) test that fails on Jenkin 219 NUC on D455 + #pytest.param(test_params_tf_static_change_d455, marks=pytest.mark.d455), pytest.param(test_params_tf_static_change_d435i, marks=pytest.mark.d435i), pytest.param(test_params_tf_static_change_d415, marks=pytest.mark.d415), ],indirect=True) @@ -160,7 +161,8 @@ def process_data(self, themes, enable_infra1): 'tf_publish_rate': '1.1', } @pytest.mark.parametrize("launch_descr_with_parameters", [ - pytest.param(test_params_tf_d455, marks=pytest.mark.d455), + #LRS-1181 [ROS2] To debug inconsistent TF (transform) test that fails on Jenkin 219 NUC on D455 + #pytest.param(test_params_tf_d455, marks=pytest.mark.d455), pytest.param(test_params_tf_d435i, marks=pytest.mark.d435i), pytest.param(test_params_tf_d415, marks=pytest.mark.d415), ],indirect=True) From 911c77b5342b26823511f4e4f946cf1b28fce97d Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Wed, 21 Aug 2024 00:18:46 +0300 Subject: [PATCH 090/111] Update README.md --- README.md | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/README.md b/README.md index d3acb8d674..c40321bc4c 100644 --- a/README.md +++ b/README.md @@ -656,8 +656,6 @@ Each of the above filters have it's own parameters, following the naming convent
- - [JSON safety interface config example](realsense2_camera/examples/jsons/calib_config_example.json) - ### calib_config_write (for specific camera modules): - Write calibration config. - Note that writing calibration config is applicable only in Safey Service Mode @@ -671,10 +669,9 @@ Each of the above filters have it's own parameters, following the naming convent - - [JSON safety interface config example](realsense2_camera/examples/jsons/calib_config_example.json) + - [JSON calib config example](realsense2_camera/examples/d500_tables/calib_config_example.json) - Result example: `realsense2_camera_msgs.srv.CalibConfigWrite_Response(success=True, error_message='')` -
## Available actions From f62e29cc30bd54e07be27f3d631a8e8be0ad5072 Mon Sep 17 00:00:00 2001 From: administrator Date: Sun, 8 Sep 2024 07:56:36 +0300 Subject: [PATCH 091/111] retry thrice finding devices with Ykush reset --- realsense2_camera/test/live_camera/rosci.py | 39 ++++++++++++--------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/realsense2_camera/test/live_camera/rosci.py b/realsense2_camera/test/live_camera/rosci.py index 5fb98c1554..b403b397d6 100755 --- a/realsense2_camera/test/live_camera/rosci.py +++ b/realsense2_camera/test/live_camera/rosci.py @@ -25,6 +25,8 @@ #For running this script locally #Extract the root where both realsense-ros and librealsense are cloned ws_local = '/'.join(os.path.abspath( __file__ ).split( os.path.sep )[0:-5]) + print("##Check point") + print(ws_local) #expected to have 'librealsense' repo in parallel to 'realsense-ros' assert os.path.exists( os.path.join(ws_local, 'librealsense')), f" 'librealsense' doesn't exist at {ws_local} " sys.path.append( os.path.join( ws_local, 'librealsense/unit-tests/py' )) @@ -50,7 +52,7 @@ def usage(): print( ' --device <> Run only on the specified device(s); list of devices separated by ',', No white spaces' ) print( ' e.g.: --device=d455,d435i,d585 (or) --device d455,d435i,d585 ') print( ' Note: if --device option not used, tests run on all connected devices ') - + sys.exit( 2 ) def command(dev_name, test=None): @@ -58,7 +60,7 @@ def command(dev_name, test=None): cmd += ['-s'] cmd += ['-m', ''.join(dev_name)] if test: - cmd += ['-k', f'{test}'] + cmd += ['-k', f'{test}'] cmd += [''.join(dir_live_tests)] cmd += ['--debug'] cmd += [f'--junit-xml={logdir}/{dev_name.upper()}_pytest.xml'] @@ -70,7 +72,7 @@ def run_test(cmd, test=None, dev_name=None, stdout=None, append =False): if test: stdout = stdout + os.sep + str(dev_name.upper()) + '_' + test + '.log' else: - stdout = stdout + os.sep + str(dev_name.upper()) + '_' + 'full.log' + stdout = stdout + os.sep + str(dev_name.upper()) + '_' + 'full.log' if stdout is None: sys.stdout.flush() elif stdout and stdout != subprocess.PIPE: @@ -93,10 +95,10 @@ def run_test(cmd, test=None, dev_name=None, stdout=None, append =False): except Exception as e: log.e("---Test Failed---") log.w( "Error Exception:\n ",e ) - + finally: if handle: - handle.close() + handle.close() junit_xml_parsing(f'{dev_name.upper()}_pytest.xml') def junit_xml_parsing(xml_file): @@ -121,24 +123,29 @@ def junit_xml_parsing(xml_file): new_xml = xml_file.split('.')[0] tree.write(f'{logdir}/{new_xml}_refined.xml') -def find_devices_run_tests(): - global logdir, device_set +def find_devices_run_tests(): + from rspy import devices + global logdir, device_set, _device_by_sn + max_retry = 3 try: os.makedirs( logdir, exist_ok=True ) - - from rspy import devices + #Update dict '_device_by_sn' from devices module of rspy - devices.query( hub_reset = hub_reset ) - global _device_by_sn + while(max_retry and not devices._device_by_sn): + subprocess.run('ykushcmd ykush3 --reset', shell=True) + time.sleep(2.0) + devices.query( hub_reset = hub_reset ) + max_retry -= 1 + if not devices._device_by_sn: assert False, 'No Camera device detected!' else: - connected_devices = [device.name for device in devices._device_by_sn.values()] - log.i('Connected devices:', connected_devices) - + connected_devices = [device.name for device in devices._device_by_sn.values()] + log.i('Connected devices:', connected_devices) + testname = regex if regex else None - + if device_set: #Loop in for user specified devices and run tests only on them devices_not_found = [] @@ -183,7 +190,7 @@ def find_devices_run_tests(): regex = arg elif opt == '--device': device_set = arg.split(',') - + find_devices_run_tests() sys.exit( 0 ) From 667b23caa998ddefb95299f54ecf4b8c8196ca92 Mon Sep 17 00:00:00 2001 From: administrator Date: Sun, 8 Sep 2024 08:07:00 +0300 Subject: [PATCH 092/111] retry thrice finding devices with Ykush reset --- realsense2_camera/test/live_camera/rosci.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/realsense2_camera/test/live_camera/rosci.py b/realsense2_camera/test/live_camera/rosci.py index b403b397d6..d6acd50c35 100755 --- a/realsense2_camera/test/live_camera/rosci.py +++ b/realsense2_camera/test/live_camera/rosci.py @@ -25,8 +25,6 @@ #For running this script locally #Extract the root where both realsense-ros and librealsense are cloned ws_local = '/'.join(os.path.abspath( __file__ ).split( os.path.sep )[0:-5]) - print("##Check point") - print(ws_local) #expected to have 'librealsense' repo in parallel to 'realsense-ros' assert os.path.exists( os.path.join(ws_local, 'librealsense')), f" 'librealsense' doesn't exist at {ws_local} " sys.path.append( os.path.join( ws_local, 'librealsense/unit-tests/py' )) From b9626dfcf87e9725e7481c938f2d077973b6e613 Mon Sep 17 00:00:00 2001 From: Noy-Zini Date: Tue, 24 Sep 2024 17:04:06 +0300 Subject: [PATCH 093/111] replace plugins versions withe commit hash --- .github/workflows/main.yml | 8 ++++---- .github/workflows/pre-release.yml | 2 +- .github/workflows/static_analysis.yaml | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 3cfe428f71..d2fbe21bf2 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -46,7 +46,7 @@ jobs: run: | mkdir -p ${{github.workspace}}/ros2/src - - uses: actions/checkout@v4 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 with: path: 'ros2/src/realsense-ros' @@ -61,17 +61,17 @@ jobs: # so, seperating steps with if conditions - name: build ROS2 for foxy if: ${{ matrix.ros_distro == 'foxy' }} - uses: ros-tooling/setup-ros@v0.6 + uses: ros-tooling/setup-ros@0395475ab6416edc274bb3c43673b2091a217e02 #v0.6 with: required-ros-distributions: ${{ matrix.ros_distro }} - name: build ROS2 for humble/iron/rolling/jazzy if: ${{ matrix.ros_distro != 'foxy' }} - uses: ros-tooling/setup-ros@v0.7 + uses: ros-tooling/setup-ros@a6ce30ecca1e5dcc10ae5e6a44fe2169115bf852 #v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} - name: Checkout librealsense/development - uses: actions/checkout@v4 + uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 with: repository: IntelRealSense/librealsense path: librealsense diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index 29122ffe10..689f0db7dc 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -45,6 +45,6 @@ jobs: BASEDIR: ${{ github.workspace }}/.work steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 - name: industrial_ci uses: ros-industrial/industrial_ci@master diff --git a/.github/workflows/static_analysis.yaml b/.github/workflows/static_analysis.yaml index 26cb9bb20d..710b9df92c 100644 --- a/.github/workflows/static_analysis.yaml +++ b/.github/workflows/static_analysis.yaml @@ -13,7 +13,7 @@ jobs: name: cppcheck runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@f43a0e5ff2bd294095638e18286ca9a3d1956744 #v3 - name: Install shell: bash From 3151f4ebdc4904276b981eab98c204a9e61ef871 Mon Sep 17 00:00:00 2001 From: Noy-Zini Date: Tue, 24 Sep 2024 18:21:39 +0300 Subject: [PATCH 094/111] Replace reference to master with latest commit hash --- .github/workflows/pre-release.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index 689f0db7dc..54c1d78d64 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -47,4 +47,4 @@ jobs: steps: - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 - name: industrial_ci - uses: ros-industrial/industrial_ci@master + uses: ros-industrial/industrial_ci@d23b9ad2c63bfad638a2b1fe3df34b8df9a2f17b #Replace reference to 'master' with the latest commit hash From 63a303d9760c9fafac3ccbc848ff931ac7d0bbea Mon Sep 17 00:00:00 2001 From: Noy-Zini Date: Wed, 25 Sep 2024 11:05:55 +0300 Subject: [PATCH 095/111] adjustments --- .github/workflows/main.yml | 2 +- .github/workflows/static_analysis.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index d2fbe21bf2..bae808d38e 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -61,7 +61,7 @@ jobs: # so, seperating steps with if conditions - name: build ROS2 for foxy if: ${{ matrix.ros_distro == 'foxy' }} - uses: ros-tooling/setup-ros@0395475ab6416edc274bb3c43673b2091a217e02 #v0.6 + uses: ros-tooling/setup-ros@236ab287884fd5a314fc030e91b2017abb46719e #v0.6 with: required-ros-distributions: ${{ matrix.ros_distro }} - name: build ROS2 for humble/iron/rolling/jazzy diff --git a/.github/workflows/static_analysis.yaml b/.github/workflows/static_analysis.yaml index 710b9df92c..b8cdeca85e 100644 --- a/.github/workflows/static_analysis.yaml +++ b/.github/workflows/static_analysis.yaml @@ -13,7 +13,7 @@ jobs: name: cppcheck runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@f43a0e5ff2bd294095638e18286ca9a3d1956744 #v3 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 - name: Install shell: bash From 4243224436dc1580aeab89a1cbba47ea3ab7be79 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 26 Sep 2024 21:30:16 +0530 Subject: [PATCH 096/111] Added hw_reset service call to reset the device --- README.md | 6 ++++ realsense2_camera/CMakeLists.txt | 2 ++ .../include/base_realsense_node.h | 5 +++ realsense2_camera/src/rs_node_setup.cpp | 32 +++++++++++++++++++ 4 files changed, 45 insertions(+) diff --git a/README.md b/README.md index c40321bc4c..49f4a1c474 100644 --- a/README.md +++ b/README.md @@ -298,6 +298,7 @@ User can set the camera name and camera namespace, to distinguish between camera /robot1/D455_1/imu > ros2 service list + /robot1/D455_1/hw_reset /robot1/D455_1/device_info ``` @@ -320,6 +321,7 @@ User can set the camera name and camera namespace, to distinguish between camera /camera/camera/imu > ros2 service list +/camera/camera/hw_reset /camera/camera/device_info ``` @@ -639,6 +641,10 @@ Each of the above filters have it's own parameters, following the naming convent ## Available services +### hw_reset: + - reset the device. The call stops all the streams too. + - Call example: `ros2 service call /camera/camera/hw_reset std_srvs/srv/Empty` + ### device_info: - retrieve information about the device - serial_number, firmware_version etc. - Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 4391297776..1cb78f4910 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -110,6 +110,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(realsense2_camera_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -245,6 +246,7 @@ set(dependencies rclcpp rclcpp_components realsense2_camera_msgs + std_srvs std_msgs sensor_msgs nav_msgs diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index c1da386cf2..e6b4287a81 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -27,6 +27,7 @@ #include #include +#include #include "realsense2_camera_msgs/msg/imu_info.hpp" #include "realsense2_camera_msgs/msg/extrinsics.hpp" #include "realsense2_camera_msgs/msg/metadata.hpp" @@ -155,6 +156,7 @@ namespace realsense2_camera std::string _camera_name; std::vector _monitor_options; rclcpp::Logger _logger; + rclcpp::Service::SharedPtr _reset_srv; rclcpp::Service::SharedPtr _device_info_srv; rclcpp::Service::SharedPtr _calib_config_read_srv; rclcpp::Service::SharedPtr _calib_config_write_srv; @@ -166,6 +168,9 @@ namespace realsense2_camera void restartStaticTransformBroadcaster(); void publishExtrinsicsTopic(const stream_index_pair& sip, const rs2_extrinsics& ex); void calcAndAppendTransformMsgs(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile); + + void handleHWReset(const std_srvs::srv::Empty::Request::SharedPtr req, + const std_srvs::srv::Empty::Response::SharedPtr res); void getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req, realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res); void CalibConfigReadService(const realsense2_camera_msgs::srv::CalibConfigRead::Request::SharedPtr req, diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 2f9d9bc1dc..d95c8b52f9 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -499,6 +499,12 @@ void BaseRealSenseNode::publishServices() { // adding "~/" to the service name will add node namespace and node name to the service // see "Private Namespace Substitution Character" section on https://design.ros2.org/articles/topic_and_service_names.html + _reset_srv = _node.create_service( + "~/hw_reset", + [&](const std_srvs::srv::Empty::Request::SharedPtr req, + std_srvs::srv::Empty::Response::SharedPtr res) + {handleHWReset(req, res);}); + _device_info_srv = _node.create_service( "~/device_info", [&](const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req, @@ -535,6 +541,32 @@ void BaseRealSenseNode::publishActions() } +void BaseRealSenseNode::handleHWReset(const std_srvs::srv::Empty::Request::SharedPtr req, + const std_srvs::srv::Empty::Response::SharedPtr res) +{ + (void)req; + (void)res; + ROS_INFO_STREAM("Reset requested through service call"); + if (_dev) + { + try + { + for(auto&& sensor : _available_ros_sensors) + { + std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); + ROS_INFO_STREAM("Stopping Sensor: " << module_name); + sensor->stop(); + } + ROS_INFO("Resetting device..."); + _dev.hardware_reset(); + } + catch(const std::exception& ex) + { + ROS_WARN_STREAM("An exception has been thrown: " << __FILE__ << ":" << __LINE__ << ":" << ex.what()); + } + } +} + void BaseRealSenseNode::getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr, realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res) { From 3d540ec57750dc1d9c57268a0bdb008dca4d9141 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 26 Sep 2024 21:30:16 +0530 Subject: [PATCH 097/111] Added hw_reset service call to reset the device Added a test in D455 to test the reset device --- README.md | 6 ++ realsense2_camera/CMakeLists.txt | 3 + .../include/base_realsense_node.h | 5 ++ realsense2_camera/src/rs_node_setup.cpp | 32 ++++++++ .../test/live_camera/test_d455_basic_tests.py | 74 +++++++++++++++++++ .../test/utils/pytest_rs_utils.py | 16 +++- 6 files changed, 134 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index c40321bc4c..49f4a1c474 100644 --- a/README.md +++ b/README.md @@ -298,6 +298,7 @@ User can set the camera name and camera namespace, to distinguish between camera /robot1/D455_1/imu > ros2 service list + /robot1/D455_1/hw_reset /robot1/D455_1/device_info ``` @@ -320,6 +321,7 @@ User can set the camera name and camera namespace, to distinguish between camera /camera/camera/imu > ros2 service list +/camera/camera/hw_reset /camera/camera/device_info ``` @@ -639,6 +641,10 @@ Each of the above filters have it's own parameters, following the naming convent ## Available services +### hw_reset: + - reset the device. The call stops all the streams too. + - Call example: `ros2 service call /camera/camera/hw_reset std_srvs/srv/Empty` + ### device_info: - retrieve information about the device - serial_number, firmware_version etc. - Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 4391297776..3453a665de 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -110,6 +110,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(realsense2_camera_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -245,6 +246,7 @@ set(dependencies rclcpp rclcpp_components realsense2_camera_msgs + std_srvs std_msgs sensor_msgs nav_msgs @@ -318,6 +320,7 @@ if(BUILD_TESTING) $ ) ament_target_dependencies(${_test_name} + std_srvs std_msgs ) #target_link_libraries(${_test_name} name_of_local_library) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index c1da386cf2..e6b4287a81 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -27,6 +27,7 @@ #include #include +#include #include "realsense2_camera_msgs/msg/imu_info.hpp" #include "realsense2_camera_msgs/msg/extrinsics.hpp" #include "realsense2_camera_msgs/msg/metadata.hpp" @@ -155,6 +156,7 @@ namespace realsense2_camera std::string _camera_name; std::vector _monitor_options; rclcpp::Logger _logger; + rclcpp::Service::SharedPtr _reset_srv; rclcpp::Service::SharedPtr _device_info_srv; rclcpp::Service::SharedPtr _calib_config_read_srv; rclcpp::Service::SharedPtr _calib_config_write_srv; @@ -166,6 +168,9 @@ namespace realsense2_camera void restartStaticTransformBroadcaster(); void publishExtrinsicsTopic(const stream_index_pair& sip, const rs2_extrinsics& ex); void calcAndAppendTransformMsgs(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile); + + void handleHWReset(const std_srvs::srv::Empty::Request::SharedPtr req, + const std_srvs::srv::Empty::Response::SharedPtr res); void getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req, realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res); void CalibConfigReadService(const realsense2_camera_msgs::srv::CalibConfigRead::Request::SharedPtr req, diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 2f9d9bc1dc..d95c8b52f9 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -499,6 +499,12 @@ void BaseRealSenseNode::publishServices() { // adding "~/" to the service name will add node namespace and node name to the service // see "Private Namespace Substitution Character" section on https://design.ros2.org/articles/topic_and_service_names.html + _reset_srv = _node.create_service( + "~/hw_reset", + [&](const std_srvs::srv::Empty::Request::SharedPtr req, + std_srvs::srv::Empty::Response::SharedPtr res) + {handleHWReset(req, res);}); + _device_info_srv = _node.create_service( "~/device_info", [&](const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req, @@ -535,6 +541,32 @@ void BaseRealSenseNode::publishActions() } +void BaseRealSenseNode::handleHWReset(const std_srvs::srv::Empty::Request::SharedPtr req, + const std_srvs::srv::Empty::Response::SharedPtr res) +{ + (void)req; + (void)res; + ROS_INFO_STREAM("Reset requested through service call"); + if (_dev) + { + try + { + for(auto&& sensor : _available_ros_sensors) + { + std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); + ROS_INFO_STREAM("Stopping Sensor: " << module_name); + sensor->stop(); + } + ROS_INFO("Resetting device..."); + _dev.hardware_reset(); + } + catch(const std::exception& ex) + { + ROS_WARN_STREAM("An exception has been thrown: " << __FILE__ << ":" << __LINE__ << ":" << ex.what()); + } + } +} + void BaseRealSenseNode::getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr, realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res) { diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index 687b4949ec..81b66493fc 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -156,3 +156,77 @@ def test_D455_Seq_ID_update(self,launch_descr_with_parameters): pytest_rs_utils.kill_realsense2_camera_node() self.shutdown() + + +test_params_reset_device = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D455 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.d455 +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestD455_reset_device(pytest_rs_utils.RsTestBaseClass): + def test_D455_Reset_Device(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + assert False + return + + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + #'data':data + } + ] + try: + ''' + initialize, run and check the data + ''' + print("Starting camera test...") + self.init_test("RsTest"+params['camera_name']) + self.wait_for_node(params['camera_name']) + self.create_service_client_ifs(get_node_heirarchy(params)) + self.spin_for_time(0.5) + assert self.set_bool_param('enable_color', False) + self.spin_for_time(0.5) + assert self.set_string_param('rgb_camera.color_profile', '640x480x30') + self.spin_for_time(0.5) + assert self.set_bool_param('enable_color', True) + self.spin_for_time(0.5) + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + self.set_string_param('rgb_camera.color_profile', '1280x800x5') + self.set_bool_param('enable_color', True) + themes[0]['width'] = 1280 + themes[0]['height'] = 800 + + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + + self.reset_device() + + #default value, the test will fail if default value is changed in librealsense + themes[0]['width'] = 1280 + themes[0]['height'] = 720 + + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 75cfb78ecd..4129ae66c0 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -46,6 +46,7 @@ from rcl_interfaces.msg import Parameter from rcl_interfaces.msg import ParameterValue from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from std_srvs.srv import Empty ''' humble doesn't have the SetParametersResult and SetParameters_Response imported using __init__.py. The below two lines can be used for iron and hopefully succeeding ROS2 versions @@ -784,18 +785,29 @@ def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, else: self.node.reset_data(topic) - - def create_service_client_ifs(self, camera_name): self.set_param_if = self.node.create_client(SetParameters, camera_name + '/set_parameters') self.get_param_if = self.node.create_client(GetParameters, camera_name + '/get_parameters') self.get_device_info = self.node.create_client(DeviceInfo, camera_name + '/device_info') + self.reset_if = self.node.create_client(Empty, camera_name + '/hw_reset') while not self.get_param_if.wait_for_service(timeout_sec=1.0): print('service not available, waiting again...') while not self.set_param_if.wait_for_service(timeout_sec=1.0): print('service not available, waiting again...') while not self.get_device_info.wait_for_service(timeout_sec=1.0): print('service not available, waiting again...') + while not self.reset_if.wait_for_service(timeout_sec=1.0): + print('hw_reset service not available, waiting again...') + + def reset_device(self): + req = Empty.Request() + future = self.reset_if.call_async(req) + while rclpy.ok(): + rclpy.spin_once(self.node) + if future.done(): + return True + return False + def send_param(self, req): future = self.set_param_if.call_async(req) From 75612a177495f1da5ad445fc9f6f3edc473aa46e Mon Sep 17 00:00:00 2001 From: Patrick Wspanialy Date: Fri, 4 Oct 2024 13:31:56 -0400 Subject: [PATCH 098/111] fix config typo --- realsense2_camera/examples/launch_params_from_file/README.md | 2 +- .../examples/launch_params_from_file/config/config.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/examples/launch_params_from_file/README.md b/realsense2_camera/examples/launch_params_from_file/README.md index ce9980aa5b..f12903a25f 100644 --- a/realsense2_camera/examples/launch_params_from_file/README.md +++ b/realsense2_camera/examples/launch_params_from_file/README.md @@ -24,7 +24,7 @@ param2: value Example: ``` enable_color: true -rgb_camera.profile: 1280x720x15 +rgb_camera.color_profile: 1280x720x15 enable_depth: true align_depth.enable: true enable_sync: true diff --git a/realsense2_camera/examples/launch_params_from_file/config/config.yaml b/realsense2_camera/examples/launch_params_from_file/config/config.yaml index d15a19a0fe..9d63fec7fe 100644 --- a/realsense2_camera/examples/launch_params_from_file/config/config.yaml +++ b/realsense2_camera/examples/launch_params_from_file/config/config.yaml @@ -1,5 +1,5 @@ enable_color: true -rgb_camera.profile: 1280x720x15 +rgb_camera.color_profile: 1280x720x15 enable_depth: true align_depth.enable: true enable_sync: true From 10f9c62b84fe30612d43ac7b1840282cf0f902cf Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sat, 5 Oct 2024 23:33:29 +0300 Subject: [PATCH 099/111] Add D555 PID --- realsense2_camera/CMakeLists.txt | 1 + realsense2_camera/include/constants.h | 3 +- realsense2_camera/include/ros_utils.h | 1 + realsense2_camera/launch/rs_launch.py | 1 + realsense2_camera/src/base_realsense_node.cpp | 55 +++++++++++++++---- .../src/realsense_node_factory.cpp | 44 +++++++++++---- realsense2_camera/src/rs_node_setup.cpp | 10 +++- 7 files changed, 90 insertions(+), 25 deletions(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 3453a665de..864cb25ae4 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -118,6 +118,7 @@ find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core) +find_package(fastrtps REQUIRED) # a workaround, till librealsense2 cmake config will be fixed find_package(realsense2 2.56.0) if (BUILD_ACCELERATE_GPU_WITH_GLSL) diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 7f5b28cee1..c8904a14eb 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -65,7 +65,8 @@ namespace realsense2_camera const uint16_t RS430i_PID = 0x0b4b; // D430i const uint16_t RS405_PID = 0x0B5B; // DS5U const uint16_t RS455_PID = 0x0B5C; // D455 - const uint16_t RS457_PID = 0xABCD; // D457 + const uint16_t RS457_PID = 0xABCD; // D457 + const uint16_t RS555_PID = 0x0B56; // D555 const bool ALLOW_NO_TEXTURE_POINTS = false; const bool ORDERED_PC = false; diff --git a/realsense2_camera/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index d31b0d42bf..937162dcb9 100644 --- a/realsense2_camera/include/ros_utils.h +++ b/realsense2_camera/include/ros_utils.h @@ -33,6 +33,7 @@ namespace realsense2_camera const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2}; const stream_index_pair GYRO{RS2_STREAM_GYRO, 0}; const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0}; + const stream_index_pair IMU{RS2_STREAM_MOTION, 0}; bool isValidCharInName(char c); diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index a79df0f84b..14f3139c4b 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -62,6 +62,7 @@ {'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"}, {'name': 'enable_accel', 'default': 'false', 'description': "'enable accel stream'"}, {'name': 'gyro_fps', 'default': '0', 'description': "''"}, + {'name': 'enable_motion', 'default': 'false', 'description': "'enable motion stream (IMU) for DDS devices'"}, {'name': 'accel_fps', 'default': '0', 'description': "''"}, {'name': 'unite_imu_method', 'default': "0", 'description': '[0-None, 1-copy, 2-linear_interpolation]'}, {'name': 'clip_distance', 'default': '-2.', 'description': "''"}, diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 4b88ec7df1..c18d4d89d0 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -480,7 +480,26 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) frame.get_profile().stream_index(), rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain())); - auto stream_index = (stream == GYRO.first)?GYRO:ACCEL; + stream_index_pair stream_index; + + if(stream == GYRO.first) + { + stream_index = GYRO; + } + else if(stream == ACCEL.first) + { + stream_index = ACCEL; + } + else if(stream == IMU.first) + { + stream_index = IMU; + } + else + { + ROS_ERROR("Unknown IMU stream type."); + return; + } + rclcpp::Time t(frameSystemTimeSec(frame)); if(_imu_publishers.find(stream_index) == _imu_publishers.end()) @@ -495,19 +514,35 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) ImuMessage_AddDefaultValues(imu_msg); imu_msg.header.frame_id = OPTICAL_FRAME_ID(stream_index); - auto crnt_reading = *(reinterpret_cast(frame.get_data())); - if (GYRO == stream_index) + const float3 *crnt_reading = reinterpret_cast(frame.get_data()); + if (IMU == stream_index) + { + // Expecting two float3 objects: first for accel, second for gyro + const float3 &accel_data = crnt_reading[0]; + const float3 &gyro_data = crnt_reading[1]; + + // Fill the IMU ROS2 message with both accel and gyro data + imu_msg.linear_acceleration.x = accel_data.x; + imu_msg.linear_acceleration.y = accel_data.y; + imu_msg.linear_acceleration.z = accel_data.z; + + imu_msg.angular_velocity.x = gyro_data.x; + imu_msg.angular_velocity.y = gyro_data.y; + imu_msg.angular_velocity.z = gyro_data.z; + } + else if (GYRO == stream_index) { - imu_msg.angular_velocity.x = crnt_reading.x; - imu_msg.angular_velocity.y = crnt_reading.y; - imu_msg.angular_velocity.z = crnt_reading.z; + imu_msg.angular_velocity.x = crnt_reading->x; + imu_msg.angular_velocity.y = crnt_reading->y; + imu_msg.angular_velocity.z = crnt_reading->z; } - else if (ACCEL == stream_index) + else // ACCEL == stream_index { - imu_msg.linear_acceleration.x = crnt_reading.x; - imu_msg.linear_acceleration.y = crnt_reading.y; - imu_msg.linear_acceleration.z = crnt_reading.z; + imu_msg.linear_acceleration.x = crnt_reading->x; + imu_msg.linear_acceleration.y = crnt_reading->y; + imu_msg.linear_acceleration.z = crnt_reading->z; } + imu_msg.header.stamp = t; _imu_publishers[stream_index]->publish(imu_msg); ROS_DEBUG("Publish %s stream", ros_stream_to_string(frame.get_profile().stream_type()).c_str()); diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index f7ea18bca7..8be212daf5 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -107,24 +107,31 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) std::vector results; ROS_INFO_STREAM("Device with name " << name << " was found."); std::string port_id = parseUsbPort(pn); - if (port_id.empty()) + + std::string pid_str(dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID)); + if(pid_str != "DDS") { - std::stringstream msg; - msg << "Error extracting usb port from device with physical ID: " << pn << std::endl << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros"; - if (_usb_port_id.empty()) + if (port_id.empty()) { - ROS_WARN_STREAM(msg.str()); + std::stringstream msg; + msg << "Error extracting usb port from device with physical ID: " << pn << std::endl + << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros"; + if (_usb_port_id.empty()) + { + ROS_WARN_STREAM(msg.str()); + } + else + { + ROS_ERROR_STREAM(msg.str()); + ROS_ERROR_STREAM("Please use serial number instead of usb port."); + } } else { - ROS_ERROR_STREAM(msg.str()); - ROS_ERROR_STREAM("Please use serial number instead of usb port."); + ROS_INFO_STREAM("Device with port number " << port_id << " was found."); } } - else - { - ROS_INFO_STREAM("Device with port number " << port_id << " was found."); - } + bool found_device_type(true); if (!_device_type.empty()) { @@ -352,8 +359,20 @@ void RealSenseNodeFactory::init() void RealSenseNodeFactory::startDevice() { if (_realSenseNode) _realSenseNode.reset(); + std::string device_name = _device.get_info(RS2_CAMERA_INFO_NAME); std::string pid_str(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID)); - uint16_t pid = std::stoi(pid_str, 0, 16); + uint16_t pid; + + if (device_name == "Intel RealSense D555") + { + // currently the PID of DDS devices is hardcoded as "DDS" + // need to be fixed in librealsense + pid = RS555_PID; + } + else + { + pid = std::stoi(pid_str, 0, 16); + } try { switch(pid) @@ -374,6 +393,7 @@ void RealSenseNodeFactory::startDevice() case RS435i_RGB_PID: case RS455_PID: case RS457_PID: + case RS555_PID: case RS_USB2_PID: _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); break; diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index d95c8b52f9..8bf6bba63b 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -295,8 +295,14 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); // Publish Intrinsics: info_topic_name << "~/" << stream_name << "/imu_info"; - _imu_info_publishers[sip] = _node.create_publisher(info_topic_name.str(), - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); + + // QoS settings for Latching-like behavior for the imu_info topic + // History: KeepLast(1) - Retains only the last message + // Durability: Transient Local - Ensures that late subscribers get the last message that was published + // Reliability: Ensures reliable delivery of messages + auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); + _imu_info_publishers[sip] = _node.create_publisher(info_topic_name.str(), qos); + IMUInfo info_msg = getImuInfo(profile); _imu_info_publishers[sip]->publish(info_msg); } From f57a6170b8ad31ba502d5fd50ea88d904f4fd5ee Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 8 Oct 2024 17:27:48 +0300 Subject: [PATCH 100/111] adjusments to DDS Support for ROS Wrapper --- realsense2_camera/CMakeLists.txt | 2 +- realsense2_camera/src/base_realsense_node.cpp | 31 +++++++++++++------ .../src/realsense_node_factory.cpp | 2 +- 3 files changed, 24 insertions(+), 11 deletions(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 864cb25ae4..0e78eb1957 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -118,7 +118,7 @@ find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core) -find_package(fastrtps REQUIRED) # a workaround, till librealsense2 cmake config will be fixed +find_package(fastrtps REQUIRED) # TODO: LRS-1203 - This is a workaround, till librealsense2 cmake config will be fixed find_package(realsense2 2.56.0) if (BUILD_ACCELERATE_GPU_WITH_GLSL) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index c18d4d89d0..72cc1c3ee0 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -515,20 +515,33 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) imu_msg.header.frame_id = OPTICAL_FRAME_ID(stream_index); const float3 *crnt_reading = reinterpret_cast(frame.get_data()); + size_t data_size = frame.get_data_size(); + if (IMU == stream_index) { // Expecting two float3 objects: first for accel, second for gyro - const float3 &accel_data = crnt_reading[0]; - const float3 &gyro_data = crnt_reading[1]; + // Check if we have at least two float3s + if (data_size >= 2 * sizeof(float3)) + { + const float3 &accel_data = crnt_reading[0]; + const float3 &gyro_data = crnt_reading[1]; - // Fill the IMU ROS2 message with both accel and gyro data - imu_msg.linear_acceleration.x = accel_data.x; - imu_msg.linear_acceleration.y = accel_data.y; - imu_msg.linear_acceleration.z = accel_data.z; + // Fill the IMU ROS2 message with both accel and gyro data + imu_msg.linear_acceleration.x = accel_data.x; + imu_msg.linear_acceleration.y = accel_data.y; + imu_msg.linear_acceleration.z = accel_data.z; - imu_msg.angular_velocity.x = gyro_data.x; - imu_msg.angular_velocity.y = gyro_data.y; - imu_msg.angular_velocity.z = gyro_data.z; + imu_msg.angular_velocity.x = gyro_data.x; + imu_msg.angular_velocity.y = gyro_data.y; + imu_msg.angular_velocity.z = gyro_data.z; + } + else + { + ROS_ERROR_STREAM("Insufficient data for IMU (Motion) frame: expected at least " + << 2 * sizeof(float3) << " bytes, but got " + << data_size << " bytes."); + return; + } } else if (GYRO == stream_index) { diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 8be212daf5..f80ed002e4 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -359,7 +359,7 @@ void RealSenseNodeFactory::init() void RealSenseNodeFactory::startDevice() { if (_realSenseNode) _realSenseNode.reset(); - std::string device_name = _device.get_info(RS2_CAMERA_INFO_NAME); + std::string device_name(_device.get_info(RS2_CAMERA_INFO_NAME)); std::string pid_str(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID)); uint16_t pid; From 9f37e29d31ca718c3dc0a7c9d1636b3df2263082 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 8 Oct 2024 18:06:18 +0300 Subject: [PATCH 101/111] support latched QoS for imu_info --- realsense2_camera/src/rs_node_setup.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 8bf6bba63b..df0c15d8c7 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -296,13 +296,13 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi // Publish Intrinsics: info_topic_name << "~/" << stream_name << "/imu_info"; - // QoS settings for Latching-like behavior for the imu_info topic - // History: KeepLast(1) - Retains only the last message - // Durability: Transient Local - Ensures that late subscribers get the last message that was published - // Reliability: Ensures reliable delivery of messages - auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); - _imu_info_publishers[sip] = _node.create_publisher(info_topic_name.str(), qos); - + // IMU Info will have latched QoS, and it will publish its data only once during the ROS Node lifetime. + // intra-process do not support latched QoS, so we need to disable intra-process for this topic + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + _imu_info_publishers[sip] = _node.create_publisher(info_topic_name.str(), + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_latched), rmw_qos_profile_latched), + std::move(options)); IMUInfo info_msg = getImuInfo(profile); _imu_info_publishers[sip]->publish(info_msg); } From 5286d263860a1e732f05dde9e8c140918171ed32 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 10 Oct 2024 10:56:47 +0300 Subject: [PATCH 102/111] use new APIs for motion, accel and gryo streams --- realsense2_camera/include/ros_utils.h | 2 +- realsense2_camera/src/base_realsense_node.cpp | 65 +++++++++---------- 2 files changed, 30 insertions(+), 37 deletions(-) diff --git a/realsense2_camera/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index 937162dcb9..6980d2bf32 100644 --- a/realsense2_camera/include/ros_utils.h +++ b/realsense2_camera/include/ros_utils.h @@ -33,7 +33,7 @@ namespace realsense2_camera const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2}; const stream_index_pair GYRO{RS2_STREAM_GYRO, 0}; const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0}; - const stream_index_pair IMU{RS2_STREAM_MOTION, 0}; + const stream_index_pair MOTION{RS2_STREAM_MOTION, 0}; bool isValidCharInName(char c); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 72cc1c3ee0..11126452b0 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -490,9 +490,9 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) { stream_index = ACCEL; } - else if(stream == IMU.first) + else if(stream == MOTION.first) { - stream_index = IMU; + stream_index = MOTION; } else { @@ -514,47 +514,40 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) ImuMessage_AddDefaultValues(imu_msg); imu_msg.header.frame_id = OPTICAL_FRAME_ID(stream_index); - const float3 *crnt_reading = reinterpret_cast(frame.get_data()); - size_t data_size = frame.get_data_size(); - - if (IMU == stream_index) + if (MOTION == stream_index) { - // Expecting two float3 objects: first for accel, second for gyro - // Check if we have at least two float3s - if (data_size >= 2 * sizeof(float3)) - { - const float3 &accel_data = crnt_reading[0]; - const float3 &gyro_data = crnt_reading[1]; + auto combined_motion_data = frame.as().get_combined_motion_data(); + + imu_msg.linear_acceleration.x = combined_motion_data.linear_acceleration.x; + imu_msg.linear_acceleration.y = combined_motion_data.linear_acceleration.y; + imu_msg.linear_acceleration.z = combined_motion_data.linear_acceleration.z; + + imu_msg.angular_velocity.x = combined_motion_data.angular_velocity.x; + imu_msg.angular_velocity.y = combined_motion_data.angular_velocity.y; + imu_msg.angular_velocity.z = combined_motion_data.angular_velocity.z; - // Fill the IMU ROS2 message with both accel and gyro data - imu_msg.linear_acceleration.x = accel_data.x; - imu_msg.linear_acceleration.y = accel_data.y; - imu_msg.linear_acceleration.z = accel_data.z; + imu_msg.orientation.x = combined_motion_data.orientation.x; + imu_msg.orientation.y = combined_motion_data.orientation.y; + imu_msg.orientation.z = combined_motion_data.orientation.z; + imu_msg.orientation.w = combined_motion_data.orientation.w; - imu_msg.angular_velocity.x = gyro_data.x; - imu_msg.angular_velocity.y = gyro_data.y; - imu_msg.angular_velocity.z = gyro_data.z; + } + else + { + auto motion_data = frame.as().get_motion_data(); + if (GYRO == stream_index) + { + imu_msg.angular_velocity.x = motion_data.x; + imu_msg.angular_velocity.y = motion_data.y; + imu_msg.angular_velocity.z = motion_data.z; } - else + else // ACCEL == stream_index { - ROS_ERROR_STREAM("Insufficient data for IMU (Motion) frame: expected at least " - << 2 * sizeof(float3) << " bytes, but got " - << data_size << " bytes."); - return; + imu_msg.linear_acceleration.x = motion_data.x; + imu_msg.linear_acceleration.y = motion_data.y; + imu_msg.linear_acceleration.z = motion_data.z; } } - else if (GYRO == stream_index) - { - imu_msg.angular_velocity.x = crnt_reading->x; - imu_msg.angular_velocity.y = crnt_reading->y; - imu_msg.angular_velocity.z = crnt_reading->z; - } - else // ACCEL == stream_index - { - imu_msg.linear_acceleration.x = crnt_reading->x; - imu_msg.linear_acceleration.y = crnt_reading->y; - imu_msg.linear_acceleration.z = crnt_reading->z; - } imu_msg.header.stamp = t; _imu_publishers[stream_index]->publish(imu_msg); From 85435dd1f7eadd48e27e533db6932edddb744291 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Tue, 29 Oct 2024 11:52:00 +0200 Subject: [PATCH 103/111] Update CMakeLists.txt --- realsense2_camera/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 0e78eb1957..d9544cc197 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -118,7 +118,7 @@ find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core) -find_package(fastrtps REQUIRED) # TODO: LRS-1203 - This is a workaround, till librealsense2 cmake config will be fixed +# find_package(fastrtps REQUIRED) # TODO: LRS-1203 - This is a workaround, till librealsense2 cmake config will be fixed find_package(realsense2 2.56.0) if (BUILD_ACCELERATE_GPU_WITH_GLSL) From b9c999ac2b054444d834d7ef55c58f14782fe1ba Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Wed, 30 Oct 2024 08:56:05 +0200 Subject: [PATCH 104/111] Update CMakeLists.txt --- realsense2_camera/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index d9544cc197..3453a665de 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -118,7 +118,6 @@ find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core) -# find_package(fastrtps REQUIRED) # TODO: LRS-1203 - This is a workaround, till librealsense2 cmake config will be fixed find_package(realsense2 2.56.0) if (BUILD_ACCELERATE_GPU_WITH_GLSL) From fe65054d8a098dc8df673cc7cf71c57bb77d3e05 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 31 Oct 2024 11:00:57 +0200 Subject: [PATCH 105/111] bump version to 4.56.3 --- README.md | 2 +- realsense2_camera/CHANGELOG.rst | 23 +++++++++++++++++++++++ realsense2_camera/CMakeLists.txt | 4 ++-- realsense2_camera/include/constants.h | 4 ++-- realsense2_camera_msgs/CHANGELOG.rst | 8 ++++++++ realsense2_description/CHANGELOG.rst | 3 +++ 6 files changed, 39 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 49f4a1c474..8c8d757a5e 100644 --- a/README.md +++ b/README.md @@ -119,7 +119,7 @@ - For example, for Humble distro: ```sudo apt install ros-humble-librealsense2*``` - #### Option 3: Build from source - - Download the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.55.1) + - Download the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.56.3) - Follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) diff --git a/realsense2_camera/CHANGELOG.rst b/realsense2_camera/CHANGELOG.rst index 9098646e47..2b85fffe2d 100644 --- a/realsense2_camera/CHANGELOG.rst +++ b/realsense2_camera/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package realsense2_camera ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* PR `#3239 `_ from SamerKhshiboun: Update CMakeLists.txt - remove find_package(fastrtps REQUIRED) +* PR `#3225 `_ from SamerKhshiboun: Use new APIs for motion, accel and gryo streams +* PR `#3222 `_ from SamerKhshiboun: Support D555 and its motion profiles +* PR `#3221 `_ from patrickwasp: fix config typo +* PR `#3216 `_ from PrasRsRos: hw_reset implementation +* PR `#3200 `_ from kadiredd: retry thrice finding devices with Ykush reset +* PR `#3178 `_ from kadiredd: disabling FPS & TF tests for ROS-CI +* PR `#3166 `_ from SamerKhshiboun: Update Calibration Config API +* PR `#3159 `_ from noacoohen: Add D421 PID +* PR `#3153 `_ from SamerKhshiboun: TC | Fix feedback and update readme +* PR `#3147 `_ from SamerKhshiboun: Update READMEs and CONTRIBUTING files regarding ros2-master +* PR `#3138 `_ from SamerKhshiboun: Support Triggered Calibration as ROS2 Action +* PR `#3135 `_ from kadiredd: Casefolding device name instead of strict case sensitive comparison +* PR `#3133 `_ from SamerKhshiboun: update librealsense2 version to 2.56.0 +* PR `#3124 `_ from kadiredd: Support testing ROS2 service call device_info +* PR `#3125 `_ from SamerKhshiboun: Support calibration config read/write services +* PR `#3114 `_ from Arun-Prasad-V: Ubuntu 24.04 support for Rolling and Jazzy distros +* PR `#3102 `_ from fortizcuesta: Allow hw synchronization of several realsense using a synchonization cable +* PR `#3096 `_ from anisotropicity: Update rs_launch.py to add depth_module.color_profile +* Contributors: Arun-Prasad-V, Madhukar Reddy Kadireddy, Ortiz Cuesta, Fernando, Patrick Wspanialy, PrasRsRos, Samer Khshiboun, SamerKhshiboun, anisotropicity, noacoohen + 4.55.1 (2024-05-28) ------------------- * PR `#3106 `_ from SamerKhshiboun: Remove unused parameter _is_profile_exist diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 3453a665de..9532ce571b 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -119,9 +119,9 @@ find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core) -find_package(realsense2 2.56.0) +find_package(realsense2 2.56.3) if (BUILD_ACCELERATE_GPU_WITH_GLSL) - find_package(realsense2-gl 2.56.0) + find_package(realsense2-gl 2.56.3) endif() if(NOT realsense2_FOUND) message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n") diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index c8904a14eb..d9b82f4741 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -18,8 +18,8 @@ #include #define REALSENSE_ROS_MAJOR_VERSION 4 -#define REALSENSE_ROS_MINOR_VERSION 55 -#define REALSENSE_ROS_PATCH_VERSION 1 +#define REALSENSE_ROS_MINOR_VERSION 56 +#define REALSENSE_ROS_PATCH_VERSION 3 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_camera_msgs/CHANGELOG.rst b/realsense2_camera_msgs/CHANGELOG.rst index 8a9d9a5778..b436639521 100644 --- a/realsense2_camera_msgs/CHANGELOG.rst +++ b/realsense2_camera_msgs/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package realsense2_camera_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* PR `#3153 `_ from SamerKhshiboun: TC | Fix feedback and update readme +* PR `#3138 `_ from SamerKhshiboun: Support Triggered Calibration as ROS2 Action +* PR `#3125 `_ from SamerKhshiboun: Support calibration config read/write services +* PR `#3100 `_ from jiuguangw: Suppress CMake warnings +* Contributors: Jiuguang Wang, Samer Khshiboun, SamerKhshiboun + 4.55.1 (2024-05-28) ------------------- * PR `#2830 `_ from SamerKhshiboun: Add RGBD + reduce changes between hkr and development diff --git a/realsense2_description/CHANGELOG.rst b/realsense2_description/CHANGELOG.rst index 40a0ccae03..948f43f079 100644 --- a/realsense2_description/CHANGELOG.rst +++ b/realsense2_description/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package realsense2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.55.1 (2024-05-28) ------------------- * PR `#2957 `_ from hellototoro: to_urdf fun retrun a str, not a BufferedRandom From d7b549d324f31906cfc7da29d0db88ec32487f16 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 31 Oct 2024 11:01:55 +0200 Subject: [PATCH 106/111] 4.56.3 --- realsense2_camera/CHANGELOG.rst | 4 ++-- realsense2_camera/package.xml | 2 +- realsense2_camera_msgs/CHANGELOG.rst | 4 ++-- realsense2_camera_msgs/package.xml | 2 +- realsense2_description/CHANGELOG.rst | 4 ++-- realsense2_description/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/realsense2_camera/CHANGELOG.rst b/realsense2_camera/CHANGELOG.rst index 2b85fffe2d..bc67ed8907 100644 --- a/realsense2_camera/CHANGELOG.rst +++ b/realsense2_camera/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package realsense2_camera ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.56.3 (2024-10-31) +------------------- * PR `#3239 `_ from SamerKhshiboun: Update CMakeLists.txt - remove find_package(fastrtps REQUIRED) * PR `#3225 `_ from SamerKhshiboun: Use new APIs for motion, accel and gryo streams * PR `#3222 `_ from SamerKhshiboun: Support D555 and its motion profiles diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index d4d53ae3eb..5438cf01c4 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -2,7 +2,7 @@ realsense2_camera - 4.55.1 + 4.56.3 RealSense camera package allowing access to Intel D400 3D cameras LibRealSense ROS Team Apache License 2.0 diff --git a/realsense2_camera_msgs/CHANGELOG.rst b/realsense2_camera_msgs/CHANGELOG.rst index b436639521..0f1fcf57e7 100644 --- a/realsense2_camera_msgs/CHANGELOG.rst +++ b/realsense2_camera_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package realsense2_camera_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.56.3 (2024-10-31) +------------------- * PR `#3153 `_ from SamerKhshiboun: TC | Fix feedback and update readme * PR `#3138 `_ from SamerKhshiboun: Support Triggered Calibration as ROS2 Action * PR `#3125 `_ from SamerKhshiboun: Support calibration config read/write services diff --git a/realsense2_camera_msgs/package.xml b/realsense2_camera_msgs/package.xml index acaa572626..250501dc2c 100644 --- a/realsense2_camera_msgs/package.xml +++ b/realsense2_camera_msgs/package.xml @@ -2,7 +2,7 @@ realsense2_camera_msgs - 4.55.1 + 4.56.3 RealSense camera_msgs package containing realsense camera messages definitions LibRealSense ROS Team Apache License 2.0 diff --git a/realsense2_description/CHANGELOG.rst b/realsense2_description/CHANGELOG.rst index 948f43f079..ee9a1eafe3 100644 --- a/realsense2_description/CHANGELOG.rst +++ b/realsense2_description/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package realsense2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.56.3 (2024-10-31) +------------------- 4.55.1 (2024-05-28) ------------------- diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index 6104a506a9..08a968048c 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -2,7 +2,7 @@ realsense2_description - 4.55.1 + 4.56.3 RealSense description package for Intel 3D D400 cameras LibRealSense ROS Team Apache License 2.0 From 5f335b0400e3d9657fb37acbd81e6c4e6cb77cbe Mon Sep 17 00:00:00 2001 From: Elias De Coninck Date: Fri, 4 Dec 2020 23:35:10 +0100 Subject: [PATCH 107/111] add gazebo plugins --- .../urdf/_d435.gazebo.xacro | 147 ++++++++++++ realsense2_description/urdf/_d435.urdf.xacro | 6 +- .../urdf/_d435i_imu_modules.gazebo.xacro | 97 ++++++++ .../urdf/_d435i_imu_modules.urdf.xacro | 7 + .../urdf/_d455.gazebo.xacro | 217 ++++++++++++++++++ realsense2_description/urdf/_d455.urdf.xacro | 4 + .../urdf/_t265.gazebo.xacro | 140 +++++++++++ 7 files changed, 617 insertions(+), 1 deletion(-) create mode 100644 realsense2_description/urdf/_d435.gazebo.xacro create mode 100644 realsense2_description/urdf/_d435i_imu_modules.gazebo.xacro create mode 100644 realsense2_description/urdf/_d455.gazebo.xacro create mode 100644 realsense2_description/urdf/_t265.gazebo.xacro diff --git a/realsense2_description/urdf/_d435.gazebo.xacro b/realsense2_description/urdf/_d435.gazebo.xacro new file mode 100644 index 0000000000..9f835b608d --- /dev/null +++ b/realsense2_description/urdf/_d435.gazebo.xacro @@ -0,0 +1,147 @@ + + + + + + + + + + + + 0 + 0 + 0 + 1 + + 1 + 0 0 0 + + 1e+13 + 1 + + + + ${69.4*deg_to_rad} + + 1920 + 1080 + RGB_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.007 + + + 1 + 30 + 1 + + + + ${85.2*deg_to_rad} + + 1280 + 720 + L_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.05 + + + 1 + 1 + 0 + + + + ${85.2*deg_to_rad} + + 1280 + 720 + L_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.05 + + + 1 + 1 + 0 + + + + ${85.2*deg_to_rad} + + 1280 + 720 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.100 + + + 1 + 30 + 0 + + + + + + ${camera_name} + 30.0 + 30.0 + 1.0 + ${camera_name}/depth/image_raw + ${camera_name}/depth/camera_info + ${camera_name}/color/image_raw + ${camera_name}/color/camera_info + ${camera_name}/infra1/image_raw + ${camera_name}/infra1/camera_info + ${camera_name}/infra2/image_raw + ${camera_name}/infra2/camera_info + ${color_optical_frame} + ${depth_optical_frame} + ${infrared1_optical_frame} + ${infrared2_optical_frame} + 0.2 + 10.0 + ${publish_pointcloud} + ${camera_name}/depth/color/points + 0.25 + 9.0 + + + + + diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index 722c6e3281..1cf021cfdb 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -25,8 +25,9 @@ aluminum peripherial evaluation case. + - + + + diff --git a/realsense2_description/urdf/_d435i_imu_modules.gazebo.xacro b/realsense2_description/urdf/_d435i_imu_modules.gazebo.xacro new file mode 100644 index 0000000000..fcf80f8d3d --- /dev/null +++ b/realsense2_description/urdf/_d435i_imu_modules.gazebo.xacro @@ -0,0 +1,97 @@ + + + + + + + + + + 1 + + 1 + + 100 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + + + + ~/out:=${camera_name}/camera/gyro/sample + + ${gyro_optical_frame} + false + + + + ~/out:=${camera_name}/camera/accel/sample + + ${accel_optical_frame} + false + + + + + + diff --git a/realsense2_description/urdf/_d435i_imu_modules.urdf.xacro b/realsense2_description/urdf/_d435i_imu_modules.urdf.xacro index cac7f4b367..a462c807db 100644 --- a/realsense2_description/urdf/_d435i_imu_modules.urdf.xacro +++ b/realsense2_description/urdf/_d435i_imu_modules.urdf.xacro @@ -21,6 +21,9 @@ This is the URDF model for the inertial modules of the Intel RealSense 435i camera. --> + + + @@ -54,6 +57,10 @@ Intel RealSense 435i camera. + + + + diff --git a/realsense2_description/urdf/_d455.gazebo.xacro b/realsense2_description/urdf/_d455.gazebo.xacro new file mode 100644 index 0000000000..fc117a1a55 --- /dev/null +++ b/realsense2_description/urdf/_d455.gazebo.xacro @@ -0,0 +1,217 @@ + + + + + + + + + + + + 0 + 0 + 0 + 1 + + 1 + 0 0 0 + + 1e+13 + 1 + + + + ${90*deg_to_rad} + + 1280 + 800 + RGB_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.007 + + + 1 + 30 + 1 + + + + ${87*deg_to_rad} + + 1280 + 720 + L_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.05 + + + 1 + 90 + 0 + + + + ${87*deg_to_rad} + + 1280 + 720 + L_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.05 + + + 1 + 90 + 0 + + + + ${87*deg_to_rad} + + 1280 + 720 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.100 + + + 1 + 90 + 0 + + + 1 + + 100 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + + + ${camera_name} + 30.0 + 30.0 + 30.0 + ${camera_name}/depth/image_raw + ${camera_name}/depth/camera_info + ${camera_name}/color/image_raw + ${camera_name}/color/camera_info + ${camera_name}/infra1/image_raw + ${camera_name}/infra1/camera_info + ${camera_name}/infra2/image_raw + ${camera_name}/infra2/camera_info + ${color_optical_frame} + ${depth_optical_frame} + ${infrared1_optical_frame} + ${infrared2_optical_frame} + 0.2 + 10.0 + ${publish_pointcloud} + ${camera_name}/depth/color/points + 0.25 + 9.0 + + + + ~/out:=${camera_name}/camera/gyro/sample + + ${gyro_optical_frame} + false + + + + ~/out:=${camera_name}/camera/accel/sample + + ${accel_optical_frame} + false + + + + + diff --git a/realsense2_description/urdf/_d455.urdf.xacro b/realsense2_description/urdf/_d455.urdf.xacro index 8804ab880b..2867dff3d9 100644 --- a/realsense2_description/urdf/_d455.urdf.xacro +++ b/realsense2_description/urdf/_d455.urdf.xacro @@ -25,6 +25,7 @@ aluminum peripherial evaluation case. + @@ -201,6 +202,9 @@ aluminum peripherial evaluation case. + + + diff --git a/realsense2_description/urdf/_t265.gazebo.xacro b/realsense2_description/urdf/_t265.gazebo.xacro new file mode 100644 index 0000000000..d3f542438b --- /dev/null +++ b/realsense2_description/urdf/_t265.gazebo.xacro @@ -0,0 +1,140 @@ + + + + + + + + + + + 0 + 0 + 0 + 1 + + 1 + 0 0 0 + + 1e+13 + 1 + + + + 30 + 1 + + 1.95 + + 848 + 800 + + + 0.2 + 100 + + + + orthographic + + + true + + + 2.20 + + 1024 + + + -0.00040996368625201285 + 0.03653175011277199 + 0.0 + -0.034823670983314514 + 0.0052825710736215115 +
0.5 0.5
+ + + + + ${camera_name}/fisheye1 + + + ${fisheye1_optical_frame} + + 0.07 + + + + + + + 0 + 0 + 0 + 1 + + 1 + 0 0 0 + + 1e+13 + 1 + + + + 30 + 1 + + 2.84 + + 848 + 800 + + + 0.1 + 100 + + + + orthographic + + + true + + + 2.20 + + 1024 + + + -0.00040996368625201285 + 0.03653175011277199 + 0.0 + -0.034823670983314514 + 0.0052825710736215115 +
0.5 0.5
+ + + + + ${camera_name}/fisheye2 + + + ${fisheye2_optical_frame} + + 0.07 + + + + + + + From 1ddad935e217a95dfdec0c9e367a14d34d89ad09 Mon Sep 17 00:00:00 2001 From: Elias De Coninck Date: Fri, 4 Dec 2020 23:35:25 +0100 Subject: [PATCH 108/111] add t265 camera --- realsense2_description/meshes/t265.stl | Bin 0 -> 42484 bytes realsense2_description/urdf/_t265.urdf.xacro | 135 ++++++++++++++++++ .../urdf/test_t265_camera.urdf.xacro | 10 ++ 3 files changed, 145 insertions(+) create mode 100644 realsense2_description/meshes/t265.stl create mode 100644 realsense2_description/urdf/_t265.urdf.xacro create mode 100644 realsense2_description/urdf/test_t265_camera.urdf.xacro diff --git a/realsense2_description/meshes/t265.stl b/realsense2_description/meshes/t265.stl new file mode 100644 index 0000000000000000000000000000000000000000..69e7e5a8068307a47f75fc4240bdd69af240ef91 GIT binary patch literal 42484 zcmb`Q3A7zmmG{d`0tp}xz?L~oMu`N70rKvxVuldGAVUZtCV;YH18M^jBA{1-3MkT% z7POh66GcP}ga-2Nt)iVkL2P-~0_&;c1wfbAP#9EDm<;`0$9j6^Oo;cMt>94ER{W$Va(&U3_cb^vB9IR+C6bn+Aq#dF*GmavvxkKYim$ zTL&v9Afs>IneZM&n|vgyBlXqUc}Dwz+a|UUtHoE&Xy3i%IF*NAGpqgnahs^@*1;ER zn0R{NnD(Sa>%IfQR^DD}zR#KcFeS~V5F!}#RsFg*e+ush> zaJkl|cOclx`l*3nE9(!nY9u-sU#wvwzu@rp37_7z!3Ps;b?JrE+rOK?PnA#~tYKp2 ziHEkQt!h;X>6A{!1Y7O7?4Y*%HV~|-6B{nvt4e6Cn1Jk7BKPFCN@%TEQzx7j!|>BV zJ8DGFt)zXuzE}yTEsq+|PDq=2eX&v>TozkNKlNlbH?)JTq(6Gz&wYfHpQsPkFd-wR zC#yt*4<^`3WQ1d#$Rx;0few7G?>tFG~8YW~uR*3-A z2)4442I4&L)O@Ih36o2r?t=-HEh4SxBND9Pa;;4z5o^T+TUkGqMD)Q{)*ousM?@d2 zVZugCgAXRy%Eoj9!5SuPOqWEg6%%Y_bF(C3%UM$=WImQe^udJ9TD1}x>s3jxkEp2= z6(2(pSg*8PX&=_z1|O_JO`MQ6VcmTPf~};VVrF0!(tqOhL(5`9`UcmJ1|M7&Tgixt z8AruO@ui~=?~Olp{gBOvRZZnhpE$Jl@XT#gt`e;AF$N1C3v3O!Q!v3+=$orv-Lb(3 zYwCpP-hlw^R`PFqNFPyCC!FTCbP6sDvRg^}+C#=FYMAiR!{x_VMS`uQpW5C}N_7Go zquW*9u1k=uXZ7`98~%2@&-)0!N<`~U6Tw!my?KPL?fnF6nDDDaBuwZOOt6)`t{J!* zcLEX#&)Z{7{Q6-<$A20LrFj;pc_% zt#}r&pV)GZJC|#1YUmFxi><70j9Br=S=nzcot)0O@Q&6kj~v_n{3|0{Tiv;6V9YJY zwx3wER_j;o%W5YR-@Iu-Hfi@c>FJkm(0lYt$F^_0adhjycb*Z6t?!>5iO_~C#mDWJ zPfj-cnUDkYe)v@h8_Z`v7Z$6{u zh`pCo*!f%O#JBOW82xy!&|@HXrcJXcKc#B`zP17(S=N~mA@V`Hh<*P*SanknlvQh8-b^4wKKWnpwi6y(tYJYCn`&t7(|Bm=bR?N;u zfAp>Njy3zYS@Vl;9@D=5{w-U-pYX2(TvC$!%f&}!PtPBnjky1*7HgQ;^MqsC?aj7s zEq&zz@$uyMre})|7@PfZt(g^qtwwz8nD&SMb!*8*F|NM5WO}x2>e%eb!K1sZ887AD zh;mOoGuA51kF&mURCfG=(b@TTJk@0@uhp8DPpT2aq~-6Hmj7(c{ylF8%3{JJvVRXJ zcPBk|ME32;>t*|2HnYnFTX{c)T8+5#q-@2qyVJFPaQhV2FyVc(>FtM!j~g~PE4%Y2 z2d4YHv{meLl*Lv)LW7Uh7oM73x9QdC%Dt}Wv4#mBW1TyW5FZcAXy4_q^V8Qx9akaP zidUs1`S30WXGe_KLhm+ke{d9B4kPi=J=e=7{q_hYSmQG4&R4r68H!*lmxGTV{_McC z7<-obV2#W0VcCFQACqJ#f~{N*KBS0_{HH$bs;~Pcw({5E&wr`}Uab)^>(^T4-VQGd z-dzSQZ9@ZiUxn79fY0k&d6C2)1%L_>f2xfsJJiLc>-j z54v4H0Zju@tyL~YxN@St>_f}7Hg$s!YX=A~R|&RqIkdx~i?(!F<1$LJzA+y}cu#dv z7F)R-eAp;aAFOd1K5UGc4l*Lvq2OlzPG1}D!Yg~p8n=9sni0B<vYIYsmO2)a#PY;k@`nu0cM$RQA$s2Oz+-~~$`IE!ql~ZfQ zgk43u;aWQ@F1~TcQ!TdgT2a1h1X{k9wEV}CA$t_DW-#Fq2{|?L z#P3XAuaGk<*Z@C`zGuSkS8wx$$`1#iLuX77F+q)4L+Wi zJn`C1ug;&|>k8lF=&WVJ$5`0oAWz)wu=8`t6DtH;@v0Q_M699-_XkJ8r;X? zE~DT#QHdLD`w6^iG!uG#jI~lfRx8YB%n$UMuJcwyeZ5Msrip;>7+v+jgv+q8 zNShDq4<%UBM1W2b8&|sjV8UhCa1^QpYnlkq`qS|UduuILZN#b`H)Cj*>Jb&136+Dj z&01}T)XK9v^s~yak{aS(k;RG$)-(~phpj#y-B^~>u#!mY9SF2Zdk`y}_h8gC5y6M8 zqJ4yy3oDL7Tz8dVO%oA(*jlCksub&X}S7f z4HFitYOOl(!Es-3^!uf?!_}&hP#>&e;+}60qA#jG`Utj?lH!~f!}t^LJ8M}?ygcmJ zt;#3?(?L6|<*0$L)GAwls2m*t@_WL41dp*us1GiStz@oX^ysLJg!*6&6W0IlK(G~$ z>8cNlqPF9}V_uVg7izhGyJKZbW>>QhT{#h}Ew+85B{S9M2VNhVPz@6%m-AfWgr9hq zj0v@}^|7HpxW3lUN^|={>EUl2C0Z5}HeyO2ohE{pz`>dyQM$7u{GZ)-YjLhK5=(!B#`}2hTO@n?ajh|A$%2gtbrUYRj$eU6jjKRzrQg zO0b3rYoCtep$WFKmN)od4HI@vw01oE;4b;B&Cf2xTc;cu3#XLw@G0e}TqrpM6Rc?_ zkY&97_S^Xrx6W1{OqBBQDdoI@4<=aCOu)z4o9$N&UbLP1V4{>`-^hG~4<^d7yO3(a z1bi&J__IZOi<@`J3&R{rIBo)CUu#ESXcxcj1Ex)-)6FA+t70B}Mz%>=ar>s1q>D}&W=Ag)(vxe}}~A2k9#EV>1JFj2~} z2h9f))(5I-Cg8(HiTYrol;hYnA57TjQ%y4gA2!<62NR_{d`dZ^7S;&XG!yV4KOOJI zau&t8-8UbV0ECf}ltgvKDc2bDVHod<7W$mfD3m3k9pz7_U`^Q{3w@7k(RBw>v%*$2 z)z~Vka#^dokFpQqJpTu3RYrGVtv`lphqbAN5k7RSSkpi#U3x&vnJ`(cI3iV|oT&?a z{m>a~ksb_luw*1}r}mo-cb*^X@O8y{`0^Z2&Q ze|6tS^qvP!ZXmbUdAyY*6SsvNnyi0iuCoR^3nxq-h=c}8V%0>bmHD1hArRf{*o%K0 zy^E};!LVBN%1>f#Lw&qftf>)QZ(o&=`r zWpuUW9kiS^Uw-kSZtvNP)QU@LY=_pDiC^7!pS0=vs*k>Qu$9HIfnW_2V}E#etYNiQ zOt2OAY?WXQ6H*fTrlD3$uod@z^wB|dYqe_qW6_mbMM9#`szg`H$4_|wAkKF%8rVvD zP|tlDM@iH$A-x&>h0?=c^$`iSlF=izYKoP`XF0rEbXB%m=`3jUp&BNv|CJt^U@IHD z%|7BBWWvUAvyVuy70<_Ntt=*5mc{BHXgL$sH_f#|e`pKYilbhw6>FHVF;@EM1Wv~; z=7DP1%Eq)2|2}PHZy)(R=x1l^J6XL}Pk;1~-d}eb(H*tR)Yif4ex&!2-Pcw*N%pwr z(0sRd|0TcuC&ws(_l9nHN2N0mmKlH|@a-jiRw`z1ZpisQpI;gaO3+i^$cC+_f2 ze$BZjWQgK@Z*Qfs;)s53!;kio75}8K%ahBmFU~(HzgG#t8Yb9xl6+aLPnX~8Y6M%2 zIrFgI^?Sd^d>kM(x=(5bf;CK3+wnOe&JyCv8o^d?zw2-vF&GO;a^a>==Eq;VasJGv zpU7ClL^ZmX?ldO9a_`wi{?PxVi0I7crgUda-Acz5-o1S8ybrZd!?hRgta6gfdFHj= zsr&6+Jn^3GwLe?~+6l!O1Z(O<*@K6O^$htPT_d-Sgaj6hvYXn<8@XS%VG6WyneB+_suaDop_~-xF zyAoaWfv*xAw8$jrs8 zWepSX=PO^5oWAdt`Ed^{D6ZOfOC5>wo?6+aBiuD8*9pizrjz92Q~sX+*T}1Sr!W0@#`6I0cScPxi#XCr@^gvoWfI|2 zCAzF(;^*BXl@6o*t*H}>Eo8+1vy65o*s9utUzFK&&mUgv-7hnWHB8`L->6~e^BZNx z?j#1NnduSJw!(LPW4)pf{7`vRk$(u9II7tYL!ZjjV8DeS`eoRwLL75y6@t zeC#4M+DK}4mk_L}6BxT8x`zny2WkEJHG(5OFn6Y2YlDv;N@QPo?L+x~5?$6XQSFcU zGH%}^!vse? zN#+SLM~KBWf~_#}{c|mnWc?Mp7Q4vrjY6=7366S_j1z189$F*V%0A;j=fBiSYBW=7 zwyzMZVS;0rBp(;zG$A@Qg01XR7|aJ&p7SKa@|&`zPL%y|tPpn#A-@%ZdyPM(B4mii zPTV@*{t3pHc2FNokc>}Tu$7@sxCTCmha@)`w( z3D?vJx~hSgIpMR#x|5fx4<<;)CsWv(SSMTqA4J5fDD0C&#%&>^QGGB$GCm2zW>lSU z4SWz0KZhqtX35+z3jLvLAI*;e$h=Re609+riGi@kIY#CNd`y-3!Gy_b#hHP;XOsxm zG!O$}$8xsJ4fuFT<_8lds}<)Da@Udw)-(_UA=BSU<_3HW|NcH16DF$_=MPE31Zx_I zfso0(`}_N-kMS};m@rwbIO9kXCRo!z41{a#D>66Gj?-j*Fk!M<@ij3?m|#rJa=#RV9E5kEVh#Q7;Bp(v!B1W<>w5nf$Y~P*(bCsXBlle%ZM5# zAp5l>No4=gru|2Z5Bsb7ko`@1itW!Od(kK&Rvb~uOWWms@bcFer!(aY0J|dAFu{G3 zBwrWe^Fmx*BiKszpY8HwV(Pdf(=%n~1%fq9RNHZe5FZx;wPJ#;K>MkP-21-nFwbR=lhGb0YM78!Lg~1V%C@w9 z6l=I|sy^O6Xk2!>?437}y)$c=;C@OH5Lej#oC&s)eS-E!XvfTZlWdxt0oaZ{YM9`t zC&|Zzm@NcOCb%rN0$rX=ESkGG9U~_PBZXiM6WmWpa)A&V2=T5O!B!ZRepZttcOSof zcB$-ZPm_HuN0(O#j+;Dxvtm^`>X~o$a2mxLCV2iQ$?-Cyo|hSQd5vHzo-2CRm(p2Z zT*;W=87!Ip0}HZqB@4etvT#{Nb^KtplvPw^tj?0d>xvOpYT0>ZwDXD@CU{II$!A7h z)&8EG9DG_%4R{_v_RNwaog~|R^>yXcfXiYl$esbpd)#-Qo4qXi+Wlo;%Q4}(QjKnU*R*U~ z*=>JW_Rg$ff@gu8ObGF)?6wDM1Y3bFcl4LsvQ2iioB`luf;CL=yh)Oagt$qFYia~r zVN91N6R%zSK)R2d16&~lYnb2|N)KN5Kss57Wi^7WFpm9{O3!3uw1+bp)-X}+4>|El z>BK9R#a56#zYg<5=0^VI+Wg?Y!N|vTH)I)~oON7wlR?#i_3t_<&lbjKpK z)skY?mrK^yvaBzb#a7Y|C9rSPtPgjWQk)302C|Gkt-E9#J<0m49d-vRr8`)vVFI#b zeXX$bvQuaE!SP{#RUbIjmaH$uT^Sh}8b!?YM7XJ|yeQCF@gyHB407Az5E8S)bO53AO?)tEl;qtS^_WFSV>M zYM7`-7pKmW^`$s<=ADqtVC`qi`gByrtPdyKZ93U*LBj;^4U+_?=aTiMI6Y?#_l>L) z%7zCyCT%(A{% z7F&UqtWQfy63O~<$@((O`l5!3Y7a`*mrK@{S=JW`w!)~CtD5tS^_WFSD#KYM7|@hh%-ZWPO=s zeUV@*jAP0A%!l2H%Wx--HB40d!|rotbe~hpVk^j!mzWRP6&08pmi5K?!M%o&kLzy8 zWF+e=Bx&vDs{J8ZUm;mvYFS?-*b3`B=1TA(SzjSpUuIcf)G$%)4`h7E{4DE> z1Y2Px!e|dZB&qYDZClJ#Yl^+kfMkkuf%!G~mfxD%II))zHQRQp4+ zy z@vcwLMI*tQ2Exu><+N4_obNJWvRYvWfOdpyEfcJ1AnXiTPMy^U&Z(I&S*`etL(i!r z!I}ob&cfyNTnU`FGhwn?VV8k+gglW6)-(`yrZ0B^QuI0Q8!%zA=kV$y-f4&gpU^fC zc5g!NW+;LCA556+nZEjncT6II9Sd3rO#@-O3AvA>1nzS(VX|7uXxEa$-hc_#G!VM? z!AX14!M!NEm)etBsm5rRUN;hGwO^DE0_ zK*+t+%PnfoSSIrTXeH-o$*lt^%FiqtN)6||YjT$DnZ}bV%RGu)^lL;=1 ztt2nS{V4MxPnf3igsBp&VWQd&dBQZ6Crnd&!Zen}R+2esM8Z`~o-j@23DeY`FpU}} zs?n7vOmXU**%PKbi{h+B4a;4sooxFaCu-0iG|%I@qEDFO^j!B*u<{iL(e=Km`j96~ zQ+dKPvnNcW4<)YOTx2jvOVOr9{!>&OjF!}wI@s?!B#w1;vHT;>x(NH6Y%FVSf4OW z=M^G9gcxX7Yq-YEPI(O`X8CHq0n_!Zed7OjCQpG!h(X*?H>~ zHJnV~PMkbpn%WblQNu*FKkPoIpLj)rt$5#-B=UqQ?q+26glU`~+&8$2Vy=XpusmU! z$`huUJz*L(OmNio3DZ=bFwGX1_dm2Ow!+BAIv;$<6Q-FwVVc-4`iYta?he^#q;5~|K z4NTUSGg0+ngtnYp$aIyk{z&Wn;qOoS|H%tAzD= z3LpNyI}qIeRpKx5`J&g$XQg8A0;^N**;ZraGIsGiVv>Xj)-(~}3}BUfM(W$)b5au~ zT!s~o1^J9dB3RQz1Rt+S8(s*X_nI)_GOR3aI>zL)|A=5s6A^qoDWCoMviV@bWmxgZ zmrso#f;CM<@R5xCaE8{SKbUYCRy;z*j0n~=5y8jv@>#5JS)ViEGORH2|LqH5-XsYV ztZ5=DKJ>JR374rIHb2aV5>~Einuy@TTCS&eOt=gyi-P%Jf;CM<@L@fuKA3PBR@R&5 zg9+9&5y6LzMEFRUa2ZxMD$NHItZ5>G51Ucyg9(>mWwXnCFu|H8BKWYC0X^vRT+4DA zR=!Gvm6{3GG!emvTn%!%Ms?u}*Fot)eV01Ql}PKm;)4%a+Y7lG=-Mu0NeO6}khsN? z^je$JwKi&`<*{6;Vf2?IvbO7W(AIX*gzF#^RUfjp>vho9_E;{goURgb#X+t7iW6&P zHPJ|$KkGrc8f20I=+!`?+m#4zgO)Q<^_ptQT1Wb)hj2rkm)L6eXg})@7(`YLRO!`*F;!7ap$sY5G$84`gp|XnivVzG!bF! z%IZ`2+RlW_u&Rz-CRo!%1Rt{c|q zUe%aznc89V!)m33m8+U2BKWXa={1T8mtkd5Fds~?rilnXtk2a46E4HbdeeL`!I~x_ z_^^@a*Fmg)G6u)7!I_76Yrc*ipY9#bvy{$?LxySbFOO`&4TtT!U|=a00Tw zjw@@D{8@5Q`BfjTVJrA}@$Zie%vze64<=m01Z00b_;^v)-j%YZa9M1H*B{((#lVC6 z;alJE!-Q*?fb6d;(9ys0=`bkETWj)QJ#N9)36%A}5K60UM{1RMJH-1S>T5Mo+1fW4 zV^tv}CaPfqvcDdDpf9{XT*FrIA^kKM`@F-1YnXuSuLmD8O0duIKG(9?isRN#NUUN+ zyse2xnbe^ApP|Ufp6#fUkT8Guyzz)-__r`#plN$WW0_Vj+}OL z1K}F4yVL%9@L|zaAFlCOpmFVF+o{9~zJW4UOh`;rV|9{bbG zxyOqA_1B6CkF@*fYk7<=G+eIB%`NwcvK3^s&)4>9J6uyI&?aB0mFOVMR+e53Ad8i* z!H~tbUP~HV95mEl4Xl`e+}!7=FB3klBqD=8&--Gf8u#G@ z+7xE(Z$I??fp5ti94+gjfA-3|AqMywj6-;f4bzPZ=Fkqs^W;Oq(cpAR{^k8msh z-mUN*ZXo_gu7mPjKN)MdT*&gxz4nc4PRP!vk8msh{;=>JZXj05^?aC|;IW3wg)HCu ztKa7a`#`*_fp9DT9yELdTp-X6?AN^R_{K8#hi^!8fA$T;0^vJf@NF@yVZwdj+u4GT z)k3^cBiPE@Q4+&GadMV^<)=AonBZ@NlTT3o{rv+s>_0!7DE-a^Tj6#2&f>0olBd<` zil3g7J@wKkC0xS=&ui0&uXKe*Mp?VRkVF|?^}!J$4zS=TAcUlrdlhvLb-0$PpsPJl;Q`g z@6!HoD<&X6de^A#r}tPOwR+=<L+fK=q`A5 zQynF4#ROzTeeGxeO=|Vt^===SC_T8Ztqe@K6@2)Zu8b1(;TkR%vcDdPmyUQWpL5Q< zKEka)$alqe!}lou=&|wH%OAfc-|#OJ3jV%CwyOI0;+(y*aesRze{R<`d7W?{{*9&h z4%j3)_pRNsS2uq)|KNe^6|CWMA$t^(WXnS*WoO^EQE^@O;k-_`6@MRWlI(T(uGz01 z-l&)`@;5naxLn8{^(1-UUX!z@FW9lTW!|}cgj@Odv*LSZ<#W8oPs)Bd|NX^;^)AR+ z!{tK8$PXiN=JZL~CWlQbHal%uuTHoXe`~BfS#!vw?8w6>72mmZS&ubbE@U6mNwVYN zld?I7Oe&V&w5(kx+{%52Z==0jYW1<{lZrDhS=MF^m+St*Jl|Y=tlb)4yfWjwv`+Xu zuYP~+-#BssAxAL)oZ^D%)IZm6D zowEIg#aAzUJY@}+3mGFOtfG6)os{i()QkByPFyFe6K=)dmzyLv4Qpp(KlOM%_fMlT z)^NFyG4ewuGiJs3Y>dpPKmU0`AK_LYFbBi;@V;yA%xsq2%Q;i-XiV%LpjR(kw|zB0 zxxV^@Sb^9>?wZV(yBg4V-o^x%BrAguCy0+THxhnA5aMIf7yr`GAD-2?ztHC(&=+gu4ihwSLr4BQ3h#LZ+((Vzr&L{7ma} z&!a(j|5u4KLbjC3-k`7%*YXTWqR{f_3f(GmAIX+dWJ`6ztt<~JiH>AT`u(!3v0AAW zWT~zC3)d(R$inM{TUmZp5*>V(DY9_ZaJi5rhOP2?J`~|rmRS;EwelI&!tTfY`5Z(I zL!YB1vSTT*WAU0m!-V??v(^dhSeRfdZ%0Xt{KUz{#IM8*xdjaqmczG}o{I0OzE$=H zH%jiaqh(}hEeN)P4;i8I#1`!hibq}=u&Jy+N^J z+*-Ppcpp@Vus3KeeFZrjeq?V@$ljnr*xo>`JPN^w?Sxa=8)$Sf!qFc-j(t~*aUA!x zotN$t*L^ESYdClq6SC8tsz3X2@Q-EXZ!vPe_KG$=*Qw!>yQrEZIOSjs za9@+X!S6SJHr?jH^*kd}4VMeKjMa{^H<*9hM%iWEhdt|ygj?ATpmo=y^JFbaWp8lL z!y9Gok-teoQCVPWE!mTV1ZiSrM_6C{k4Ln0u4VMcUBR`Bp+Z$xEH}EVx z5^iPreCwARVWr02;4sO=&%88d`l{h_A^Vt?_v(xFaYFTOio1zOxRtjf>~XFLdxPwf zOX7~D1r3+${=z)By+I~>1K$-z!sof|09sq!xk%b!dxK2&203fET*!!YSlexHkjdVl zk8mp=S7Fb2nd}V~Y`V7lG{8dHJl&kSFGzC;C;53Ck00-L)({rz|}B@GG^`p&gdJ7M{I&2H-V8uT_0m zhFo}t?DYlVbd|6jOG-PISSybeXzzbn;iTn~>GR$o=ATYfePC~}gJj^ooACS-<$Au` zPsm;^rM+6Lm9Gq-;m=lT@ge)D%y&gT@4W7~epG#+9Uy$? + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense2_description/urdf/test_t265_camera.urdf.xacro b/realsense2_description/urdf/test_t265_camera.urdf.xacro new file mode 100644 index 0000000000..bfa079ad2c --- /dev/null +++ b/realsense2_description/urdf/test_t265_camera.urdf.xacro @@ -0,0 +1,10 @@ + + + + + + + + + + From 8be117a97368ffb03f4d693df0760d4f6abcf928 Mon Sep 17 00:00:00 2001 From: Elias De Coninck Date: Fri, 4 Dec 2020 23:36:09 +0100 Subject: [PATCH 109/111] allow meshes in package to be used by others --- realsense2_description/CMakeLists.txt | 1 + realsense2_description/env-hooks/realsense2_description.dsv.in | 1 + 2 files changed, 2 insertions(+) create mode 100644 realsense2_description/env-hooks/realsense2_description.dsv.in diff --git a/realsense2_description/CMakeLists.txt b/realsense2_description/CMakeLists.txt index 30c04d9fa8..4fdb5f75d7 100644 --- a/realsense2_description/CMakeLists.txt +++ b/realsense2_description/CMakeLists.txt @@ -25,4 +25,5 @@ install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in") ament_package() diff --git a/realsense2_description/env-hooks/realsense2_description.dsv.in b/realsense2_description/env-hooks/realsense2_description.dsv.in new file mode 100644 index 0000000000..cd4ca90514 --- /dev/null +++ b/realsense2_description/env-hooks/realsense2_description.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;GAZEBO_MODEL_PATH;share \ No newline at end of file From 6efbd6a98c715b658044e16a2080c75d5de86f63 Mon Sep 17 00:00:00 2001 From: Elias De Coninck Date: Thu, 7 Mar 2024 20:56:36 +0100 Subject: [PATCH 110/111] fix gazebo sensor plugins for imu --- .../urdf/_d435i_imu_modules.gazebo.xacro | 33 ++++++++---------- .../urdf/_d455.gazebo.xacro | 34 +++++++++---------- 2 files changed, 32 insertions(+), 35 deletions(-) diff --git a/realsense2_description/urdf/_d435i_imu_modules.gazebo.xacro b/realsense2_description/urdf/_d435i_imu_modules.gazebo.xacro index fcf80f8d3d..454d877ca0 100644 --- a/realsense2_description/urdf/_d435i_imu_modules.gazebo.xacro +++ b/realsense2_description/urdf/_d435i_imu_modules.gazebo.xacro @@ -10,7 +10,7 @@ Intel RealSense 435i camera. - + 1 @@ -72,26 +72,23 @@ Intel RealSense 435i camera. + + + ~/out:=${camera_name}/gyro/data_raw + + ${gyro_optical_frame} + false + + + + ~/out:=${camera_name}/accel/data_raw + + ${accel_optical_frame} + false + - - - - ~/out:=${camera_name}/camera/gyro/sample - - ${gyro_optical_frame} - false - - - - ~/out:=${camera_name}/camera/accel/sample - - ${accel_optical_frame} - false - - - diff --git a/realsense2_description/urdf/_d455.gazebo.xacro b/realsense2_description/urdf/_d455.gazebo.xacro index fc117a1a55..18efa5e716 100644 --- a/realsense2_description/urdf/_d455.gazebo.xacro +++ b/realsense2_description/urdf/_d455.gazebo.xacro @@ -3,10 +3,10 @@ - + - - + + @@ -169,6 +169,20 @@ This is the Gazebo URDF model for the Intel RealSense D455 camera + + + ~/out:=${camera_name}/gyro/data_raw + + ${gyro_optical_frame} + false + + + + ~/out:=${camera_name}/accel/data_raw + + ${accel_optical_frame} + false + @@ -197,20 +211,6 @@ This is the Gazebo URDF model for the Intel RealSense D455 camera 0.25 9.0 - - - ~/out:=${camera_name}/camera/gyro/sample - - ${gyro_optical_frame} - false - - - - ~/out:=${camera_name}/camera/accel/sample - - ${accel_optical_frame} - false - From 460b36daac90c3f57652ee89a2a952605a5208c3 Mon Sep 17 00:00:00 2001 From: Elias De Coninck Date: Wed, 27 Mar 2024 18:00:17 +0100 Subject: [PATCH 111/111] use find package instead of file --- realsense2_description/urdf/_d435.urdf.xacro | 2 +- realsense2_description/urdf/_d455.urdf.xacro | 81 ++++++++++--------- .../urdf/_usb_plug.urdf.xacro | 13 +-- 3 files changed, 49 insertions(+), 47 deletions(-) diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index 1cf021cfdb..36924de7bd 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -76,7 +76,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_d455.urdf.xacro b/realsense2_description/urdf/_d455.urdf.xacro index 2867dff3d9..671e9bfd3e 100644 --- a/realsense2_description/urdf/_d455.urdf.xacro +++ b/realsense2_description/urdf/_d455.urdf.xacro @@ -21,11 +21,12 @@ This is the URDF model for the Intel RealSense 430 camera, in its aluminum peripherial evaluation case. --> - + - + @@ -33,42 +34,42 @@ aluminum peripherial evaluation case. - - - + + + - - - - + + + + - + - + - + - - - + + + - + - + - - + + @@ -76,17 +77,17 @@ aluminum peripherial evaluation case. - + - + - + - + - + @@ -101,18 +102,18 @@ aluminum peripherial evaluation case. - - + + - + - + @@ -120,14 +121,14 @@ aluminum peripherial evaluation case. - + - + @@ -135,14 +136,14 @@ aluminum peripherial evaluation case. - + - + @@ -150,20 +151,20 @@ aluminum peripherial evaluation case. - + - - + + - - - + + + @@ -203,11 +204,11 @@ aluminum peripherial evaluation case. - + - + diff --git a/realsense2_description/urdf/_usb_plug.urdf.xacro b/realsense2_description/urdf/_usb_plug.urdf.xacro index 7381756e17..0d62d78080 100644 --- a/realsense2_description/urdf/_usb_plug.urdf.xacro +++ b/realsense2_description/urdf/_usb_plug.urdf.xacro @@ -20,7 +20,8 @@ This is the URDF model for the Intel RealSense 415 camera, in it's aluminum peripherial evaluation case. --> - + @@ -33,18 +34,18 @@ aluminum peripherial evaluation case. - + - + - + - + - +