GridMap integration (#1082)

* GridMap integration

* Updated with upstream changes

* updated with upstream changes

* some fixes

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
@@ -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);

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,58 +74,71 @@ 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
 ############################
 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")
 
 #############

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 <ros/time.h>
@@ -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<int, rtabmap::Transform> assembledGroundPoses_;
 	std::map<int, rtabmap::Transform> assembledObstaclePoses_;
@@ -122,18 +127,19 @@ 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_;
 
 	rtabmap::OctoMap * octomap_;
 	int octomapTreeDepth_;
 	bool octomapUpdated_;
 
+	rtabmap::GridMap * elevationMap_;
+	bool elevationMapUpdated_;
+
 	rtabmap::ParametersMap parameters_;
 
 	bool latching_;

rtabmap_util/package.xml

@@ -28,6 +28,7 @@
   <depend>pluginlib</depend>
   <depend>rtabmap_msgs</depend>
   <depend>rtabmap_conversions</depend>
+  <depend>grid_map_ros</depend>
 
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />