Skip to content

Commit

Permalink
rename depth/image_raw to depth_raw/image
Browse files Browse the repository at this point in the history
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
  • Loading branch information
mikeferguson committed Aug 7, 2024
1 parent 215a85c commit b9b4743
Showing 1 changed file with 2 additions and 5 deletions.
7 changes: 2 additions & 5 deletions openni2_camera/src/openni2_driver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -194,7 +194,8 @@ 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<sensor_msgs::msg::CameraInfo>("projector/camera_info", 1);
}
Expand Down Expand Up @@ -398,7 +399,6 @@ void OpenNI2Driver::colorConnectCb()
}
std::lock_guard<std::mutex> lock(connect_mutex_);

// This does not appear to work
color_subscribers_ = pub_color_.getNumSubscribers() > 0;

if (color_subscribers_ && !device_->isColorStreamStarted())
Expand Down Expand Up @@ -451,7 +451,6 @@ void OpenNI2Driver::depthConnectCb()
}
std::lock_guard<std::mutex> 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;
Expand Down Expand Up @@ -481,8 +480,6 @@ void OpenNI2Driver::irConnectCb()
}
std::lock_guard<std::mutex> 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;

Expand Down

0 comments on commit b9b4743

Please sign in to comment.