dynamic masking map

homer_map_manager/CMakeLists.txt

@@ -5,6 +5,8 @@ find_package(catkin REQUIRED COMPONENTS roscpp roslib tf homer_mapnav_msgs homer
 
 find_package( Eigen3 REQUIRED )
 
+set(CMAKE_BUILD_TYPE Release)
+
 find_package(OpenMP)
 if (OPENMP_FOUND)
     set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")

homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h

@@ -23,7 +23,7 @@ class MaskingManager {
     /** @brief The destructor. */
     virtual ~MaskingManager();
 
-    void updateMapInfo(const nav_msgs::MapMetaData &mapInfo);
+    nav_msgs::OccupancyGrid::ConstPtr updateMapInfo(const nav_msgs::MapMetaData &mapInfo);
 
     /** modifies either the masking layer or the slam layer (accordingly to the
      * given map layer in the msg */

homer_map_manager/src/Managers/MapManager.cpp

@@ -52,12 +52,6 @@ MapManager::~MapManager()
 
 void MapManager::updateMapLayer(int type, nav_msgs::OccupancyGrid::ConstPtr layer)
 {
-    //TODO possible crash
-    if(m_MapLayers[type]->info.width != layer->info.width)
-    {
-        
-        ROS_INFO_STREAM("Map_manager: map size changed!");
-    }
     m_MapLayers[type] 	= layer;
     if(type == homer_mapnav_msgs::MapLayers::SLAM_LAYER)
     {

homer_map_manager/src/Managers/MaskingManager.cpp

@@ -17,16 +17,17 @@ MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo) {
 
 MaskingManager::~MaskingManager() {}
 
-void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
-    if (m_SlamMap.info.width != mapInfo.width) {
+nav_msgs::OccupancyGrid::ConstPtr MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
+    if (m_SlamMap.info.width < mapInfo.width || m_SlamMap.info.height < mapInfo.height) {
         m_SlamMap.info = mapInfo;
 
         int x_add_left =
-            (m_MaskingMap.info.origin.position.x - mapInfo.origin.position.x) /
+            (m_MaskingMap.info.origin.position.x - mapInfo.origin.position.x  + 0.025) /
             mapInfo.resolution;
         int y_add_up =
-            (m_MaskingMap.info.origin.position.y - mapInfo.origin.position.y) /
+            (m_MaskingMap.info.origin.position.y - mapInfo.origin.position.y  + 0.025) /
             mapInfo.resolution;
+        ROS_INFO_STREAM("x add "<<x_add_left<<" y add "<<y_add_up);
 
         std::vector<signed char> tmpData = m_MaskingMap.data;
 
@@ -43,11 +44,13 @@ void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
         }
         m_MaskingMap.info = mapInfo;
     }
+        return boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
 }
 
 nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(
     homer_mapnav_msgs::ModifyMap::ConstPtr msg) {
     // reset SLAM mask map before each masking
+    m_SlamMap.data.resize(m_SlamMap.info.height * m_SlamMap.info.width);
     std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
               homer_mapnav_msgs::ModifyMap::NOT_MASKED);
     drawPolygon(msg->region, msg->maskAction, msg->mapLayer);

homer_map_manager/src/map_manager_node.cpp

@@ -132,8 +132,9 @@ MapManagerNode::~MapManagerNode() {
 
 void MapManagerNode::callbackSLAMMap(
     const nav_msgs::OccupancyGrid::ConstPtr& msg) {
+    nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->updateMapInfo(msg->info);
+    m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
     m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, msg);
-    m_MaskingManager->updateMapInfo(msg->info);
 }
 
 void MapManagerNode::callbackRapidMap(

homer_mapping/src/OccupancyMap/OccupancyMap.cpp

@@ -818,15 +818,15 @@ void OccupancyMap::applyChanges()
       if ( m_CurrentChanges[i] == ::OCCUPIED && m_OccupancyCount[i] < USHRT_MAX )
       {
 		
-		if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1)
-				m_MeasurementCount[x + m_metaData.width * (y+1)]++;
-		if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1)
-				m_MeasurementCount[x + m_metaData.width * (y-1)]++;
-		if(m_MeasurementCount[i-1] > 1)
-			m_MeasurementCount[i-1]++;
-		if(m_MeasurementCount[i+1] > 1)
-			m_MeasurementCount[i+1]++;
-        m_OccupancyCount[i] += 4;
+		//if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1)
+				//m_MeasurementCount[x + m_metaData.width * (y+1)]++;
+		//if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1)
+				//m_MeasurementCount[x + m_metaData.width * (y-1)]++;
+		//if(m_MeasurementCount[i-1] > 1)
+			//m_MeasurementCount[i-1]++;
+		//if(m_MeasurementCount[i+1] > 1)
+			//m_MeasurementCount[i+1]++;
+        m_OccupancyCount[i] ++;
       }
     }
   }