From 3927ec067a9d98cc008fab47c4f7c32bb87d2ef0 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Sat, 10 Aug 2024 15:44:54 -0400 Subject: [PATCH] rename depth/image_raw to depth_raw/image (#140) Without this change, both depth/image and depth/image_raw publish the same camera_info topic (depth/camera_info) - this has several issues: * subscribing to either image causes both to be published along with their respective camera_info * remapping either camera_info, remaps both * In Jazzy, it appears that the double camera_info also causes minor issues with message_filters in downstream packages --- openni2_camera/src/openni2_driver.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/openni2_camera/src/openni2_driver.cpp b/openni2_camera/src/openni2_driver.cpp index 71f98cc..61a577f 100644 --- a/openni2_camera/src/openni2_driver.cpp +++ b/openni2_camera/src/openni2_driver.cpp @@ -194,7 +194,7 @@ void OpenNI2Driver::advertiseROSTopics() }; rmw_qos_profile_t custom_qos = rmw_qos_profile_default; custom_qos.depth = 1; - pub_depth_raw_ = image_transport::create_camera_publisher(this, "depth/image_raw", custom_qos, pub_options); + pub_depth_raw_ = image_transport::create_camera_publisher(this, "depth_raw/image", custom_qos, pub_options); pub_depth_ = image_transport::create_camera_publisher(this, "depth/image", custom_qos, pub_options); pub_projector_info_ = this->create_publisher("projector/camera_info", 1); } @@ -398,7 +398,6 @@ void OpenNI2Driver::colorConnectCb() } std::lock_guard lock(connect_mutex_); - // This does not appear to work color_subscribers_ = pub_color_.getNumSubscribers() > 0; if (color_subscribers_ && !device_->isColorStreamStarted()) @@ -451,7 +450,6 @@ void OpenNI2Driver::depthConnectCb() } std::lock_guard lock(connect_mutex_); - // These does not appear to work depth_subscribers_ = pub_depth_.getNumSubscribers() > 0; depth_raw_subscribers_ = pub_depth_raw_.getNumSubscribers() > 0; projector_info_subscribers_ = pub_projector_info_->get_subscription_count() > 0; @@ -481,8 +479,6 @@ void OpenNI2Driver::irConnectCb() } std::lock_guard lock(connect_mutex_); - // This does not appear to work - // ir_subscribers_ = pub_ir_.getNumSubscribers() > 0; ir_subscribers_ = this->count_subscribers("ir/image") > 0 || this->count_subscribers("ir/camera_info") > 0;