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: | diff --git a/README.md b/README.md index dff65f6105..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) @@ -50,9 +51,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 +121,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 +137,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 +169,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 +186,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 +195,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
@@ -637,6 +638,58 @@ 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='')` + + +
+ +## 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
diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 4e02573cb6..59d8f875b2 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) @@ -117,9 +118,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") @@ -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 24c2ce941f..c1da386cf2 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -32,6 +32,10 @@ #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 "rclcpp_action/rclcpp_action.hpp" +#include "realsense2_camera_msgs/action/triggered_calibration.hpp" #include #include @@ -118,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: @@ -150,6 +156,10 @@ 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; + rclcpp_action::Server::SharedPtr _triggered_calibration_action_server; + std::shared_ptr _parameters; std::list _parameters_names; @@ -158,6 +168,16 @@ 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); + + 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, @@ -263,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 191644b83e..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() @@ -503,6 +504,35 @@ 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::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, @@ -524,3 +554,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/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_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 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 diff --git a/realsense2_camera_msgs/CMakeLists.txt b/realsense2_camera_msgs/CMakeLists.txt index 2fcd91c6b1..ec8788cddc 100644 --- a/realsense2_camera_msgs/CMakeLists.txt +++ b/realsense2_camera_msgs/CMakeLists.txt @@ -43,6 +43,9 @@ set(msg_files rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} "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 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