PR #3125 from SamerKhshiboun: Support calibration config read/write s…

…ervices

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`
+    <details>
+    <summary>Click to see the full response of the call example</summary>
+
+    `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]]]}}')`
+
+    </details>
+
+  - [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.,:
+
+    <details>
+    <summary>Click to see full call example</summary>
+
+    `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]]]}}' }"`
+
+    </details>
+
+  - [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='')`
+
 
 <hr>
 

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 <librealsense2/hpp/rs_processing.hpp>
 #include <librealsense2/rs_advanced_mode.hpp>
 
@@ -150,6 +152,8 @@ namespace realsense2_camera
         std::vector<rs2_option> _monitor_options;
         rclcpp::Logger _logger;
         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;
         std::shared_ptr<Parameters> _parameters;
         std::list<std::string> _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,

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<realsense2_camera_msgs::srv::CalibConfigRead>(
+            "~/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<realsense2_camera_msgs::srv::CalibConfigWrite>(
+            "~/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<rs2::auto_calibrated_device>().get_calibration_config();
+        res->calib_config = _dev.as<rs2::auto_calibrated_device>().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<rs2::auto_calibrated_device>().json_string_to_calibration_config(req->calib_config);
+        _dev.as<rs2::auto_calibrated_device>().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();
+    }
+}

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
 )

realsense2_camera_msgs/srv/CalibConfigRead.srv

@@ -0,0 +1,4 @@
+---
+bool success
+string error_message
+string calib_config

realsense2_camera_msgs/srv/CalibConfigWrite.srv

@@ -0,0 +1,4 @@
+string calib_config
+---
+bool success
+string error_message