feat(gemini2): add OrbbecSDK_ROS2/
This commit is contained in:
X-lanni
@@ -0,0 +1,68 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
||||
*
|
||||
* 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.
|
||||
*******************************************************************************/
|
||||
|
||||
#if __has_include(<cv_bridge/cv_bridge.hpp>)
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#elif __has_include(<cv_bridge/cv_bridge.h>)
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#endif
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include "orbbec_camera/d2c_viewer.h"
|
||||
|
||||
namespace orbbec_camera {
|
||||
D2CViewer::D2CViewer(rclcpp::Node* const node, rmw_qos_profile_t rgb_qos,
|
||||
rmw_qos_profile_t depth_qos)
|
||||
: node_(node), logger_(rclcpp::get_logger("d2c_viewer")) {
|
||||
rgb_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
|
||||
node_, "color/image_raw", rgb_qos);
|
||||
depth_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
|
||||
node_, "depth/image_raw", depth_qos);
|
||||
sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(MySyncPolicy(10), *rgb_sub_,
|
||||
*depth_sub_);
|
||||
sync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(1.0)); // 1s
|
||||
|
||||
using std::placeholders::_1;
|
||||
using std::placeholders::_2;
|
||||
sync_->registerCallback(std::bind(&D2CViewer::messageCallback, this, _1, _2));
|
||||
d2c_viewer_pub_ =
|
||||
node_->create_publisher<sensor_msgs::msg::Image>("depth_to_color/image_raw", rclcpp::QoS(1));
|
||||
}
|
||||
D2CViewer::~D2CViewer() = default;
|
||||
|
||||
void D2CViewer::messageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg) {
|
||||
if (rgb_msg->width != depth_msg->width || rgb_msg->height != depth_msg->height) {
|
||||
RCLCPP_ERROR(logger_, "rgb and depth image size not match(%d, %d) vs (%d, %d)", rgb_msg->width,
|
||||
rgb_msg->height, depth_msg->width, depth_msg->height);
|
||||
return;
|
||||
}
|
||||
auto rgb_img_ptr = cv_bridge::toCvCopy(rgb_msg, sensor_msgs::image_encodings::RGB8);
|
||||
auto depth_img_ptr = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
cv::Mat gray_depth, depth_img, d2c_img;
|
||||
depth_img_ptr->image.convertTo(gray_depth, CV_8UC1);
|
||||
cv::cvtColor(gray_depth, depth_img, cv::COLOR_GRAY2RGB);
|
||||
depth_img.setTo(cv::Scalar(255, 255, 0), depth_img);
|
||||
cv::bitwise_or(rgb_img_ptr->image, depth_img, d2c_img);
|
||||
sensor_msgs::msg::Image::SharedPtr d2c_msg =
|
||||
cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::RGB8, d2c_img)
|
||||
.toImageMsg();
|
||||
d2c_msg->header = rgb_msg->header;
|
||||
d2c_viewer_pub_->publish(*d2c_msg);
|
||||
}
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,158 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
||||
*
|
||||
* 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 "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> ¶meters) {
|
||||
for (const auto ¶meter : 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() noexcept {
|
||||
for (auto const ¶m : param_functions_) {
|
||||
try {
|
||||
node_->undeclare_parameter(param.first);
|
||||
} catch (const rclcpp::exceptions::InvalidParameterTypeException &e) {
|
||||
// ignore
|
||||
} catch (const std::exception &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, e.what());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
rclcpp::ParameterValue Parameters::setParam(
|
||||
const std::string ¶m_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 ¶m,
|
||||
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(
|
||||
[¶m, func](const rclcpp::Parameter ¶meter) { param = parameter.get_value<T>(); });
|
||||
if (func) {
|
||||
param_functions_[param_name].push_back(func);
|
||||
}
|
||||
param_names_[¶m] = param_name;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
void Parameters::setParamValue(T ¶m, const T &value) {
|
||||
param = value;
|
||||
try {
|
||||
std::string param_name = param_names_.at(¶m);
|
||||
|
||||
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(¶m);
|
||||
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 ¶m_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 ¶m,
|
||||
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 ¶m,
|
||||
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 ¶m,
|
||||
std::function<void(const rclcpp::Parameter &)> func,
|
||||
rcl_interfaces::msg::ParameterDescriptor descriptor);
|
||||
|
||||
template void Parameters::setParamValue<int>(int ¶m, const int &value);
|
||||
|
||||
template void Parameters::setParamValue<bool>(bool ¶m, const bool &value);
|
||||
|
||||
template void Parameters::setParamValue<double>(double ¶m, const double &value);
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,48 @@
|
||||
// Copyright 2023 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 "orbbec_camera/image_publisher.h"
|
||||
|
||||
namespace orbbec_camera {
|
||||
|
||||
// --- image_rcl_publisher implementation ---
|
||||
image_rcl_publisher::image_rcl_publisher(rclcpp::Node& node, const std::string& topic_name,
|
||||
const rmw_qos_profile_t& qos) {
|
||||
image_publisher_impl = node.create_publisher<sensor_msgs::msg::Image>(
|
||||
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos));
|
||||
}
|
||||
|
||||
void image_rcl_publisher::publish(sensor_msgs::msg::Image::UniquePtr image_ptr) {
|
||||
image_publisher_impl->publish(std::move(image_ptr));
|
||||
}
|
||||
|
||||
size_t image_rcl_publisher::get_subscription_count() const {
|
||||
return image_publisher_impl->get_subscription_count();
|
||||
}
|
||||
|
||||
// --- image_transport_publisher implementation ---
|
||||
image_transport_publisher::image_transport_publisher(rclcpp::Node& node,
|
||||
const std::string& topic_name,
|
||||
const rmw_qos_profile_t& qos) {
|
||||
image_publisher_impl = std::make_shared<image_transport::Publisher>(
|
||||
image_transport::create_publisher(&node, topic_name, qos));
|
||||
}
|
||||
void image_transport_publisher::publish(sensor_msgs::msg::Image::UniquePtr image_ptr) {
|
||||
image_publisher_impl->publish(*image_ptr);
|
||||
}
|
||||
|
||||
size_t image_transport_publisher::get_subscription_count() const {
|
||||
return image_publisher_impl->getNumSubscribers();
|
||||
}
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,119 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
||||
*
|
||||
* 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 "orbbec_camera/jetson_nv_decoder.h"
|
||||
#include <NvJpegDecoder.h>
|
||||
#include <NvV4l2Element.h>
|
||||
#include <algorithm>
|
||||
#include <nvbufsurface.h>
|
||||
#include <nvbufsurftransform.h>
|
||||
#include <NvBufSurface.h>
|
||||
#include <fstream>
|
||||
#include <libyuv.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "orbbec_camera/utils.h"
|
||||
|
||||
namespace orbbec_camera {
|
||||
|
||||
JetsonNvJPEGDecoder::JetsonNvJPEGDecoder(int width, int height) : JPEGDecoder(width, height) {}
|
||||
|
||||
JetsonNvJPEGDecoder::~JetsonNvJPEGDecoder() { delete decoder_; }
|
||||
|
||||
bool JetsonNvJPEGDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, uint8_t *dest) {
|
||||
if (!isValidJPEG(frame)) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Invalid JPEG frame");
|
||||
return false;
|
||||
}
|
||||
uint32_t pixfmt = 0;
|
||||
auto *data = static_cast<uint8_t *>(frame->data());
|
||||
uint32_t width = 0;
|
||||
uint32_t height = 0;
|
||||
auto data_size = frame->dataSize();
|
||||
while (data_size > 4 && data[data_size - 1] == 0x00) {
|
||||
data_size--;
|
||||
}
|
||||
int fd = -1;
|
||||
decoder_ = NvJPEGDecoder::createJPEGDecoder("jpegdec");
|
||||
std::shared_ptr<int> decoder_deleter(nullptr, [&](int *) { delete decoder_; });
|
||||
decoder_->decodeToFd(fd, data, data_size, pixfmt, width, height);
|
||||
if (pixfmt != V4L2_PIX_FMT_YUV422M) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Unexpected pixfmt: " << pixfmt);
|
||||
if (fd != -1) {
|
||||
close(fd);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if (width != static_cast<uint32_t>(width_) || height != static_cast<uint32_t>(height_)) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"),
|
||||
"Unexpected width/height: " << width << "x" << height);
|
||||
if (fd != -1) {
|
||||
close(fd);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
NvBufSurf::NvCommonAllocateParams nvbufParams;
|
||||
memset(&nvbufParams, 0, sizeof(nvbufParams));
|
||||
|
||||
nvbufParams.memType = NVBUF_MEM_SURFACE_ARRAY;
|
||||
nvbufParams.width = width;
|
||||
nvbufParams.height = height;
|
||||
nvbufParams.layout = NVBUF_LAYOUT_PITCH;
|
||||
nvbufParams.colorFormat = NVBUF_COLOR_FORMAT_RGBA;
|
||||
int rgba_fd = -1;
|
||||
int ret = NvBufSurf::NvAllocate(&nvbufParams, 1, &rgba_fd);
|
||||
if (ret != 0) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Failed to allocate buffer");
|
||||
return false;
|
||||
}
|
||||
NvBufSurf::NvCommonTransformParams transform_params;
|
||||
transform_params.src_top = 0;
|
||||
transform_params.src_left = 0;
|
||||
transform_params.src_width = width;
|
||||
transform_params.src_height = height;
|
||||
transform_params.dst_top = 0;
|
||||
transform_params.dst_left = 0;
|
||||
transform_params.dst_width = width;
|
||||
transform_params.dst_height = height;
|
||||
transform_params.flag = NVBUFSURF_TRANSFORM_FILTER;
|
||||
transform_params.flip = NvBufSurfTransform_None;
|
||||
transform_params.filter = NvBufSurfTransformInter_Nearest;
|
||||
ret = NvBufSurf::NvTransform(&transform_params, fd, rgba_fd);
|
||||
if (ret != 0) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Failed to transform buffer");
|
||||
if (rgba_fd != -1) {
|
||||
NvBufSurf::NvDestroy(rgba_fd);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
NvBufSurface *nvbuf_surf = 0;
|
||||
NvBufSurfaceFromFd(rgba_fd, (void **)&nvbuf_surf);
|
||||
ret = NvBufSurfaceMap(nvbuf_surf, 0, 0, NVBUF_MAP_READ_WRITE);
|
||||
if (ret < 0) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Failed to map buffer");
|
||||
return false;
|
||||
}
|
||||
NvBufSurfaceSyncForCpu(nvbuf_surf, 0, 0);
|
||||
uint8_t *rgba = (uint8_t *)nvbuf_surf->surfaceList[0].mappedAddr.addr[0];
|
||||
int src_stride_argb = width * 4;
|
||||
int dst_stride_rgb24 = width * 3;
|
||||
|
||||
libyuv::ARGBToRGB24(rgba, src_stride_argb, dest, dst_stride_rgb24, width, height);
|
||||
NvBufSurfaceUnMap(nvbuf_surf, 0, 0);
|
||||
if (rgba_fd != -1) {
|
||||
NvBufSurf::NvDestroy(rgba_fd);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,22 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
||||
*
|
||||
* 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 <orbbec_camera/jpeg_decoder.h>
|
||||
|
||||
namespace orbbec_camera {
|
||||
JPEGDecoder::JPEGDecoder(int width, int height) : width_(width), height_(height) {}
|
||||
JPEGDecoder::~JPEGDecoder() {}
|
||||
|
||||
} // namespace orbbec_camera
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,537 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
||||
*
|
||||
* 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 "orbbec_camera/ob_camera_node_driver.h"
|
||||
#include "orbbec_camera/utils.h"
|
||||
#include <fcntl.h>
|
||||
#include <semaphore.h>
|
||||
#include <sys/shm.h>
|
||||
#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
#include <rclcpp_components/register_node_macro.hpp>
|
||||
#include <csignal>
|
||||
#include <sys/mman.h>
|
||||
#include <unistd.h>
|
||||
#include <filesystem>
|
||||
|
||||
#include <fstream>
|
||||
#include <iomanip> // For std::put_time
|
||||
|
||||
std::string g_camera_name = "orbbec_camera"; // Assuming this is declared elsewhere
|
||||
|
||||
void signalHandler(int sig) {
|
||||
std::cout << "Received signal: " << sig << std::endl;
|
||||
|
||||
std::string log_dir = "Log/";
|
||||
|
||||
// get current time
|
||||
std::time_t now = std::time(nullptr);
|
||||
std::tm *local_time = std::localtime(&now);
|
||||
|
||||
// format date and time to string, format as "2024_05_20_12_34_56"
|
||||
std::ostringstream time_stream;
|
||||
time_stream << std::put_time(local_time, "%Y_%m_%d_%H_%M_%S");
|
||||
|
||||
// generate log file name
|
||||
std::string log_file_name = g_camera_name + "_crash_stack_trace_" + time_stream.str() + ".log";
|
||||
std::string log_file_path = log_dir + log_file_name;
|
||||
|
||||
if (!std::filesystem::exists(log_dir)) {
|
||||
std::filesystem::create_directories(log_dir);
|
||||
}
|
||||
|
||||
std::cout << "Log crash stack trace to " << log_file_path << std::endl;
|
||||
std::ofstream log_file(log_file_path, std::ios::app);
|
||||
|
||||
if (log_file.is_open()) {
|
||||
log_file << "Received signal: " << sig << std::endl;
|
||||
|
||||
backward::StackTrace st;
|
||||
st.load_here(32); // Capture stack
|
||||
backward::Printer p;
|
||||
p.print(st, log_file); // Print stack to log file
|
||||
}
|
||||
|
||||
log_file.close();
|
||||
exit(sig); // Exit program
|
||||
}
|
||||
|
||||
namespace orbbec_camera {
|
||||
|
||||
backward::SignalHandling OBCameraNodeDriver::sh;
|
||||
OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
|
||||
: Node("orbbec_camera_node", "/", node_options),
|
||||
node_options_(node_options),
|
||||
config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
|
||||
"/config/OrbbecSDKConfig_v1.0.xml"),
|
||||
ctx_(std::make_unique<ob::Context>(config_path_.c_str())),
|
||||
logger_(this->get_logger()) {
|
||||
init();
|
||||
}
|
||||
|
||||
OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::string &ns,
|
||||
const rclcpp::NodeOptions &node_options)
|
||||
: Node(node_name, ns, node_options),
|
||||
node_options_(node_options),
|
||||
ctx_(std::make_unique<ob::Context>()),
|
||||
logger_(this->get_logger()) {
|
||||
init();
|
||||
}
|
||||
|
||||
OBCameraNodeDriver::~OBCameraNodeDriver() {
|
||||
is_alive_.store(false);
|
||||
if (device_count_update_thread_ && device_count_update_thread_->joinable()) {
|
||||
device_count_update_thread_->join();
|
||||
}
|
||||
if (query_thread_ && query_thread_->joinable()) {
|
||||
query_thread_->join();
|
||||
}
|
||||
if (reset_device_thread_ && reset_device_thread_->joinable()) {
|
||||
reset_device_cond_.notify_all();
|
||||
reset_device_thread_->join();
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNodeDriver::init() {
|
||||
signal(SIGSEGV, signalHandler); // segment fault
|
||||
signal(SIGABRT, signalHandler); // abort
|
||||
signal(SIGFPE, signalHandler); // float point exception
|
||||
signal(SIGILL, signalHandler); // illegal instruction
|
||||
auto log_level_str = declare_parameter<std::string>("log_level", "none");
|
||||
auto log_level = obLogSeverityFromString(log_level_str);
|
||||
connection_delay_ = static_cast<int>(declare_parameter<int>("connection_delay", 100));
|
||||
enable_sync_host_time_ = declare_parameter<bool>("enable_sync_host_time", true);
|
||||
g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
|
||||
enable_hardware_reset_ = declare_parameter<bool>("enable_hardware_reset", false);
|
||||
ob::Context::setLoggerToConsole(log_level);
|
||||
orb_device_lock_shm_fd_ = shm_open(ORB_DEFAULT_LOCK_NAME.c_str(), O_CREAT | O_RDWR, 0666);
|
||||
if (orb_device_lock_shm_fd_ < 0) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to open shared memory " << ORB_DEFAULT_LOCK_NAME);
|
||||
return;
|
||||
}
|
||||
int ret = ftruncate(orb_device_lock_shm_fd_, sizeof(pthread_mutex_t));
|
||||
if (ret < 0) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to truncate shared memory " << ORB_DEFAULT_LOCK_NAME);
|
||||
return;
|
||||
}
|
||||
orb_device_lock_shm_addr_ =
|
||||
static_cast<uint8_t *>(mmap(NULL, sizeof(pthread_mutex_t), PROT_READ | PROT_WRITE, MAP_SHARED,
|
||||
orb_device_lock_shm_fd_, 0));
|
||||
if (orb_device_lock_shm_addr_ == MAP_FAILED) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to map shared memory " << ORB_DEFAULT_LOCK_NAME);
|
||||
return;
|
||||
}
|
||||
reboot_device_srv_ = this->create_service<std_srvs::srv::Empty>(
|
||||
"reboot_device", std::bind(&OBCameraNodeDriver::rebootDeviceCallback, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
pthread_mutexattr_init(&orb_device_lock_attr_);
|
||||
pthread_mutexattr_setpshared(&orb_device_lock_attr_, PTHREAD_PROCESS_SHARED);
|
||||
orb_device_lock_ = (pthread_mutex_t *)orb_device_lock_shm_addr_;
|
||||
pthread_mutex_init(orb_device_lock_, &orb_device_lock_attr_);
|
||||
is_alive_.store(true);
|
||||
parameters_ = std::make_shared<Parameters>(this);
|
||||
serial_number_ = declare_parameter<std::string>("serial_number", "");
|
||||
device_num_ = static_cast<int>(declare_parameter<int>("device_num", 1));
|
||||
usb_port_ = declare_parameter<std::string>("usb_port", "");
|
||||
net_device_ip_ = declare_parameter<std::string>("net_device_ip", "");
|
||||
net_device_port_ = static_cast<int>(declare_parameter<int>("net_device_port", 0));
|
||||
enumerate_net_device_ = declare_parameter<bool>("enumerate_net_device", false);
|
||||
ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
|
||||
ctx_->setDeviceChangedCallback([this](const std::shared_ptr<ob::DeviceList> &removed_list,
|
||||
const std::shared_ptr<ob::DeviceList> &added_list) {
|
||||
onDeviceConnected(added_list);
|
||||
onDeviceDisconnected(removed_list);
|
||||
});
|
||||
check_connect_timer_ =
|
||||
this->create_wall_timer(std::chrono::milliseconds(1000), [this]() { checkConnectTimer(); });
|
||||
CHECK_NOTNULL(check_connect_timer_);
|
||||
query_thread_ = std::make_shared<std::thread>([this]() { queryDevice(); });
|
||||
reset_device_thread_ = std::make_shared<std::thread>([this]() { resetDevice(); });
|
||||
}
|
||||
|
||||
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
|
||||
CHECK_NOTNULL(device_list);
|
||||
if (device_list->deviceCount() == 0) {
|
||||
return;
|
||||
}
|
||||
if (!device_) {
|
||||
startDevice(device_list);
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceList> &device_list) {
|
||||
CHECK_NOTNULL(device_list);
|
||||
if (device_list->deviceCount() == 0) {
|
||||
return;
|
||||
}
|
||||
RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected");
|
||||
for (size_t i = 0; i < device_list->deviceCount(); i++) {
|
||||
std::string uid = device_list->uid(i);
|
||||
std::string serial_number = device_list->serialNumber(i);
|
||||
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
|
||||
RCLCPP_INFO_STREAM(logger_, "device with " << uid << " disconnected");
|
||||
if (uid == device_unique_id_ || serial_number_ == serial_number) {
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"device with " << uid << " disconnected, notify reset device thread.");
|
||||
std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
|
||||
reset_device_flag_ = true;
|
||||
reset_device_cond_.notify_all();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
OBLogSeverity OBCameraNodeDriver::obLogSeverityFromString(const std::string_view &log_level) {
|
||||
if (log_level == "debug") {
|
||||
return OBLogSeverity::OB_LOG_SEVERITY_DEBUG;
|
||||
} else if (log_level == "info") {
|
||||
return OBLogSeverity::OB_LOG_SEVERITY_INFO;
|
||||
} else if (log_level == "warn") {
|
||||
return OBLogSeverity::OB_LOG_SEVERITY_WARN;
|
||||
} else if (log_level == "error") {
|
||||
return OBLogSeverity::OB_LOG_SEVERITY_ERROR;
|
||||
} else if (log_level == "fatal") {
|
||||
return OBLogSeverity::OB_LOG_SEVERITY_FATAL;
|
||||
} else {
|
||||
return OBLogSeverity::OB_LOG_SEVERITY_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNodeDriver::checkConnectTimer() {
|
||||
if (!device_connected_.load()) {
|
||||
RCLCPP_DEBUG_STREAM(logger_,
|
||||
"checkConnectTimer: device " << serial_number_ << " not connected");
|
||||
return;
|
||||
} else if (!ob_camera_node_) {
|
||||
device_connected_.store(false);
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNodeDriver::queryDevice() {
|
||||
while (is_alive_ && rclcpp::ok() && !device_connected_.load()) {
|
||||
if (!net_device_ip_.empty() && net_device_port_ != 0) {
|
||||
connectNetDevice(net_device_ip_, net_device_port_);
|
||||
} else {
|
||||
auto device_list = ctx_->queryDeviceList();
|
||||
if (device_list->deviceCount() == 0) {
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"queryDevice :No Device found, using usb event to trigger "
|
||||
"OBCameraNodeDriver::onDeviceConnected");
|
||||
return;
|
||||
}
|
||||
startDevice(device_list);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNodeDriver::resetDevice() {
|
||||
while (is_alive_ && rclcpp::ok()) {
|
||||
std::unique_lock<decltype(reset_device_mutex_)> lock(reset_device_mutex_);
|
||||
reset_device_cond_.wait(lock,
|
||||
[this]() { return !is_alive_ || !rclcpp::ok() || reset_device_flag_; });
|
||||
if (!is_alive_ || !rclcpp::ok()) {
|
||||
break;
|
||||
}
|
||||
RCLCPP_INFO_STREAM(logger_, "resetDevice : Reset device uid: " << device_unique_id_);
|
||||
std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
|
||||
{
|
||||
ob_camera_node_.reset();
|
||||
device_.reset();
|
||||
device_info_.reset();
|
||||
device_connected_ = false;
|
||||
device_unique_id_.clear();
|
||||
reset_device_flag_ = false;
|
||||
}
|
||||
RCLCPP_INFO_STREAM(logger_, "Reset device uid: " << device_unique_id_ << " done");
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNodeDriver::rebootDeviceCallback(
|
||||
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
|
||||
std::shared_ptr<std_srvs::srv::Empty::Response> response) {
|
||||
(void)request;
|
||||
(void)response;
|
||||
if (!device_connected_) {
|
||||
RCLCPP_WARN(logger_, "Device not connected");
|
||||
return;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Reboot device");
|
||||
ob_camera_node_->rebootDevice();
|
||||
device_connected_ = false;
|
||||
device_ = nullptr;
|
||||
}
|
||||
|
||||
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
|
||||
const std::shared_ptr<ob::DeviceList> &list) {
|
||||
if (device_num_ == 1) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Connecting to the default device");
|
||||
return list->getDevice(0);
|
||||
}
|
||||
|
||||
std::shared_ptr<ob::Device> device = nullptr;
|
||||
if (!serial_number_.empty()) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Connecting to device with serial number: " << serial_number_);
|
||||
device = selectDeviceBySerialNumber(list, serial_number_);
|
||||
} else if (!usb_port_.empty()) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Connecting to device with usb port: " << usb_port_);
|
||||
device = selectDeviceByUSBPort(list, usb_port_);
|
||||
}
|
||||
if (device == nullptr) {
|
||||
RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Device with serial number %s not found",
|
||||
serial_number_.c_str());
|
||||
device_connected_ = false;
|
||||
return nullptr;
|
||||
}
|
||||
return device;
|
||||
}
|
||||
|
||||
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
|
||||
const std::shared_ptr<ob::DeviceList> &list, const std::string &serial_number) {
|
||||
std::string lower_sn;
|
||||
std::transform(serial_number.begin(), serial_number.end(), std::back_inserter(lower_sn),
|
||||
[](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); });
|
||||
for (size_t i = 0; i < list->deviceCount(); i++) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Before lock: Select device serial number: " << serial_number);
|
||||
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
|
||||
RCLCPP_INFO_STREAM(logger_, "After lock: Select device serial number: " << serial_number);
|
||||
try {
|
||||
auto pid = list->pid(i);
|
||||
if (isOpenNIDevice(pid)) {
|
||||
// openNI device
|
||||
auto device = list->getDevice(i);
|
||||
auto device_info = device->getDeviceInfo();
|
||||
if (device_info->serialNumber() == serial_number) {
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Device serial number " << device_info->serialNumber() << " matched");
|
||||
return device;
|
||||
}
|
||||
} else {
|
||||
std::string sn = list->serialNumber(i);
|
||||
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "Device serial number: " << sn);
|
||||
if (sn == serial_number) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Device serial number " << sn << " matched");
|
||||
return list->getDevice(i);
|
||||
}
|
||||
}
|
||||
} catch (ob::Error &e) {
|
||||
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 1000,
|
||||
"Failed to get device info " << e.getMessage());
|
||||
} catch (std::exception &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.what());
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info");
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
|
||||
const std::shared_ptr<ob::DeviceList> &list, const std::string &usb_port) {
|
||||
try {
|
||||
RCLCPP_INFO_STREAM(logger_, "Before lock: Select device usb port: " << usb_port);
|
||||
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
|
||||
RCLCPP_INFO_STREAM(logger_, "After lock: Select device usb port: " << usb_port);
|
||||
auto device = list->getDeviceByUid(usb_port.c_str());
|
||||
if (device) {
|
||||
RCLCPP_INFO_STREAM(logger_, "getDeviceByUid device usb port " << usb_port << " done");
|
||||
} else {
|
||||
RCLCPP_ERROR_STREAM(logger_, "getDeviceByUid device usb port " << usb_port << " failed");
|
||||
RCLCPP_ERROR_STREAM(logger_,
|
||||
"Please use script to get usb port: "
|
||||
"ros2 run orbbec_camera list_devices_node");
|
||||
}
|
||||
return device;
|
||||
} catch (ob::Error &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.getMessage());
|
||||
} catch (std::exception &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.what());
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info");
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
|
||||
if (device_) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Device is not nullptr, reset device");
|
||||
device_.reset();
|
||||
}
|
||||
if (enable_hardware_reset_ && !hardware_reset_done_) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Enable hardware reset, reboot device");
|
||||
device->reboot();
|
||||
RCLCPP_INFO_STREAM(logger_, "Reboot device done");
|
||||
hardware_reset_done_ = true;
|
||||
device_connected_ = false;
|
||||
return;
|
||||
}
|
||||
device_ = device;
|
||||
CHECK_NOTNULL(device_);
|
||||
CHECK_NOTNULL(device_.get());
|
||||
if (ob_camera_node_) {
|
||||
ob_camera_node_.reset();
|
||||
}
|
||||
int retry_count = 0;
|
||||
constexpr int max_retries = 3;
|
||||
bool initialized = false;
|
||||
device_info_ = device_->getDeviceInfo();
|
||||
RCLCPP_INFO_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
|
||||
|
||||
while (retry_count < max_retries && !initialized) {
|
||||
try {
|
||||
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
|
||||
node_options_.use_intra_process_comms());
|
||||
initialized = true;
|
||||
} catch (const ob::Error &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
|
||||
<< retry_count + 1 << " of " << max_retries
|
||||
<< "): " << e.getMessage());
|
||||
} catch (const std::exception &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt " << retry_count + 1
|
||||
<< " of " << max_retries
|
||||
<< "): " << e.what());
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
|
||||
<< retry_count + 1 << " of " << max_retries << ")");
|
||||
}
|
||||
retry_count++;
|
||||
}
|
||||
|
||||
if (!initialized) {
|
||||
RCLCPP_ERROR_STREAM(logger_,
|
||||
"Device initialization failed after " << max_retries << " attempts.");
|
||||
throw std::runtime_error("Device initialization failed after " + std::to_string(max_retries) +
|
||||
" attempts.");
|
||||
}
|
||||
|
||||
ob_camera_node_->startIMU();
|
||||
ob_camera_node_->startStreams();
|
||||
device_connected_ = true;
|
||||
serial_number_ = device_info_->serialNumber();
|
||||
CHECK_NOTNULL(device_info_.get());
|
||||
device_unique_id_ = device_info_->uid();
|
||||
if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid())) {
|
||||
TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
|
||||
sync_host_time_timer_ = this->create_wall_timer(std::chrono::milliseconds(30000), [this]() {
|
||||
if (device_) {
|
||||
TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
|
||||
}
|
||||
});
|
||||
}
|
||||
RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->name() << " connected");
|
||||
RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->serialNumber());
|
||||
RCLCPP_INFO_STREAM(logger_, "Firmware version: " << device_info_->firmwareVersion());
|
||||
RCLCPP_INFO_STREAM(logger_, "Hardware version: " << device_info_->hardwareVersion());
|
||||
RCLCPP_INFO_STREAM(logger_, "device unique id: " << device_unique_id_);
|
||||
RCLCPP_INFO_STREAM(logger_, "Current node pid: " << getpid());
|
||||
RCLCPP_INFO_STREAM(logger_, "usb connect type: " << device_info_->connectionType());
|
||||
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::high_resolution_clock::now() - start_time_);
|
||||
RCLCPP_INFO_STREAM(logger_, "Start device cost " << time_cost.count() << " ms");
|
||||
} // namespace orbbec_camera
|
||||
|
||||
void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int net_device_port) {
|
||||
if (net_device_ip.empty() || net_device_port == 0) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Invalid net device ip or port");
|
||||
return;
|
||||
}
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_, "Connecting to net device ip: " << net_device_ip << " port: " << net_device_port);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_));
|
||||
auto device = ctx_->createNetDevice(net_device_ip.c_str(), net_device_port);
|
||||
if (device == nullptr) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to connect to net device " << net_device_ip);
|
||||
return;
|
||||
}
|
||||
initializeDevice(device);
|
||||
}
|
||||
|
||||
void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list) {
|
||||
if (device_connected_) {
|
||||
return;
|
||||
}
|
||||
if (list->deviceCount() == 0) {
|
||||
RCLCPP_WARN(logger_, "No device found");
|
||||
return;
|
||||
}
|
||||
start_time_ = std::chrono::high_resolution_clock::now();
|
||||
if (device_) {
|
||||
device_.reset();
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_));
|
||||
int try_lock_count = 0;
|
||||
int max_try_lock_count = 50;
|
||||
|
||||
while (try_lock_count < max_try_lock_count) {
|
||||
int try_lock_result = pthread_mutex_trylock(orb_device_lock_);
|
||||
|
||||
if (try_lock_result == 0) {
|
||||
// success get lock,break
|
||||
break;
|
||||
} else if (try_lock_result == EBUSY) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Device lock is held by another process, waiting 100ms");
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
} else {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_");
|
||||
return; // Not EBUSY, return
|
||||
}
|
||||
|
||||
try_lock_count++;
|
||||
}
|
||||
if (try_lock_count >= max_try_lock_count) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_");
|
||||
return;
|
||||
}
|
||||
|
||||
std::shared_ptr<int> lock_holder(nullptr,
|
||||
[this](int *) { pthread_mutex_unlock(orb_device_lock_); });
|
||||
bool start_device_failed = false;
|
||||
try {
|
||||
auto start_time = std::chrono::high_resolution_clock::now();
|
||||
auto device = selectDevice(list);
|
||||
auto end_time = std::chrono::high_resolution_clock::now();
|
||||
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
|
||||
RCLCPP_INFO_STREAM(logger_, "Select device cost " << time_cost.count() << " ms");
|
||||
if (device == nullptr) {
|
||||
RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Device with serial number %s not found",
|
||||
serial_number_.c_str());
|
||||
device_connected_ = false;
|
||||
return;
|
||||
}
|
||||
start_time = std::chrono::high_resolution_clock::now();
|
||||
initializeDevice(device);
|
||||
end_time = std::chrono::high_resolution_clock::now();
|
||||
time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
|
||||
RCLCPP_INFO_STREAM(logger_, "Initialize device cost " << time_cost.count() << " ms");
|
||||
} catch (ob::Error &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.getMessage());
|
||||
start_device_failed = true;
|
||||
} catch (std::exception &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.what());
|
||||
start_device_failed = true;
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device");
|
||||
start_device_failed = true;
|
||||
}
|
||||
if (start_device_failed) {
|
||||
device_connected_ = false;
|
||||
std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
|
||||
reset_device_flag_ = true;
|
||||
reset_device_cond_.notify_all();
|
||||
}
|
||||
}
|
||||
} // namespace orbbec_camera
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::OBCameraNodeDriver)
|
||||
@@ -0,0 +1,237 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
||||
*
|
||||
* 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 "orbbec_camera/rk_mpp_decoder.h"
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <magic_enum/magic_enum.hpp>
|
||||
|
||||
namespace orbbec_camera {
|
||||
|
||||
RKJPEGDecoder::RKJPEGDecoder(int width, int height) : JPEGDecoder(width, height) {
|
||||
rgb_buffer_ = new uint8_t[width_ * height_ * 3];
|
||||
MPP_RET ret = mpp_create(&mpp_ctx_, &mpp_api_);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"), "mpp_create failed, ret = " << ret);
|
||||
throw std::runtime_error("mpp_create failed");
|
||||
}
|
||||
MpiCmd mpi_cmd = MPP_CMD_BASE;
|
||||
MppParam mpp_param = nullptr;
|
||||
|
||||
mpi_cmd = MPP_DEC_SET_PARSER_SPLIT_MODE;
|
||||
mpp_param = &need_split_;
|
||||
ret = mpp_api_->control(mpp_ctx_, mpi_cmd, mpp_param);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
|
||||
"mpp_api_->control failed, ret = " << ret);
|
||||
throw std::runtime_error("mpp_api_->control failed");
|
||||
}
|
||||
ret = mpp_init(mpp_ctx_, MPP_CTX_DEC, MPP_VIDEO_CodingMJPEG);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"), "mpp_init failed, ret = " << ret);
|
||||
throw std::runtime_error("mpp_init failed");
|
||||
}
|
||||
MppFrameFormat fmt = MPP_FMT_YUV420SP_VU;
|
||||
mpp_param = &fmt;
|
||||
ret = mpp_api_->control(mpp_ctx_, MPP_DEC_SET_OUTPUT_FORMAT, mpp_param);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
|
||||
"mpp_api_->control failed, ret = " << ret);
|
||||
throw std::runtime_error("mpp_api_->control failed");
|
||||
}
|
||||
ret = mpp_frame_init(&mpp_frame_);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
|
||||
"mpp_frame_init failed, ret = " << ret);
|
||||
throw std::runtime_error("mpp_frame_init failed");
|
||||
}
|
||||
ret = mpp_buffer_group_get_internal(&mpp_frame_group_, MPP_BUFFER_TYPE_ION);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
|
||||
"mpp_buffer_group_get_internal failed, ret = " << ret);
|
||||
throw std::runtime_error("mpp_buffer_group_get_internal failed");
|
||||
}
|
||||
ret = mpp_buffer_group_get_internal(&mpp_packet_group_, MPP_BUFFER_TYPE_ION);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
|
||||
"mpp_buffer_group_get_internal failed, ret = " << ret);
|
||||
throw std::runtime_error("mpp_buffer_group_get_internal failed");
|
||||
}
|
||||
RK_U32 hor_stride = MPP_ALIGN(width_, 16);
|
||||
RK_U32 ver_stride = MPP_ALIGN(height_, 16);
|
||||
ret = mpp_buffer_get(mpp_frame_group_, &mpp_frame_buffer_, hor_stride * ver_stride * 4);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
|
||||
"mpp_buffer_get failed, ret = " << ret);
|
||||
throw std::runtime_error("mpp_buffer_get failed");
|
||||
}
|
||||
mpp_frame_set_buffer(mpp_frame_, mpp_frame_buffer_);
|
||||
ret = mpp_buffer_get(mpp_packet_group_, &mpp_packet_buffer_, width_ * height_ * 3);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
|
||||
"mpp_buffer_get failed, ret = " << ret);
|
||||
throw std::runtime_error("mpp_buffer_get failed");
|
||||
}
|
||||
mpp_packet_init_with_buffer(&mpp_packet_, mpp_packet_buffer_);
|
||||
data_buffer_ = (uint8_t *)mpp_buffer_get_ptr(mpp_packet_buffer_);
|
||||
}
|
||||
|
||||
RKJPEGDecoder::~RKJPEGDecoder() {
|
||||
if (mpp_frame_buffer_) {
|
||||
mpp_buffer_put(mpp_frame_buffer_);
|
||||
mpp_frame_buffer_ = nullptr;
|
||||
}
|
||||
if (mpp_packet_buffer_) {
|
||||
mpp_buffer_put(mpp_packet_buffer_);
|
||||
mpp_packet_buffer_ = nullptr;
|
||||
}
|
||||
if (mpp_frame_group_) {
|
||||
mpp_buffer_group_put(mpp_frame_group_);
|
||||
mpp_frame_group_ = nullptr;
|
||||
}
|
||||
if (mpp_packet_group_) {
|
||||
mpp_buffer_group_put(mpp_packet_group_);
|
||||
mpp_packet_group_ = nullptr;
|
||||
}
|
||||
if (mpp_frame_) {
|
||||
mpp_frame_deinit(&mpp_frame_);
|
||||
mpp_frame_ = nullptr;
|
||||
}
|
||||
if (mpp_packet_) {
|
||||
mpp_packet_deinit(&mpp_packet_);
|
||||
mpp_packet_ = nullptr;
|
||||
}
|
||||
if (mpp_ctx_) {
|
||||
mpp_destroy(mpp_ctx_);
|
||||
mpp_ctx_ = nullptr;
|
||||
}
|
||||
if (rgb_buffer_) {
|
||||
delete[] rgb_buffer_;
|
||||
}
|
||||
}
|
||||
|
||||
bool RKJPEGDecoder::mppFrame2RGB(const MppFrame frame, uint8_t *data) {
|
||||
int width = mpp_frame_get_width(frame);
|
||||
int height = mpp_frame_get_height(frame);
|
||||
MppBuffer buffer = mpp_frame_get_buffer(frame);
|
||||
CHECK_EQ(width, width_);
|
||||
CHECK_EQ(height, height_);
|
||||
CHECK_NOTNULL(data);
|
||||
CHECK_EQ(width, width_);
|
||||
CHECK_EQ(height, height_);
|
||||
memset(data, 0, width * height * 3);
|
||||
auto buffer_ptr = mpp_buffer_get_ptr(buffer);
|
||||
#if defined(USE_LIBYUV)
|
||||
auto *y = (const uint8_t *)buffer_ptr;
|
||||
auto *uv = y + width * height;
|
||||
int ret = libyuv::NV12ToRGB24(y, width, uv, width, data, width * 3, width, height);
|
||||
if (ret) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"), "libyuv error " << ret);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
#else
|
||||
rga_info_t src_info;
|
||||
rga_info_t dst_info;
|
||||
// NOTE: memset to zero is MUST
|
||||
memset(&src_info, 0, sizeof(rga_info_t));
|
||||
memset(&dst_info, 0, sizeof(rga_info_t));
|
||||
src_info.fd = -1;
|
||||
src_info.mmuFlag = 1;
|
||||
src_info.virAddr = buffer_ptr;
|
||||
src_info.format = RK_FORMAT_YCbCr_420_SP;
|
||||
dst_info.fd = -1;
|
||||
dst_info.mmuFlag = 1;
|
||||
dst_info.virAddr = data;
|
||||
dst_info.format = RK_FORMAT_BGR_888;
|
||||
rga_set_rect(&src_info.rect, 0, 0, width, height, width, height, RK_FORMAT_YCbCr_420_SP);
|
||||
rga_set_rect(&dst_info.rect, 0, 0, width, height, width, height, RK_FORMAT_BGR_888);
|
||||
int ret = c_RkRgaBlit(&src_info, &dst_info, nullptr);
|
||||
if (ret) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
|
||||
"c_RkRgaBlit error " << ret << " errno " << strerror(errno));
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool RKJPEGDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, uint8_t *dest) {
|
||||
MPP_RET ret = MPP_OK;
|
||||
memset(data_buffer_, 0, width_ * height_ * 3);
|
||||
memcpy(data_buffer_, frame->data(), frame->dataSize());
|
||||
mpp_packet_set_pos(mpp_packet_, data_buffer_);
|
||||
mpp_packet_set_length(mpp_packet_, frame->dataSize());
|
||||
mpp_packet_set_eos(mpp_packet_);
|
||||
CHECK_NOTNULL(mpp_ctx_);
|
||||
ret = mpp_api_->poll(mpp_ctx_, MPP_PORT_INPUT, MPP_POLL_BLOCK);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp poll failed %d", ret);
|
||||
return false;
|
||||
}
|
||||
ret = mpp_api_->dequeue(mpp_ctx_, MPP_PORT_INPUT, &mpp_task_);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp dequeue failed %d", ret);
|
||||
return false;
|
||||
}
|
||||
mpp_task_meta_set_packet(mpp_task_, KEY_INPUT_PACKET, mpp_packet_);
|
||||
mpp_task_meta_set_frame(mpp_task_, KEY_OUTPUT_FRAME, mpp_frame_);
|
||||
ret = mpp_api_->enqueue(mpp_ctx_, MPP_PORT_INPUT, mpp_task_);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp enqueue failed %d", ret);
|
||||
return false;
|
||||
}
|
||||
ret = mpp_api_->poll(mpp_ctx_, MPP_PORT_OUTPUT, MPP_POLL_BLOCK);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp poll failed %d", ret);
|
||||
return false;
|
||||
}
|
||||
ret = mpp_api_->dequeue(mpp_ctx_, MPP_PORT_OUTPUT, &mpp_task_);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp dequeue failed %d", ret);
|
||||
return false;
|
||||
}
|
||||
if (mpp_task_) {
|
||||
MppFrame output_frame = nullptr;
|
||||
mpp_task_meta_get_frame(mpp_task_, KEY_OUTPUT_FRAME, &output_frame);
|
||||
if (mpp_frame_) {
|
||||
int width = mpp_frame_get_width(mpp_frame_);
|
||||
int height = mpp_frame_get_height(mpp_frame_);
|
||||
if (width != width_ || height != height_) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
|
||||
"mpp frame size error " << width << " " << height);
|
||||
return false;
|
||||
}
|
||||
if (!mppFrame2RGB(mpp_frame_, rgb_buffer_)) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"), "mpp frame to rgb error");
|
||||
return false;
|
||||
}
|
||||
if (mpp_frame_get_eos(output_frame)) {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("rk_mpp_decoder"), "mpp frame get eos");
|
||||
}
|
||||
}
|
||||
ret = mpp_api_->enqueue(mpp_ctx_, MPP_PORT_OUTPUT, mpp_task_);
|
||||
if (ret != MPP_OK) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp enqueue failed %d", ret);
|
||||
return false;
|
||||
}
|
||||
CHECK_NOTNULL(dest);
|
||||
memcpy(dest, rgb_buffer_, width_ * height_ * 3);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,30 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
||||
*
|
||||
* 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 "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();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,781 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
||||
*
|
||||
* 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 "orbbec_camera/ob_camera_node.h"
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <thread>
|
||||
|
||||
#include "orbbec_camera/utils.h"
|
||||
namespace orbbec_camera {
|
||||
|
||||
void OBCameraNode::setupCameraCtrlServices() {
|
||||
using std_srvs::srv::SetBool;
|
||||
for (auto stream_index : IMAGE_STREAMS) {
|
||||
if (!enable_stream_[stream_index]) {
|
||||
continue;
|
||||
}
|
||||
auto stream_name = stream_name_[stream_index];
|
||||
std::string service_name = "get_" + stream_name + "_exposure";
|
||||
get_exposure_srv_[stream_index] = node_->create_service<GetInt32>(
|
||||
service_name,
|
||||
[this, stream_index = stream_index](const std::shared_ptr<GetInt32::Request> request,
|
||||
std::shared_ptr<GetInt32::Response> response) {
|
||||
getExposureCallback(request, response, stream_index);
|
||||
});
|
||||
|
||||
service_name = "set_" + stream_name + "_exposure";
|
||||
set_exposure_srv_[stream_index] = node_->create_service<SetInt32>(
|
||||
service_name,
|
||||
[this, stream_index = stream_index](const std::shared_ptr<SetInt32::Request> request,
|
||||
std::shared_ptr<SetInt32::Response> response) {
|
||||
setExposureCallback(request, response, stream_index);
|
||||
});
|
||||
service_name = "get_" + stream_name + "_gain";
|
||||
get_gain_srv_[stream_index] = node_->create_service<GetInt32>(
|
||||
service_name,
|
||||
[this, stream_index = stream_index](const std::shared_ptr<GetInt32::Request> request,
|
||||
std::shared_ptr<GetInt32::Response> response) {
|
||||
getGainCallback(request, response, stream_index);
|
||||
});
|
||||
|
||||
service_name = "set_" + stream_name + "_gain";
|
||||
set_gain_srv_[stream_index] = node_->create_service<SetInt32>(
|
||||
service_name,
|
||||
[this, stream_index = stream_index](const std::shared_ptr<SetInt32::Request> request,
|
||||
std::shared_ptr<SetInt32::Response> response) {
|
||||
setGainCallback(request, response, stream_index);
|
||||
});
|
||||
service_name = "set_" + stream_name + "_auto_exposure";
|
||||
set_auto_exposure_srv_[stream_index] = node_->create_service<SetBool>(
|
||||
service_name,
|
||||
[this, stream_index = stream_index](const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setAutoExposureCallback(request, response, stream_index);
|
||||
});
|
||||
|
||||
service_name = "toggle_" + stream_name;
|
||||
|
||||
toggle_sensor_srv_[stream_index] = node_->create_service<SetBool>(
|
||||
service_name,
|
||||
[this, stream_index = stream_index](const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
toggleSensorCallback(request, response, stream_index);
|
||||
});
|
||||
service_name = "set_" + stream_name + "_mirror";
|
||||
set_mirror_srv_[stream_index] = node_->create_service<SetBool>(
|
||||
service_name,
|
||||
[this, stream_index = stream_index](const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setMirrorCallback(request, response, stream_index);
|
||||
});
|
||||
}
|
||||
set_fan_work_mode_srv_ = node_->create_service<SetInt32>(
|
||||
"set_fan_work_mode", [this](const std::shared_ptr<SetInt32::Request> request,
|
||||
std::shared_ptr<SetInt32::Response> response) {
|
||||
setFanWorkModeCallback(request, response);
|
||||
});
|
||||
set_floor_enable_srv_ = node_->create_service<SetBool>(
|
||||
"set_floor_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setFloorEnableCallback(request_header, request, response);
|
||||
});
|
||||
set_laser_enable_srv_ = node_->create_service<SetBool>(
|
||||
"set_laser_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setLaserEnableCallback(request_header, request, response);
|
||||
});
|
||||
set_ldp_enable_srv_ = node_->create_service<SetBool>(
|
||||
"set_ldp_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setLdpEnableCallback(request_header, request, response);
|
||||
});
|
||||
get_ldp_status_srv_ = node_->create_service<GetBool>(
|
||||
"get_ldp_status", [this](const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<GetBool::Request> request,
|
||||
std::shared_ptr<GetBool::Response> response) {
|
||||
(void)request_header;
|
||||
getLdpStatusCallback(request, response);
|
||||
});
|
||||
|
||||
get_white_balance_srv_ = node_->create_service<GetInt32>(
|
||||
"get_white_balance", [this](const std::shared_ptr<GetInt32::Request> request,
|
||||
std::shared_ptr<GetInt32::Response> response) {
|
||||
getWhiteBalanceCallback(request, response);
|
||||
});
|
||||
|
||||
set_white_balance_srv_ = node_->create_service<SetInt32>(
|
||||
"set_white_balance", [this](const std::shared_ptr<SetInt32::Request> request,
|
||||
std::shared_ptr<SetInt32::Response> response) {
|
||||
setWhiteBalanceCallback(request, response);
|
||||
});
|
||||
get_auto_white_balance_srv_ = node_->create_service<GetInt32>(
|
||||
"get_auto_white_balance", [this](const std::shared_ptr<GetInt32::Request> request,
|
||||
std::shared_ptr<GetInt32::Response> response) {
|
||||
getAutoWhiteBalanceCallback(request, response);
|
||||
});
|
||||
set_auto_white_balance_srv_ = node_->create_service<SetBool>(
|
||||
"set_auto_white_balance", [this](const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setAutoWhiteBalanceCallback(request, response);
|
||||
});
|
||||
get_device_srv_ = node_->create_service<GetDeviceInfo>(
|
||||
"get_device_info", [this](const std::shared_ptr<GetDeviceInfo::Request> request,
|
||||
std::shared_ptr<GetDeviceInfo::Response> response) {
|
||||
getDeviceInfoCallback(request, response);
|
||||
});
|
||||
get_sdk_version_srv_ = node_->create_service<GetString>(
|
||||
"get_sdk_version",
|
||||
[this](const std::shared_ptr<GetString::Request> request,
|
||||
std::shared_ptr<GetString::Response> response) { getSDKVersion(request, response); });
|
||||
save_images_srv_ = node_->create_service<std_srvs::srv::Empty>(
|
||||
"save_images", [this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
|
||||
std::shared_ptr<std_srvs::srv::Empty::Response> response) {
|
||||
saveImageCallback(request, response);
|
||||
});
|
||||
save_point_cloud_srv_ = node_->create_service<std_srvs::srv::Empty>(
|
||||
"save_point_cloud", [this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
|
||||
std::shared_ptr<std_srvs::srv::Empty::Response> response) {
|
||||
savePointCloudCallback(request, response);
|
||||
});
|
||||
switch_ir_camera_srv_ = node_->create_service<SetString>(
|
||||
"switch_ir", [this](const std::shared_ptr<SetString::Request> request,
|
||||
std::shared_ptr<SetString::Response> response) {
|
||||
switchIRCameraCallback(request, response);
|
||||
});
|
||||
set_ir_long_exposure_srv_ = node_->create_service<SetBool>(
|
||||
"set_ir_long_exposure", [this](const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setIRLongExposureCallback(request, response);
|
||||
});
|
||||
get_ldp_measure_distance_srv_ = node_->create_service<GetInt32>(
|
||||
"get_ldp_measure_distance", [this](const std::shared_ptr<GetInt32::Request> request,
|
||||
std::shared_ptr<GetInt32::Response> response) {
|
||||
getLdpMeasureDistanceCallback(request, response);
|
||||
});
|
||||
}
|
||||
|
||||
void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>& request,
|
||||
std::shared_ptr<SetInt32::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
auto stream = stream_index.first;
|
||||
try {
|
||||
switch (stream) {
|
||||
case OB_STREAM_IR_LEFT:
|
||||
case OB_STREAM_IR_RIGHT:
|
||||
case OB_STREAM_IR:
|
||||
device_->setIntProperty(OB_PROP_IR_EXPOSURE_INT, request->data);
|
||||
break;
|
||||
case OB_STREAM_DEPTH:
|
||||
device_->setIntProperty(OB_PROP_DEPTH_EXPOSURE_INT, request->data);
|
||||
break;
|
||||
case OB_STREAM_COLOR:
|
||||
device_->setIntProperty(OB_PROP_COLOR_EXPOSURE_INT, request->data);
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
|
||||
break;
|
||||
}
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->success = false;
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR(logger_, "%s unknown error %d", __FUNCTION__, __LINE__);
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::getGainCallback(const std::shared_ptr<GetInt32::Request>& request,
|
||||
std::shared_ptr<GetInt32::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
(void)request;
|
||||
auto stream = stream_index.first;
|
||||
try {
|
||||
switch (stream) {
|
||||
case OB_STREAM_IR_LEFT:
|
||||
case OB_STREAM_IR_RIGHT:
|
||||
case OB_STREAM_IR:
|
||||
response->data = device_->getIntProperty(OB_PROP_IR_GAIN_INT);
|
||||
break;
|
||||
case OB_STREAM_DEPTH:
|
||||
response->data = device_->getIntProperty(OB_PROP_DEPTH_GAIN_INT);
|
||||
break;
|
||||
case OB_STREAM_COLOR:
|
||||
response->data = device_->getIntProperty(OB_PROP_COLOR_GAIN_INT);
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
|
||||
break;
|
||||
}
|
||||
response->success = true;
|
||||
} catch (ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->success = false;
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& request,
|
||||
std::shared_ptr<SetInt32::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
auto stream = stream_index.first;
|
||||
OBPropertyID prop_id = OB_PROP_IR_GAIN_INT;
|
||||
try {
|
||||
switch (stream) {
|
||||
case OB_STREAM_IR_LEFT:
|
||||
case OB_STREAM_IR_RIGHT:
|
||||
case OB_STREAM_IR:
|
||||
prop_id = OB_PROP_IR_GAIN_INT;
|
||||
break;
|
||||
case OB_STREAM_DEPTH:
|
||||
prop_id = OB_PROP_DEPTH_GAIN_INT;
|
||||
break;
|
||||
case OB_STREAM_COLOR:
|
||||
prop_id = OB_PROP_COLOR_GAIN_INT;
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
|
||||
response->success = false;
|
||||
response->message = "NOT a video stream";
|
||||
return;
|
||||
}
|
||||
auto range = device_->getIntPropertyRange(prop_id);
|
||||
if (request->data < range.min || request->data > range.max) {
|
||||
response->success = false;
|
||||
RCLCPP_INFO_STREAM(logger_, "set gain value out of range");
|
||||
response->message = "value out of range";
|
||||
return;
|
||||
}
|
||||
device_->setIntProperty(prop_id, request->data);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->success = false;
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::getWhiteBalanceCallback(const std::shared_ptr<GetInt32::Request>& request,
|
||||
std::shared_ptr<GetInt32::Response>& response) {
|
||||
(void)request;
|
||||
try {
|
||||
response->data = device_->getIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->success = false;
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Request>& request,
|
||||
std::shared_ptr<SetInt32 ::Response>& response) {
|
||||
try {
|
||||
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_WHITE_BALANCE_INT);
|
||||
if (request->data < range.min || request->data > range.max) {
|
||||
response->success = false;
|
||||
RCLCPP_INFO_STREAM(logger_, "set white balance value out of range");
|
||||
response->message = "value out of range";
|
||||
return;
|
||||
}
|
||||
bool auto_white_balance = device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL);
|
||||
if (auto_white_balance) {
|
||||
RCLCPP_WARN(logger_, "auto white balance is enabled, set white balance will be ignored");
|
||||
response->success = false;
|
||||
response->message = "auto white balance is enabled";
|
||||
return;
|
||||
}
|
||||
device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, request->data);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->message = e.what();
|
||||
response->success = false;
|
||||
} catch (...) {
|
||||
response->message = "unknown error";
|
||||
response->success = false;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::getAutoWhiteBalanceCallback(const std::shared_ptr<GetInt32::Request>& request,
|
||||
std::shared_ptr<GetInt32::Response>& response) {
|
||||
(void)request;
|
||||
try {
|
||||
response->data = device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setAutoWhiteBalanceCallback(const std::shared_ptr<SetBool::Request>& request,
|
||||
std::shared_ptr<SetBool::Response>& response) {
|
||||
try {
|
||||
device_->setBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, request->data);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setAutoExposureCallback(
|
||||
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
auto stream = stream_index.first;
|
||||
OBPropertyID prop_id = OB_PROP_IR_AUTO_EXPOSURE_BOOL;
|
||||
try {
|
||||
switch (stream) {
|
||||
case OB_STREAM_IR_LEFT:
|
||||
case OB_STREAM_IR_RIGHT:
|
||||
case OB_STREAM_IR:
|
||||
prop_id = OB_PROP_IR_AUTO_EXPOSURE_BOOL;
|
||||
break;
|
||||
case OB_STREAM_DEPTH:
|
||||
prop_id = OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL;
|
||||
break;
|
||||
case OB_STREAM_COLOR:
|
||||
prop_id = OB_PROP_COLOR_AUTO_EXPOSURE_BOOL;
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
|
||||
response->success = false;
|
||||
response->message = "NOT a video stream";
|
||||
return;
|
||||
}
|
||||
auto range = device_->getIntPropertyRange(prop_id);
|
||||
if (request->data < range.min || request->data > range.max) {
|
||||
response->success = false;
|
||||
RCLCPP_INFO_STREAM(logger_, "set auto exposure value out of range");
|
||||
response->message = "value out of range";
|
||||
return;
|
||||
}
|
||||
device_->setIntProperty(prop_id, request->data);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setFanWorkModeCallback(const std::shared_ptr<SetInt32::Request>& request,
|
||||
std::shared_ptr<SetInt32::Response>& response) {
|
||||
(void)response;
|
||||
bool fan_mode = request->data;
|
||||
try {
|
||||
device_->setBoolProperty(OB_PROP_FAN_WORK_MODE_INT, fan_mode);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->success = false;
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setFloorEnableCallback(
|
||||
const std::shared_ptr<rmw_request_id_t>& request_header,
|
||||
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
|
||||
(void)request_header;
|
||||
(void)response;
|
||||
bool floor_enable = request->data;
|
||||
try {
|
||||
device_->setBoolProperty(OB_PROP_FLOOD_BOOL, floor_enable);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->success = false;
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setLaserEnableCallback(
|
||||
const std::shared_ptr<rmw_request_id_t>& request_header,
|
||||
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
|
||||
(void)request_header;
|
||||
(void)response;
|
||||
bool laser_enable = request->data;
|
||||
try {
|
||||
if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
|
||||
device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, laser_enable);
|
||||
} else if (device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
|
||||
device_->setIntProperty(OB_PROP_LASER_BOOL, laser_enable);
|
||||
}
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->message = e.getMessage();
|
||||
response->success = false;
|
||||
} catch (const std::exception& e) {
|
||||
response->message = e.what();
|
||||
response->success = false;
|
||||
} catch (...) {
|
||||
response->message = "unknown error";
|
||||
response->success = false;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setLdpEnableCallback(
|
||||
const std::shared_ptr<rmw_request_id_t>& request_header,
|
||||
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
|
||||
(void)request_header;
|
||||
(void)response;
|
||||
bool ldp_enable = request->data;
|
||||
try {
|
||||
if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
|
||||
auto laser_enable = device_->getIntProperty(OB_PROP_LASER_CONTROL_INT);
|
||||
device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
|
||||
device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, laser_enable);
|
||||
} else if (device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
|
||||
if (!ldp_enable) {
|
||||
auto laser_enable = device_->getIntProperty(OB_PROP_LASER_BOOL);
|
||||
device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(3));
|
||||
device_->setIntProperty(OB_PROP_LASER_BOOL, laser_enable);
|
||||
} else {
|
||||
device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
|
||||
}
|
||||
}
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->success = false;
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::getExposureCallback(const std::shared_ptr<GetInt32::Request>& request,
|
||||
std::shared_ptr<GetInt32 ::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
(void)request;
|
||||
auto stream = stream_index.first;
|
||||
try {
|
||||
switch (stream) {
|
||||
case OB_STREAM_IR_LEFT:
|
||||
case OB_STREAM_IR_RIGHT:
|
||||
case OB_STREAM_IR:
|
||||
response->data = device_->getIntProperty(OB_PROP_IR_EXPOSURE_INT);
|
||||
break;
|
||||
case OB_STREAM_DEPTH:
|
||||
response->data = device_->getIntProperty(OB_PROP_DEPTH_EXPOSURE_INT);
|
||||
break;
|
||||
case OB_STREAM_COLOR:
|
||||
response->data = device_->getIntProperty(OB_PROP_COLOR_EXPOSURE_INT);
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
|
||||
break;
|
||||
}
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->message = e.getMessage();
|
||||
response->success = false;
|
||||
} catch (const std::exception& e) {
|
||||
response->message = e.what();
|
||||
response->success = false;
|
||||
} catch (...) {
|
||||
response->message = "unknown error";
|
||||
response->success = false;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr<GetDeviceInfo::Request>& request,
|
||||
std::shared_ptr<GetDeviceInfo::Response>& response) {
|
||||
(void)request;
|
||||
try {
|
||||
auto device_info = device_->getDeviceInfo();
|
||||
response->info.name = device_info->name();
|
||||
response->info.serial_number = device_info->serialNumber();
|
||||
response->info.firmware_version = device_info->firmwareVersion();
|
||||
response->info.supported_min_sdk_version = device_info->supportedMinSdkVersion();
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->success = false;
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::getSDKVersion(const std::shared_ptr<GetString::Request>& request,
|
||||
std::shared_ptr<GetString::Response>& response) {
|
||||
(void)request;
|
||||
try {
|
||||
auto device_info = device_->getDeviceInfo();
|
||||
nlohmann::json data;
|
||||
data["firmware_version"] = device_info->firmwareVersion();
|
||||
data["supported_min_sdk_version"] = device_info->supportedMinSdkVersion();
|
||||
data["ros_sdk_version"] = OB_ROS_VERSION_STR;
|
||||
data["ob_sdk_version"] = getObSDKVersion();
|
||||
response->data = data.dump(2);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->success = false;
|
||||
response->message = e.getMessage();
|
||||
} catch (const std::exception& e) {
|
||||
response->success = false;
|
||||
response->message = e.what();
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "unknown error";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setMirrorCallback(const std::shared_ptr<SetBool::Request>& request,
|
||||
std::shared_ptr<SetBool::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
(void)request;
|
||||
auto stream = stream_index.first;
|
||||
try {
|
||||
switch (stream) {
|
||||
case OB_STREAM_IR_RIGHT:
|
||||
device_->setBoolProperty(OB_PROP_IR_RIGHT_MIRROR_BOOL, request->data);
|
||||
break;
|
||||
case OB_STREAM_IR_LEFT:
|
||||
case OB_STREAM_IR:
|
||||
device_->setBoolProperty(OB_PROP_IR_MIRROR_BOOL, request->data);
|
||||
break;
|
||||
case OB_STREAM_DEPTH:
|
||||
device_->setBoolProperty(OB_PROP_DEPTH_MIRROR_BOOL, request->data);
|
||||
break;
|
||||
case OB_STREAM_COLOR:
|
||||
device_->setBoolProperty(OB_PROP_COLOR_MIRROR_BOOL, request->data);
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
|
||||
break;
|
||||
}
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->message = e.getMessage();
|
||||
response->success = false;
|
||||
} catch (const std::exception& e) {
|
||||
response->message = e.what();
|
||||
response->success = false;
|
||||
} catch (...) {
|
||||
response->message = "unknown error";
|
||||
response->success = false;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::getLdpStatusCallback(const std::shared_ptr<GetBool::Request>& request,
|
||||
std::shared_ptr<GetBool::Response>& response) {
|
||||
(void)request;
|
||||
try {
|
||||
response->data = device_->getBoolProperty(OB_PROP_LDP_STATUS_BOOL);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->message = e.getMessage();
|
||||
response->success = false;
|
||||
} catch (const std::exception& e) {
|
||||
response->message = e.what();
|
||||
response->success = false;
|
||||
} catch (...) {
|
||||
response->message = "unknown error";
|
||||
response->success = false;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::getLdpMeasureDistanceCallback(const std::shared_ptr<GetInt32::Request>& request,
|
||||
std::shared_ptr<GetInt32::Response>& response) {
|
||||
(void)request;
|
||||
try {
|
||||
response->data = device_->getIntProperty(OB_PROP_LDP_MEASURE_DISTANCE_INT);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->message = e.getMessage();
|
||||
response->success = false;
|
||||
} catch (const std::exception& e) {
|
||||
response->message = e.what();
|
||||
response->success = false;
|
||||
} catch (...) {
|
||||
response->message = "unknown error";
|
||||
response->success = false;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::toggleSensorCallback(const std::shared_ptr<SetBool::Request>& request,
|
||||
std::shared_ptr<SetBool::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
std::string msg;
|
||||
if (request->data) {
|
||||
if (enable_stream_[stream_index]) {
|
||||
msg = stream_name_[stream_index] + " Already ON";
|
||||
}
|
||||
RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index] << " ON");
|
||||
|
||||
} else {
|
||||
if (!enable_stream_[stream_index]) {
|
||||
msg = stream_name_[stream_index] + " Already OFF";
|
||||
}
|
||||
RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index] << " OFF");
|
||||
}
|
||||
if (!msg.empty()) {
|
||||
RCLCPP_ERROR_STREAM(logger_, msg);
|
||||
response->success = false;
|
||||
response->message = msg;
|
||||
return;
|
||||
}
|
||||
response->success = toggleSensor(stream_index, request->data, response->message);
|
||||
}
|
||||
|
||||
bool OBCameraNode::toggleSensor(const stream_index_pair& stream_index, bool enabled,
|
||||
std::string& msg) {
|
||||
try {
|
||||
pipeline_->stop();
|
||||
enable_stream_[stream_index] = enabled;
|
||||
setupProfiles();
|
||||
startStreams();
|
||||
return true;
|
||||
} catch (const ob::Error& e) {
|
||||
msg = e.getMessage();
|
||||
return false;
|
||||
} catch (const std::exception& e) {
|
||||
msg = e.what();
|
||||
return false;
|
||||
} catch (...) {
|
||||
msg = "unknown error";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
|
||||
std::shared_ptr<std_srvs::srv::Empty::Response>& response) {
|
||||
(void)request;
|
||||
(void)response;
|
||||
for (const auto& stream_index : IMAGE_STREAMS) {
|
||||
if (enable_stream_[stream_index]) {
|
||||
save_images_[stream_index] = true;
|
||||
save_images_count_[stream_index] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::savePointCloudCallback(
|
||||
const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
|
||||
std::shared_ptr<std_srvs::srv::Empty::Response>& response) {
|
||||
(void)request;
|
||||
(void)response;
|
||||
if (enable_point_cloud_) {
|
||||
save_point_cloud_ = true;
|
||||
}
|
||||
if (enable_colored_point_cloud_) {
|
||||
save_colored_point_cloud_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::switchIRCameraCallback(const std::shared_ptr<SetString::Request>& request,
|
||||
std::shared_ptr<SetString::Response>& response) {
|
||||
if (request->data != "left" && request->data != "right") {
|
||||
response->success = false;
|
||||
response->message = "invalid ir camera name";
|
||||
return;
|
||||
}
|
||||
try {
|
||||
int data = request->data == "left" ? 0 : 1;
|
||||
device_->setIntProperty(OB_PROP_IR_CHANNEL_DATA_SOURCE_INT, data);
|
||||
response->success = true;
|
||||
return;
|
||||
} catch (const ob::Error& e) {
|
||||
response->message = e.getMessage();
|
||||
response->success = false;
|
||||
} catch (const std::exception& e) {
|
||||
response->message = e.what();
|
||||
response->success = false;
|
||||
} catch (...) {
|
||||
response->message = "unknown error";
|
||||
response->success = false;
|
||||
}
|
||||
}
|
||||
void OBCameraNode::setIRLongExposureCallback(
|
||||
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
|
||||
try {
|
||||
device_->setBoolProperty(OB_PROP_IR_LONG_EXPOSURE_BOOL, request->data);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->message = e.getMessage();
|
||||
response->success = false;
|
||||
} catch (const std::exception& e) {
|
||||
response->message = e.what();
|
||||
response->success = false;
|
||||
} catch (...) {
|
||||
response->message = "unknown error";
|
||||
response->success = false;
|
||||
}
|
||||
}
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,69 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
||||
*
|
||||
* 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 "orbbec_camera/utils.h"
|
||||
#include "orbbec_camera/synced_imu_publisher.h"
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
namespace orbbec_camera {
|
||||
SyncedImuPublisher::SyncedImuPublisher(
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher, size_t queue_size)
|
||||
: imu_publisher_(imu_publisher), queue_size_(queue_size) {}
|
||||
|
||||
SyncedImuPublisher::~SyncedImuPublisher() { publishPendingMessages(); }
|
||||
|
||||
void SyncedImuPublisher::publish(const sensor_msgs::msg::Imu &imu_msg) {
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
auto sub_num = imu_publisher_->get_subscription_count();
|
||||
if (sub_num == 0 || !is_enabled_) {
|
||||
return;
|
||||
}
|
||||
if (is_paused_) {
|
||||
while (queue_.size() >= queue_size_) {
|
||||
queue_.pop();
|
||||
}
|
||||
queue_.push(imu_msg);
|
||||
} else {
|
||||
imu_publisher_->publish(imu_msg);
|
||||
}
|
||||
}
|
||||
|
||||
void SyncedImuPublisher::pause() {
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
is_paused_ = true;
|
||||
}
|
||||
|
||||
void SyncedImuPublisher::resume() {
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
is_paused_ = false;
|
||||
publishPendingMessages();
|
||||
}
|
||||
|
||||
void SyncedImuPublisher::setQueueSize(size_t queue_size) {
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
queue_size_ = queue_size;
|
||||
}
|
||||
|
||||
void SyncedImuPublisher::enable(bool enable) { is_enabled_ = enable; }
|
||||
|
||||
void SyncedImuPublisher::publishPendingMessages() {
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
while (!queue_.empty()) {
|
||||
imu_publisher_->publish(queue_.front());
|
||||
queue_.pop();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,875 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
||||
*
|
||||
* 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 <regex>
|
||||
#include "orbbec_camera/utils.h"
|
||||
#include <sensor_msgs/point_cloud2_iterator.hpp>
|
||||
#include "orbbec_camera/constants.h"
|
||||
namespace orbbec_camera {
|
||||
sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
|
||||
OBCameraDistortion distortion, int width) {
|
||||
(void)width;
|
||||
sensor_msgs::msg::CameraInfo info;
|
||||
info.distortion_model = sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;
|
||||
info.width = intrinsic.width;
|
||||
info.height = intrinsic.height;
|
||||
info.d.resize(8, 0.0);
|
||||
info.d[0] = distortion.k1;
|
||||
info.d[1] = distortion.k2;
|
||||
info.d[2] = distortion.p1;
|
||||
info.d[3] = distortion.p2;
|
||||
info.d[4] = distortion.k3;
|
||||
info.d[5] = distortion.k4;
|
||||
info.d[6] = distortion.k5;
|
||||
info.d[7] = distortion.k6;
|
||||
|
||||
info.k.fill(0.0);
|
||||
info.k[0] = intrinsic.fx;
|
||||
info.k[2] = intrinsic.cx;
|
||||
info.k[4] = intrinsic.fy;
|
||||
info.k[5] = intrinsic.cy;
|
||||
info.k[8] = 1.0;
|
||||
|
||||
info.r.fill(0.0);
|
||||
info.r[0] = 1;
|
||||
info.r[4] = 1;
|
||||
info.r[8] = 1;
|
||||
|
||||
info.p.fill(0.0);
|
||||
info.p[0] = info.k[0];
|
||||
info.p[2] = info.k[2];
|
||||
info.p[5] = info.k[4];
|
||||
info.p[6] = info.k[5];
|
||||
info.p[10] = 1.0;
|
||||
return info;
|
||||
}
|
||||
|
||||
void saveRGBPointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::string &fileName) {
|
||||
size_t point_size = frame->dataSize() / sizeof(OBColorPoint);
|
||||
FILE *fp = fopen(fileName.c_str(), "wb+");
|
||||
std::shared_ptr<int> fp_guard(nullptr, [&fp](int *) {
|
||||
fflush(fp);
|
||||
fclose(fp);
|
||||
});
|
||||
fprintf(fp, "ply\n");
|
||||
fprintf(fp, "format ascii 1.0\n");
|
||||
fprintf(fp, "element vertex %zu\n", point_size);
|
||||
fprintf(fp, "property float x\n");
|
||||
fprintf(fp, "property float y\n");
|
||||
fprintf(fp, "property float z\n");
|
||||
fprintf(fp, "property uchar red\n");
|
||||
fprintf(fp, "property uchar green\n");
|
||||
fprintf(fp, "property uchar blue\n");
|
||||
fprintf(fp, "end_header\n");
|
||||
|
||||
const auto *points = (OBColorPoint *)frame->data();
|
||||
CHECK_NOTNULL(points);
|
||||
for (size_t i = 0; i < point_size; i++) {
|
||||
fprintf(fp, "%.3f %.3f %.3f %d %d %d\n", points[i].x, points[i].y, points[i].z,
|
||||
(int)points[i].r, (int)points[i].g, (int)points[i].b);
|
||||
}
|
||||
}
|
||||
|
||||
void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2::UniquePtr &msg,
|
||||
const std::string &fileName) {
|
||||
FILE *fp = fopen(fileName.c_str(), "wb+");
|
||||
CHECK_NOTNULL(fp);
|
||||
CHECK_NOTNULL(msg);
|
||||
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
|
||||
|
||||
// First, count the actual number of valid points
|
||||
size_t valid_points = 0;
|
||||
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
|
||||
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
|
||||
++valid_points;
|
||||
}
|
||||
}
|
||||
|
||||
// Reset the iterators
|
||||
iter_x = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "x");
|
||||
iter_y = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "y");
|
||||
iter_z = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "z");
|
||||
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_r(*msg, "r");
|
||||
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_g(*msg, "g");
|
||||
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_b(*msg, "b");
|
||||
|
||||
fprintf(fp, "ply\n");
|
||||
fprintf(fp, "format ascii 1.0\n");
|
||||
fprintf(fp, "element vertex %zu\n", valid_points);
|
||||
fprintf(fp, "property float x\n");
|
||||
fprintf(fp, "property float y\n");
|
||||
fprintf(fp, "property float z\n");
|
||||
fprintf(fp, "property uchar red\n");
|
||||
fprintf(fp, "property uchar green\n");
|
||||
fprintf(fp, "property uchar blue\n");
|
||||
fprintf(fp, "end_header\n");
|
||||
|
||||
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) {
|
||||
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
|
||||
fprintf(fp, "%.3f %.3f %.3f %d %d %d\n", *iter_x, *iter_y, *iter_z, (int)*iter_r,
|
||||
(int)*iter_g, (int)*iter_b);
|
||||
}
|
||||
}
|
||||
|
||||
fflush(fp);
|
||||
fclose(fp);
|
||||
}
|
||||
|
||||
void saveDepthPointsToPly(const sensor_msgs::msg::PointCloud2::UniquePtr &msg,
|
||||
const std::string &fileName) {
|
||||
FILE *fp = fopen(fileName.c_str(), "wb+");
|
||||
CHECK_NOTNULL(fp);
|
||||
CHECK_NOTNULL(msg);
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
|
||||
|
||||
// First, count the actual number of valid points
|
||||
size_t valid_points = 0;
|
||||
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
|
||||
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
|
||||
++valid_points;
|
||||
}
|
||||
}
|
||||
|
||||
// Reset the iterators
|
||||
iter_x = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "x");
|
||||
iter_y = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "y");
|
||||
iter_z = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "z");
|
||||
|
||||
fprintf(fp, "ply\n");
|
||||
fprintf(fp, "format ascii 1.0\n");
|
||||
fprintf(fp, "element vertex %zu\n", valid_points);
|
||||
fprintf(fp, "property float x\n");
|
||||
fprintf(fp, "property float y\n");
|
||||
fprintf(fp, "property float z\n");
|
||||
fprintf(fp, "end_header\n");
|
||||
|
||||
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
|
||||
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
|
||||
fprintf(fp, "%.3f %.3f %.3f\n", *iter_x, *iter_y, *iter_z);
|
||||
}
|
||||
}
|
||||
|
||||
fflush(fp);
|
||||
fclose(fp);
|
||||
}
|
||||
|
||||
void savePointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::string &fileName) {
|
||||
size_t point_size = frame->dataSize() / sizeof(OBPoint);
|
||||
FILE *fp = fopen(fileName.c_str(), "wb+");
|
||||
std::shared_ptr<int> fp_guard(nullptr, [&fp](int *) {
|
||||
fflush(fp);
|
||||
fclose(fp);
|
||||
});
|
||||
CHECK_NOTNULL(fp);
|
||||
CHECK_NOTNULL(frame);
|
||||
fprintf(fp, "ply\n");
|
||||
fprintf(fp, "format ascii 1.0\n");
|
||||
fprintf(fp, "element vertex %zu\n", point_size);
|
||||
fprintf(fp, "property float x\n");
|
||||
fprintf(fp, "property float y\n");
|
||||
fprintf(fp, "property float z\n");
|
||||
fprintf(fp, "end_header\n");
|
||||
|
||||
const auto *points = (OBPoint *)frame->data();
|
||||
CHECK_NOTNULL(points);
|
||||
for (size_t i = 0; i < point_size; i++) {
|
||||
fprintf(fp, "%.3f %.3f %.3f\n", points[i].x, points[i].y, points[i].z);
|
||||
}
|
||||
}
|
||||
|
||||
tf2::Quaternion rotationMatrixToQuaternion(const float rotation[9]) {
|
||||
Eigen::Matrix3f m;
|
||||
// We need to be careful about the order, as RS2 rotation matrix is
|
||||
// column-major, while Eigen::Matrix3f expects row-major.
|
||||
m << rotation[0], rotation[1], rotation[2], rotation[3], rotation[4], rotation[5], rotation[6],
|
||||
rotation[7], rotation[8];
|
||||
Eigen::Quaternionf q(m);
|
||||
return {q.x(), q.y(), q.z(), q.w()};
|
||||
}
|
||||
|
||||
std::ostream &operator<<(std::ostream &os, const OBCameraParam &rhs) {
|
||||
auto depth_intrinsic = rhs.depthIntrinsic;
|
||||
auto rgb_intrinsic = rhs.rgbIntrinsic;
|
||||
os << "=====depth intrinsic=====\n";
|
||||
os << "fx : " << depth_intrinsic.fx << "\n";
|
||||
os << "fy : " << depth_intrinsic.fy << "\n";
|
||||
os << "cx : " << depth_intrinsic.cx << "\n";
|
||||
os << "cy : " << depth_intrinsic.cy << "\n";
|
||||
os << "width : " << depth_intrinsic.width << "\n";
|
||||
os << "height : " << depth_intrinsic.height << "\n";
|
||||
os << "=====rgb intrinsic=====\n";
|
||||
os << "fx : " << rgb_intrinsic.fx << "\n";
|
||||
os << "fy : " << rgb_intrinsic.fy << "\n";
|
||||
os << "cx : " << rgb_intrinsic.cx << "\n";
|
||||
os << "cy : " << rgb_intrinsic.cy << "\n";
|
||||
os << "width : " << rgb_intrinsic.width << "\n";
|
||||
os << "height : " << rgb_intrinsic.height << "\n";
|
||||
return os;
|
||||
}
|
||||
|
||||
orbbec_camera_msgs::msg::Extrinsics obExtrinsicsToMsg(const OBD2CTransform &extrinsics,
|
||||
const std::string &frame_id) {
|
||||
orbbec_camera_msgs::msg::Extrinsics msg;
|
||||
for (int i = 0; i < 9; ++i) {
|
||||
msg.rotation[i] = extrinsics.rot[i];
|
||||
if (i < 3) {
|
||||
msg.translation[i] = extrinsics.trans[i] / 1000.0;
|
||||
}
|
||||
}
|
||||
|
||||
msg.header.frame_id = frame_id;
|
||||
return msg;
|
||||
}
|
||||
|
||||
rclcpp::Time fromMsToROSTime(uint64_t ms) {
|
||||
auto total = static_cast<uint64_t>(ms * 1e6);
|
||||
uint64_t sec = total / 1000000000;
|
||||
uint64_t nano_sec = total % 1000000000;
|
||||
rclcpp::Time stamp(sec, nano_sec);
|
||||
return stamp;
|
||||
}
|
||||
|
||||
rclcpp::Time fromUsToROSTime(uint64_t us) {
|
||||
auto total = static_cast<uint64_t>(us * 1e3);
|
||||
uint64_t sec = total / 1000000000;
|
||||
uint64_t nano_sec = total % 1000000000;
|
||||
rclcpp::Time stamp(sec, nano_sec);
|
||||
return stamp;
|
||||
}
|
||||
|
||||
std::string getObSDKVersion() {
|
||||
std::string major = std::to_string(ob::Version::getMajor());
|
||||
std::string minor = std::to_string(ob::Version::getMinor());
|
||||
std::string patch = std::to_string(ob::Version::getPatch());
|
||||
std::string version = major + "." + minor + "." + patch;
|
||||
return version;
|
||||
}
|
||||
|
||||
OBFormat OBFormatFromString(const std::string &format) {
|
||||
if (format.empty()) {
|
||||
return OB_FORMAT_UNKNOWN;
|
||||
}
|
||||
std::string fixed_format;
|
||||
std::transform(format.begin(), format.end(), std::back_inserter(fixed_format),
|
||||
[](const auto ch) { return std::isalpha(ch) ? toupper(ch) : ch; });
|
||||
if (fixed_format == "MJPG") {
|
||||
return OB_FORMAT_MJPG;
|
||||
} else if (fixed_format == "MJPEG") {
|
||||
return OB_FORMAT_MJPEG;
|
||||
} else if (fixed_format == "YUYV") {
|
||||
return OB_FORMAT_YUYV;
|
||||
} else if (fixed_format == "YUYV2") {
|
||||
return OB_FORMAT_YUY2;
|
||||
} else if (fixed_format == "UYVY") {
|
||||
return OB_FORMAT_UYVY;
|
||||
} else if (fixed_format == "NV12") {
|
||||
return OB_FORMAT_NV12;
|
||||
} else if (fixed_format == "NV21") {
|
||||
return OB_FORMAT_NV21;
|
||||
} else if (fixed_format == "H264") {
|
||||
return OB_FORMAT_H264;
|
||||
} else if (fixed_format == "H265") {
|
||||
return OB_FORMAT_H265;
|
||||
} else if (fixed_format == "Y16") {
|
||||
return OB_FORMAT_Y16;
|
||||
} else if (fixed_format == "Y8") {
|
||||
return OB_FORMAT_Y8;
|
||||
} else if (fixed_format == "Y10") {
|
||||
return OB_FORMAT_Y10;
|
||||
} else if (fixed_format == "Y11") {
|
||||
return OB_FORMAT_Y11;
|
||||
} else if (fixed_format == "Y12") {
|
||||
return OB_FORMAT_Y12;
|
||||
} else if (fixed_format == "GRAY") {
|
||||
return OB_FORMAT_GRAY;
|
||||
} else if (fixed_format == "HEVC") {
|
||||
return OB_FORMAT_HEVC;
|
||||
} else if (fixed_format == "I420") {
|
||||
return OB_FORMAT_I420;
|
||||
} else if (fixed_format == "ACCEL") {
|
||||
return OB_FORMAT_ACCEL;
|
||||
} else if (fixed_format == "GYRO") {
|
||||
return OB_FORMAT_GYRO;
|
||||
} else if (fixed_format == "POINT") {
|
||||
return OB_FORMAT_POINT;
|
||||
} else if (fixed_format == "RGB_POINT") {
|
||||
return OB_FORMAT_RGB_POINT;
|
||||
} else if (fixed_format == "REL") {
|
||||
return OB_FORMAT_RLE;
|
||||
} else if (fixed_format == "RGB888" || fixed_format == "RGB") {
|
||||
return OB_FORMAT_RGB888;
|
||||
} else if (fixed_format == "BGR") {
|
||||
return OB_FORMAT_BGR;
|
||||
} else if (fixed_format == "Y14") {
|
||||
return OB_FORMAT_Y14;
|
||||
} else if (fixed_format == "BGRA") {
|
||||
return OB_FORMAT_BGRA;
|
||||
} else if (fixed_format == "COMPRESSED") {
|
||||
return OB_FORMAT_COMPRESSED;
|
||||
} else if (fixed_format == "RVL") {
|
||||
return OB_FORMAT_RVL;
|
||||
} else if (fixed_format == "Z16") {
|
||||
return OB_FORMAT_Z16;
|
||||
} else if (fixed_format == "YV12") {
|
||||
return OB_FORMAT_YV12;
|
||||
} else if (fixed_format == "BA81") {
|
||||
return OB_FORMAT_BA81;
|
||||
} else if (fixed_format == "RGBA") {
|
||||
return OB_FORMAT_RGBA;
|
||||
} else if (fixed_format == "BYR2") {
|
||||
return OB_FORMAT_BYR2;
|
||||
} else if (fixed_format == "RW16") {
|
||||
return OB_FORMAT_RW16;
|
||||
} else if (fixed_format == "DISP16") {
|
||||
return OB_FORMAT_DISP16;
|
||||
} else {
|
||||
return OB_FORMAT_UNKNOWN;
|
||||
}
|
||||
}
|
||||
|
||||
std::string OBFormatToString(const OBFormat &format) {
|
||||
switch (format) {
|
||||
case OB_FORMAT_MJPG:
|
||||
return "MJPG";
|
||||
case OB_FORMAT_YUYV:
|
||||
return "YUYV";
|
||||
case OB_FORMAT_YUY2:
|
||||
return "YUYV2";
|
||||
case OB_FORMAT_UYVY:
|
||||
return "UYVY";
|
||||
case OB_FORMAT_NV12:
|
||||
return "NV12";
|
||||
case OB_FORMAT_NV21:
|
||||
return "NV21";
|
||||
case OB_FORMAT_H264:
|
||||
return "H264";
|
||||
case OB_FORMAT_H265:
|
||||
return "H265";
|
||||
case OB_FORMAT_Y16:
|
||||
return "Y16";
|
||||
case OB_FORMAT_Y8:
|
||||
return "Y8";
|
||||
case OB_FORMAT_Y10:
|
||||
return "Y10";
|
||||
case OB_FORMAT_Y11:
|
||||
return "Y11";
|
||||
case OB_FORMAT_Y12:
|
||||
return "Y12";
|
||||
case OB_FORMAT_GRAY:
|
||||
return "GRAY";
|
||||
case OB_FORMAT_HEVC:
|
||||
return "HEVC";
|
||||
case OB_FORMAT_I420:
|
||||
return "I420";
|
||||
case OB_FORMAT_ACCEL:
|
||||
return "ACCEL";
|
||||
case OB_FORMAT_GYRO:
|
||||
return "GYRO";
|
||||
case OB_FORMAT_POINT:
|
||||
return "POINT";
|
||||
case OB_FORMAT_RGB_POINT:
|
||||
return "RGB_POINT";
|
||||
case OB_FORMAT_RLE:
|
||||
return "REL";
|
||||
case OB_FORMAT_RGB888:
|
||||
return "RGB888";
|
||||
case OB_FORMAT_BGR:
|
||||
return "BGR";
|
||||
case OB_FORMAT_Y14:
|
||||
return "Y14";
|
||||
case OB_FORMAT_BGRA:
|
||||
return "BGRA";
|
||||
case OB_FORMAT_COMPRESSED:
|
||||
return "COMPRESSED";
|
||||
case OB_FORMAT_RVL:
|
||||
return "RVL";
|
||||
case OB_FORMAT_Z16:
|
||||
return "Z16";
|
||||
case OB_FORMAT_YV12:
|
||||
return "YV12";
|
||||
case OB_FORMAT_BA81:
|
||||
return "BA81";
|
||||
case OB_FORMAT_RGBA:
|
||||
return "RGBA";
|
||||
case OB_FORMAT_BYR2:
|
||||
return "BYR2";
|
||||
case OB_FORMAT_RW16:
|
||||
return "RW16";
|
||||
case OB_FORMAT_DISP16:
|
||||
return "DISP16";
|
||||
default:
|
||||
return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
std::ostream &operator<<(std::ostream &os, const OBFormat &rhs) {
|
||||
os << OBFormatToString(rhs);
|
||||
return os;
|
||||
}
|
||||
|
||||
std::string ObDeviceTypeToString(const OBDeviceType &type) {
|
||||
switch (type) {
|
||||
case OBDeviceType::OB_STRUCTURED_LIGHT_BINOCULAR_CAMERA:
|
||||
return "structured light binocular camera";
|
||||
case OBDeviceType::OB_STRUCTURED_LIGHT_MONOCULAR_CAMERA:
|
||||
return "structured light monocular camera";
|
||||
case OBDeviceType::OB_TOF_CAMERA:
|
||||
return "tof camera";
|
||||
}
|
||||
return "unknown technology camera";
|
||||
}
|
||||
|
||||
rmw_qos_profile_t getRMWQosProfileFromString(const std::string &str_qos) {
|
||||
std::string upper_str_qos = str_qos;
|
||||
std::transform(upper_str_qos.begin(), upper_str_qos.end(), upper_str_qos.begin(), ::toupper);
|
||||
if (upper_str_qos == "SYSTEM_DEFAULT") {
|
||||
return rmw_qos_profile_system_default;
|
||||
} else if (upper_str_qos == "DEFAULT") {
|
||||
return rmw_qos_profile_default;
|
||||
} else if (upper_str_qos == "PARAMETER_EVENTS") {
|
||||
return rmw_qos_profile_parameter_events;
|
||||
} else if (upper_str_qos == "SERVICES_DEFAULT") {
|
||||
return rmw_qos_profile_services_default;
|
||||
} else if (upper_str_qos == "PARAMETERS") {
|
||||
return rmw_qos_profile_parameters;
|
||||
} else if (upper_str_qos == "SENSOR_DATA") {
|
||||
return rmw_qos_profile_sensor_data;
|
||||
} else {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("astra_camera"),
|
||||
"Invalid QoS profile: " << upper_str_qos << ". Using default QoS profile.");
|
||||
return rmw_qos_profile_default;
|
||||
}
|
||||
}
|
||||
|
||||
bool isOpenNIDevice(int pid) {
|
||||
static const std::vector<int> OPENNI_DEVICE_PIDS = {
|
||||
0x0300, 0x0301, 0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0407, 0x0601, 0x060b, 0x060e,
|
||||
0x060f, 0x0610, 0x0613, 0x0614, 0x0616, 0x0617, 0x0618, 0x061b, 0x062b, 0x062c, 0x062d,
|
||||
0x0632, 0x0633, 0x0634, 0x0635, 0x0636, 0x0637, 0x0638, 0x0639, 0x063a, 0x0650, 0x0651,
|
||||
0x0654, 0x0655, 0x0656, 0x0657, 0x0658, 0x0659, 0x065a, 0x065b, 0x065c, 0x065d, 0x0698,
|
||||
0x0699, 0x069a, 0x055c, 0x065e, 0x069a, 0x069f, 0x06a0, 0x069e, 0x06aa, 0x06a6, 0x06a7};
|
||||
|
||||
return std::any_of(OPENNI_DEVICE_PIDS.begin(), OPENNI_DEVICE_PIDS.end(),
|
||||
[pid](int pid_openni) { return pid == pid_openni; });
|
||||
}
|
||||
|
||||
OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString(
|
||||
const std::string &depth_precision_level_str) {
|
||||
if (depth_precision_level_str == "1mm") {
|
||||
return OB_PRECISION_1MM;
|
||||
} else if (depth_precision_level_str == "0.8mm") {
|
||||
return OB_PRECISION_0MM8;
|
||||
} else if (depth_precision_level_str == "0.4mm") {
|
||||
return OB_PRECISION_0MM4;
|
||||
} else if (depth_precision_level_str == "0.2mm") {
|
||||
return OB_PRECISION_0MM2;
|
||||
} else if (depth_precision_level_str == "0.1mm") {
|
||||
return OB_PRECISION_0MM1;
|
||||
} else {
|
||||
return OB_PRECISION_0MM8;
|
||||
}
|
||||
}
|
||||
|
||||
float depthPrecisionFromString(const std::string &depth_precision_level_str) {
|
||||
// covert 0.8mm to 0.8
|
||||
if (depth_precision_level_str.size() < 2) {
|
||||
RCLCPP_WARN_STREAM(rclcpp::get_logger("utils"),
|
||||
"Invalid depth precision level: " << depth_precision_level_str
|
||||
<< ". Using default precision level 1mm");
|
||||
return 1.0;
|
||||
}
|
||||
std::string depth_precision_level_str_num =
|
||||
depth_precision_level_str.substr(0, depth_precision_level_str.size() - 2);
|
||||
return std::stof(depth_precision_level_str_num);
|
||||
}
|
||||
|
||||
OBMultiDeviceSyncMode OBSyncModeFromString(const std::string &mode) {
|
||||
if (mode == "FREE_RUN") {
|
||||
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN;
|
||||
} else if (mode == "STANDALONE") {
|
||||
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_STANDALONE;
|
||||
} else if (mode == "PRIMARY") {
|
||||
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_PRIMARY;
|
||||
} else if (mode == "SECONDARY") {
|
||||
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SECONDARY;
|
||||
} else if (mode == "SECONDARY_SYNCED") {
|
||||
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SECONDARY_SYNCED;
|
||||
} else if (mode == "SOFTWARE_TRIGGERING") {
|
||||
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING;
|
||||
} else if (mode == "HARDWARE_TRIGGERING") {
|
||||
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING;
|
||||
} else {
|
||||
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN;
|
||||
}
|
||||
}
|
||||
|
||||
OB_SAMPLE_RATE sampleRateFromString(std::string &sample_rate) {
|
||||
// covert to lower case
|
||||
std::transform(sample_rate.begin(), sample_rate.end(), sample_rate.begin(), ::tolower);
|
||||
if (sample_rate == "1.5625hz") {
|
||||
return OB_SAMPLE_RATE_1_5625_HZ;
|
||||
} else if (sample_rate == "3.125hz") {
|
||||
return OB_SAMPLE_RATE_3_125_HZ;
|
||||
} else if (sample_rate == "6.25hz") {
|
||||
return OB_SAMPLE_RATE_6_25_HZ;
|
||||
} else if (sample_rate == "12.5hz") {
|
||||
return OB_SAMPLE_RATE_12_5_HZ;
|
||||
} else if (sample_rate == "25hz") {
|
||||
return OB_SAMPLE_RATE_25_HZ;
|
||||
} else if (sample_rate == "50hz") {
|
||||
return OB_SAMPLE_RATE_50_HZ;
|
||||
} else if (sample_rate == "100hz") {
|
||||
return OB_SAMPLE_RATE_100_HZ;
|
||||
} else if (sample_rate == "200hz") {
|
||||
return OB_SAMPLE_RATE_200_HZ;
|
||||
} else if (sample_rate == "500hz") {
|
||||
return OB_SAMPLE_RATE_500_HZ;
|
||||
} else if (sample_rate == "1khz") {
|
||||
return OB_SAMPLE_RATE_1_KHZ;
|
||||
} else if (sample_rate == "2khz") {
|
||||
return OB_SAMPLE_RATE_2_KHZ;
|
||||
} else if (sample_rate == "4khz") {
|
||||
return OB_SAMPLE_RATE_4_KHZ;
|
||||
} else if (sample_rate == "8khz") {
|
||||
return OB_SAMPLE_RATE_8_KHZ;
|
||||
} else if (sample_rate == "16khz") {
|
||||
return OB_SAMPLE_RATE_16_KHZ;
|
||||
} else if (sample_rate == "32khz") {
|
||||
return OB_SAMPLE_RATE_32_KHZ;
|
||||
} else {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"), "Unknown OB_SAMPLE_RATE: " << sample_rate);
|
||||
return OB_SAMPLE_RATE_100_HZ;
|
||||
}
|
||||
}
|
||||
|
||||
std::string sampleRateToString(const OB_SAMPLE_RATE &sample_rate) {
|
||||
switch (sample_rate) {
|
||||
case OB_SAMPLE_RATE_1_5625_HZ:
|
||||
return "1.5625hz";
|
||||
case OB_SAMPLE_RATE_3_125_HZ:
|
||||
return "3.125hz";
|
||||
case OB_SAMPLE_RATE_6_25_HZ:
|
||||
return "6.25hz";
|
||||
case OB_SAMPLE_RATE_12_5_HZ:
|
||||
return "12.5hz";
|
||||
case OB_SAMPLE_RATE_25_HZ:
|
||||
return "25hz";
|
||||
case OB_SAMPLE_RATE_50_HZ:
|
||||
return "50hz";
|
||||
case OB_SAMPLE_RATE_100_HZ:
|
||||
return "100hz";
|
||||
case OB_SAMPLE_RATE_200_HZ:
|
||||
return "200hz";
|
||||
case OB_SAMPLE_RATE_500_HZ:
|
||||
return "500hz";
|
||||
case OB_SAMPLE_RATE_1_KHZ:
|
||||
return "1khz";
|
||||
case OB_SAMPLE_RATE_2_KHZ:
|
||||
return "2khz";
|
||||
case OB_SAMPLE_RATE_4_KHZ:
|
||||
return "4khz";
|
||||
case OB_SAMPLE_RATE_8_KHZ:
|
||||
return "8khz";
|
||||
case OB_SAMPLE_RATE_16_KHZ:
|
||||
return "16khz";
|
||||
case OB_SAMPLE_RATE_32_KHZ:
|
||||
return "32khz";
|
||||
default:
|
||||
return "100hz";
|
||||
}
|
||||
}
|
||||
|
||||
std::ostream &operator<<(std::ostream &os, const OB_SAMPLE_RATE &rhs) {
|
||||
os << sampleRateToString(rhs);
|
||||
return os;
|
||||
}
|
||||
|
||||
OB_GYRO_FULL_SCALE_RANGE fullGyroScaleRangeFromString(std::string &full_scale_range) {
|
||||
std::transform(full_scale_range.begin(), full_scale_range.end(), full_scale_range.begin(),
|
||||
::tolower);
|
||||
if (full_scale_range == "16dps") {
|
||||
return OB_GYRO_FS_16dps;
|
||||
} else if (full_scale_range == "31dps") {
|
||||
return OB_GYRO_FS_31dps;
|
||||
} else if (full_scale_range == "62dps") {
|
||||
return OB_GYRO_FS_62dps;
|
||||
} else if (full_scale_range == "125dps") {
|
||||
return OB_GYRO_FS_125dps;
|
||||
} else if (full_scale_range == "250dps") {
|
||||
return OB_GYRO_FS_250dps;
|
||||
} else if (full_scale_range == "500dps") {
|
||||
return OB_GYRO_FS_500dps;
|
||||
} else if (full_scale_range == "1000dps") {
|
||||
return OB_GYRO_FS_1000dps;
|
||||
} else if (full_scale_range == "2000dps") {
|
||||
return OB_GYRO_FS_2000dps;
|
||||
} else {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"),
|
||||
"Unknown OB_GYRO_FULL_SCALE_RANGE: " << full_scale_range);
|
||||
return OB_GYRO_FS_2000dps;
|
||||
}
|
||||
}
|
||||
|
||||
std::string fullGyroScaleRangeToString(const OB_GYRO_FULL_SCALE_RANGE &full_scale_range) {
|
||||
switch (full_scale_range) {
|
||||
case OB_GYRO_FS_16dps:
|
||||
return "16dps";
|
||||
case OB_GYRO_FS_31dps:
|
||||
return "31dps";
|
||||
case OB_GYRO_FS_62dps:
|
||||
return "62dps";
|
||||
case OB_GYRO_FS_125dps:
|
||||
return "125dps";
|
||||
case OB_GYRO_FS_250dps:
|
||||
return "250dps";
|
||||
case OB_GYRO_FS_500dps:
|
||||
return "500dps";
|
||||
case OB_GYRO_FS_1000dps:
|
||||
return "1000dps";
|
||||
case OB_GYRO_FS_2000dps:
|
||||
return "2000dps";
|
||||
default:
|
||||
return "16dps";
|
||||
}
|
||||
}
|
||||
|
||||
std::ostream &operator<<(std::ostream &os, const OB_GYRO_FULL_SCALE_RANGE &rhs) {
|
||||
os << fullGyroScaleRangeToString(rhs);
|
||||
return os;
|
||||
}
|
||||
|
||||
OBAccelFullScaleRange fullAccelScaleRangeFromString(std::string &full_scale_range) {
|
||||
std::transform(full_scale_range.begin(), full_scale_range.end(), full_scale_range.begin(),
|
||||
::tolower);
|
||||
if (full_scale_range == "2g") {
|
||||
return OB_ACCEL_FS_2g;
|
||||
} else if (full_scale_range == "4g") {
|
||||
return OB_ACCEL_FS_4g;
|
||||
} else if (full_scale_range == "8g") {
|
||||
return OB_ACCEL_FS_8g;
|
||||
} else if (full_scale_range == "16g") {
|
||||
return OB_ACCEL_FS_16g;
|
||||
} else {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"),
|
||||
"Unknown OB_ACCEL_FULL_SCALE_RANGE: " << full_scale_range);
|
||||
return OB_ACCEL_FS_16g;
|
||||
}
|
||||
}
|
||||
|
||||
std::string fullAccelScaleRangeToString(const OBAccelFullScaleRange &full_scale_range) {
|
||||
switch (full_scale_range) {
|
||||
case OB_ACCEL_FS_2g:
|
||||
return "2g";
|
||||
case OB_ACCEL_FS_4g:
|
||||
return "4g";
|
||||
case OB_ACCEL_FS_8g:
|
||||
return "8g";
|
||||
case OB_ACCEL_FS_16g:
|
||||
return "16g";
|
||||
default:
|
||||
return "2g";
|
||||
}
|
||||
}
|
||||
|
||||
std::ostream &operator<<(std::ostream &os, const OBAccelFullScaleRange &rhs) {
|
||||
os << fullAccelScaleRangeToString(rhs);
|
||||
return os;
|
||||
}
|
||||
|
||||
std::string parseUsbPort(const std::string &line) {
|
||||
std::string port_id;
|
||||
std::regex self_regex("(?:[^ ]+/usb[0-9]+[0-9./-]*/){0,1}([0-9.-]+)(:){0,1}[^ ]*",
|
||||
std::regex_constants::ECMAScript);
|
||||
std::smatch base_match;
|
||||
bool found = std::regex_match(line, base_match, self_regex);
|
||||
if (found) {
|
||||
port_id = base_match[1].str();
|
||||
if (base_match[2].str().empty()) // This is libuvc string. Remove counter is exists.
|
||||
{
|
||||
std::regex end_regex = std::regex(".+(-[0-9]+$)", std::regex_constants::ECMAScript);
|
||||
bool found_end = std::regex_match(port_id, base_match, end_regex);
|
||||
if (found_end) {
|
||||
port_id = port_id.substr(0, port_id.size() - base_match[1].str().size());
|
||||
}
|
||||
}
|
||||
}
|
||||
return port_id;
|
||||
}
|
||||
|
||||
bool isValidJPEG(const std::shared_ptr<ob::ColorFrame> &frame) {
|
||||
if (frame->dataSize() < 2) { // Checking both start and end markers, so minimal size is 4
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto *data = static_cast<const uint8_t *>(frame->data());
|
||||
|
||||
// Check for JPEG start marker
|
||||
if (data[0] != 0xFF || data[1] != 0xD8) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string metaDataTypeToString(const OBFrameMetadataType &meta_data_type) {
|
||||
switch (meta_data_type) {
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_TIMESTAMP:
|
||||
return "frame_timestamp";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_SENSOR_TIMESTAMP:
|
||||
return "sensor_timestamp";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_FRAME_NUMBER:
|
||||
return "frame_number";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AUTO_EXPOSURE:
|
||||
return "auto_exposure";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_EXPOSURE:
|
||||
return "exposure";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_GAIN:
|
||||
return "gain";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AUTO_WHITE_BALANCE:
|
||||
return "auto_white_balance";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_WHITE_BALANCE:
|
||||
return "white_balance";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_BRIGHTNESS:
|
||||
return "brightness";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_CONTRAST:
|
||||
return "contrast";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_SATURATION:
|
||||
return "saturation";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_SHARPNESS:
|
||||
return "sharpness";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_BACKLIGHT_COMPENSATION:
|
||||
return "backlight_compensation";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HUE:
|
||||
return "hue";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_GAMMA:
|
||||
return "gamma";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_POWER_LINE_FREQUENCY:
|
||||
return "power_line_frequency";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_LOW_LIGHT_COMPENSATION:
|
||||
return "low_light_compensation";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_MANUAL_WHITE_BALANCE:
|
||||
return "manual_white_balance";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_ACTUAL_FRAME_RATE:
|
||||
return "actual_frame_rate";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_FRAME_RATE:
|
||||
return "frame_rate";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_LEFT:
|
||||
return "ae_roi_left";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_TOP:
|
||||
return "ae_roi_top";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_RIGHT:
|
||||
return "ae_roi_right";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_BOTTOM:
|
||||
return "ae_roi_bottom";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_EXPOSURE_PRIORITY:
|
||||
return "exposure_priority";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_NAME:
|
||||
return "hdr_sequence_name";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_SIZE:
|
||||
return "hdr_sequence_size";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX:
|
||||
return "hdr_sequence_index";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_LASER_POWER:
|
||||
return "frame_laser_power";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_LASER_POWER_MODE:
|
||||
return "frame_laser_power_mode";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_EMITTER_MODE:
|
||||
return "frame_emitter_mode";
|
||||
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_GPIO_INPUT_DATA:
|
||||
return "gpio_input_data";
|
||||
default:
|
||||
return "unknown_field";
|
||||
}
|
||||
}
|
||||
|
||||
std::ostream &operator<<(std::ostream &os, const OBFrameMetadataType &rhs) {
|
||||
os << metaDataTypeToString(rhs);
|
||||
return os;
|
||||
}
|
||||
|
||||
OBHoleFillingMode holeFillingModeFromString(const std::string &hole_filling_mode) {
|
||||
if (hole_filling_mode == "FILL_TOP") {
|
||||
return OB_HOLE_FILL_TOP;
|
||||
} else if (hole_filling_mode == "FILL_NEAREST") {
|
||||
return OB_HOLE_FILL_NEAREST;
|
||||
} else if (hole_filling_mode == "FILL_FAREST") {
|
||||
return OB_HOLE_FILL_FAREST;
|
||||
} else {
|
||||
return OB_HOLE_FILL_NEAREST;
|
||||
}
|
||||
}
|
||||
|
||||
bool isGemini2R(int pid) {
|
||||
if (pid == GEMINI2R_PID || pid == GEMINI2RL_PID) {
|
||||
return true;
|
||||
}
|
||||
if (pid == GEMINI2R_PID2 || pid == GEMINI2RL_PID2) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
OBStreamType obStreamTypeFromString(const std::string &stream_type) {
|
||||
std::string upper_stream_type = stream_type;
|
||||
std::transform(upper_stream_type.begin(), upper_stream_type.end(), upper_stream_type.begin(),
|
||||
::toupper);
|
||||
if (upper_stream_type == "VIDEO") {
|
||||
return OB_STREAM_VIDEO;
|
||||
} else if (upper_stream_type == "IR") {
|
||||
return OB_STREAM_IR;
|
||||
} else if (upper_stream_type == "COLOR") {
|
||||
return OB_STREAM_COLOR;
|
||||
} else if (upper_stream_type == "DEPTH") {
|
||||
return OB_STREAM_DEPTH;
|
||||
} else if (upper_stream_type == "ACCEL") {
|
||||
return OB_STREAM_ACCEL;
|
||||
} else if (upper_stream_type == "GYRO") {
|
||||
return OB_STREAM_GYRO;
|
||||
} else if (upper_stream_type == "IR_LEFT") {
|
||||
return OB_STREAM_IR_LEFT;
|
||||
} else if (upper_stream_type == "IR_RIGHT") {
|
||||
return OB_STREAM_IR_RIGHT;
|
||||
} else if (upper_stream_type == "RAW_PHASE") {
|
||||
return OB_STREAM_RAW_PHASE;
|
||||
} else {
|
||||
return OB_STREAM_UNKNOWN;
|
||||
}
|
||||
}
|
||||
|
||||
cv::Mat undistortImage(const cv::Mat &image, const OBCameraIntrinsic &intrinsic,
|
||||
const OBCameraDistortion &distortion) {
|
||||
cv::Mat undistorted_image;
|
||||
cv::Mat camera_matrix = cv::Mat::eye(3, 3, CV_64F);
|
||||
camera_matrix.at<double>(0, 0) = intrinsic.fx;
|
||||
camera_matrix.at<double>(1, 1) = intrinsic.fy;
|
||||
camera_matrix.at<double>(0, 2) = intrinsic.cx;
|
||||
camera_matrix.at<double>(1, 2) = intrinsic.cy;
|
||||
|
||||
// Create the distortion coefficients matrix using the extended distortion model
|
||||
cv::Mat dist_coeffs = (cv::Mat_<float>(8, 1) << distortion.k1, distortion.k2, distortion.p1,
|
||||
distortion.p2, distortion.k3, distortion.k4, distortion.k5, distortion.k6);
|
||||
|
||||
// Undistort the image using OpenCV's undistort function
|
||||
// This function corrects for lens distortion
|
||||
cv::undistort(image, undistorted_image, camera_matrix, dist_coeffs);
|
||||
|
||||
return undistorted_image;
|
||||
}
|
||||
} // namespace orbbec_camera
|
||||
Reference in New Issue
Block a user