Updated with upstream changes

introlab · Dec 18, 2023 · 2f365ab · 2f365ab
1 parent e2b70a6
commit 2f365ab
Show file tree

Hide file tree

Showing 5 changed files with 57 additions and 134 deletions.
diff --git 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 <rtabmap/core/DBDriver.h>
 #include <rtabmap/core/Registration.h>
 #include <rtabmap/core/Graph.h>
+#include <rtabmap/core/LocalGridMaker.h>
 #include <rtabmap/core/Optimizer.h>
 
 #ifdef WITH_OCTOMAP_MSGS
@@ -2118,7 +2119,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/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 <rtabmap/core/Signature.h>
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/FlannIndex.h>
+#include <rtabmap/core/LocalGrid.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <rclcpp/rclcpp.hpp>
@@ -47,6 +48,7 @@ namespace rtabmap {
 class OctoMap;
 class Memory;
 class OccupancyGrid;
+class LocalGridMaker;
 
 }  // namespace rtabmap
 
@@ -94,6 +96,7 @@ class MapsManager {
 	const rtabmap::OctoMap * getOctomap() const {return octomap_;}
 #endif
 	const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;}
+	const rtabmap::LocalGridMaker * getLocalMapMaker() const {return localMapMaker_;}
 
 private:
 	// mapping stuff
@@ -133,12 +136,10 @@ class MapsManager {
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > groundClouds_;
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > obstacleClouds_;
 
-	std::map<int, rtabmap::Transform> gridPoses_;
-	cv::Mat gridMap_;
-	std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> > gridMaps_; // < <ground, obstacles>, empty cells >
-	std::map<int, cv::Point3f> gridMapsViewpoints_;
+	rtabmap::LocalGridCache localMaps_;
 
 	rtabmap::OccupancyGrid * occupancyGrid_;
+	rtabmap::LocalGridMaker * localMapMaker_;
 	bool gridUpdated_;
 
 #ifdef RTABMAP_OCTOMAP

diff --git a/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp b/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp
@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
-#include <rtabmap/core/OccupancyGrid.h>
+#include <rtabmap/core/LocalGridMaker.h>
 
 namespace rtabmap_util
 {
@@ -53,7 +53,7 @@ class ObstaclesDetection : public rclcpp::Node
 	std::string mapFrameId_;
 	double waitForTransform_;
 
-	rtabmap::OccupancyGrid grid_;
+	rtabmap::LocalGridMaker localMapMaker_;
 	bool mapFrameProjection_;
 	bool warned_;