i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 12:07:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed hotplug

Browse Source
This commit is contained in:
Joe Dong
2022-06-08 14:51:00 +08:00
parent ad4cb96b0c
commit c6ded28f8e
12 changed files with 366 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/dynamic_params.cpp
+148
View File
@@ -0,0 +1,148 @@
/**************************************************************************/
/* */
/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc */
/* */
/* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the */
/* subject matter of this material. All manufacturing, reproduction, use, */
/* and sales rights pertaining to this subject matter are governed by the */
/* license agreement. The recipient of this software implicitly accepts */
/* the terms of the license. */
/* */
/**************************************************************************/
#include "orbbec_camera/dynamic_params.h"
namespace orbbec_camera {
Parameters::Parameters(rclcpp::Node *node)
: node_(node), logger_(node_->get_logger()), params_backend_(node) {
params_backend_.addOnSetParametersCallback(
[this](const std::vector<rclcpp::Parameter> &parameters) {
for (const auto &parameter : parameters) {
if (param_functions_.find(parameter.get_name()) != param_functions_.end()) {
auto functions = param_functions_[parameter.get_name()];
if (functions.empty()) {
RCLCPP_WARN_STREAM(logger_, "Parameter " << parameter.get_name()
<< " can not be changed in runtime.");
} else {
for (const auto &func : param_functions_[parameter.get_name()]) {
func(parameter);
}
}
}
}
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
return result;
});
}
Parameters::~Parameters() {
for (auto const &param : param_functions_) {
node_->undeclare_parameter(param.first);
}
}
rclcpp::ParameterValue Parameters::setParam(
const std::string &param_name, rclcpp::ParameterValue initial_value,
const std::function<void(const rclcpp::Parameter &)> &func,
const rcl_interfaces::msg::ParameterDescriptor &descriptor) {
rclcpp::ParameterValue result_value(initial_value);
try {
if (!node_->has_parameter(param_name)) {
result_value = node_->declare_parameter(param_name, initial_value, descriptor);
} else {
result_value = node_->get_parameter(param_name).get_parameter_value();
}
} catch (const std::exception &e) {
std::stringstream range;
for (auto val : descriptor.floating_point_range) {
range << val.from_value << ", " << val.to_value;
}
for (auto val : descriptor.integer_range) {
range << val.from_value << ", " << val.to_value;
}
RCLCPP_WARN_STREAM(
logger_,
"Could not set param: " << param_name << " with "
<< rclcpp::Parameter(param_name, initial_value).value_to_string()
<< "Range: [" << range.str() << "]"
<< ": " << e.what());
return initial_value;
}
if (func) {
param_functions_[param_name].push_back(func);
} else {
param_functions_[param_name] = std::vector<std::function<void(const rclcpp::Parameter &)>>();
}
if (result_value != initial_value && func) {
func(rclcpp::Parameter(param_name, result_value));
}
return result_value;
}
template <class T>
void Parameters::setParamT(std::string param_name, rclcpp::ParameterValue initial_value, T &param,
std::function<void(const rclcpp::Parameter &)> func,
rcl_interfaces::msg::ParameterDescriptor descriptor) {
// NOTICE: callback function is set AFTER the parameter is declared!!!
if (!node_->has_parameter(param_name))
param = node_->declare_parameter(param_name, initial_value, descriptor).get<T>();
else {
param = node_->get_parameter(param_name).get_parameter_value().get<T>();
}
param_functions_[param_name].push_back(
[&param, func](const rclcpp::Parameter &parameter) { param = parameter.get_value<T>(); });
if (func) {
param_functions_[param_name].push_back(func);
}
param_names_[&param] = param_name;
}
template <class T>
void Parameters::setParamValue(T &param, const T &value) {
param = value;
try {
std::string param_name = param_names_.at(&param);
rcl_interfaces::msg::SetParametersResult results =
node_->set_parameter(rclcpp::Parameter(param_name, value));
if (!results.successful) {
RCLCPP_WARN_STREAM(logger_, "Parameter: " << param_name << " was not set:" << results.reason);
}
} catch (const std::out_of_range &e) {
RCLCPP_WARN_STREAM(logger_, "Parameter was not internally declared.");
} catch (const rclcpp::exceptions::ParameterNotDeclaredException &e) {
std::string param_name = param_names_.at(&param);
RCLCPP_WARN_STREAM(logger_, "Parameter: " << param_name << " was not declared:" << e.what());
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, e.what());
}
}
void Parameters::removeParam(const std::string &param_name) {
node_->undeclare_parameter(param_name);
param_functions_.erase(param_name);
}
template void Parameters::setParamT<bool>(std::string param_name,
rclcpp::ParameterValue initial_value, bool &param,
std::function<void(const rclcpp::Parameter &)> func,
rcl_interfaces::msg::ParameterDescriptor descriptor);
template void Parameters::setParamT<int>(std::string param_name,
rclcpp::ParameterValue initial_value, int &param,
std::function<void(const rclcpp::Parameter &)> func,
rcl_interfaces::msg::ParameterDescriptor descriptor);
template void Parameters::setParamT<double>(std::string param_name,
rclcpp::ParameterValue initial_value, double &param,
std::function<void(const rclcpp::Parameter &)> func,
rcl_interfaces::msg::ParameterDescriptor descriptor);
template void Parameters::setParamValue<int>(int &param, const int &value);
template void Parameters::setParamValue<bool>(bool &param, const bool &value);
template void Parameters::setParamValue<double>(double &param, const double &value);
} // namespace orbbec_camera
orbbec_camera/src/ob_camera_node.cpp
+39 -12
View File
@@ -1,3 +1,15 @@
/**************************************************************************/
/* */
/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc */
/* */
/* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the */
/* subject matter of this material. All manufacturing, reproduction, use, */
/* and sales rights pertaining to this subject matter are governed by the */
/* license agreement. The recipient of this software implicitly accepts */
/* the terms of the license. */
/* */
/**************************************************************************/
#include "orbbec_camera/ob_camera_node.h"
#include <rclcpp/rclcpp.hpp>
#include <thread>
@@ -7,8 +19,9 @@
namespace orbbec_camera {
using namespace std::chrono_literals;
OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device)
: node_(node), device_(device), logger_(node->get_logger()) {
OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device,
std::shared_ptr<Parameters> parameters)
: node_(node), device_(device), parameters_(parameters), logger_(node->get_logger()) {
static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node);
dynamic_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node);
// FIXME:
@@ -38,6 +51,21 @@ OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> devic
startPipeline();
}
template <class T>
void OBCameraNode::setNgetNodeParameter(
T& param, const std::string& param_name, const T& default_value,
const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor) {
try {
param = parameters_
->setParam(param_name, rclcpp::ParameterValue(default_value),
std::function<void(const rclcpp::Parameter&)>(), parameter_descriptor)
.get<T>();
} catch (const rclcpp::ParameterTypeException& ex) {
RCLCPP_ERROR_STREAM(logger_, "Failed to set parameter: " << param_name << ". " << ex.what());
throw;
}
}
OBCameraNode::~OBCameraNode() { clean(); }
void OBCameraNode::clean() {
@@ -136,26 +164,25 @@ void OBCameraNode::startPipeline() {
void OBCameraNode::getParameters() {
for (auto stream_index : IMAGE_STREAMS) {
std::string param_name = stream_name_[stream_index.first] + "_width";
width_[stream_index] = node_->declare_parameter<int>(param_name, IMAGE_WIDTH);
setNgetNodeParameter(width_[stream_index], param_name, IMAGE_WIDTH);
param_name = stream_name_[stream_index.first] + "_height";
height_[stream_index] = node_->declare_parameter<int>(param_name, IMAGE_HEIGHT);
setNgetNodeParameter(height_[stream_index], param_name, IMAGE_HEIGHT);
param_name = stream_name_[stream_index.first] + "_fps";
fps_[stream_index] = node_->declare_parameter<int>(param_name, IMAGE_FPS);
setNgetNodeParameter(fps_[stream_index], param_name, IMAGE_FPS);
param_name = "enable_" + stream_name_[stream_index.first];
enable_[stream_index] = node_->declare_parameter<bool>(param_name, true);
setNgetNodeParameter(enable_[stream_index], param_name, true);
param_name = stream_name_[stream_index.first] + "_frame_id";
std::string default_frame_id = "camera_" + stream_name_[stream_index.first] + "_frame";
frame_id_[stream_index] = node_->declare_parameter<std::string>(param_name, default_frame_id);
setNgetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
std::string default_optical_frame_id =
"camera_" + stream_name_[stream_index.first] + "_optical_frame";
param_name = stream_name_[stream_index.first] + "_optical_frame_id";
optical_frame_id_[stream_index] =
node_->declare_parameter<std::string>(param_name, default_optical_frame_id);
setNgetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
depth_aligned_frame_id_[stream_index] = stream_name_[OB_STREAM_COLOR] + "_optical_frame";
}
publish_tf_ = node_->declare_parameter<bool>("publish_tf", true);
align_depth_ = node_->declare_parameter<bool>("align_depth", true);
tf_publish_rate_ = node_->declare_parameter<double>("tf_publish_rate", 10.0);
setNgetNodeParameter(publish_tf_, "publish_tf", true);
setNgetNodeParameter(align_depth_, "align_depth", true);
setNgetNodeParameter(tf_publish_rate_, "tf_publish_rate", 10.0);
}
void OBCameraNode::setupTopics() {
orbbec_camera/src/ob_camera_node_factory.cpp
+28 -15
View File
@@ -1,3 +1,15 @@
/**************************************************************************/
/* */
/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc */
/* */
/* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the */
/* subject matter of this material. All manufacturing, reproduction, use, */
/* and sales rights pertaining to this subject matter are governed by the */
/* license agreement. The recipient of this software implicitly accepts */
/* the terms of the license. */
/* */
/**************************************************************************/
#include "orbbec_camera/ob_camera_node_factory.h"
namespace orbbec_camera {
OBCameraNodeFactory::OBCameraNodeFactory(const rclcpp::NodeOptions &node_options)
@@ -23,6 +35,7 @@ OBCameraNodeFactory::~OBCameraNodeFactory() {
void OBCameraNodeFactory::init() {
ctx_->setLoggerSeverity(OB_LOG_SEVERITY_NONE);
is_alive_.store(true);
parameters_ = std::make_shared<Parameters>(this);
serial_number_ = declare_parameter<std::string>("serial_number", "");
query_thread_ = std::thread([=]() {
std::chrono::milliseconds timespan(static_cast<int>(reconnect_timeout_ * 1e3));
@@ -85,20 +98,20 @@ void OBCameraNodeFactory::deviceDisconnectCallback(
CHECK_NOTNULL(device_list);
ob_camera_node_.reset(nullptr);
device_.reset();
// try {
// for (size_t i = 0; i < device_list->deviceCount(); i++) {
// auto dev = device_list->getDevice(i);
// std::string sn1 = dev->getDeviceInfo()->serialNumber();
// std::string sn2 = device_->getDeviceInfo()->serialNumber();
// if (sn1 == sn2) {
// RCLCPP_ERROR(logger_, "The device with SN %s was disconnected!", sn1.c_str());
// ob_camera_node_.reset(nullptr);
// device_.reset();
// }
// }
// } catch (const ob::Error &e) {
// RCLCPP_ERROR_STREAM(logger_, e.getMessage());
// }
// try {
// for (size_t i = 0; i < device_list->deviceCount(); i++) {
// auto dev = device_list->getDevice(i);
// std::string sn1 = dev->getDeviceInfo()->serialNumber();
// std::string sn2 = device_->getDeviceInfo()->serialNumber();
// if (sn1 == sn2) {
// RCLCPP_ERROR(logger_, "The device with SN %s was disconnected!", sn1.c_str());
// ob_camera_node_.reset(nullptr);
// device_.reset();
// }
// }
// } catch (const ob::Error &e) {
// RCLCPP_ERROR_STREAM(logger_, e.getMessage());
// }
}
void OBCameraNodeFactory::printDeviceInfo(const std::shared_ptr<ob::DeviceInfo> &device_info) {
@@ -143,6 +156,6 @@ void OBCameraNodeFactory::startDevice() {
ob_camera_node_.reset();
}
CHECK_NOTNULL(device_);
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_);
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_);
}
} // namespace orbbec_camera
orbbec_camera/src/ros_param_backend.cpp
+31
View File
@@ -0,0 +1,31 @@
/**************************************************************************/
/* */
/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc */
/* */
/* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the */
/* subject matter of this material. All manufacturing, reproduction, use, */
/* and sales rights pertaining to this subject matter are governed by the */
/* license agreement. The recipient of this software implicitly accepts */
/* the terms of the license. */
/* */
/**************************************************************************/
#include "orbbec_camera/ros_param_backend.h"
namespace orbbec_camera {
ParametersBackend::ParametersBackend(rclcpp::Node *node)
: node_(node), logger_(node_->get_logger()) {}
ParametersBackend::~ParametersBackend() {
if (ros_callback_) {
node_->remove_on_set_parameters_callback(
(rclcpp::node_interfaces::OnSetParametersCallbackHandle *)(ros_callback_.get()));
ros_callback_.reset();
}
}
void ParametersBackend::addOnSetParametersCallback(
rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType callback) {
ros_callback_ = node_->add_on_set_parameters_callback(callback);
}
} // namespace orbbec_camera
orbbec_camera/src/device_ctrl_service.cpp → orbbec_camera/src/ros_service.cpp
+12
View File
@@ -1,3 +1,15 @@
/**************************************************************************/
/* */
/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc */
/* */
/* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the */
/* subject matter of this material. All manufacturing, reproduction, use, */
/* and sales rights pertaining to this subject matter are governed by the */
/* license agreement. The recipient of this software implicitly accepts */
/* the terms of the license. */
/* */
/**************************************************************************/
#include "orbbec_camera/ob_camera_node.h"
#include <rclcpp/rclcpp.hpp>
#include <thread>
orbbec_camera/src/utils.cpp
+12
View File
@@ -1,3 +1,15 @@
/**************************************************************************/
/* */
/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc */
/* */
/* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the */
/* subject matter of this material. All manufacturing, reproduction, use, */
/* and sales rights pertaining to this subject matter are governed by the */
/* license agreement. The recipient of this software implicitly accepts */
/* the terms of the license. */
/* */
/**************************************************************************/
#include "orbbec_camera/utils.h"
namespace orbbec_camera {
sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,