Skip to content

Commit

Permalink
PR #3138 from SamerKhshiboun: Support Triggered Calibration as ROS2 A…
Browse files Browse the repository at this point in the history
…ction
  • Loading branch information
SamerKhshiboun authored Jun 23, 2024
2 parents f688fba + cce5cf2 commit 598756b
Show file tree
Hide file tree
Showing 9 changed files with 201 additions and 1 deletion.
23 changes: 23 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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.
Expand Down
2 changes: 2 additions & 0 deletions realsense2_camera/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down
13 changes: 13 additions & 0 deletions realsense2_camera/include/base_realsense_node.h
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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;

Expand All @@ -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,
Expand Down Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion realsense2_camera/include/ros_utils.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

121 changes: 121 additions & 0 deletions realsense2_camera/src/actions.cpp
Original file line number Diff line number Diff line change
@@ -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");
}
}
13 changes: 13 additions & 0 deletions realsense2_camera/src/ros_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

}
18 changes: 18 additions & 0 deletions realsense2_camera/src/rs_node_setup.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ void BaseRealSenseNode::setup()
monitoringProfileChanges();
updateSensors();
publishServices();
publishActions();
}

void BaseRealSenseNode::monitoringProfileChanges()
Expand Down Expand Up @@ -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,
Expand Down
1 change: 1 addition & 0 deletions realsense2_camera_msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
Expand Down
9 changes: 9 additions & 0 deletions realsense2_camera_msgs/action/TriggeredCalibration.action
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
# request
string json "calib run"
---
# result
string calibration
float32 health
---
# feedback
float32 progress

0 comments on commit 598756b

Please sign in to comment.