From 64a2656052ae4afd41acb05ab99cea0031d439e2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 21 Nov 2023 14:01:45 -0800 Subject: [PATCH 01/12] Odom: Added 5 and 6 cameras internal sync callbacks for convenience. --- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 169 ++++++++---- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 244 +++++++++++++++++- 2 files changed, 360 insertions(+), 53 deletions(-) diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 1cb190897..f1932c7dc 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -72,6 +72,8 @@ class RGBDOdometry : public OdometryROS exactSync4_(0), approxSync5_(0), exactSync5_(0), + approxSync6_(0), + exactSync6_(0), queueSize_(5), keepColor_(false) { @@ -81,46 +83,18 @@ class RGBDOdometry : public OdometryROS { rgbdSub_.shutdown(); rgbdxSub_.shutdown(); - if(approxSync_) - { - delete approxSync_; - } - if(exactSync_) - { - delete exactSync_; - } - if(approxSync2_) - { - delete approxSync2_; - } - if(exactSync2_) - { - delete exactSync2_; - } - if(approxSync3_) - { - delete approxSync3_; - } - if(exactSync3_) - { - delete exactSync3_; - } - if(approxSync4_) - { - delete approxSync4_; - } - if(exactSync4_) - { - delete exactSync4_; - } - if(approxSync5_) - { - delete approxSync5_; - } - if(exactSync5_) - { - delete exactSync5_; - } + delete approxSync_; + delete exactSync_; + delete approxSync2_; + delete exactSync2_; + delete approxSync3_; + delete exactSync3_; + delete approxSync4_; + delete exactSync4_; + delete approxSync5_; + delete exactSync5_; + delete approxSync6_; + delete exactSync6_; } private: @@ -147,10 +121,6 @@ class RGBDOdometry : public OdometryROS { rgbdCameras = 0; } - if(rgbdCameras > 5) - { - NODELET_FATAL("Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras)."); - } pnh.param("keep_color", keepColor_, keepColor_); NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); @@ -181,6 +151,10 @@ class RGBDOdometry : public OdometryROS { rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1); } + if(rgbdCameras >= 6) + { + rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1); + } if(rgbdCameras == 2) { @@ -308,6 +282,53 @@ class RGBDOdometry : public OdometryROS rgbd_image4_sub_.getTopic().c_str(), rgbd_image5_sub_.getTopic().c_str()); } + else if(rgbdCameras == 6) + { + if(approxSync) + { + approxSync6_ = new message_filters::Synchronizer( + MyApproxSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + if(approxSyncMaxInterval > 0.0) + approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + approxSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + else + { + exactSync6_ = new message_filters::Synchronizer( + MyExactSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + exactSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", + getName().c_str(), + approxSync?"approx":"exact", + approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str(), + rgbd_image3_sub_.getTopic().c_str(), + rgbd_image4_sub_.getTopic().c_str(), + rgbd_image5_sub_.getTopic().c_str(), + rgbd_image6_sub_.getTopic().c_str()); + } + else + { + NODELET_FATAL("%s doesn't support more than 6 cameras (rgbd_cameras=%d) with " + "internal synchronization interface, set rgbd_cameras=0 and use " + "rgbd_images input topic instead for more cameras (for which " + "rgbdx_sync node can sync up to 8 cameras).", + getName().c_str(), rgbdCameras); + } } else if(rgbdCameras == 0) @@ -700,6 +721,35 @@ class RGBDOdometry : public OdometryROS this->commonCallback(imageMsgs, depthMsgs, infoMsgs); } } + void callbackRGBD6( + const rtabmap_msgs::RGBDImageConstPtr& image, + const rtabmap_msgs::RGBDImageConstPtr& image2, + const rtabmap_msgs::RGBDImageConstPtr& image3, + const rtabmap_msgs::RGBDImageConstPtr& image4, + const rtabmap_msgs::RGBDImageConstPtr& image5, + const rtabmap_msgs::RGBDImageConstPtr& image6) + { + if(!this->isPaused()) + { + std::vector imageMsgs(6); + std::vector depthMsgs(6); + std::vector infoMsgs; + rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]); + rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]); + rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]); + rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]); + rtabmap_conversions::toCvShare(image5, imageMsgs[4], depthMsgs[4]); + rtabmap_conversions::toCvShare(image6, imageMsgs[5], depthMsgs[5]); + infoMsgs.push_back(image->rgb_camera_info); + infoMsgs.push_back(image2->rgb_camera_info); + infoMsgs.push_back(image3->rgb_camera_info); + infoMsgs.push_back(image4->rgb_camera_info); + infoMsgs.push_back(image5->rgb_camera_info); + infoMsgs.push_back(image6->rgb_camera_info); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + } + } protected: virtual void flushCallbacks() @@ -801,6 +851,32 @@ class RGBDOdometry : public OdometryROS rgbd_image5_sub_); exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); } + if(approxSync6_) + { + delete approxSync6_; + approxSync6_ = new message_filters::Synchronizer( + MyApproxSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + approxSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + if(exactSync6_) + { + delete exactSync6_; + exactSync6_ = new message_filters::Synchronizer( + MyExactSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + exactSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } } private: @@ -815,6 +891,7 @@ class RGBDOdometry : public OdometryROS message_filters::Subscriber rgbd_image3_sub_; message_filters::Subscriber rgbd_image4_sub_; message_filters::Subscriber rgbd_image5_sub_; + message_filters::Subscriber rgbd_image6_sub_; typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; message_filters::Synchronizer * approxSync_; @@ -836,6 +913,10 @@ class RGBDOdometry : public OdometryROS message_filters::Synchronizer * approxSync5_; typedef message_filters::sync_policies::ExactTime MyExactSync5Policy; message_filters::Synchronizer * exactSync5_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync6Policy; + message_filters::Synchronizer * approxSync6_; + typedef message_filters::sync_policies::ExactTime MyExactSync6Policy; + message_filters::Synchronizer * exactSync6_; int queueSize_; bool keepColor_; }; diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index 6d65f2f91..889de3656 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -70,6 +70,10 @@ class StereoOdometry : public OdometryROS exactSync3_(0), approxSync4_(0), exactSync4_(0), + approxSync5_(0), + exactSync5_(0), + approxSync6_(0), + exactSync6_(0), queueSize_(5), keepColor_(false) { @@ -77,14 +81,20 @@ class StereoOdometry : public OdometryROS virtual ~StereoOdometry() { - if(approxSync_) - { - delete approxSync_; - } - if(exactSync_) - { - delete exactSync_; - } + rgbdSub_.shutdown(); + rgbdxSub_.shutdown(); + delete approxSync_; + delete exactSync_; + delete approxSync2_; + delete exactSync2_; + delete approxSync3_; + delete exactSync3_; + delete approxSync4_; + delete exactSync4_; + delete approxSync5_; + delete exactSync5_; + delete approxSync6_; + delete exactSync6_; } private: @@ -127,6 +137,14 @@ class StereoOdometry : public OdometryROS { rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); } + if(rgbdCameras >= 5) + { + rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1); + } + if(rgbdCameras >= 6) + { + rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1); + } if(rgbdCameras == 2) { @@ -218,9 +236,88 @@ class StereoOdometry : public OdometryROS rgbd_image3_sub_.getTopic().c_str(), rgbd_image4_sub_.getTopic().c_str()); } + else if(rgbdCameras == 5) + { + if(approxSync) + { + approxSync5_ = new message_filters::Synchronizer( + MyApproxSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + if(approxSyncMaxInterval > 0.0) + approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + approxSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); + } + else + { + exactSync5_ = new message_filters::Synchronizer( + MyExactSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + exactSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", + getName().c_str(), + approxSync?"approx":"exact", + approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str(), + rgbd_image3_sub_.getTopic().c_str(), + rgbd_image4_sub_.getTopic().c_str(), + rgbd_image5_sub_.getTopic().c_str()); + } + else if(rgbdCameras == 6) + { + if(approxSync) + { + approxSync6_ = new message_filters::Synchronizer( + MyApproxSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + if(approxSyncMaxInterval > 0.0) + approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + approxSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + else + { + exactSync6_ = new message_filters::Synchronizer( + MyExactSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + exactSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", + getName().c_str(), + approxSync?"approx":"exact", + approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str(), + rgbd_image3_sub_.getTopic().c_str(), + rgbd_image4_sub_.getTopic().c_str(), + rgbd_image5_sub_.getTopic().c_str(), + rgbd_image6_sub_.getTopic().c_str()); + } else { - ROS_FATAL("%s doesn't support more than 4 cameras (rgbd_cameras=%d) with internal synchronization interface, set rgbd_cameras=0 and use rgbd_images input topic instead for more cameras.", getName().c_str(), rgbdCameras); + NODELET_FATAL("%s doesn't support more than 6 cameras (rgbd_cameras=%d) with " + "internal synchronization interface, set rgbd_cameras=0 and use " + "rgbd_images input topic instead for more cameras (for which " + "rgbdx_sync node can sync up to 8 cameras).", + getName().c_str(), rgbdCameras); } } @@ -723,6 +820,76 @@ class StereoOdometry : public OdometryROS } } + void callbackRGBD5( + const rtabmap_msgs::RGBDImageConstPtr& image, + const rtabmap_msgs::RGBDImageConstPtr& image2, + const rtabmap_msgs::RGBDImageConstPtr& image3, + const rtabmap_msgs::RGBDImageConstPtr& image4, + const rtabmap_msgs::RGBDImageConstPtr& image5) + { + if(!this->isPaused()) + { + std::vector leftMsgs(5); + std::vector rightMsgs(5); + std::vector leftInfoMsgs; + std::vector rightInfoMsgs; + rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]); + rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]); + rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]); + rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]); + rtabmap_conversions::toCvShare(image5, leftMsgs[4], rightMsgs[4]); + leftInfoMsgs.push_back(image->rgb_camera_info); + leftInfoMsgs.push_back(image2->rgb_camera_info); + leftInfoMsgs.push_back(image3->rgb_camera_info); + leftInfoMsgs.push_back(image4->rgb_camera_info); + leftInfoMsgs.push_back(image5->rgb_camera_info); + rightInfoMsgs.push_back(image->depth_camera_info); + rightInfoMsgs.push_back(image2->depth_camera_info); + rightInfoMsgs.push_back(image3->depth_camera_info); + rightInfoMsgs.push_back(image4->depth_camera_info); + rightInfoMsgs.push_back(image5->depth_camera_info); + + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs); + } + } + + void callbackRGBD6( + const rtabmap_msgs::RGBDImageConstPtr& image, + const rtabmap_msgs::RGBDImageConstPtr& image2, + const rtabmap_msgs::RGBDImageConstPtr& image3, + const rtabmap_msgs::RGBDImageConstPtr& image4, + const rtabmap_msgs::RGBDImageConstPtr& image5, + const rtabmap_msgs::RGBDImageConstPtr& image6) + { + if(!this->isPaused()) + { + std::vector leftMsgs(6); + std::vector rightMsgs(6); + std::vector leftInfoMsgs; + std::vector rightInfoMsgs; + rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]); + rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]); + rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]); + rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]); + rtabmap_conversions::toCvShare(image5, leftMsgs[4], rightMsgs[4]); + rtabmap_conversions::toCvShare(image6, leftMsgs[5], rightMsgs[5]); + leftInfoMsgs.push_back(image->rgb_camera_info); + leftInfoMsgs.push_back(image2->rgb_camera_info); + leftInfoMsgs.push_back(image3->rgb_camera_info); + leftInfoMsgs.push_back(image4->rgb_camera_info); + leftInfoMsgs.push_back(image5->rgb_camera_info); + leftInfoMsgs.push_back(image6->rgb_camera_info); + rightInfoMsgs.push_back(image->depth_camera_info); + rightInfoMsgs.push_back(image2->depth_camera_info); + rightInfoMsgs.push_back(image3->depth_camera_info); + rightInfoMsgs.push_back(image4->depth_camera_info); + rightInfoMsgs.push_back(image5->depth_camera_info); + rightInfoMsgs.push_back(image6->depth_camera_info); + + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs); + } + } + protected: virtual void flushCallbacks() { @@ -799,6 +966,56 @@ class StereoOdometry : public OdometryROS rgbd_image4_sub_); exactSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } + if(approxSync5_) + { + delete approxSync5_; + approxSync5_ = new message_filters::Synchronizer( + MyApproxSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + approxSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); + } + if(exactSync5_) + { + delete exactSync5_; + exactSync5_ = new message_filters::Synchronizer( + MyExactSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + exactSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); + } + if(approxSync6_) + { + delete approxSync6_; + approxSync6_ = new message_filters::Synchronizer( + MyApproxSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + approxSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + if(exactSync6_) + { + delete exactSync6_; + exactSync6_ = new message_filters::Synchronizer( + MyExactSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + exactSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } } private: @@ -814,6 +1031,7 @@ class StereoOdometry : public OdometryROS message_filters::Subscriber rgbd_image3_sub_; message_filters::Subscriber rgbd_image4_sub_; message_filters::Subscriber rgbd_image5_sub_; + message_filters::Subscriber rgbd_image6_sub_; typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; message_filters::Synchronizer * approxSync_; @@ -831,6 +1049,14 @@ class StereoOdometry : public OdometryROS message_filters::Synchronizer * approxSync4_; typedef message_filters::sync_policies::ExactTime MyExactSync4Policy; message_filters::Synchronizer * exactSync4_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync5Policy; + message_filters::Synchronizer * approxSync5_; + typedef message_filters::sync_policies::ExactTime MyExactSync5Policy; + message_filters::Synchronizer * exactSync5_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync6Policy; + message_filters::Synchronizer * approxSync6_; + typedef message_filters::sync_policies::ExactTime MyExactSync6Policy; + message_filters::Synchronizer * exactSync6_; int queueSize_; bool keepColor_; From 8129ace10a9926a9ac9a1a498eb1d2630313e324 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 26 Nov 2023 14:36:08 -0800 Subject: [PATCH 02/12] Added republish_tf_static.py for convenience when replaying rosbags with /tf_static not latched --- rtabmap_util/CMakeLists.txt | 1 + rtabmap_util/scripts/republish_tf_static.py | 20 ++++++++++++++++++++ 2 files changed, 21 insertions(+) create mode 100755 rtabmap_util/scripts/republish_tf_static.py diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index bb4086469..5a17b9887 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -131,6 +131,7 @@ catkin_install_python(PROGRAMS scripts/yaml_to_camera_info.py scripts/netvlad_tf_ros.py scripts/gazebo_ground_truth.py + scripts/republish_tf_static.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/rtabmap_util/scripts/republish_tf_static.py b/rtabmap_util/scripts/republish_tf_static.py new file mode 100755 index 000000000..13874cae6 --- /dev/null +++ b/rtabmap_util/scripts/republish_tf_static.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python +import rospy +from tf2_msgs.msg import TFMessage + +msg=TFMessage() + +def callback(data): + global msg + if len(msg.transforms) == 0: + msg = data + else: + msg.transforms = msg.transforms+ data.transforms + rospy.loginfo("Received /tf_static_old and republising latched /tf_static") + pub.publish(msg) + +if __name__ == '__main__': + rospy.init_node('listener', anonymous=True) + rospy.Subscriber("tf_static_old", TFMessage, callback) + pub = rospy.Publisher('tf_static', TFMessage, queue_size=10, latch=True) + rospy.spin() From e3452e1962c395ac2098a31af516e25347670b7e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 26 Nov 2023 19:23:37 -0800 Subject: [PATCH 03/12] icp_odom: Fixed scan without stamps field after projecting to point cloud before deskewing --- rtabmap_conversions/src/MsgConversion.cpp | 6 ++++++ rtabmap_odom/src/nodelets/icp_odometry.cpp | 1 + 2 files changed, 7 insertions(+) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 3b586c387..33236c93d 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -2853,6 +2853,12 @@ bool deskew_impl( if(offsetTime < 0) { ROS_ERROR("Input cloud doesn't have \"t\", \"time\", \"stamps\" or \"timestamp\" field!"); + std::string fieldsReceived; + for(size_t i=0; itfListener(), + -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp); if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull()) From 0748b91accf9bd173645f321a4ff498caf7ba2b1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 9 Dec 2023 15:57:33 -0800 Subject: [PATCH 04/12] Updated osuter example with option to set wait tf duration, increased too low Icp/PMOutlierRatio --- rtabmap_examples/launch/test_ouster_gen2.launch | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rtabmap_examples/launch/test_ouster_gen2.launch b/rtabmap_examples/launch/test_ouster_gen2.launch index 9e1942488..df0918d53 100644 --- a/rtabmap_examples/launch/test_ouster_gen2.launch +++ b/rtabmap_examples/launch/test_ouster_gen2.launch @@ -49,6 +49,7 @@ + @@ -77,11 +78,12 @@ + - + @@ -101,6 +103,7 @@ + @@ -113,7 +116,7 @@ - + From 752e7524f5a43717ef012a7b21594644f178c29e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 17 Dec 2023 22:44:40 -0800 Subject: [PATCH 05/12] GridMap integration (#1082) * GridMap integration * Updated with upstream changes * updated with upstream changes * some fixes --- rtabmap_slam/src/CoreWrapper.cpp | 3 +- rtabmap_util/CMakeLists.txt | 46 ++- .../include/rtabmap_util/MapsManager.h | 14 +- rtabmap_util/package.xml | 1 + rtabmap_util/src/MapsManager.cpp | 281 ++++++++---------- .../src/nodelets/obstacles_detection.cpp | 8 +- 6 files changed, 171 insertions(+), 182 deletions(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 0c565782e..9f09ab599 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -60,6 +60,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #ifdef WITH_OCTOMAP_MSGS @@ -2140,7 +2141,7 @@ void CoreWrapper::process( rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first || rtabmap_.getMemory()->getLastWorkingSignature() == 0 || rtabmap_.getMemory()->getLastWorkingSignature()->sensorData().gridCellSize() == 0 || - (!mapsManager_.getOccupancyGrid()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data + (!mapsManager_.getLocalMapMaker()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data { SensorData tmpData = data; tmpData.setId(0); diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 5a17b9887..f7aca0e1b 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -9,11 +9,15 @@ find_package(catkin REQUIRED COMPONENTS # Optional components find_package(octomap_msgs) +find_package(grid_map_ros) SET(optional_dependencies "") IF(octomap_msgs_FOUND) SET(optional_dependencies ${optional_dependencies} octomap_msgs) ENDIF(octomap_msgs_FOUND) +IF(grid_map_ros_FOUND) + SET(optional_dependencies ${optional_dependencies} grid_map_ros) +ENDIF(grid_map_ros_FOUND) catkin_package( INCLUDE_DIRS include @@ -32,6 +36,11 @@ include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/include ${catkin_INCLUDE_DIRS} ) + +# libraries +SET(Libraries + ${catkin_LIBRARIES} +) SET(rtabmap_util_plugins_lib_src src/MapsManager.cpp @@ -65,6 +74,19 @@ SET(Libraries ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS") ENDIF(octomap_msgs_FOUND) +# If grid_map is found, add definition +IF(grid_map_ros_FOUND) +MESSAGE(STATUS "WITH grid_map_ros") +include_directories( + ${grid_map_ros_INCLUDE_DIRS} +) +SET(Libraries + ${grid_map_ros_LIBRARIES} + ${Libraries} +) +ADD_DEFINITIONS("-DWITH_GRID_MAP_ROS") +ENDIF(grid_map_ros_FOUND) + ############################ ## Declare a cpp library ############################ @@ -72,51 +94,51 @@ add_library(rtabmap_util_plugins ${rtabmap_util_plugins_lib_src} ) target_link_libraries(rtabmap_util_plugins - ${catkin_LIBRARIES} + ${Libraries} ) add_executable(rtabmap_rgbd_relay src/RGBDRelayNode.cpp) -target_link_libraries(rtabmap_rgbd_relay ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_rgbd_relay ${Libraries}) set_target_properties(rtabmap_rgbd_relay PROPERTIES OUTPUT_NAME "rgbd_relay") add_executable(rtabmap_rgbd_split src/RGBDSplitNode.cpp) -target_link_libraries(rtabmap_rgbd_split ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_rgbd_split ${Libraries}) set_target_properties(rtabmap_rgbd_split PROPERTIES OUTPUT_NAME "rgbd_split") add_executable(rtabmap_map_optimizer src/MapOptimizerNode.cpp) -target_link_libraries(rtabmap_map_optimizer ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_map_optimizer ${Libraries}) set_target_properties(rtabmap_map_optimizer PROPERTIES OUTPUT_NAME "map_optimizer") add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp) -target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins ${Libraries}) set_target_properties(rtabmap_map_assembler PROPERTIES OUTPUT_NAME "map_assembler") add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp) -target_link_libraries(rtabmap_imu_to_tf ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_imu_to_tf ${Libraries}) set_target_properties(rtabmap_imu_to_tf PROPERTIES OUTPUT_NAME "imu_to_tf") add_executable(rtabmap_lidar_deskewing src/LidarDeskewingNode.cpp) -target_link_libraries(rtabmap_lidar_deskewing ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_lidar_deskewing ${Libraries}) set_target_properties(rtabmap_lidar_deskewing PROPERTIES OUTPUT_NAME "lidar_deskewing") add_executable(rtabmap_data_player src/DbPlayerNode.cpp) -target_link_libraries(rtabmap_data_player ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_data_player ${Libraries}) set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player") add_executable(rtabmap_odom_msg_to_tf src/OdomMsgToTFNode.cpp) -target_link_libraries(rtabmap_odom_msg_to_tf ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_odom_msg_to_tf ${Libraries}) set_target_properties(rtabmap_odom_msg_to_tf PROPERTIES OUTPUT_NAME "odom_msg_to_tf") add_executable(rtabmap_pointcloud_to_depthimage src/PointCloudToDepthImageNode.cpp) -target_link_libraries(rtabmap_pointcloud_to_depthimage ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_pointcloud_to_depthimage ${Libraries}) set_target_properties(rtabmap_pointcloud_to_depthimage PROPERTIES OUTPUT_NAME "pointcloud_to_depthimage") add_executable(rtabmap_point_cloud_aggregator src/PointCloudAggregatorNode.cpp) -target_link_libraries(rtabmap_point_cloud_aggregator ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_point_cloud_aggregator ${Libraries}) set_target_properties(rtabmap_point_cloud_aggregator PROPERTIES OUTPUT_NAME "point_cloud_aggregator") add_executable(rtabmap_point_cloud_assembler src/PointCloudAssemblerNode.cpp) -target_link_libraries(rtabmap_point_cloud_assembler ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_point_cloud_assembler ${Libraries}) set_target_properties(rtabmap_point_cloud_assembler PROPERTIES OUTPUT_NAME "point_cloud_assembler") ############# diff --git a/rtabmap_util/include/rtabmap_util/MapsManager.h b/rtabmap_util/include/rtabmap_util/MapsManager.h index c7150e1ad..311ca24c0 100644 --- a/rtabmap_util/include/rtabmap_util/MapsManager.h +++ b/rtabmap_util/include/rtabmap_util/MapsManager.h @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -40,6 +41,8 @@ namespace rtabmap { class OctoMap; class Memory; class OccupancyGrid; +class LocalGridMaker; +class GridMap; } // namespace rtabmap @@ -85,6 +88,7 @@ class MapsManager { const rtabmap::OctoMap * getOctomap() const {return octomap_;} const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;} + const rtabmap::LocalGridMaker * getLocalMapMaker() const {return localMapMaker_;} private: // mapping stuff @@ -112,6 +116,7 @@ class MapsManager { ros::Publisher octoMapObstacleCloud_; ros::Publisher octoMapEmptySpace_; ros::Publisher octoMapProj_; + ros::Publisher elevationMapPub_; std::map assembledGroundPoses_; std::map assembledObstaclePoses_; @@ -122,18 +127,19 @@ class MapsManager { std::map::Ptr > groundClouds_; std::map::Ptr > obstacleClouds_; - std::map gridPoses_; - cv::Mat gridMap_; - std::map, cv::Mat> > gridMaps_; // < , empty cells > - std::map gridMapsViewpoints_; + rtabmap::LocalGridCache localMaps_; rtabmap::OccupancyGrid * occupancyGrid_; + rtabmap::LocalGridMaker * localMapMaker_; bool gridUpdated_; rtabmap::OctoMap * octomap_; int octomapTreeDepth_; bool octomapUpdated_; + rtabmap::GridMap * elevationMap_; + bool elevationMapUpdated_; + rtabmap::ParametersMap parameters_; bool latching_; diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index cf55452b8..4eed22a82 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -28,6 +28,7 @@ pluginlib rtabmap_msgs rtabmap_conversions + grid_map_ros diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 1e9e63d4f..3b82b4340 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -38,20 +38,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include - #include #include #include #include +#include -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) #include #include #include #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) +#include +#include #endif using namespace rtabmap; @@ -69,15 +72,22 @@ MapsManager::MapsManager() : scanEmptyRayTracing_(true), assembledObstacles_(new pcl::PointCloud), assembledGround_(new pcl::PointCloud), - occupancyGrid_(new OccupancyGrid), + occupancyGrid_(new OccupancyGrid(&localMaps_)), + localMapMaker_(new LocalGridMaker), gridUpdated_(true), -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - octomap_(new OctoMap), -#endif +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) + octomap_(new OctoMap(&localMaps_)), +#else + octomap_(0), #endif octomapTreeDepth_(16), octomapUpdated_(true), +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + elevationMap_(new GridMap(&localMaps_)), +#else + elevationMap_(0), +#endif + elevationMapUpdated_(true), latching_(true) { } @@ -135,8 +145,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s ROS_INFO("%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false"); ROS_INFO("%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_); -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_); if(octomapTreeDepth_ > 16) { @@ -149,7 +158,6 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s octomapTreeDepth_ = 16; } ROS_INFO("%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_); -#endif #endif // If true, the last message published on @@ -185,8 +193,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s scanMapPub_ = nht->advertise("scan_map", 1, latching_); latched_.insert(std::make_pair((void*)&scanMapPub_, false)); -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) octoMapPubBin_ = nht->advertise("octomap_binary", 1, latching_); latched_.insert(std::make_pair((void*)&octoMapPubBin_, false)); octoMapPubFull_ = nht->advertise("octomap_full", 1, latching_); @@ -204,6 +211,10 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s octoMapProj_ = nht->advertise("octomap_grid", 1, latching_); latched_.insert(std::make_pair((void*)&octoMapProj_, false)); #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + elevationMapPub_ = nht->advertise("elevation_map", 1, latching_); + latched_.insert(std::make_pair((void*)&elevationMapPub_, false)); #endif } @@ -211,15 +222,14 @@ MapsManager::~MapsManager() { clear(); delete occupancyGrid_; + delete localMapMaker_; -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - if(octomap_) - { - delete octomap_; - octomap_ = 0; - } +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) + delete octomap_; #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + delete elevationMap_; #endif } @@ -306,33 +316,32 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet parameterMoved(pnh, "proj_map_frame", Parameters::kGridMapFrameProjection(), parameters); parameterMoved(pnh, "grid_unknown_space_filled", Parameters::kGridScan2dUnknownSpaceFilled(), parameters); parameterMoved(pnh, "grid_cell_size", Parameters::kGridCellSize(), parameters); - parameterMoved(pnh, "grid_incremental", Parameters::kGridGlobalFullUpdate(), parameters); parameterMoved(pnh, "grid_size", Parameters::kGridGlobalMinSize(), parameters); parameterMoved(pnh, "grid_eroded", Parameters::kGridGlobalEroded(), parameters); parameterMoved(pnh, "grid_footprint_radius", Parameters::kGridGlobalFootprintRadius(), parameters); -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) parameterMoved(pnh, "octomap_ground_is_obstacle", Parameters::kGridGroundIsObstacle(), parameters); parameterMoved(pnh, "octomap_occupancy_thr", Parameters::kGridGlobalOccupancyThr(), parameters); #endif -#endif } void MapsManager::setParameters(const rtabmap::ParametersMap & parameters) { parameters_ = parameters; - occupancyGrid_->parseParameters(parameters_); + delete occupancyGrid_; + occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_); + + localMapMaker_->parseParameters(parameters_); -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - if(octomap_) - { - delete octomap_; - octomap_ = 0; - } - octomap_ = new OctoMap(parameters_); +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) + delete octomap_; + octomap_ = new OctoMap(&localMaps_, parameters_); #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + delete elevationMap_; + elevationMap_ = new GridMap(&localMaps_, parameters_); #endif } @@ -350,8 +359,8 @@ void MapsManager::set2DMap( { for(std::map::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter) { - std::map, cv::Mat> >::iterator jter = gridMaps_.find(iter->first); - if(!uContains(gridMaps_, iter->first)) + std::map::const_iterator jter = localMaps_.find(iter->first); + if(!uContains(localMaps_.localGrids(), iter->first)) { rtabmap::SensorData data; data = memory->getNodeData(iter->first, false, false, false, true); @@ -371,23 +380,16 @@ void MapsManager::set2DMap( &obstacles, &emptyCells); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(gridMapsViewpoints_, std::make_pair(iter->first, data.gridViewPoint())); - occupancyGrid_->addToCache(iter->first, ground, obstacles, emptyCells); + localMaps_.add(iter->first, ground, obstacles, emptyCells, data.gridCellSize(), data.gridViewPoint()); } } - else - { - occupancyGrid_->addToCache(iter->first, jter->second.first.first, jter->second.first.second, jter->second.second); - } } } } void MapsManager::clear() { - gridMaps_.clear(); - gridMapsViewpoints_.clear(); + localMaps_.clear(); assembledGround_->clear(); assembledObstacles_->clear(); assembledGroundPoses_.clear(); @@ -397,10 +399,11 @@ void MapsManager::clear() groundClouds_.clear(); obstacleClouds_.clear(); occupancyGrid_->clear(); -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) octomap_->clear(); #endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + elevationMap_->clear(); #endif for(std::map::iterator iter=latched_.begin(); iter!=latched_.end(); ++iter) { @@ -458,7 +461,8 @@ std::map MapsManager::updateMapCaches( const std::map & signatures) { bool updateGridCache = updateGrid || updateOctomap; - if(!updateGrid && !updateOctomap) + bool updateElevation = false; + if(!updateGrid && !updateOctomap && !updateOctomap) { // all false, update only those where we have subscribers updateOctomap = @@ -475,22 +479,25 @@ std::map MapsManager::updateMapCaches( gridMapPub_.getNumSubscribers() != 0 || gridProbMapPub_.getNumSubscribers() != 0; - updateGridCache = updateOctomap || updateGrid || + updateElevation = elevationMapPub_.getNumSubscribers() != 0; + + updateGridCache = updateOctomap || updateGrid || updateElevation || cloudMapPub_.getNumSubscribers() != 0 || cloudObstaclesPub_.getNumSubscribers() != 0 || cloudGroundPub_.getNumSubscribers() != 0 || scanMapPub_.getNumSubscribers() != 0; } -#ifndef WITH_OCTOMAP_MSGS +#if not (defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)) updateOctomap = false; #endif -#ifndef RTABMAP_OCTOMAP - updateOctomap = false; +#if not (defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)) + updateElevation = false; #endif gridUpdated_ = updateGrid; octomapUpdated_ = updateOctomap; + elevationMapUpdated_ = updateElevation; UDEBUG("Updating map caches..."); @@ -542,19 +549,17 @@ std::map MapsManager::updateMapCaches( UTimer longUpdateTimer; if(filteredPoses.size() > 20) { - if(updateGridCache && gridMaps_.size() < 5) + if(updateGridCache && localMaps_.size() < 5) { - ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size())); + ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size())); longUpdate = true; } -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) if(updateOctomap && octomap_->addedNodes().size() < 5) { ROS_WARN("Many clouds should be added to octomap (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size())); longUpdate = true; } -#endif #endif } @@ -565,7 +570,7 @@ std::map MapsManager::updateMapCaches( if(!iter->second.isNull()) { rtabmap::SensorData data; - if(updateGridCache && (iter->first == 0 || !uContains(gridMaps_, iter->first))) + if(updateGridCache && (iter->first == 0 || !uContains(localMaps_.localGrids(), iter->first))) { ROS_DEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); @@ -575,7 +580,7 @@ std::map MapsManager::updateMapCaches( } else if(memory) { - data = memory->getNodeData(iter->first, occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, !occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, false, true); + data = memory->getNodeData(iter->first, localMapMaker_->isGridFromDepth() && !occupancySavedInDB, !localMapMaker_->isGridFromDepth() && !occupancySavedInDB, false, true); } ROS_DEBUG("Adding grid map %d to cache...", iter->first); @@ -604,12 +609,12 @@ std::map MapsManager::updateMapCaches( { // if we are here, it is because we loaded a database with old nodes not having occupancy grid set // try reload again - data = memory->getNodeData(iter->first, occupancyGrid_->isGridFromDepth(), !occupancyGrid_->isGridFromDepth(), false, false); + data = memory->getNodeData(iter->first, localMapMaker_->isGridFromDepth(), !localMapMaker_->isGridFromDepth(), false, false); } data.uncompressData( - occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0, - occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0, - !occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0, + localMapMaker_->isGridFromDepth() && generateGrid?&rgb:0, + localMapMaker_->isGridFromDepth() && generateGrid?&depth:0, + !localMapMaker_->isGridFromDepth() && generateGrid?&scan:0, 0, generateGrid?0:&ground, generateGrid?0:&obstacles, @@ -619,15 +624,13 @@ std::map MapsManager::updateMapCaches( { Signature tmp(data); tmp.setPose(iter->second); - occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } else { viewPoint = data.gridViewPoint(); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } } else @@ -641,16 +644,16 @@ std::map MapsManager::updateMapCaches( { ParametersMap parameters; parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(scanEmptyRayTracing_))); - occupancyGrid_->parseParameters(parameters); + localMapMaker_->parseParameters(parameters); } cv::Mat rgb, depth; LaserScan scan; bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_); data.uncompressData( - occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0, - occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0, - !occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0, + localMapMaker_->isGridFromDepth() && generateGrid?&rgb:0, + localMapMaker_->isGridFromDepth() && generateGrid?&depth:0, + !localMapMaker_->isGridFromDepth() && generateGrid?&scan:0, 0, generateGrid?0:&ground, generateGrid?0:&obstacles, @@ -660,15 +663,13 @@ std::map MapsManager::updateMapCaches( { Signature tmp(data); tmp.setPose(iter->second); - occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } else { viewPoint = data.gridViewPoint(); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } // put back @@ -676,52 +677,10 @@ std::map MapsManager::updateMapCaches( { ParametersMap parameters; parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled))); - occupancyGrid_->parseParameters(parameters); + localMapMaker_->parseParameters(parameters); } } } - - if(updateGrid && - (iter->first == 0 || - occupancyGrid_->addedNodes().find(iter->first) == occupancyGrid_->addedNodes().end())) - { - std::map, cv::Mat> >::iterator mter = gridMaps_.find(iter->first); - if(mter != gridMaps_.end()) - { - if(!mter->second.first.first.empty() || !mter->second.first.second.empty() || !mter->second.second.empty()) - { - occupancyGrid_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second); - } - } - } - -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - if(updateOctomap && - (iter->first == 0 || - octomap_->addedNodes().find(iter->first) == octomap_->addedNodes().end())) - { - std::map, cv::Mat> >::iterator mter = gridMaps_.find(iter->first); - std::map::iterator pter = gridMapsViewpoints_.find(iter->first); - if(mter != gridMaps_.end() && pter!=gridMapsViewpoints_.end()) - { - if((mter->second.first.first.empty() || mter->second.first.first.channels() > 2) && - (mter->second.first.second.empty() || mter->second.first.second.channels() > 2) && - (mter->second.second.empty() || mter->second.second.channels() > 2)) - { - octomap_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second, pter->second); - } - else if(!mter->second.first.first.empty() && !mter->second.first.second.empty() && !mter->second.second.empty()) - { - ROS_WARN("Node %d: Cannot update octomap with 2D occupancy grids. " - "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " - "all occupancy grid parameters.", - iter->first); - } - } - } -#endif -#endif } else { @@ -734,8 +693,7 @@ std::map MapsManager::updateMapCaches( gridUpdated_ = occupancyGrid_->update(filteredPoses); } -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) if(updateOctomap) { UTimer time; @@ -743,20 +701,17 @@ std::map MapsManager::updateMapCaches( ROS_INFO("Octomap update time = %fs", time.ticks()); } #endif -#endif - for(std::map, cv::Mat> >::iterator iter=gridMaps_.begin(); - iter!=gridMaps_.end();) + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + if(updateElevation) { - if(!uContains(poses, iter->first)) - { - UASSERT(gridMapsViewpoints_.erase(iter->first) != 0); - gridMaps_.erase(iter++); - } - else - { - ++iter; - } + UTimer time; + elevationMapUpdated_ = elevationMap_->update(filteredPoses); + ROS_INFO("GridMap (elevation map) update time = %fs", time.ticks()); } +#endif + + localMaps_.clear(true); for(std::map::Ptr >::iterator iter=groundClouds_.begin(); iter!=groundClouds_.end();) @@ -979,10 +934,6 @@ void MapsManager::publishMaps( } ++countObstacles; } - else - { - std::map, cv::Mat> >::iterator jter = gridMaps_.find(iter->first); - } } } double addingPointsTime = t.ticks(); @@ -1005,16 +956,16 @@ void MapsManager::publishMaps( for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { - std::map, cv::Mat> >::iterator jter = gridMaps_.find(iter->first); + std::map::const_iterator jter = localMaps_.localGrids().find(iter->first); if(updateGround && assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end()) { if(iter->first > 0) { assembledGroundPoses_.insert(*iter); } - if(jter!=gridMaps_.end() && jter->second.first.first.cols) + if(jter!=localMaps_.end() && jter->second.groundCells.cols) { - pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.first), iter->second, 0, 255, 0); + pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.groundCells), iter->second, 0, 255, 0); pcl::PointCloud::Ptr subtractedCloud = transformed; if(cloudSubtractFiltering_) { @@ -1059,9 +1010,9 @@ void MapsManager::publishMaps( { assembledObstaclePoses_.insert(*iter); } - if(jter!=gridMaps_.end() && jter->second.first.second.cols) + if(jter!=localMaps_.end() && jter->second.obstacleCells.cols) { - pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.second), iter->second, 255, 0, 0); + pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.obstacleCells), iter->second, 255, 0, 0); pcl::PointCloud::Ptr subtractedCloud = transformed; if(cloudSubtractFiltering_) { @@ -1214,8 +1165,7 @@ void MapsManager::publishMaps( latched_.at(&cloudObstaclesPub_) = false; } -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) if( octomapUpdated_ || !latching_ || (octoMapPubBin_.getNumSubscribers() && !latched_.at(&octoMapPubBin_)) || @@ -1405,8 +1355,6 @@ void MapsManager::publishMaps( { latched_.at(&octoMapProj_) = false; } - -#endif #endif if( gridUpdated_ || @@ -1469,7 +1417,7 @@ void MapsManager::publishMaps( } else if(poses.size()) { - ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size()); + ROS_WARN("Grid map is empty! (local maps=%d)", (int)localMaps_.size()); } } if(gridMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers()) @@ -1515,7 +1463,7 @@ void MapsManager::publishMaps( } else if(poses.size()) { - ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size()); + ROS_WARN("Grid map is empty! (local maps=%d)", (int)localMaps_.size()); } } } @@ -1533,23 +1481,34 @@ void MapsManager::publishMaps( latched_.at(&gridProbMapPub_) = false; } + if( elevationMapUpdated_ || + !latching_ || + (elevationMapPub_.getNumSubscribers() && !latched_.at(&elevationMapPub_))) + { + grid_map_msgs::GridMap msg; + grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap(), msg); + msg.info.header.frame_id = mapFrameId; + msg.info.header.stamp = stamp; + elevationMapPub_.publish(msg); + } + if(elevationMapPub_.getNumSubscribers() == 0) + { + latched_.at(&elevationMapPub_) = false; + } + if( mapCacheCleanup_ && + elevationMapPub_.getNumSubscribers() == 0) + { + elevationMap_->clear(); + } + if(!this->hasSubscribers() && mapCacheCleanup_) { - if(!gridMaps_.empty()) + if(!localMaps_.empty()) { - size_t totalBytes = 0; - for(std::map, cv::Mat> >::iterator iter=gridMaps_.begin(); iter!=gridMaps_.end(); ++iter) - { - totalBytes+= sizeof(int)+ - iter->second.first.first.total()*iter->second.first.first.elemSize() + - iter->second.first.second.total()*iter->second.first.second.elemSize() + - iter->second.second.total()*iter->second.second.elemSize(); - } - totalBytes += gridMapsViewpoints_.size()*sizeof(int) + gridMapsViewpoints_.size() * sizeof(cv::Point3f); - ROS_INFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", gridMaps_.size(), totalBytes/1048576); + size_t totalBytes = localMaps_.getMemoryUsed(); + ROS_INFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", localMaps_.size(), totalBytes/1048576); } - gridMaps_.clear(); - gridMapsViewpoints_.clear(); + localMaps_.clear(); } } diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index 9474bccce..1fd0038b8 100644 --- a/rtabmap_util/src/nodelets/obstacles_detection.cpp +++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -40,7 +41,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "rtabmap/core/OccupancyGrid.h" #include "rtabmap/utilite/UStl.h" namespace rtabmap_util @@ -226,7 +226,7 @@ class ObstaclesDetection : public nodelet::Nodelet NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str()); } - grid_.parseParameters(parameters); + localMapMaker_.parseParameters(parameters); cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this); @@ -325,7 +325,7 @@ class ObstaclesDetection : public nodelet::Nodelet inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform); pcl::IndicesPtr flatObstacles(new std::vector); - pcl::PointCloud::Ptr cloud = grid_.segmentCloud( + pcl::PointCloud::Ptr cloud = localMapMaker_.segmentCloud( inputCloud, pcl::IndicesPtr(new std::vector), pose, @@ -441,7 +441,7 @@ class ObstaclesDetection : public nodelet::Nodelet std::string mapFrameId_; bool waitForTransform_; - rtabmap::OccupancyGrid grid_; + rtabmap::LocalGridMaker localMapMaker_; bool mapFrameProjection_; bool warned_; From f9e6fe1cd01d56427f9db2e4880ce8f5b0e0114f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 18 Dec 2023 18:52:17 -0800 Subject: [PATCH 06/12] Adding suggestion of #1081 --- .github/workflows/ros1.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml index e0ea3245a..7acc0f0e4 100644 --- a/.github/workflows/ros1.yml +++ b/.github/workflows/ros1.yml @@ -36,6 +36,8 @@ jobs: sudo apt-get update sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros python3-catkin-tools sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap + sudo pip3 uninstall empy + sudo pip3 install empy==3.3.4 - name: Setup catkin workspace run: | From 0af3e2d15d2a2d17fc31e8ffa7c51fa8d5776c55 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 18 Dec 2023 19:04:21 -0800 Subject: [PATCH 07/12] try number2 #1081 --- .github/workflows/ros1.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml index 7acc0f0e4..4b998fd56 100644 --- a/.github/workflows/ros1.yml +++ b/.github/workflows/ros1.yml @@ -37,7 +37,7 @@ jobs: sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros python3-catkin-tools sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap sudo pip3 uninstall empy - sudo pip3 install empy==3.3.4 + yes | sudo pip3 install empy==3.3.4 - name: Setup catkin workspace run: | From f7b04612b07ab09fa4ab90bf8fc46394e293a038 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 18 Dec 2023 19:33:24 -0800 Subject: [PATCH 08/12] try number 3 #1081 --- .github/workflows/ros1.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml index 4b998fd56..59d44c4f7 100644 --- a/.github/workflows/ros1.yml +++ b/.github/workflows/ros1.yml @@ -36,8 +36,8 @@ jobs: sudo apt-get update sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros python3-catkin-tools sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap - sudo pip3 uninstall empy - yes | sudo pip3 install empy==3.3.4 + sudo pip3 uninstall empy --yes + sudo pip3 install empy==3.3.4 - name: Setup catkin workspace run: | From 377c9e1a2219b6f7c1aa1c9c602042d12b506df3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 18 Dec 2023 19:38:31 -0800 Subject: [PATCH 09/12] try number 4 #1081 --- .github/workflows/ros1.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml index 59d44c4f7..3bf57a781 100644 --- a/.github/workflows/ros1.yml +++ b/.github/workflows/ros1.yml @@ -37,7 +37,6 @@ jobs: sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros python3-catkin-tools sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap sudo pip3 uninstall empy --yes - sudo pip3 install empy==3.3.4 - name: Setup catkin workspace run: | From 5afc069a0a66fab658e60c6a30199fd30658a2ed Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 19 Dec 2023 09:02:57 -0800 Subject: [PATCH 10/12] fixing build without grid_map_msgs --- rtabmap_util/src/MapsManager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 3b82b4340..a895f3625 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -1480,7 +1480,7 @@ void MapsManager::publishMaps( { latched_.at(&gridProbMapPub_) = false; } - +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) if( elevationMapUpdated_ || !latching_ || (elevationMapPub_.getNumSubscribers() && !latched_.at(&elevationMapPub_))) @@ -1500,7 +1500,7 @@ void MapsManager::publishMaps( { elevationMap_->clear(); } - +#endif if(!this->hasSubscribers() && mapCacheCleanup_) { if(!localMaps_.empty()) From eed4180f97438defc2623bb145b08248d54be3bf Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 3 Jan 2024 13:16:56 -0800 Subject: [PATCH 11/12] Required 0.21.23 to build --- rtabmap_conversions/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 3ddb50c25..0e3e77e50 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS image_geometry rtabmap_msgs ) -find_package(RTABMap 0.21.0 REQUIRED) +find_package(RTABMap 0.21.23 REQUIRED) catkin_package( INCLUDE_DIRS include From c70f9f1b14ead8f51ee724bd0b2c88ff091d74fa Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 3 Jan 2024 18:06:01 -0800 Subject: [PATCH 12/12] fixed missing grid_map_core definitions on rtabmap_slam side --- rtabmap_conversions/CMakeLists.txt | 2 +- rtabmap_slam/CMakeLists.txt | 6 ++++++ rtabmap_util/src/MapsManager.cpp | 3 ++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 0e3e77e50..adbc48dde 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS image_geometry rtabmap_msgs ) -find_package(RTABMap 0.21.23 REQUIRED) +find_package(RTABMap 0.21.3 REQUIRED) catkin_package( INCLUDE_DIRS include diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index 7860b8140..c66d6367c 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -60,6 +60,12 @@ MESSAGE(STATUS "WITH octomap_msgs") ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS") ENDIF(octomap_msgs_FOUND) +# If grid_map_core is found, add definition +IF(grid_map_core_FOUND) +MESSAGE(STATUS "WITH grid_map_core") +ADD_DEFINITIONS("-DWITH_GRID_MAP_CORE") +ENDIF(grid_map_core_FOUND) + # If apriltag_ros is found, add definition IF(apriltag_ros_FOUND) MESSAGE(STATUS "WITH apriltag_ros") diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index a895f3625..9beae049e 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -427,7 +427,8 @@ bool MapsManager::hasSubscribers() const octoMapObstacleCloud_.getNumSubscribers() != 0 || octoMapGroundCloud_.getNumSubscribers() != 0 || octoMapEmptySpace_.getNumSubscribers() != 0 || - octoMapProj_.getNumSubscribers() != 0; + octoMapProj_.getNumSubscribers() != 0 || + elevationMapPub_.getNumSubscribers() != 0; } bool MapsManager::isMapUpdated() const