i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-10 11:39:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added error messages when subscribe_odom and odom_frame_id are both not set. rgb_sync: added option "fill_empty_depth" to add fake empty depth for monocular cameras. See also #1363

Browse Source
This commit is contained in:
matlabbe
2025-10-18 13:59:55 -07:00
parent bc5f0a7146
commit 3b5a4ed675
8 changed files with 82 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_sync/src/CommonDataSubscriber.cpp
+7 -1
View File
@@ -531,6 +531,13 @@ void CommonDataSubscriber::setupCallbacks(
RCLCPP_INFO(node.get_logger(), "%s: subscribe_stereo = %s", name_.c_str(), subscribedToStereo_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s: subscribe_rgbd = %s (rgbd_cameras=%d)", name_.c_str(), subscribedToRGBD_?"true":"false", rgbdCameras_);
RCLCPP_INFO(node.get_logger(), "%s: subscribe_sensor_data = %s", name_.c_str(), subscribedToSensorData_?"true":"false");
if(subscribedToOdom_ && !odomFrameId_.empty()) {
RCLCPP_INFO(node.get_logger(), "%s: subscribe_odom = false (\"odom_frame_id\" is set)", name_.c_str());
subscribedToOdom_ = false;
}
else {
RCLCPP_INFO(node.get_logger(), "%s: subscribe_odom = %s", name_.c_str(), subscribedToOdom_?"true":"false");
}
RCLCPP_INFO(node.get_logger(), "%s: subscribe_odom_info = %s", name_.c_str(), subscribedToOdomInfo_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s: subscribe_user_data = %s", name_.c_str(), subscribedToUserData_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan = %s", name_.c_str(), subscribedToScan2d_?"true":"false");
@@ -550,7 +557,6 @@ void CommonDataSubscriber::setupCallbacks(
rclcpp::SubscriptionOptions callbackOptions;
callbackOptions.callback_group = syncCallbackGroup_;
subscribedToOdom_ = odomFrameId_.empty() && subscribedToOdom_;
if(subscribedToDepth_)
{
setupDepthCallbacks(
rtabmap_sync/src/nodelets/rgb_sync.cpp
+22
View File
@@ -48,6 +48,7 @@ namespace rtabmap_sync
RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
Node("rgbd_sync", options),
compressedRate_(0),
fillEmptyDepth_(false),
approxSync_(0),
exactSync_(0)
{
@@ -73,6 +74,7 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
int qosCaminfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
std::string imageTransport = this->declare_parameter("image_transport", std::string("raw"));
fillEmptyDepth_ = this->declare_parameter("fill_empty_depth", fillEmptyDepth_);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
@@ -83,6 +85,7 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCaminfo);
RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s: image_transport = %s", get_name(), imageTransport.c_str());
RCLCPP_INFO(this->get_logger(), "%s: fill_empty_depth = %s", get_name(), fillEmptyDepth_?"true":"false");
rgbdImagePub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
rgbdImageCompressedPub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
@@ -150,6 +153,14 @@ void RGBSync::callback(
msg.header.frame_id = cameraInfo->header.frame_id;
msg.header.stamp = image->header.stamp;
msg.rgb_camera_info = *cameraInfo;
cv_bridge::CvImage fakeDepthImage;
if(fillEmptyDepth_)
{
msg.depth_camera_info = *cameraInfo;
fakeDepthImage.header = image->header;
fakeDepthImage.image = cv::Mat::zeros(image->height, image->width, CV_16UC1);
fakeDepthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
}
if(rgbdImageCompressedPub_->get_subscription_count())
{
@@ -172,6 +183,13 @@ void RGBSync::callback(
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG);
if(fillEmptyDepth_)
{
msgCompressed.depth_compressed.header = image->header;
msgCompressed.depth_compressed.data = rtabmap::compressImage(fakeDepthImage.image, ".png");
msgCompressed.depth_compressed.format = "png";
}
rgbdImageCompressedPub_->publish(msgCompressed);
}
}
@@ -179,6 +197,10 @@ void RGBSync::callback(
if(rgbdImagePub_->get_subscription_count())
{
msg.rgb = *image;
if(fillEmptyDepth_)
{
fakeDepthImage.toImageMsg(msg.depth);
}
rgbdImagePub_->publish(msg);
}