From ec8ef109924b9c4a8e2c5f7a44b44d9bb40f6cfa Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 13 Jun 2024 09:45:08 +0000 Subject: [PATCH] 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