implement Triggered Calibration action

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,61 @@
+#include "../include/base_realsense_node.h"
+using namespace realsense2_camera;
+using namespace rs2;
+
+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;
+}
+
+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;
+}
+
+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();
+}
+
+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>();
+
+    rs2::auto_calibrated_device ac_dev = _dev.as<auto_calibrated_device>();
+    float health = 0.f;
+    int timeout_ms = 120000;  // 2 minutes
+    auto ans = ac_dev.run_on_chip_calibration(goal->json,
+                                              &health,
+                                              [&](const float progress) {_progress = progress; },
+                                              timeout_ms);
+
+    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: Goal succeeded");
+    }
+    else
+    {
+        // exception must have been thrown from run_on_chip_calibration call
+        result->calibration = "";
+        goal_handle->canceled(result);
+        ROS_WARN("TriggeredCalibrationExecute: Goal failed");
+    }
+}

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