diff --git a/homer_map_manager/include/homer_map_manager/Managers/MapManager.h b/homer_map_manager/include/homer_map_manager/Managers/MapManager.h
index 3d82a8820a0a295e521b83ac9bf8c96deeb0606f..d5a74ce4be03bccc050bb037035a18b37e859814 100644
--- a/homer_map_manager/include/homer_map_manager/Managers/MapManager.h
+++ b/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;
diff --git a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h b/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
index a372cf639d0c18d0a1125f750008f9521a0fae73..5809635a993909e01eb9ecb35a28dd2ba48adcc2 100644
--- a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
+++ b/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 );
diff --git a/homer_map_manager/src/Managers/MapManager.cpp b/homer_map_manager/src/Managers/MapManager.cpp
index b6bbf506e1fbe29e74a707bd0b10e05269e4ac41..8e3e5fe2ca2202072d29c2faccd9f5f55bf3101a 100644
--- a/homer_map_manager/src/Managers/MapManager.cpp
+++ b/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();
}
}
diff --git a/homer_map_manager/src/Managers/MaskingManager.cpp b/homer_map_manager/src/Managers/MaskingManager.cpp
index 1b3af14db71a07f09e56a1b2449f32e0540bf32b..7f72ac493df2ba4a32a379d97253582516eb59ee 100644
--- a/homer_map_manager/src/Managers/MaskingManager.cpp
+++ b/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;
diff --git a/homer_map_manager/src/map_manager_node.cpp b/homer_map_manager/src/map_manager_node.cpp
index 59bca4741e4c1462c7f7b7592a12736a7c424924..4e279befe589b7a96212bd279b5030ec378bef97 100644
--- a/homer_map_manager/src/map_manager_node.cpp
+++ b/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)