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

updata libOrbbecSDK.so , ob_camera_node and CMakeLists.txt

Browse Source
This commit is contained in:
jj
2024-09-04 12:47:36 +08:00
parent 3cd3a7fdd3
commit 9f764c8e57
58 changed files with 6218 additions and 5345 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/image_publisher.cpp
-48
View File
@@ -1,48 +0,0 @@
// 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
orbbec_camera/src/ob_camera_node.cpp
+228 -305
View File
File diff suppressed because it is too large Load Diff
orbbec_camera/src/ob_camera_node_driver.cpp
+26 -117
View File
@@ -15,7 +15,6 @@
*******************************************************************************/
#include "orbbec_camera/ob_camera_node_driver.h"
#include "orbbec_camera/utils.h"
#include <fcntl.h>
#include <semaphore.h>
#include <sys/shm.h>
@@ -24,56 +23,10 @@
#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())),
@@ -84,7 +37,6 @@ OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
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();
@@ -105,15 +57,10 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
}
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);
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) {
@@ -162,7 +109,7 @@ void OBCameraNodeDriver::init() {
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
CHECK_NOTNULL(device_list);
if (device_list->deviceCount() == 0) {
if (device_list->getCount() == 0) {
return;
}
if (!device_) {
@@ -172,13 +119,13 @@ void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList>
void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceList> &device_list) {
CHECK_NOTNULL(device_list);
if (device_list->deviceCount() == 0) {
if (device_list->getCount() == 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);
for (size_t i = 0; i < device_list->getCount(); i++) {
std::string uid = device_list->getUid(i);
std::string serial_number = device_list->getSerialNumber(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) {
@@ -224,7 +171,7 @@ void OBCameraNodeDriver::queryDevice() {
connectNetDevice(net_device_ip_, net_device_port_);
} else {
auto device_list = ctx_->queryDeviceList();
if (device_list->deviceCount() == 0) {
if (device_list->getCount() == 0) {
RCLCPP_INFO_STREAM(logger_,
"queryDevice :No Device found, using usb event to trigger "
"OBCameraNodeDriver::onDeviceConnected");
@@ -302,23 +249,23 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
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++) {
for (size_t i = 0; i < list->getCount(); 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);
auto pid = list->getPid(i);
if (isOpenNIDevice(pid)) {
// openNI device
auto device = list->getDevice(i);
auto device_info = device->getDeviceInfo();
if (device_info->serialNumber() == serial_number) {
if (device_info->getSerialNumber() == serial_number) {
RCLCPP_INFO_STREAM(logger_,
"Device serial number " << device_info->serialNumber() << " matched");
"Device serial number " << device_info->getSerialNumber() << " matched");
return device;
}
} else {
std::string sn = list->serialNumber(i);
std::string sn = list->getSerialNumber(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");
@@ -344,14 +291,7 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
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");
}
RCLCPP_INFO_STREAM(logger_, "Device usb port " << usb_port << " done");
return device;
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.getMessage());
@@ -371,64 +311,33 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
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_ = std::make_unique<OBCameraNode>(this, device_, parameters_);
ob_camera_node_->startIMU();
ob_camera_node_->startStreams();
device_connected_ = true;
serial_number_ = device_info_->serialNumber();
device_info_ = device_->getDeviceInfo();
serial_number_ = device_info_->getSerialNumber();
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());
device_unique_id_ = device_info_->getUid();
if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->getPid())) {
device_->timerSyncWithHost();
sync_host_time_timer_ = this->create_wall_timer(std::chrono::milliseconds(30000), [this]() {
if (device_) {
TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
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 " << device_info_->getName() << " connected");
RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->getSerialNumber());
RCLCPP_INFO_STREAM(logger_, "Firmware version: " << device_info_->getFirmwareVersion());
RCLCPP_INFO_STREAM(logger_, "Hardware version: " << device_info_->getHardwareVersion());
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());
RCLCPP_INFO_STREAM(logger_, "usb connect type: " << device_info_->getConnectionType());
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) {
@@ -448,7 +357,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
if (device_connected_) {
return;
}
if (list->deviceCount() == 0) {
if (list->getCount() == 0) {
RCLCPP_WARN(logger_, "No device found");
return;
}
orbbec_camera/src/ros_service.cpp
+6 -6
View File
@@ -537,10 +537,10 @@ void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr<GetDeviceInfo::Re
(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->info.name = device_info->getName();
response->info.serial_number = device_info->getSerialNumber();
response->info.firmware_version = device_info->getFirmwareVersion();
response->info.supported_min_sdk_version = device_info->getSupportedMinSdkVersion();
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
@@ -560,8 +560,8 @@ void OBCameraNode::getSDKVersion(const std::shared_ptr<GetString::Request>& requ
try {
auto device_info = device_->getDeviceInfo();
nlohmann::json data;
data["firmware_version"] = device_info->firmwareVersion();
data["supported_min_sdk_version"] = device_info->supportedMinSdkVersion();
data["firmware_version"] = device_info->getFirmwareVersion();
data["supported_min_sdk_version"] = device_info->getSupportedMinSdkVersion();
data["ros_sdk_version"] = OB_ROS_VERSION_STR;
data["ob_sdk_version"] = getObSDKVersion();
response->data = data.dump(2);
orbbec_camera/src/utils.cpp
+16 -11
View File
@@ -58,7 +58,7 @@ sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
}
void saveRGBPointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::string &fileName) {
size_t point_size = frame->dataSize() / sizeof(OBColorPoint);
size_t point_size = frame->getDataSize() / sizeof(OBColorPoint);
FILE *fp = fopen(fileName.c_str(), "wb+");
std::shared_ptr<int> fp_guard(nullptr, [&fp](int *) {
fflush(fp);
@@ -75,7 +75,7 @@ void saveRGBPointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::stri
fprintf(fp, "property uchar blue\n");
fprintf(fp, "end_header\n");
const auto *points = (OBColorPoint *)frame->data();
const auto *points = (OBColorPoint *)frame->getData();
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,
@@ -172,7 +172,7 @@ void saveDepthPointsToPly(const sensor_msgs::msg::PointCloud2::UniquePtr &msg,
}
void savePointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::string &fileName) {
size_t point_size = frame->dataSize() / sizeof(OBPoint);
size_t point_size = frame->getDataSize() / sizeof(OBPoint);
FILE *fp = fopen(fileName.c_str(), "wb+");
std::shared_ptr<int> fp_guard(nullptr, [&fp](int *) {
fflush(fp);
@@ -188,7 +188,7 @@ void savePointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::string
fprintf(fp, "property float z\n");
fprintf(fp, "end_header\n");
const auto *points = (OBPoint *)frame->data();
const auto *points = (OBPoint *)frame->getData();
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);
@@ -338,9 +338,11 @@ OBFormat OBFormatFromString(const std::string &format) {
return OB_FORMAT_BYR2;
} else if (fixed_format == "RW16") {
return OB_FORMAT_RW16;
} else if (fixed_format == "DISP16") {
return OB_FORMAT_DISP16;
} else {
}
// else if (fixed_format == "DISP16") {
// return OB_FORMAT_DISP16;
// }
else {
return OB_FORMAT_UNKNOWN;
}
}
@@ -413,8 +415,8 @@ std::string OBFormatToString(const OBFormat &format) {
return "BYR2";
case OB_FORMAT_RW16:
return "RW16";
case OB_FORMAT_DISP16:
return "DISP16";
// case OB_FORMAT_DISP16:
// return "DISP16";
default:
return "UNKNOWN";
}
@@ -433,6 +435,9 @@ std::string ObDeviceTypeToString(const OBDeviceType &type) {
return "structured light monocular camera";
case OBDeviceType::OB_TOF_CAMERA:
return "tof camera";
default:
// 处理其他未预见的情况
break;
}
return "unknown technology camera";
}
@@ -715,11 +720,11 @@ std::string parseUsbPort(const std::string &line) {
}
bool isValidJPEG(const std::shared_ptr<ob::ColorFrame> &frame) {
if (frame->dataSize() < 2) { // Checking both start and end markers, so minimal size is 4
if (frame->getDataSize() < 2) { // Checking both start and end markers, so minimal size is 4
return false;
}
const auto *data = static_cast<const uint8_t *>(frame->data());
const auto *data = static_cast<const uint8_t *>(frame->getData());
// Check for JPEG start marker
if (data[0] != 0xFF || data[1] != 0xD8) {