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

Update orbbec_camera package to be compatible with cuvslam

Browse Source
This commit is contained in:
jj
2025-04-26 18:12:18 +08:00
parent 3c4764f8ca
commit 5bd616cb90
213 changed files with 2964 additions and 7751 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/d2c_viewer.cpp
+4 -9
View File
@@ -52,23 +52,18 @@ void D2CViewer::messageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& r
rgb_msg->height, depth_msg->width, depth_msg->height);
return;
}
auto rgb_encode = (rgb_msg->step == 5760) ? sensor_msgs::image_encodings::RGB8
: sensor_msgs::image_encodings::RGBA8;
auto rgb_encode = (rgb_msg->step == 5760) ? sensor_msgs::image_encodings::RGB8 : sensor_msgs::image_encodings::RGBA8;
auto gray_type = (rgb_msg->step == 5760) ? cv::COLOR_GRAY2RGB : cv::COLOR_GRAY2RGBA;
auto rgb_img_ptr = cv_bridge::toCvCopy(rgb_msg, rgb_encode);
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, gray_type);
depth_img.setTo(cv::Scalar(255, 255, 0), depth_img);
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(), rgb_encode, d2c_img).toImageMsg();
if (d2c_msg != nullptr) {
d2c_msg->header = rgb_msg->header;
d2c_viewer_pub_->publish(*d2c_msg);
}else{
RCLCPP_ERROR(logger_, "-----------------------d2c_viewer publishing failed-----------------------");
}
d2c_msg->header = rgb_msg->header;
d2c_viewer_pub_->publish(*d2c_msg);
}
} // namespace orbbec_camera
orbbec_camera/src/ob_camera_node.cpp
+218 -750
View File
File diff suppressed because it is too large Load Diff
orbbec_camera/src/ob_camera_node_driver.cpp
+2 -140
View File
@@ -31,7 +31,6 @@
#include <iomanip> // For std::put_time
std::string g_camera_name = "orbbec_camera"; // Assuming this is declared elsewhere
std::string g_time_domain = "global"; // Assuming this is declared elsewhere
void signalHandler(int sig) {
std::cout << "Received signal: " << sig << std::endl;
@@ -130,9 +129,6 @@ void OBCameraNodeDriver::init() {
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);
g_time_domain = declare_parameter<std::string>("time_domain", g_time_domain);
preset_firmware_path_ =
declare_parameter<std::string>("preset_firmware_path", preset_firmware_path_);
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) {
@@ -281,6 +277,7 @@ void OBCameraNodeDriver::resetDevice() {
device_info_.reset();
device_connected_ = false;
device_unique_id_.clear();
serial_number_.clear();
reset_device_flag_ = false;
}
RCLCPP_INFO_STREAM(logger_, "Reset device uid: " << device_unique_id_ << " done");
@@ -393,7 +390,6 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
device_ = device;
updatePresetFirmware(preset_firmware_path_);
CHECK_NOTNULL(device_);
CHECK_NOTNULL(device_.get());
if (ob_camera_node_) {
@@ -443,13 +439,6 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid())) {
TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
if (g_time_domain != "global") {
sync_host_time_timer_ = this->create_wall_timer(std::chrono::milliseconds(60000), [this]() {
if (device_) {
TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
}
});
}
}
RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->getName() << " connected");
@@ -491,30 +480,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
device_.reset();
}
std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_));
int try_lock_count = 0;
int max_try_lock_count = 5;
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;
}
pthread_mutex_lock(orb_device_lock_);
std::shared_ptr<int> lock_holder(nullptr,
[this](int *) { pthread_mutex_unlock(orb_device_lock_); });
bool start_device_failed = false;
@@ -557,110 +523,6 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
reset_device_cond_.notify_all();
}
}
void OBCameraNodeDriver::updatePresetFirmware(std::string path) {
if (path.empty()) {
return;
} else {
std::stringstream ss(path);
std::string path_segment;
std::vector<std::string> paths;
OBFwUpdateState updateState = STAT_START;
bool firstCall = true;
while (std::getline(ss, path_segment, ',')) {
paths.push_back(path_segment);
}
uint8_t index = 0;
uint8_t count = static_cast<uint8_t>(paths.size());
char(*filePaths)[OB_PATH_MAX] = new char[count][OB_PATH_MAX];
RCLCPP_INFO_STREAM(this->get_logger(), "paths.cout : " << (uint32_t)count);
for (const auto &p : paths) {
strcpy(filePaths[index], p.c_str());
RCLCPP_INFO_STREAM(this->get_logger(),
"path: " << (uint32_t)index << ":" << filePaths[index]);
index++;
}
RCLCPP_INFO_STREAM(this->get_logger(),
"Start to update optional depth preset, please wait a moment...");
try {
device_->updateOptionalDepthPresets(
filePaths, count,
[this, &updateState, &firstCall](OBFwUpdateState state, const char *message,
uint8_t percent) {
updateState = state;
presetUpdateCallback(firstCall, state, message, percent);
// firstCall = false;
});
delete[] filePaths;
filePaths = nullptr;
if (updateState == STAT_DONE || updateState == STAT_DONE_WITH_DUPLICATES) {
RCLCPP_INFO_STREAM(this->get_logger(), "After updating the preset: ");
auto presetList = device_->getAvailablePresetList();
RCLCPP_INFO_STREAM(this->get_logger(), "Preset count: " << presetList->getCount());
for (uint32_t i = 0; i < presetList->getCount(); ++i) {
RCLCPP_INFO_STREAM(this->get_logger(), " - " << presetList->getName(i));
}
RCLCPP_INFO_STREAM(this->get_logger(),
"Current preset: " << device_->getCurrentPresetName());
std::string key = "PresetVer";
if (device_->isExtensionInfoExist(key)) {
std::string value = device_->getExtensionInfo(key);
RCLCPP_INFO_STREAM(this->get_logger(), "Preset version: " << value);
} else {
RCLCPP_INFO_STREAM(this->get_logger(), "PresetVer: ");
}
}
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware " << e.getMessage());
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware");
}
}
}
void OBCameraNodeDriver::presetUpdateCallback(bool firstCall, OBFwUpdateState state,
const char *message, uint8_t percent) {
if (!firstCall) {
std::cout << "\033[3F";
}
std::cout << "\033[K";
std::cout << "Progress: " << static_cast<uint32_t>(percent) << "%" << std::endl;
std::cout << "\033[K";
std::cout << "Status : ";
switch (state) {
case STAT_VERIFY_SUCCESS:
std::cout << "Image file verification success" << std::endl;
break;
case STAT_FILE_TRANSFER:
std::cout << "File transfer in progress" << std::endl;
break;
case STAT_DONE:
std::cout << "Update completed" << std::endl;
break;
case STAT_DONE_WITH_DUPLICATES:
std::cout << "Update completed, duplicated presets have been ignored" << std::endl;
break;
case STAT_IN_PROGRESS:
std::cout << "Update in progress" << std::endl;
break;
case STAT_START:
std::cout << "Starting the update" << std::endl;
break;
case STAT_VERIFY_IMAGE:
std::cout << "Verifying image file" << std::endl;
break;
default:
std::cout << "Unknown status or error" << std::endl;
break;
}
std::cout << "\033[K";
std::cout << "Message : " << message << std::endl << std::flush;
}
} // namespace orbbec_camera
RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::OBCameraNodeDriver)
orbbec_camera/src/ros_service.cpp
+5 -80
View File
@@ -67,14 +67,6 @@ void OBCameraNode::setupCameraCtrlServices() {
setAutoExposureCallback(request, response, stream_index);
});
service_name = "set_" + stream_name + "_ae_roi";
set_ae_roi_srv_[stream_index] = node_->create_service<SetArrays>(
service_name,
[this, stream_index = stream_index](const std::shared_ptr<SetArrays::Request> request,
std::shared_ptr<SetArrays::Response> response) {
setAeRoiCallback(request, response, stream_index);
});
service_name = "toggle_" + stream_name;
toggle_sensor_srv_[stream_index] = node_->create_service<SetBool>(
@@ -172,10 +164,10 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<SetBool::Response> response) {
setIRLongExposureCallback(request, response);
});
get_lrm_measure_distance_srv_ = node_->create_service<GetInt32>(
"get_lrm_measure_distance", [this](const std::shared_ptr<GetInt32::Request> request,
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) {
getLrmMeasureDistanceCallback(request, response);
getLdpMeasureDistanceCallback(request, response);
});
set_reset_timestamp_srv_ = node_->create_service<SetBool>(
"set_reset_timestamp", [this](const std::shared_ptr<SetBool::Request> request,
@@ -309,59 +301,6 @@ void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& re
}
}
void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>& request,
std::shared_ptr<SetArrays::Response>& response,
const stream_index_pair& stream_index) {
auto stream = stream_index.first;
auto config = OBRegionOfInterest();
try {
switch (stream) {
case OB_STREAM_IR_LEFT:
case OB_STREAM_IR_RIGHT:
case OB_STREAM_IR:
case OB_STREAM_DEPTH:
config.x0_left = static_cast<short int>(request->data_param[0]);
config.x1_right = static_cast<short int>(request->data_param[1]);
config.y0_top = static_cast<short int>(request->data_param[2]);
config.y1_bottom = static_cast<short int>(request->data_param[3]);
device_->setStructuredData(OB_STRUCT_DEPTH_AE_ROI,
reinterpret_cast<const uint8_t*>(&config), sizeof(config));
RCLCPP_INFO_STREAM(logger_,
"set depth AE ROI : " << "[Left: " << config.x0_left << ", Right: "
<< config.x1_right << ", Top: " << config.y0_top
<< ", Bottom: " << config.y1_bottom << " ]");
break;
case OB_STREAM_COLOR:
config.x0_left = static_cast<short int>(request->data_param[0]);
config.x1_right = static_cast<short int>(request->data_param[1]);
config.y0_top = static_cast<short int>(request->data_param[2]);
config.y1_bottom = static_cast<short int>(request->data_param[3]);
device_->setStructuredData(OB_STRUCT_COLOR_AE_ROI,
reinterpret_cast<const uint8_t*>(&config), sizeof(config));
RCLCPP_INFO_STREAM(logger_,
"set color AE ROI : " << "[Left: " << config.x0_left << ", Right: "
<< config.x1_right << ", Top: " << config.y0_top
<< ", Bottom: " << config.y1_bottom << " ]");
break;
default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
response->success = false;
response->message = "NOT a video stream";
return;
}
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;
@@ -563,20 +502,7 @@ void OBCameraNode::setLdpEnableCallback(
(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);
}
}
device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
@@ -726,7 +652,7 @@ void OBCameraNode::getLdpStatusCallback(const std::shared_ptr<GetBool::Request>&
}
}
void OBCameraNode::getLrmMeasureDistanceCallback(const std::shared_ptr<GetInt32::Request>& request,
void OBCameraNode::getLdpMeasureDistanceCallback(const std::shared_ptr<GetInt32::Request>& request,
std::shared_ptr<GetInt32::Response>& response) {
(void)request;
try {
@@ -910,5 +836,4 @@ void OBCameraNode::setSYNCHostimeCallback(
response->success = false;
}
}
} // namespace orbbec_camera
orbbec_camera/src/utils.cpp
+8 -31
View File
@@ -23,7 +23,7 @@ sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
OBCameraDistortion distortion, int width) {
(void)width;
sensor_msgs::msg::CameraInfo info;
info.distortion_model = getDistortionModels(distortion);
info.distortion_model = sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;
info.width = intrinsic.width;
info.height = intrinsic.height;
info.d.resize(8, 0.0);
@@ -339,9 +339,9 @@ OBFormat OBFormatFromString(const std::string &format) {
} else if (fixed_format == "RW16") {
return OB_FORMAT_RW16;
}
// else if (fixed_format == "DISP16") {
// return OB_FORMAT_DISP16;
// }
// else if (fixed_format == "DISP16") {
// return OB_FORMAT_DISP16;
// }
else {
return OB_FORMAT_UNKNOWN;
}
@@ -436,8 +436,8 @@ std::string ObDeviceTypeToString(const OBDeviceType &type) {
case OBDeviceType::OB_TOF_CAMERA:
return "tof camera";
default:
// 处理其他未预见的情况
break;
// 处理其他未预见的情况
break;
}
return "unknown technology camera";
}
@@ -700,9 +700,9 @@ std::ostream &operator<<(std::ostream &os, const OBAccelFullScaleRange &rhs) {
}
std::string parseUsbPort(const std::string &line) {
std::string port_id;
std::string port_id;
std::regex usb_regex("(?:[^ ]+/usb[0-9]+[0-9./-]*/){0,1}([0-9.-]+)(:){0,1}[^ ]*",
std::regex_constants::ECMAScript);
std::regex_constants::ECMAScript);
std::smatch base_match;
bool found_usb = std::regex_match(line, base_match, usb_regex);
@@ -815,10 +815,6 @@ std::string metaDataTypeToString(const OBFrameMetadataType &meta_data_type) {
return "frame_emitter_mode";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_GPIO_INPUT_DATA:
return "gpio_input_data";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_DISPARITY_SEARCH_OFFSET:
return "disparity_search_offset";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_DISPARITY_SEARCH_RANGE:
return "disparity search range";
default:
return "unknown_field";
}
@@ -897,23 +893,4 @@ cv::Mat undistortImage(const cv::Mat &image, const OBCameraIntrinsic &intrinsic,
return undistorted_image;
}
std::string getDistortionModels(OBCameraDistortion distortion){
switch (distortion.model)
{
case OB_DISTORTION_NONE:
return "ob_distortion_none";
case OB_DISTORTION_MODIFIED_BROWN_CONRADY:
return "ob_distortion_modified_brown_conrady";
case OB_DISTORTION_INVERSE_BROWN_CONRADY:
return "ob_distortion_inverse_brown_conrady";
case OB_DISTORTION_BROWN_CONRADY:
return "ob_distortion_brown_conrady";
case OB_DISTORTION_BROWN_CONRADY_K6:
return "ob_distortion_brown_conrady_k6";
case OB_DISTORTION_KANNALA_BRANDT4:
return "ob_distortion_kannala_brandt4";
default:
return "unknown_field";
}
}
} // namespace orbbec_camera