0.11.10: Updated LaserScanInfo interface. Updated NodeData msg with occupancy grids and laser scan local transform. MapsManager is now using directly the occupancy grids saved in nodes for 3D cloud, octomap and grid map.

include/rtabmap_ros/CoreWrapper.h

@@ -77,6 +77,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <actionlib_msgs/GoalStatusArray.h>
 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
 
+namespace rtabmap {
+class StereoDense;
+}
+
 class CoreWrapper
 {
 public:
@@ -226,7 +230,8 @@ private:
 	bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
+	bool getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
+	bool getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool getProjMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
@@ -239,7 +244,7 @@ private:
 	bool octomapFullCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
 #endif
 
-	rtabmap::ParametersMap loadParameters(const std::string & configFile);
+	void loadParameters(const std::string & configFile, rtabmap::ParametersMap & parameters);
 	void saveParameters(const std::string & configFile);
 
 	void publishLoop(double tfDelay, double tfTolerance);
@@ -489,6 +494,7 @@ private:
 	ros::ServiceServer setLogErrorSrv_;
 	ros::ServiceServer getMapDataSrv_;
 	ros::ServiceServer getProjMapSrv_;
+	ros::ServiceServer getMapSrv_;
 	ros::ServiceServer getGridMapSrv_;
 	ros::ServiceServer publishMapDataSrv_;
 	ros::ServiceServer setGoalSrv_;
@@ -504,6 +510,7 @@ private:
 
 	boost::thread* transformThread_;
 
+	bool stereoToDepth_;
 	float rate_;
 	bool createIntermediateNodes_;
 	ros::Time time_;

include/rtabmap_ros/MapsManager.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define MAPSMANAGER_H_
 
 #include <rtabmap/core/Signature.h>
+#include <rtabmap/core/Parameters.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <ros/time.h>
@@ -37,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 class OctoMap;
 class Memory;
+class OccupancyGrid;
 
 }  // namespace rtabmap
 
@@ -46,6 +48,8 @@ public:
 	virtual ~MapsManager();
 	void clear();
 	bool hasSubscribers() const;
+	void backwardCompatibilityParameters(rtabmap::ParametersMap & parameters) const;
+	void setParameters(const rtabmap::ParametersMap & parameters);
 
 	std::map<int, rtabmap::Transform> getFilteredPoses(
 			const std::map<int, rtabmap::Transform> & poses);
@@ -53,10 +57,7 @@ public:
 	std::map<int, rtabmap::Transform> updateMapCaches(
 			const std::map<int, rtabmap::Transform> & poses,
 			const rtabmap::Memory * memory,
-			bool updateCloud,
-			bool updateProj,
 			bool updateGrid,
-			bool updateScan,
 			bool updateOctomap,
 			const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
 
@@ -65,12 +66,6 @@ public:
 			const ros::Time & stamp,
 			const std::string & mapFrameId);
 
-	cv::Mat generateProjMap(
-			const std::map<int, rtabmap::Transform> & filteredPoses,
-			float & xMin,
-			float & yMin,
-			float & gridCellSize);
-
 	cv::Mat generateGridMap(
 			const std::map<int, rtabmap::Transform> & filteredPoses,
 			float & xMin,
@@ -81,37 +76,20 @@ public:
 
 private:
 	// mapping stuff
-	int cloudDecimation_;
-	double cloudMaxDepth_;
-	double cloudMinDepth_;
-	double cloudVoxelSize_;
-	double cloudFloorCullingHeight_;
-	double cloudCeilingCullingHeight_;
 	bool cloudOutputVoxelized_;
-	bool cloudFrustumCulling_;
-	double cloudNoiseFilteringRadius_;
-	int cloudNoiseFilteringMinNeighbors_;
-	int scanDecimation_;
-	double scanVoxelSize_;
-	bool scanOutputVoxelized_;
-	double projMaxGroundAngle_;
-	int projMinClusterSize_;
-	double projMaxObstaclesHeight_;
-	double projMaxGroundHeight_;
-	bool projDetectFlatObstacles_;
-	bool projMapFrame_;
 	double gridCellSize_;
+	bool gridIncremental_;
 	double gridSize_;
 	bool gridEroded_;
 	double footprintRadius_;
-	bool gridUnknownSpaceFilled_;
-	double gridMaxUnknownSpaceFilledRange_;
 	double mapFilterRadius_;
 	double mapFilterAngle_;
 	bool mapCacheCleanup_;
 	bool negativePosesIgnored_;
 
 	ros::Publisher cloudMapPub_;
+	ros::Publisher cloudGroundPub_;
+	ros::Publisher cloudObstaclesPub_;
 	ros::Publisher projMapPub_;
 	ros::Publisher gridMapPub_;
 	ros::Publisher scanMapPub_;
@@ -121,15 +99,22 @@ private:
 	ros::Publisher octoMapEmptySpace_;
 	ros::Publisher octoMapProj_;
 
-	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
-	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
-	std::map<int, std::vector<rtabmap::CameraModel> > cameraModels_;
-	std::map<int, std::pair<cv::Mat, cv::Mat> > projMaps_; // <ground, obstacles>
+	std::map<int, rtabmap::Transform> assembledGroundPoses_;
+	std::map<int, rtabmap::Transform> assembledObstaclePoses_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
+
+	std::map<int, rtabmap::Transform> gridPoses_;
+	cv::Mat gridMap_;
 	std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles>
+	std::map<int, cv::Point3f> gridMapsViewpoints_;
+
+	rtabmap::OccupancyGrid * occupancyGrid_;
 
 	rtabmap::OctoMap * octomap_;
 	int octomapTreeDepth_;
-	bool octomapGroundIsObstacle_;
+
+	rtabmap::ParametersMap parameters_;
 };
 
 #endif /* MAPSMANAGER_H_ */