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

feat: add enable_publish_extrinsic parameter to control extrinsic publishing

Browse Source
This commit is contained in:
obyalian
2025-09-11 10:46:53 +08:00
parent beb2143cfa
commit 4586c4ef18
2 changed files with 17 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+1
View File
@@ -819,6 +819,7 @@ class OBCameraNode {
std::string frame_aggregate_mode_ = "ANY"; // # full_frame, color_frame, ANY or disable std::string frame_aggregate_mode_ = "ANY"; // # full_frame, color_frame, ANY or disable
bool show_fps_enable_ = false; bool show_fps_enable_ = false;
bool enable_publish_extrinsic_ = false;
std::unique_ptr<FpsCounter> fps_counter_color_{nullptr}; std::unique_ptr<FpsCounter> fps_counter_color_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_depth_{nullptr}; std::unique_ptr<FpsCounter> fps_counter_depth_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_left_ir_{nullptr}; std::unique_ptr<FpsCounter> fps_counter_left_ir_{nullptr};
orbbec_camera/src/ob_camera_node.cpp
+16 -17
View File
@@ -1945,6 +1945,7 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<std::string>(frame_aggregate_mode_, "frame_aggregate_mode", "ANY"); setAndGetNodeParameter<std::string>(frame_aggregate_mode_, "frame_aggregate_mode", "ANY");
setAndGetNodeParameter<bool>(show_fps_enable_, "show_fps_enable", false); setAndGetNodeParameter<bool>(show_fps_enable_, "show_fps_enable", false);
setAndGetNodeParameter<bool>(enable_publish_extrinsic_, "enable_publish_extrinsic", false);
RCLCPP_INFO_STREAM(logger_, "current time domain: " << time_domain_); RCLCPP_INFO_STREAM(logger_, "current time domain: " << time_domain_);
RCLCPP_INFO_STREAM(logger_, "hdr_index1_laser_control_ " RCLCPP_INFO_STREAM(logger_, "hdr_index1_laser_control_ "
@@ -2013,14 +2014,12 @@ void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapp
// Try to acquire device lock with timeout to avoid blocking during shutdown // Try to acquire device lock with timeout to avoid blocking during shutdown
std::unique_lock<decltype(device_lock_)> lock(device_lock_, std::try_to_lock); std::unique_lock<decltype(device_lock_)> lock(device_lock_, std::try_to_lock);
if (!lock.owns_lock()) { if (!lock.owns_lock()) {
status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, "Device busy or shutting down");
"Device busy or shutting down");
return; return;
} }
if (!device_) { if (!device_) {
status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, "Device not available");
"Device not available");
return; return;
} }
@@ -2287,32 +2286,32 @@ void OBCameraNode::setupPublishers() {
if (use_intra_process_) { if (use_intra_process_) {
extrinsics_qos = rclcpp::QoS(1); extrinsics_qos = rclcpp::QoS(1);
} }
if (enable_stream_[DEPTH] && enable_stream_[INFRA0]) { if (enable_stream_[DEPTH] && enable_stream_[INFRA0] && enable_publish_extrinsic_) {
depth_to_other_extrinsics_publishers_[INFRA0] = depth_to_other_extrinsics_publishers_[INFRA0] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>( node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_ir", extrinsics_qos); "/" + camera_name_ + "/depth_to_ir", extrinsics_qos);
} }
if (enable_stream_[DEPTH] && enable_stream_[COLOR]) { if (enable_stream_[DEPTH] && enable_stream_[COLOR] && enable_publish_extrinsic_) {
depth_to_other_extrinsics_publishers_[COLOR] = depth_to_other_extrinsics_publishers_[COLOR] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>( node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_color", extrinsics_qos); "/" + camera_name_ + "/depth_to_color", extrinsics_qos);
} }
if (enable_stream_[DEPTH] && enable_stream_[INFRA1]) { if (enable_stream_[DEPTH] && enable_stream_[INFRA1] && enable_publish_extrinsic_) {
depth_to_other_extrinsics_publishers_[INFRA1] = depth_to_other_extrinsics_publishers_[INFRA1] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>( node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_left_ir", extrinsics_qos); "/" + camera_name_ + "/depth_to_left_ir", extrinsics_qos);
} }
if (enable_stream_[DEPTH] && enable_stream_[INFRA2]) { if (enable_stream_[DEPTH] && enable_stream_[INFRA2] && enable_publish_extrinsic_) {
depth_to_other_extrinsics_publishers_[INFRA2] = depth_to_other_extrinsics_publishers_[INFRA2] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>( node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_right_ir", extrinsics_qos); "/" + camera_name_ + "/depth_to_right_ir", extrinsics_qos);
} }
if (enable_stream_[DEPTH] && enable_stream_[ACCEL]) { if (enable_stream_[DEPTH] && enable_stream_[ACCEL] && enable_publish_extrinsic_) {
depth_to_other_extrinsics_publishers_[ACCEL] = depth_to_other_extrinsics_publishers_[ACCEL] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>( node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_accel", extrinsics_qos); "/" + camera_name_ + "/depth_to_accel", extrinsics_qos);
} }
if (enable_stream_[DEPTH] && enable_stream_[GYRO]) { if (enable_stream_[DEPTH] && enable_stream_[GYRO] && enable_publish_extrinsic_) {
depth_to_other_extrinsics_publishers_[GYRO] = depth_to_other_extrinsics_publishers_[GYRO] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>( node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_gyro", extrinsics_qos); "/" + camera_name_ + "/depth_to_gyro", extrinsics_qos);
@@ -3492,7 +3491,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
} }
if ((pid == FEMTO_BOLT_PID || pid == FEMTO_MEGA_PID) && enable_stream_[DEPTH] && if ((pid == FEMTO_BOLT_PID || pid == FEMTO_MEGA_PID) && enable_stream_[DEPTH] &&
enable_stream_[COLOR]) { enable_stream_[COLOR] && enable_publish_extrinsic_) {
// calc depth to color // calc depth to color
CHECK_NOTNULL(stream_profile_[COLOR]); CHECK_NOTNULL(stream_profile_[COLOR]);
@@ -3505,7 +3504,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
frame_id_[base_stream_]); frame_id_[base_stream_]);
} }
if (enable_stream_[DEPTH] && enable_stream_[COLOR]) { if (enable_stream_[DEPTH] && enable_stream_[COLOR] && enable_publish_extrinsic_) {
static const char *frame_id = "depth_to_color_extrinsics"; static const char *frame_id = "depth_to_color_extrinsics";
OBExtrinsic ex; OBExtrinsic ex;
try { try {
@@ -3521,7 +3520,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
depth_to_other_extrinsics_publishers_[COLOR]->publish(ex_msg); depth_to_other_extrinsics_publishers_[COLOR]->publish(ex_msg);
} }
if (enable_stream_[DEPTH] && enable_stream_[INFRA0]) { if (enable_stream_[DEPTH] && enable_stream_[INFRA0] && enable_publish_extrinsic_) {
static const char *frame_id = "depth_to_ir_extrinsics"; static const char *frame_id = "depth_to_ir_extrinsics";
OBExtrinsic ex; OBExtrinsic ex;
try { try {
@@ -3536,7 +3535,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA0]); CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA0]);
depth_to_other_extrinsics_publishers_[INFRA0]->publish(ex_msg); depth_to_other_extrinsics_publishers_[INFRA0]->publish(ex_msg);
} }
if (enable_stream_[DEPTH] && enable_stream_[INFRA1]) { if (enable_stream_[DEPTH] && enable_stream_[INFRA1] && enable_publish_extrinsic_) {
static const char *frame_id = "depth_to_left_ir_extrinsics"; static const char *frame_id = "depth_to_left_ir_extrinsics";
OBExtrinsic ex; OBExtrinsic ex;
try { try {
@@ -3551,7 +3550,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA1]); CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA1]);
depth_to_other_extrinsics_publishers_[INFRA1]->publish(ex_msg); depth_to_other_extrinsics_publishers_[INFRA1]->publish(ex_msg);
} }
if (enable_stream_[DEPTH] && enable_stream_[INFRA2]) { if (enable_stream_[DEPTH] && enable_stream_[INFRA2] && enable_publish_extrinsic_) {
static const char *frame_id = "depth_to_right_ir_extrinsics"; static const char *frame_id = "depth_to_right_ir_extrinsics";
OBExtrinsic ex; OBExtrinsic ex;
try { try {
@@ -3567,7 +3566,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA2]); CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA2]);
depth_to_other_extrinsics_publishers_[INFRA2]->publish(ex_msg); depth_to_other_extrinsics_publishers_[INFRA2]->publish(ex_msg);
} }
if (enable_stream_[DEPTH] && enable_stream_[ACCEL]) { if (enable_stream_[DEPTH] && enable_stream_[ACCEL] && enable_publish_extrinsic_) {
static const char *frame_id = "depth_to_accel_extrinsics"; static const char *frame_id = "depth_to_accel_extrinsics";
OBExtrinsic ex; OBExtrinsic ex;
try { try {
@@ -3582,7 +3581,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[ACCEL]); CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[ACCEL]);
depth_to_other_extrinsics_publishers_[ACCEL]->publish(ex_msg); depth_to_other_extrinsics_publishers_[ACCEL]->publish(ex_msg);
} }
if (enable_stream_[DEPTH] && enable_stream_[GYRO]) { if (enable_stream_[DEPTH] && enable_stream_[GYRO] && enable_publish_extrinsic_) {
static const char *frame_id = "depth_to_gyro_extrinsics"; static const char *frame_id = "depth_to_gyro_extrinsics";
OBExtrinsic ex; OBExtrinsic ex;
try { try {