diff --git a/README.md b/README.md
index dcebde58a..2349f9bab 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)
@@ -670,6 +671,28 @@ Each of the above filters have it's own parameters, following the naming convent
+## 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
+
+
+
## 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.
diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt
index cce916163..439129777 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)
@@ -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 032f8db7e..c1da386cf 100755
--- a/realsense2_camera/include/base_realsense_node.h
+++ b/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
#include
@@ -120,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:
@@ -154,6 +158,8 @@ namespace realsense2_camera
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;
@@ -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 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,
@@ -271,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 feccd4647..d31b0d42b 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 000000000..740c7391b
--- /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 0ee217290..8c4d1528c 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 87dd48022..215309df5 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()
@@ -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(
+ _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,
diff --git a/realsense2_camera_msgs/CMakeLists.txt b/realsense2_camera_msgs/CMakeLists.txt
index 3d085d76c..fe14467a3 100644
--- a/realsense2_camera_msgs/CMakeLists.txt
+++ b/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
)
diff --git a/realsense2_camera_msgs/action/TriggeredCalibration.action b/realsense2_camera_msgs/action/TriggeredCalibration.action
new file mode 100644
index 000000000..451969080
--- /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