From 3b5a4ed675038e407d7cd2b973f821b49c742f31 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 18 Oct 2025 13:59:55 -0700 Subject: [PATCH] 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 --- rtabmap_conversions/src/MsgConversion.cpp | 2 +- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 4 + rtabmap_slam/src/CoreWrapper.cpp | 43 ++-- .../include/rtabmap_sync/rgb_sync.hpp | 1 + rtabmap_sync/src/CommonDataSubscriber.cpp | 8 +- rtabmap_sync/src/nodelets/rgb_sync.cpp | 22 ++ rtabmap_viz/include/rtabmap_viz/GuiWrapper.h | 14 -- rtabmap_viz/src/GuiWrapper.cpp | 208 ++---------------- 8 files changed, 82 insertions(+), 220 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 6eaf84ea..5a69ce00 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1997,7 +1997,7 @@ rtabmap::Transform getTransform( } catch(tf2::TransformException & ex) { - UWARN("(getting transform %s -> %s) %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), ex.what(), waitForTransform); + UWARN("(getting transform \"%s\" -> \"%s\") %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), ex.what(), waitForTransform); } return transform; diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 47b1e171..a2614e09 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -432,8 +432,10 @@ void RGBDOdometry::commonCallback( { UASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size()); rclcpp::Time higherStamp; + UASSERT_MSG(rgbImages[0], "RGB image is null!"); int imageWidth = rgbImages[0]->image.cols; int imageHeight = rgbImages[0]->image.rows; + UASSERT_MSG(depthImages[0], "Depth image is null!"); int depthWidth = depthImages[0]->image.cols; int depthHeight = depthImages[0]->image.rows; @@ -447,6 +449,8 @@ void RGBDOdometry::commonCallback( std::vector cameraModels; for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 528b6065..55cd356a 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -233,20 +233,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : stereoToDepth_ = this->declare_parameter("stereo_to_depth", stereoToDepth_); odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_); - RCLCPP_INFO(this->get_logger(), "rtabmap: frame_id = %s", frameId_.c_str()); - if(!odomFrameId_.empty()) - { - RCLCPP_INFO(this->get_logger(), "rtabmap: odom_frame_id = %s", odomFrameId_.c_str()); - } - if(!groundTruthFrameId_.empty()) - { - RCLCPP_INFO(this->get_logger(), "rtabmap: ground_truth_frame_id = %s -> ground_truth_base_frame_id = %s", - groundTruthFrameId_.c_str(), - groundTruthBaseFrameId_.c_str()); - } - RCLCPP_INFO(this->get_logger(), "rtabmap: map_frame_id = %s", mapFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: frame_id = \"%s\"", frameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: odom_frame_id = \"%s\"", odomFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: ground_truth_frame_id = \"%s\" -> ground_truth_base_frame_id = \"%s\"", + groundTruthFrameId_.c_str(), + groundTruthBaseFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: map_frame_id = \"%s\"", mapFrameId_.c_str()); RCLCPP_INFO(this->get_logger(), "rtabmap: log_to_rosout_level = %d", eventLevel); - RCLCPP_INFO(this->get_logger(), "rtabmap: initial_pose = %s", initialPoseStr.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: initial_pose = \"%s\"", initialPoseStr.c_str()); RCLCPP_INFO(this->get_logger(), "rtabmap: use_action_for_goal = %s", useActionForGoal_?"true":"false"); RCLCPP_INFO(this->get_logger(), "rtabmap: tf_delay = %f", tfDelay); RCLCPP_INFO(this->get_logger(), "rtabmap: tf_tolerance = %f", tfTolerance); @@ -840,6 +834,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : rtabmap_.parseParameters(parameters_); } } + + if(!this->isSubscribedToOdom() && odomFrameId_.empty()) + { + bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str()); + if(isRGBD) { + RCLCPP_ERROR(this->get_logger(), "\"subscribe_odom\" or \"odom_frame_id\" should be used when \"%s\" is enabled!", Parameters::kRGBDEnabled().c_str()); + } + } // Set initial pose if set if(!initialPoseStr.empty()) @@ -1356,6 +1358,11 @@ void CoreWrapper::commonMultiCameraCallback( mapToOdomMutex_.lock(); odomFrameId = odomFrameId_; mapToOdomMutex_.unlock(); + if(odomFrameId.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } if(!scan2dMsg.ranges.empty()) { if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) @@ -1742,6 +1749,11 @@ void CoreWrapper::commonLaserScanCallback( mapToOdomMutex_.lock(); odomFrameId = odomFrameId_; mapToOdomMutex_.unlock(); + if(odomFrameId.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } if(!scan2dMsg.ranges.empty()) { if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) @@ -1951,6 +1963,11 @@ void CoreWrapper::commonSensorDataCallback( mapToOdomMutex_.lock(); odomFrameId = odomFrameId_; mapToOdomMutex_.unlock(); + if(odomFrameId.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } if(!odomTFUpdate(odomFrameId, sensorDataMsg->header.stamp)) { return; diff --git a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp index 204d7370..571ef3a1 100644 --- a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp @@ -58,6 +58,7 @@ public: private: double compressedRate_; + bool fillEmptyDepth_; rclcpp::Time lastCompressedPublished_; diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index f671b439..c8cbfb06 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -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( diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index 5498135d..a7f7d740 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -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("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImageCompressedPub_ = this->create_publisher("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); } diff --git a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h index f263871d..4d7d90c5 100644 --- a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h +++ b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h @@ -89,20 +89,6 @@ private: const std::vector > & localKeyPoints = std::vector >(), const std::vector > & localPoints3d = std::vector >(), const std::vector & localDescriptors = std::vector()); - virtual void commonStereoCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, - const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, - const cv_bridge::CvImageConstPtr& leftImageMsg, - const cv_bridge::CvImageConstPtr& rightImageMsg, - const sensor_msgs::msg::CameraInfo& leftCamInfoMsg, - const sensor_msgs::msg::CameraInfo& rightCamInfoMsg, - const sensor_msgs::msg::LaserScan & scan2dMsg, - const sensor_msgs::msg::PointCloud2 & scan3dMsg, - const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, - const std::vector & globalDescriptorMsgs = std::vector(), - const std::vector & localKeyPoints = std::vector(), - const std::vector & localPoints3d = std::vector(), - const cv::Mat & localDescriptors = cv::Mat()); virtual void commonLaserScanCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index e0283d7c..872d8b95 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -590,6 +590,11 @@ void GuiWrapper::commonMultiCameraCallback( } odomHeader.frame_id = odomFrameId_; + if(odomHeader.frame_id.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); if(odomT.isNull()) { @@ -747,197 +752,6 @@ void GuiWrapper::commonMultiCameraCallback( QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); } -void GuiWrapper::commonStereoCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, - const rtabmap_msgs::msg::UserData::ConstSharedPtr &, - const cv_bridge::CvImageConstPtr& leftImageMsg, - const cv_bridge::CvImageConstPtr& rightImageMsg, - const sensor_msgs::msg::CameraInfo& leftCamInfoMsg, - const sensor_msgs::msg::CameraInfo& rightCamInfoMsg, - const sensor_msgs::msg::LaserScan & scan2dMsg, - const sensor_msgs::msg::PointCloud2 & scan3dMsg, - const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, - const std::vector &, - const std::vector &, - const std::vector &, - const cv::Mat &) -{ - std_msgs::msg::Header odomHeader; - std::string frameId = frameId_; - Transform odomT; - if(odomMsg.get()) - { - odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); - odomHeader = odomMsg->header; - if(!odomMsg->child_frame_id.empty()) - { - frameId = odomMsg->child_frame_id; - } - else - { - RCLCPP_WARN(get_logger(), "Received odom topic with child_frame_id not set! Using \"%s\" as base frame.", frameId_.c_str()); - } - } - else - { - if(!scan2dMsg.ranges.empty()) - { - odomHeader = scan2dMsg.header; - } - else if(!scan3dMsg.data.empty()) - { - odomHeader = scan3dMsg.header; - } - else - { - odomHeader = leftCamInfoMsg.header; - } - odomHeader.frame_id = odomFrameId_; - - odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); - if(odomT.isNull()) - { - RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from " - "TF for stamp %f, aborting! To show red screen in rtabmap_viz " - "when this happens (indicating potentially lost), set subscribe_odom " - "to true.", rclcpp::Time(odomHeader.stamp).seconds()); - return; - } - } - - cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); - if(odomMsg.get()) - { - UASSERT(odomMsg->twist.covariance.size() == 36); - if(odomMsg->twist.covariance[0] != 0 && - odomMsg->twist.covariance[7] != 0 && - odomMsg->twist.covariance[14] != 0 && - odomMsg->twist.covariance[21] != 0 && - odomMsg->twist.covariance[28] != 0 && - odomMsg->twist.covariance[35] != 0) - { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone(); - } - } - else if(odomInfoMsg.get() && odomInfoMsg->covariance.size() == 36) - { - if(odomInfoMsg->covariance[0] != 0 && - odomInfoMsg->covariance[7] != 0 && - odomInfoMsg->covariance[14] != 0 && - odomInfoMsg->covariance[21] != 0 && - odomInfoMsg->covariance[28] != 0 && - odomInfoMsg->covariance[35] != 0) - { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomInfoMsg->covariance.data()).clone(); - } - } - if(odomHeader.frame_id.empty()) - { - RCLCPP_ERROR(this->get_logger(), "Odometry frame not set!?"); - return; - } - - cv::Mat left; - cv::Mat right; - LaserScan scan; - rtabmap::StereoCameraModel stereoModel; - rtabmap::OdometryInfo info; - bool ignoreData = false; - - // limit update rate - if(maxOdomUpdateRate_<=0.0 || - (UTimer::now() - lastOdomInfoUpdateTime_ > 1.0/maxOdomUpdateRate_ && - !mainWindow_->isProcessingOdometry() && - !mainWindow_->isProcessingStatistics())) - { - lastOdomInfoUpdateTime_ = UTimer::now(); - - ParametersMap allParameters = prefDialog_->getAllParameters(); - bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified(); - Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified); - - if(!rtabmap_conversions::convertStereoMsg( - leftImageMsg, - rightImageMsg, - leftCamInfoMsg, - rightCamInfoMsg, - frameId, - odomSensorSync_?odomHeader.frame_id:"", - odomHeader.stamp, - left, - right, - stereoModel, - *tfBuffer_, - waitForTransform_, - imagesAlreadyRectified)) - { - RCLCPP_ERROR(this->get_logger(), "Could not convert stereo msgs! Aborting rtabmap_viz update..."); - return; - } - - if(!scan2dMsg.ranges.empty()) - { - if(!rtabmap_conversions::convertScanMsg( - scan2dMsg, - frameId, - odomSensorSync_?odomHeader.frame_id:"", - odomHeader.stamp, - scan, - *tfBuffer_, - waitForTransform_)) - { - RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap_viz update..."); - return; - } - } - else if(!scan3dMsg.data.empty()) - { - if(!rtabmap_conversions::convertScan3dMsg( - scan3dMsg, - frameId, - odomSensorSync_?odomHeader.frame_id:"", - odomHeader.stamp, - scan, - *tfBuffer_, - waitForTransform_)) - { - RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap_viz update..."); - return; - } - } - - if(odomInfoMsg.get()) - { - info = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); - } - ignoreData = false; - } - else if(odomInfoMsg.get()) - { - info = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg).copyWithoutData(); - ignoreData = true; - } - else - { - // don't update GUI odom stuff if we don't use visual odometry - return; - } - - info.reg.covariance = covariance; - rtabmap::OdometryEvent odomEvent( - rtabmap::SensorData( - scan, - left, - right, - stereoModel, - 0, - rtabmap_conversions::timestampFromROS(odomHeader.stamp)), - odomT, - info); - - QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); -} - void GuiWrapper::commonLaserScanCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr &, @@ -978,6 +792,12 @@ void GuiWrapper::commonLaserScanCallback( } odomHeader.frame_id = odomFrameId_; + if(odomHeader.frame_id.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); if(odomT.isNull()) { @@ -1194,6 +1014,12 @@ void GuiWrapper::commonSensorDataCallback( odomHeader = sensorDataMsg->header; odomHeader.frame_id = odomFrameId_; + if(odomHeader.frame_id.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); if(odomT.isNull()) {