From 80ae448d1b318f5edb6e5cf2360059b468796a6f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 3 Jan 2024 18:11:16 -0800 Subject: [PATCH] merged master->ros2 --- .github/workflows/ros1.yml | 1 + rtabmap_conversions/src/MsgConversion.cpp | 6 + .../demo_unitree_quadruped_robot.launch | 95 -------- .../include/rtabmap_odom/rgbd_odometry.hpp | 13 + .../include/rtabmap_odom/stereo_odometry.hpp | 23 ++ rtabmap_odom/src/nodelets/icp_odometry.cpp | 1 + rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 116 ++++++++- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 224 +++++++++++++++++- rtabmap_slam/CMakeLists.txt | 11 - rtabmap_util/CMakeLists.txt | 23 +- .../include/rtabmap_util/MapsManager.h | 15 +- rtabmap_util/package.xml | 3 +- rtabmap_util/src/MapsManager.cpp | 90 +++++-- .../src/nodelets/obstacles_detection.cpp | 3 +- 14 files changed, 493 insertions(+), 131 deletions(-) delete mode 100644 rtabmap_demos/launch/demo_unitree_quadruped_robot.launch diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml index e0ea3245..3bf57a78 100644 --- a/.github/workflows/ros1.yml +++ b/.github/workflows/ros1.yml @@ -36,6 +36,7 @@ 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 - name: Setup catkin workspace run: | diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 97fd6c13..0747df2e 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -2850,6 +2850,12 @@ bool deskew_impl( if(offsetTime < 0) { UERROR("Input cloud doesn't have \"t\", \"time\", \"stamps\" or \"timestamp\" field!"); + std::string fieldsReceived; + for(size_t i=0; i - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp index 53fa7657..090c3beb 100644 --- a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp @@ -95,6 +95,14 @@ private: const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5); + void callbackRGBD6( + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6); + protected: virtual void flushCallbacks(); @@ -110,6 +118,7 @@ private: 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_; @@ -131,6 +140,10 @@ private: 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/include/rtabmap_odom/stereo_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp index e45f2775..3f5dddf4 100644 --- a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp @@ -84,6 +84,19 @@ private: const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4); + void callbackRGBD5( + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5); + void callbackRGBD6( + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6); protected: @@ -101,6 +114,8 @@ private: message_filters::Subscriber rgbd_image2_sub_; 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_; @@ -118,6 +133,14 @@ private: 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_; diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 24090c93..f72dd064 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -317,6 +317,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan *scanMsg, scanOut, this->tfBuffer(), + -1.0f, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp); if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull()) diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 61c96541..b81e37a5 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -57,6 +57,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) : exactSync4_(0), approxSync5_(0), exactSync5_(0), + approxSync6_(0), + exactSync6_(0), queueSize_(5), keepColor_(false) { @@ -75,6 +77,8 @@ RGBDOdometry::~RGBDOdometry() delete exactSync4_; delete approxSync5_; delete exactSync5_; + delete approxSync6_; + delete exactSync6_; } void RGBDOdometry::onOdomInit() @@ -93,10 +97,6 @@ void RGBDOdometry::onOdomInit() { rgbdCameras = 1; } - if(rgbdCameras > 5) - { - RCLCPP_FATAL(this->get_logger(), "Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras)."); - } keepColor_ = this->declare_parameter("keep_color", keepColor_); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); @@ -129,6 +129,10 @@ void RGBDOdometry::onOdomInit() { rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); } + if(rgbdCameras >= 6) + { + rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + } if(rgbdCameras == 2) { @@ -256,6 +260,54 @@ void RGBDOdometry::onOdomInit() rgbd_image4_sub_.getSubscriber()->get_topic_name(), rgbd_image5_sub_.getSubscriber()->get_topic_name()); } + 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + approxSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::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(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", + get_name(), + 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 + { + RCLCPP_FATAL(this->get_logger(), + "%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).", + get_name(), rgbdCameras); + } } else if(rgbdCameras == 0) { @@ -649,6 +701,36 @@ void RGBDOdometry::callbackRGBD5( } } +void RGBDOdometry::callbackRGBD6( + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr 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); + } +} + void RGBDOdometry::flushCallbacks() { // flush callbacks @@ -748,6 +830,32 @@ void RGBDOdometry::flushCallbacks() rgbd_image5_sub_); exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::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(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::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(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); + } } } diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index 726c3e13..c249ac40 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -55,6 +55,10 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) : exactSync3_(0), approxSync4_(0), exactSync4_(0), + approxSync5_(0), + exactSync5_(0), + approxSync6_(0), + exactSync6_(0), queueSize_(5), keepColor_(false) { @@ -65,6 +69,16 @@ StereoOdometry::~StereoOdometry() { delete approxSync_; delete exactSync_; + delete approxSync2_; + delete exactSync2_; + delete approxSync3_; + delete exactSync3_; + delete approxSync4_; + delete exactSync4_; + delete approxSync5_; + delete exactSync5_; + delete approxSync6_; + delete exactSync6_; } void StereoOdometry::onOdomInit() @@ -106,6 +120,14 @@ void StereoOdometry::onOdomInit() { rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); } + if(rgbdCameras >= 5) + { + rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + } + if(rgbdCameras >= 6) + { + rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + } if(rgbdCameras == 2) { @@ -197,9 +219,89 @@ void StereoOdometry::onOdomInit() 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + approxSync5_->registerCallback(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::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(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", + get_name(), + 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + approxSync6_->registerCallback(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::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(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", + get_name(), + 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 { - RCLCPP_FATAL(this->get_logger(), "%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.", get_name(), rgbdCameras); + RCLCPP_FATAL(this->get_logger(), + "%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).", + get_name(), rgbdCameras); } } @@ -735,6 +837,76 @@ void StereoOdometry::callbackRGBD4( } } +void StereoOdometry::callbackRGBD5( + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr 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 StereoOdometry::callbackRGBD6( + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5, + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr 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); + } +} + void StereoOdometry::flushCallbacks() { //flush callbacks @@ -810,6 +982,56 @@ void StereoOdometry::flushCallbacks() rgbd_image4_sub_); exactSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::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(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::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(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::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(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::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(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); + } } } diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index 3e4d4cc6..946180d9 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -33,7 +33,6 @@ IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0) ENDIF() #optional -find_package(octomap_msgs) find_package(apriltag_msgs) IF(WIN32) @@ -71,16 +70,6 @@ SET(rtabmap_slam_plugins_lib_src src/CoreWrapper.cpp ) -# If octomap is found, add definition -IF(octomap_msgs_FOUND) -MESSAGE(STATUS "WITH octomap_msgs") -ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS") -SET(Libraries - ${Libraries} - octomap_msgs -) -ENDIF(octomap_msgs_FOUND) - # If apriltag_msgs is found, add definition IF(apriltag_msgs_FOUND) MESSAGE(STATUS "WITH apriltag_msgs") diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 6a6b499f..becf84dd 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -29,6 +29,7 @@ find_package(rtabmap_conversions REQUIRED) # Optional components find_package(octomap_msgs) +find_package(grid_map_ros) include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/include @@ -75,7 +76,7 @@ SET(rtabmap_util_plugins_lib_src ) -# If octomap is found, add definition +# If octomap is found, add dependency IF(octomap_msgs_FOUND) MESSAGE(STATUS "WITH octomap_msgs") include_directories( @@ -88,6 +89,18 @@ SET(Libraries ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS") ENDIF(octomap_msgs_FOUND) +# If grid_map is found, add dependency +IF(grid_map_ros_FOUND) +MESSAGE(STATUS "WITH grid_map_ros") +include_directories( + ${grid_map_ros_INCLUDE_DIRS} +) +SET(Libraries + grid_map_ros + ${Libraries} +) +ENDIF(grid_map_ros_FOUND) + ############################ ## Declare a cpp library ############################ @@ -100,6 +113,14 @@ target_include_directories(rtabmap_util_plugins $ ) +IF(octomap_msgs_FOUND) + target_compile_definitions(rtabmap_util_plugins PUBLIC -DWITH_OCTOMAP_MSGS) +ENDIF(octomap_msgs_FOUND) + +IF(grid_map_ros_FOUND) + target_compile_definitions(rtabmap_util_plugins PUBLIC -DWITH_GRID_MAP_ROS) +ENDIF(grid_map_ros_FOUND) + ament_target_dependencies(rtabmap_util_plugins ${Libraries}) rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDRelay") diff --git a/rtabmap_util/include/rtabmap_util/MapsManager.h b/rtabmap_util/include/rtabmap_util/MapsManager.h index cf80bb60..ee41af8c 100644 --- a/rtabmap_util/include/rtabmap_util/MapsManager.h +++ b/rtabmap_util/include/rtabmap_util/MapsManager.h @@ -38,10 +38,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#ifdef RTABMAP_OCTOMAP -#ifdef WITH_OCTOMAP_MSGS +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) #include #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) +#include #endif namespace rtabmap { @@ -49,6 +51,7 @@ class OctoMap; class Memory; class OccupancyGrid; class LocalGridMaker; +class GridMap; } // namespace rtabmap @@ -126,6 +129,9 @@ private: rclcpp::Publisher::SharedPtr octoMapEmptySpace_; rclcpp::Publisher::SharedPtr octoMapProj_; #endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + rclcpp::Publisher::SharedPtr elevationMapPub_; +#endif std::map assembledGroundPoses_; std::map assembledObstaclePoses_; @@ -148,6 +154,11 @@ private: int octomapTreeDepth_; bool octomapUpdated_; +#ifdef RTABMAP_GRIDMAP + rtabmap::GridMap * elevationMap_; +#endif + bool elevationMapUpdated_; + rtabmap::ParametersMap parameters_; bool latching_; diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 910b7aad..7461051b 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -30,7 +30,8 @@ message_filters rtabmap_msgs rtabmap_conversions - + grid_map_ros + ament_cmake diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 9ced2983..414906f7 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -51,6 +51,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) +#include +#include +#endif + using namespace rtabmap; namespace rtabmap_util { @@ -74,6 +79,10 @@ MapsManager::MapsManager() : #endif octomapTreeDepth_(16), octomapUpdated_(true), +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + elevationMap_(new GridMap(&localMaps_)), +#endif + elevationMapUpdated_(true), latching_(true) { } @@ -96,6 +105,7 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool) // connect latching_ = node.declare_parameter("latch", rclcpp::ParameterValue(latching_)).get(); + RCLCPP_INFO(node.get_logger(), "%s(maps): latch = %s", name.c_str(), latching_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s(maps): map_filter_radius = %f", name.c_str(), mapFilterRadius_); RCLCPP_INFO(node.get_logger(), "%s(maps): map_filter_angle = %f", name.c_str(), mapFilterAngle_); RCLCPP_INFO(node.get_logger(), "%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false"); @@ -105,7 +115,6 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool) RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_); -#ifdef WITH_OCTOMAP_MSGS #ifdef RTABMAP_OCTOMAP octomapTreeDepth_ = node.declare_parameter("octomap_tree_depth", rclcpp::ParameterValue(octomapTreeDepth_)).get(); if(octomapTreeDepth_ > 16) @@ -120,9 +129,6 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool) } RCLCPP_INFO(node.get_logger(), "%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_); #endif -#endif - - // mapping topics latched_.clear(); @@ -157,6 +163,11 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool) octoMapProj_ = node.create_publisher("octomap_grid", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); latched_.insert(std::make_pair((void*)&octoMapProj_, false)); #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + elevationMapPub_ = node.create_publisher("elevation_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + latched_.insert(std::make_pair((void*)&elevationMapPub_, false)); +#endif } MapsManager::~MapsManager() { @@ -168,6 +179,9 @@ MapsManager::~MapsManager() { #ifdef RTABMAP_OCTOMAP delete octomap_; #endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + delete elevationMap_; +#endif } void parameterMoved( @@ -236,13 +250,16 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters) parameters_ = parameters; delete occupancyGrid_; occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_); - localMapMaker_->parseParameters(parameters_); #ifdef 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 } void MapsManager::set2DMap( @@ -299,9 +316,15 @@ void MapsManager::clear() groundClouds_.clear(); obstacleClouds_.clear(); occupancyGrid_->clear(); + #ifdef 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) { iter->second = false; @@ -327,6 +350,9 @@ bool MapsManager::hasSubscribers() const octoMapGroundCloud_->get_subscription_count() != 0 || octoMapEmptySpace_->get_subscription_count() != 0 || octoMapProj_->get_subscription_count() != 0 +#endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + || elevationMapPub_->get_subscription_count() != 0 #endif ; } @@ -361,7 +387,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 #ifdef RTABMAP_OCTOMAP @@ -378,21 +405,29 @@ std::map MapsManager::updateMapCaches( octoMapProj_->get_subscription_count() != 0; #endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + updateElevation = elevationMapPub_->get_subscription_count() != 0; +#endif + updateGrid = gridMapPub_->get_subscription_count() != 0 || gridProbMapPub_->get_subscription_count() != 0; - updateGridCache = updateOctomap || updateGrid || + updateGridCache = updateOctomap || updateGrid || updateElevation || cloudMapPub_->get_subscription_count() != 0 || cloudObstaclesPub_->get_subscription_count() != 0 || cloudGroundPub_->get_subscription_count() != 0; } -#if !defined(WITH_OCTOMAP_MSGS) and !defined(RTABMAP_OCTOMAP) +#if not (defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)) updateOctomap = false; #endif +#if not (defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)) + updateElevation = false; +#endif gridUpdated_ = updateGrid; octomapUpdated_ = updateOctomap; + elevationMapUpdated_ = updateElevation; UDEBUG("Updating map caches..."); @@ -595,6 +630,18 @@ std::map MapsManager::updateMapCaches( } #endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + if(updateElevation) + { + UTimer time; + elevationMapUpdated_ = elevationMap_->update(filteredPoses); + UINFO("GridMap (elevation map) update time = %fs", time.ticks()); + } +#endif + + localMaps_.clear(true); + + for(std::map::Ptr >::iterator iter=groundClouds_.begin(); iter!=groundClouds_.end();) { @@ -796,10 +843,6 @@ void MapsManager::publishMaps( } ++countObstacles; } - else - { - //std::map, cv::Mat> >::iterator jter = gridMaps_.find(iter->first); - } } } double addingPointsTime = t.ticks(); @@ -1218,7 +1261,6 @@ void MapsManager::publishMaps( { latched_.at(&octoMapProj_) = false; } - #endif if( gridUpdated_ || @@ -1314,7 +1356,27 @@ void MapsManager::publishMaps( { latched_.at(&gridProbMapPub_) = false; } - +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + if( elevationMapUpdated_ || + !latching_ || + (elevationMapPub_->get_subscription_count() && !latched_.at(&elevationMapPub_))) + { + grid_map_msgs::msg::GridMap::UniquePtr msg; + msg = grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap()); + msg->header.frame_id = mapFrameId; + msg->header.stamp = stamp; + elevationMapPub_->publish(std::move(msg)); + } + if(elevationMapPub_->get_subscription_count() == 0) + { + latched_.at(&elevationMapPub_) = false; + } + if( mapCacheCleanup_ && + elevationMapPub_->get_subscription_count() == 0) + { + elevationMap_->clear(); + } +#endif if(!this->hasSubscribers() && mapCacheCleanup_) { if(!localMaps_.empty()) diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index ec692eba..ac535b7b 100644 --- a/rtabmap_util/src/nodelets/obstacles_detection.cpp +++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -85,8 +86,6 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) : cloudSub_ = create_subscription("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1)); } - - void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg) { rclcpp::Time time = now();