reworking map_manager to use dynamic map sizes

homer_map_manager/include/homer_map_manager/Managers/MapManager.h

@@ -60,12 +60,6 @@ class MapManager
      * @brief clearMapLayers Clear all map layers
      */
     void clearMapLayers();
-
-    /** getters */
-    double getHeight() { return m_Height; }
-    double getWidth() { return m_Width; }
-    double getResolution() { return m_Resolution; }
-    geometry_msgs::Pose getOrigin() { return m_Origin; }
     
     void updateLaser(int layer, const sensor_msgs::LaserScan::ConstPtr& msg );
 
@@ -94,11 +88,7 @@ class MapManager
     std::map<int, bool> m_MapVisibility;
 
     //sizes of the last slam map
-    double m_Height;
-    double m_Width;
-    double m_Resolution;
     bool m_got_transform;
-    geometry_msgs::Pose m_Origin;
     geometry_msgs::PoseStamped::ConstPtr m_pose;
 	tf::StampedTransform m_sick_transform;
 	tf::StampedTransform m_hokuyo_transform;

homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h

@@ -4,6 +4,7 @@
 #include <ros/ros.h>
 
 #include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/MapMetaData.h>
 #include <homer_mapnav_msgs/ModifyMap.h>
 
 #include <sstream>
@@ -18,11 +19,13 @@ class MaskingManager
   public:
 
     /** @brief The constructor. */
-    MaskingManager(int mapSize, float resolution);
+    MaskingManager(nav_msgs::MapMetaData mapInfo);
 
     /** @brief The destructor. */
     virtual ~MaskingManager();
 
+    void updateMapInfo(const nav_msgs::MapMetaData::ConstPtr mapInfo);
+
     /** modifies either the masking layer or the slam layer (accordingly to the given map layer in the msg */
     nav_msgs::OccupancyGrid::ConstPtr modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg);
 
@@ -39,9 +42,6 @@ class MaskingManager
     /** stores the masking values that are afterwards sent to the slam map */
     nav_msgs::OccupancyGrid m_SlamMap;
 
-    /** sizes of the masking map layer */
-    int m_Width, m_Height;
-    float m_CellSize;
 
     /** tools to draw masking polygons */
     void drawPolygon ( std::vector< geometry_msgs::Point > vertices, int value, int mapLayer );

homer_map_manager/src/Managers/MapManager.cpp

@@ -23,10 +23,6 @@ MapManager::MapManager(ros::NodeHandle* nh)
     {
     	m_MapVisibility[m_laser_layers[i]] = true;
     }
-
-    m_Height 			= -1;
-    m_Width 			= -1;
-    m_Resolution 		= -1;
     
 	try
 	{
@@ -56,14 +52,15 @@ 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 slam map update map sizes
     if(type == homer_mapnav_msgs::MapLayers::SLAM_LAYER)
     {
-        m_Height 		= layer->info.height;
-        m_Width 		= layer->info.width;
-        m_Resolution 	= layer->info.resolution;
-        m_Origin 		= layer->info.origin;
  	    sendMergedMap();
     }
 }

homer_map_manager/src/Managers/MaskingManager.cpp

@@ -2,32 +2,41 @@
 
 using namespace std;
 
-MaskingManager::MaskingManager(int mapSize, float resolution)
+MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo)
 {
-    m_CellSize = resolution;
-    m_Width = mapSize / m_CellSize + 1;
-    m_Height = mapSize / m_CellSize + 1;
-    ROS_INFO_STREAM( "Creating " << m_Width << " x " << m_Height << " map." );
-    m_MaskingMap.info.resolution = m_CellSize;
-    m_MaskingMap.info.height = m_Height;
-    m_MaskingMap.info.width = m_Width;
-    m_MaskingMap.info.origin.position.x = -m_Height * resolution / 2;
-    m_MaskingMap.info.origin.position.y = -m_Width  * resolution / 2;
-    m_MaskingMap.data.resize(m_Width * m_Height);
+    m_MaskingMap.info = mapInfo;
+    m_MaskingMap.data.resize(m_MaskingMap.info.width * m_MaskingMap.info.height);
     std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
 
-    m_SlamMap.info.resolution = m_CellSize;
-    m_SlamMap.info.height = m_Height;
-    m_SlamMap.info.width = m_Width;
-    m_SlamMap.info.origin.position.x = -m_Height * resolution / 2;
-    m_SlamMap.info.origin.position.y = -m_Width  * resolution / 2;
-    m_SlamMap.data.resize(m_Width * m_Height);
+    m_SlamMap.info = mapInfo;
+    m_SlamMap.data.resize(m_SlamMap.info.width * m_SlamMap.info.height);
     std::fill( m_SlamMap.data.begin(), m_SlamMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
 }
 
 MaskingManager::~MaskingManager()
 {}
 
+void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData::ConstPtr mapInfo)
+{
+    if(m_SlamMap.info.width != mapInfo.width)
+    {
+        m_SlamMap.info = mapInfo;
+
+        std::vector<int> tmpData =  m_MaskingMap.data;
+    
+        m_MaskingMap.data.resize(mapInfo.width * mapInfo.height);
+        std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(), 0);
+        
+        for(int x = 0; x < )
+        {
+            for(int y = 0;)
+            {
+            }
+        }
+        m_MaskingMap.info = mapInfo; 
+    }
+}
+
 nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg)
 {
     //reset SLAM mask map before each masking
@@ -69,11 +78,8 @@ void MaskingManager::drawPolygon ( vector< geometry_msgs::Point > vertices , int
     return;
   }
     //make temp. map
-    std::vector<int> data(m_Width * m_Height);
-    for ( int i = 0; i < data.size(); i++ )
-    {
-        data[i] = 0;
-    }
+    std::vector<int> data(m_MaskingMap.info.width * m_MaskingMap.info.height);
+    std::fill (data.begin(), data.end(), 0);
 
   //draw the lines surrounding the polygon
   for ( unsigned int i = 0; i < vertices.size(); i++ )
@@ -153,7 +159,7 @@ void MaskingManager::drawLine ( std::vector<int> &data, int startX, int startY,
   // compute cells
   for ( t = 0; t < dist; t++ )
   {
-    data[x + m_Width * y] = value;
+    data[x + m_MaskingMap.info.width * y] = value;
 
     xerr += dx;
     yerr += dy;
@@ -173,7 +179,7 @@ void MaskingManager::drawLine ( std::vector<int> &data, int startX, int startY,
 
 void MaskingManager::fillPolygon ( std::vector<int> &data, int x, int y, int value )
 {
-  int index = x + m_Width * y;
+  int index = x + m_MaskingMap.info.width * y;
   if ( value != data[index] )
   {
     data[index] = value;

homer_map_manager/src/map_manager_node.cpp

@@ -15,7 +15,20 @@ MapManagerNode::MapManagerNode(ros::NodeHandle *nh)
     m_MapManager = new MapManager(nh);
     m_POIManager = new PoiManager(nh);
     m_ROIManager = new RoiManager(nh);
-    m_MaskingManager = new MaskingManager(mapSize, resolution);
+
+    nav_msgs::MapMetaData mapInfo;
+    mapInfo.width = mapSize / resolution;
+    mapInfo.height = mapSize / resolution;
+    mapInfo.resolution = resolution;
+    mapInfo.origin.position.x = 0;
+    mapInfo.origin.position.y = 0; 
+    mapInfo.origin.position.z = 0; 
+    mapInfo.origin.orientation.x = 0; 
+    mapInfo.origin.orientation.y = 0; 
+    mapInfo.origin.orientation.z = 0; 
+    mapInfo.origin.orientation.w = 1; 
+
+    m_MaskingManager = new MaskingManager(mapInfo);
 
     //subscriptions of MapManagerModule
     m_RapidMapSubscriber 		= nh->subscribe("/rapid_mapping/map", 1, &MapManagerNode::callbackRapidMap, this);
@@ -88,6 +101,7 @@ MapManagerNode::~MapManagerNode()
 void MapManagerNode::callbackSLAMMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
 {
     m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, msg);
+    m_MaskingManager->updateMapInfo(const nav_msgs::MapMetaData::ConstPtr msg->info);
 }
 
 void MapManagerNode::callbackRapidMap( const nav_msgs::OccupancyGrid::ConstPtr& msg)