PR #3138 from SamerKhshiboun: Support Triggered Calibration as ROS2 A…

…ction

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
 
 <hr>
 
+## 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
+
+<hr>
+
 ## 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.

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)

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 <librealsense2/hpp/rs_processing.hpp>
 #include <librealsense2/rs_advanced_mode.hpp>
 
@@ -120,6 +122,8 @@ namespace realsense2_camera
         void publishTopics();
 
     public:
+        using TriggeredCalibration = realsense2_camera_msgs::action::TriggeredCalibration;
+        using GoalHandleTriggeredCalibration = rclcpp_action::ServerGoalHandle<TriggeredCalibration>;
         enum class imu_sync_method{NONE, COPY, LINEAR_INTERPOLATION};
 
     protected:
@@ -154,6 +158,8 @@ namespace realsense2_camera
         rclcpp::Service<realsense2_camera_msgs::srv::DeviceInfo>::SharedPtr _device_info_srv;
         rclcpp::Service<realsense2_camera_msgs::srv::CalibConfigRead>::SharedPtr _calib_config_read_srv;
         rclcpp::Service<realsense2_camera_msgs::srv::CalibConfigWrite>::SharedPtr _calib_config_write_srv;
+        rclcpp_action::Server<TriggeredCalibration>::SharedPtr _triggered_calibration_action_server;
+
         std::shared_ptr<Parameters> _parameters;
         std::list<std::string> _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<const TriggeredCalibration::Goal> goal);
+        rclcpp_action::CancelResponse TriggeredCalibrationHandleCancel(const std::shared_ptr<GoalHandleTriggeredCalibration> goal_handle);
+        void TriggeredCalibrationHandleAccepted(const std::shared_ptr<GoalHandleTriggeredCalibration> goal_handle);
+        void TriggeredCalibrationExecute(const std::shared_ptr<GoalHandleTriggeredCalibration> 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<rs2::stream_profile>& profiles, const RosSensor& sensor);
         void startRGBDPublisherIfNeeded();
         void stopPublishers(const std::vector<rs2::stream_profile>& profiles);

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<uint8_t>& vec);
 }
 

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<const TriggeredCalibration::Goal> 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<GoalHandleTriggeredCalibration> 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<GoalHandleTriggeredCalibration> 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<GoalHandleTriggeredCalibration> goal_handle)
+{
+    ROS_INFO("TriggeredCalibrationAction: Executing...");
+    const auto goal = goal_handle->get_goal();  // get the TriggeredCalibration srv struct
+    auto feedback = std::make_shared<TriggeredCalibration::Feedback>();
+    float & _progress = feedback->progress;
+    auto result = std::make_shared<TriggeredCalibration::Result>();
+
+    try
+    {
+        rs2::auto_calibrated_device ac_dev = _dev.as<auto_calibrated_device>();
+        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<uint8_t>(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");
+    }
+}

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<uint8_t>& vec) {
+    std::ostringstream oss;
+    oss << "[";
+    for (size_t i = 0; i < vec.size(); ++i) {
+        oss << static_cast<int>(vec[i]);
+        if (i < vec.size() - 1) {
+            oss << ",";
+        }
+    }
+    oss << "]";
+    return oss.str();
+}
+
 }

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<TriggeredCalibration>(
+      _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,

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
 )

realsense2_camera_msgs/action/TriggeredCalibration.action

@@ -0,0 +1,9 @@
+# request
+string json "calib run"
+---
+# result
+string calibration
+float32 health
+---
+# feedback
+float32 progress