Revert "Trimmed the branch to only the homer_mapnav_msgs sub directory"

This reverts commit 8e600b7a.

README.md

-# map_messages
+# mapping
 
+See:
 
-## Introduction 
-
-Dieses Package enthält alle benutzerdefinierten Messages, die neben den in ROS enthaltenen Messages für das Mapping und die Navigation verwendet werden. Das Package enthält keine Node oder Librabries.
-
-## Messages 
-
-
-### PointOfInterest
-
-
-Die PointOfInterest-Message enthält alle Informationen, um einen POI zu erstellen, zu verschicken und zu speichern.
-
-~~~~~~ {.cpp}
-PointOfInterest.msg
-
-
-
-int32 DEFAULT=100
-int32 VICTIM=200
-int32 OBJECT=300
-int32 GRIPPABLE_OBJECT=400
-int32 PERSON=600
-int32 ROOMBA=700
-int32 HAZARD_MATERIAL=800
-int32 START_POSITION=900
-int32 START_ORIENTATION=1000
-
-int32 type
-string name
-string remarks
-
-geometry_msgs/Pose pose
-~~~~~~
-
-* `type` bezeichnet den Typ des POIs. Es kann eine der in dieser Message vorhandenen Konstanten verwendet werden. 
-* `name` bezeichnet den Namen des POIs. Dieser Name muss einzigartig sein, da die POIs über ihren Namen unterschieden werden.
-* `remarks`: Hier können Anmerkungen reingeschrieben werden. Diese werden in der GUI angezeigt.
-* `pose` bezeichnet die Position und Orientierung des POIS im /map-Frame.
-
-
-### ModifyPOI
-
-
-ModifyPOI ist dafür zuständig, einen vorhandenen POI zu verändern.
-
-~~~~~~ {.cpp}
-ModifyPOI.msg
-
-PointOfInterest poi
-string old_name
-~~~~~~
-
-* `poi` beinhaltet den POI, durch den der alte ersetzt werden soll.
-* `old_name` bezeichnet den Namen des POIs, der verändert werden soll.
-
-
-### TargetUnreachable
-
-
-
-TargetUnreachable wird von der Navigation versendet, sobald kein Pfad mehr zu einem Ziel geplant werden kann.
-
-
-~~~~~~ {.cpp}
-TargetUnreachable.msg
-
-int8 UNKNOWN=0
-int8 TILT_OCCURED=10
-int8 GRAVE_TILT_OCCURED=15
-int8 STALL_OCCURED=20
-int8 LASER_OBSTACLE=30
-
-int8 reason
-~~~~~~
-
-* `reason` kann einen von den in dieser Message definierten Konstanten annehmen und beschreibt den Grund des Fehlers.
-
-
-### SaveMap
-
-SaveMap wird versendet, wenn eine Karte gespeichert oder geladen werden soll und beinhaltet den Dateipfad zum Kartenordner.
-
-
-~~~~~~ {.cpp}
-SaveMap.msg
-
-string filename
-~~~~~~
-
-* `filename` bezeichnet den Dateipfad zum Kartenordner.
-
-
-### PointsOfInterest
-
-
-PointsOfInterest wird verwendet, um alle aktuellen POIs zu versenden.
-
-
-~~~~~~ {.cpp}
-PointsOfInterest.msg
-
-PointOfInterest[] pois
-~~~~~~
-
-* `pois` beinhaltet einen Vektor mit allen aktuellen POIs.
-
-
-### StartNavigation
-
-
-StartNavigation ist eine von zwei Methoden, um eine Navigation zu starten. Hier wird Der POI mitgegeben, zu dem der Roboter navigieren soll.
-
-
-~~~~~~ {.cpp}
-StartNavigation.msg
-
-geometry_msgs/Pose goal
-float32 distance_to_target
-bool skip_final_turn
-bool fast_planning
-~~~~~~
-
-* `goal` beinhaltet den Ziel-POI
-* `distance_to_target`: Hier kann angegeben werden, ab welcher Distanz zum Ziel die Navigation als erfolgreich abgeschlossen wird. 
-* `skip_final_turn`: Hier kann eingestellt werden, ob der Roboter sich am Ziel-POI in Richtund der POI-Orientierung ausrichten soll oder nicht.
-* `fast_planning`: Mit dieser Option kann ein experimentelles "Schnelles Planen" eingeschaltet werden. Es werden nur Pfade geplant, die sich in einer Boundingbox zwischen Roboter und Zielposition befinden.
-
-
-### MapLayers
-
-MapLayers definiert die vorhanden Kartenebenen als Konstanten und kann zudem verwendet werden, um einzelne Ebenen ein- oder auszuschalten.
-
-
-~~~~~~ {.cpp}
-MapLayers.msg
-
-int32 SLAM_LAYER=0
-int32 MASKING_LAYER=1
-int32 KINECT_LAYER=2
-int32 SICK_LAYER=3
-
-int32 layer
-bool state
-~~~~~~
-
-* `layer` enthält die Kartenebenen-ID und kann einen Wert dern in dieser Message definierten Konstanten annehmen.
-* `state` besagt, ob die ausgewählte Kartenebene aktiviert sein soll.
-
-
-
-
-### NavigateToPOI
-
-
-NavigateToPOI ist die zweite Art eine Navigation zu starten. Anstatt das gesamte POI-Objekt mitzugeben, wird nur der Name eines bereits im map_manger vorhandenen POIs mitgegeben, der daraufhin von der Navigation nachgeschlagen wird.
-
-~~~~~~ {.cpp}
-NavigateToPOI.msg
-
-string poi_name
-float32 distance_to_target
-bool skip_final_turn
-~~~~~~
-
-* `poi_name` beschreibt den Namen des POIS, zu dem navigiert werden soll.
-* `distance_to_target` siehe StartNavigation
-* `skip_final_turn` siehe StartNavigation
-
-
-### ModifyMap
-
-
-Mit dieser Message können Bereiche in einzelnen Kartenebenen verändert werden.
-
-
-~~~~~~ {.cpp}
-ModifyMap.msg
-
-int32 FREE = 0         
-int32 BLOCKED = 100    
-int32 OBSTACLE = 99     
-int32 NOT_MASKED = -1 
-
-geometry_msgs/Point[] region
-int32 maskAction
-int32 mapLayer 
-~~~~~~
-
-* `region` beschreibt die Eckpunkte des Polygons, das maskiert werden soll.
-* `maskAction` kann einen Wert der in dieser Message definierten Konstanten annehmen. OBSTACLE  wird in der Karte rot dargestellt. Mit NOT_MASKED können bereits maskierte Bereiche wieder gelöscht werden.
-* `mapLayer` enthält die ID der zu verändernden Kartenebene. Die IDs sind in der Message MapLayers definiert.
-
-
-
-### DeletePointOfInterest
-
-Löscht einen vorhandenen POI.
-
-
-~~~~~~ {.cpp}
-DeletePointOfInterest.msg
-
-string name
-~~~~~~
-
-* `name` beschreibt den Namen des zu löschenden POIs.
-
-
-### DoMapping
-
-
-
-Mit dieser Message kann das Mapping ein- oder ausgeschaltet werden.
-
-~~~~~~ {.cpp}
-DoMapping.msg
-
-bool state
-~~~~~~
-
-* `state` beinhaltet den Zustand des Mappings (true = an, false = off).
-
-
-## Services 
-
-### GetPointsOfInterest
-
-
-Über diesen Service kann die aktuelle Liste der POIs angefordert werden.
-
-
-~~~~~~ {.cpp}
-GetPointsOfInterest.srv
-
----
-PointsOfInterest poi_list
-~~~~~~
-
-* `poi_list` beinhaltet einen Vektor mit allen aktuellen POIs.
+* [mapping]
+* [navigation]
+* [map_manager]
+* [map_msgs]
+* [nav_libs]
 
+[mapping]: homer_mapping/README.md
+[navigation]: homer_navigation/README.md
+[map_manager]: map_manager/README.md
+[map_msgs]: map_msgs/README.md
+[nav_libs]: nav_libs/README.md
 

homer_map_manager/CHANGELOG.rst

+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package homer_map_manager
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.1 (2016-11-03)
+------------------
+* fixes
+* initial commit
+* Contributors: Niklas Yann Wettengel

homer_map_manager/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(homer_map_manager)
+
+find_package(catkin REQUIRED COMPONENTS roscpp roslib tf homer_mapnav_msgs homer_nav_libs cmake_modules std_srvs)
+
+find_package( Eigen3 REQUIRED )
+
+find_package(OpenMP)
+if (OPENMP_FOUND)
+    set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+
+include_directories(
+	include
+	${catkin_INCLUDE_DIRS}
+	${EIGEN3_INCLUDE_DIRS}
+)
+
+set(Managers_SRC
+	src/Managers/MapManager.cpp
+	src/Managers/MaskingManager.cpp
+	src/Managers/PoiManager.cpp
+	src/Managers/RoiManager.cpp
+)
+
+add_library(homerManagers ${Managers_SRC})
+target_link_libraries(homerManagers homerImage_io)
+add_dependencies(homerManagers ${catkin_EXPORTED_TARGETS})
+
+
+add_library(homerImage_io
+	src/MapIO/image_loader.cpp
+	src/MapIO/map_saver.cpp
+	src/MapIO/map_loader.cpp
+)
+target_link_libraries(homerImage_io SDL SDL_image yaml-cpp)
+add_dependencies(homerImage_io ${catkin_EXPORTED_TARGETS})
+
+catkin_package(
+	INCLUDE_DIRS include
+	CATKIN_DEPENDS
+		roscpp
+		roslib
+		tf
+		homer_mapnav_msgs
+		homer_nav_libs
+		std_srvs
+	LIBRARIES homerImage_io homerManagers
+)
+
+add_executable(map_manager src/map_manager_node.cpp)
+
+target_link_libraries(
+	map_manager
+		${catkin_LIBRARIES}
+		homerManagers
+		homerImage_io
+)
+
+add_dependencies(
+	map_manager
+		${catkin_EXPORTED_TARGETS}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+	DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(TARGETS homerManagers homerImage_io map_manager
+	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

homer_map_manager/README.md

+# map_manager
+
+## Known Issues / Todo's 
+
+Aus bisher ungeklärten Gründen kann es in seltenen Fällen passieren, dass der map_manager die Verbindung zum roscore verliert. In diesem Fall muss er durch rosrun map_manager map_manager neugestartet werden.
+
+
+## Introduction 
+
+Der map_manager ist der Mittelpunkt der Kommunikation zwischen homer_mapping, homer_navigation, GUI und die Spiel-Nodes.
+Das Zusammenspiel dieser Nodes ist im Screenshot des rqt_graphs zu sehen.
+
+![rqt_graph](images/rosgraph.png)
+
+Er verwaltet die aktuell durch das mapping erstellte Karte sowie weitere Kartebenen. Aktuell sind das die SLAM-Karte, die aktuellen Laserdaten in einer weiteren Ebene und eine Masking-Ebene, in der mit Hilfe der GUI Hindernisse oder freie Flächen in die Karte gezeichnet werden können.
+Jedes mal, wenn eine SLAM-Karte von der mapping-Node geschickt wird, wird diese mit allen anderen Karteneben überschrieben (in der Reihenfolge SLAM, Masking, Laserdaten) und als eine zusammengefügte Karte versendet.
+Zudem verwaltet der map_manager alle erstellten Points Of Interest (POIs), die z.B. als Ziele für die Navigation verwendet werden.
+Die Node ist außerdem zuständig für das Speichern und Laden der Kartenebenen und der POIs. Dabei wird die SLAM-Ebene sowie die Masking-Ebene berücksichtigt.
+
+
+
+## Topics 
+
+
+#### Publisher 
+* `/map`: Die aktuelle Karte, die aus allen aktivierten Kartenebenen zusammengesetzt ist. Diese wird in der GUI angezeigt und für die Navigation verwendet.
+* `/map_manager/poi_list`: Verschickt einen Vektor mit allen aktuellen POIs. Dieser Publisher wird immer ausgelöst, sobald sich ein POI ändert oder ein neuer hinzugefügt wird.
+* `/map_manager/loaded_map`: Wenn eine Karte geladen wird, wird über dieses Topic die geladene SLAM-Ebene an die homer_mapping-Node verschickt.
+* `/map_manager/mask_slam`: Über die GUI kann die SLAM-Map verändert werden. Diese Modifizierungen werden über dieses Topic vom map_manager an das homer_mapping versendet.
+
+#### Subscriber
+
+* `/homer_mapping/slam_map (nav_msgs/OccupancyGrid)`: Hierüber wird die aktuelle SLAM-Map empfangen.
+* `/map_manager/save_map (map_messages/SaveMap)`: Hierüber wird der Befehl zum Speichern der Karte inklusive des Dateinamens empfangen.
+* `/map_manager/load_map (map_messages/SaveMap)`: Hiermit wird eine Karte geladen und alle bisherigen Kartenebenen durch die geladenen ersetzt.
+* `/map_manager/toggle_map_visibility (map_messages/MapLayers)`: Hierüber können einzelne Kartenebenen aktiviert beziehungsweise deaktiviert werden. Deaktivierte werden nicht mehr beim Zusammenfügen der Karte berücksichtigt und dementsprechend auch nicht in der GUI angezeigt sowie für die Navigation verwendet.
+* `/scan (nav_msgs/LaserScan)`: Der aktuelle Laserscan, der in die Laserscan-Ebene gezeichnet wird.
+* `/map_manager/add_POI (map_messages/PointOfInterest)`: Hierüber kann ein POI hinzugefügt werden.
+* `/map_manager/modify_POI (map_messages/ModifyPOI)`: Hierüber kann ein vorhandener POI verändert werden (Name, Position,...)
+* `/map_manager/delete_POI (map_messages/DeletePointOfInterest)`: Hierüber kann ein vorhander POI gelöscht werden.
+* `/map_manager/modify_map (map_messages/ModifyMap)`: Über dieses Topic werden die Koordinaten der Polygone verschickt, die über die GUI maskiert wurden. Außerdem wird die Kartenebene mitgeteilt, die verändet werden soll (SLAM oder Masking-Ebene).
+* `/map_manager/reset_maps (std_msgs/Empty)`: Hierüber werden alle Kartenebenen zurückgesetzt.
\ No newline at end of file

homer_map_manager/include/homer_map_manager/Managers/MapManager.h

+#ifndef MAPMANAGER_H
+#define MAPMANAGER_H
+
+#include <string>
+#include <map>
+#include <list>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include <nav_msgs/OccupancyGrid.h>
+#include <geometry_msgs/Pose.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <homer_mapnav_msgs/ModifyMap.h>
+#include <homer_mapnav_msgs/MapLayers.h>
+#include <sensor_msgs/LaserScan.h>
+#include <homer_nav_libs/tools.h>
+#include <tf/tf.h>
+
+#include "ros/ros.h"
+
+/** @class MapManager
+  * @author Malte Knauf, David Gossow (RX), Susanne Maur
+  * @brief This class holds all current map layers, updates them and publishes them in one merged map.
+  */
+class MapManager
+{
+  public:
+
+    /**
+     * @brief Constructor
+     * @param nh node handle
+     */
+    MapManager(ros::NodeHandle* nh);
+
+    /**
+     * @brief getMapLayer search for map layer of given type and return it
+     * @param type type of map layer to search for
+     * @return map layer
+     */
+    nav_msgs::OccupancyGrid::ConstPtr getMapLayer(int type);
+
+    /**
+     * @brief updateMapLayer replaces map layer of given type
+     * @param type type of map layer
+     * @param layer new map layer
+     */
+    void updateMapLayer(int type, nav_msgs::OccupancyGrid::ConstPtr layer);
+
+    /**
+     * @brief toggleMapVisibility toggles visibility of each map layer
+     * @param type type of map layer to toggle
+     * @param state visible or not
+     */
+    void toggleMapVisibility(int type, bool state );
+
+    /**
+     * @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 );
+
+    /** merges all map layers and publishes the merged map */
+    void sendMergedMap();
+    
+    void updatePose(const geometry_msgs::PoseStamped::ConstPtr& msg){m_pose = msg;}
+
+    /** destructor */
+    virtual ~MapManager();
+
+  private:
+    /**
+     * The map data of each available map layer ist stored in this map
+     */
+    std::map< int, nav_msgs::OccupancyGrid::ConstPtr > m_MapLayers;
+    std::map< int, sensor_msgs::LaserScan::ConstPtr > m_laserLayers;
+    std::vector< int > m_map_layers;
+    std::vector< int > m_laser_layers;
+
+	tf::TransformListener m_TransformListener;
+	
+    /**
+     * This map stores which map layers are enabled and which are disabled
+     */
+    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;
+     
+    /** map publisher */
+    ros::Publisher m_MapPublisher;
+};
+#endif

homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h

+#ifndef MaskingManager_H
+#define MaskingManager_H
+
+#include <ros/ros.h>
+
+#include <nav_msgs/OccupancyGrid.h>
+#include <homer_mapnav_msgs/ModifyMap.h>
+
+#include <sstream>
+
+/**
+ * @class  MaskingManager
+ * @brief  Manages a map that can overwrite values in the SLAM map or store it in a separate layer
+ * @author Malte Knauf, David Gossow
+ */
+class MaskingManager
+{
+  public:
+
+    /** @brief The constructor. */
+    MaskingManager(int mapSize, float resolution);
+
+    /** @brief The destructor. */
+    virtual ~MaskingManager();
+
+    /** 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);
+
+    /** resets the masking map layer */
+    nav_msgs::OccupancyGrid::ConstPtr resetMap();
+
+    /** replaces the masking map layer */
+    void replaceMap(nav_msgs::OccupancyGrid map);
+
+  private:
+
+    /** stores the masking values in the dedicated masking map */
+    nav_msgs::OccupancyGrid m_MaskingMap;
+    /** 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 );
+    void drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value );
+    void fillPolygon ( std::vector<int> &data, int x, int y, int value );
+};
+
+#endif
+

homer_map_manager/include/homer_map_manager/Managers/PoiManager.h

+#ifndef POI_MANAGER_H
+#define POI_MANAGER_H
+
+#include <list>
+
+#include <homer_mapnav_msgs/PointOfInterest.h>
+#include <homer_mapnav_msgs/ModifyPOI.h>
+
+#include <ros/ros.h>
+
+/** @class PoiManager
+ * @author Malte Knauf, David Gossow
+ * @brief This class manages the List of points of interest (POIs)
+  *
+  * This class keeps a list of all POIs within the current map. It provides the usual functions
+  * to edit the list.
+  */
+class PoiManager {
+
+  public:
+
+    /** The constructor of the class. */
+    PoiManager(ros::NodeHandle* nh);
+
+    /** constructor initializing the poi list */
+    PoiManager( std::vector< homer_mapnav_msgs::PointOfInterest > pois );
+
+    /** Does nothing. */
+    ~PoiManager() {}
+
+    /** Adds a new POI to the list if no POI with the same name exists
+      * @param poi pointer to the PointOfInterest message with the POI to be added
+      * @return true if successful, false otherwise
+      */
+    bool addPointOfInterest( const homer_mapnav_msgs::PointOfInterest::ConstPtr& poi );
+
+    /** Replaces a POI with a new one
+      * @param poi pointer with the PointOfInterest to be inserted
+      *            the POI with the same ID as the new one is first deleted
+      * @return true if the old POI was found and could be deleted
+      *         false otherwise
+      */
+    bool modifyPointOfInterest( const homer_mapnav_msgs::ModifyPOI::ConstPtr& poi );
+
+    /** Deletes a POI with a certain name from the list
+      * @param name Name of the POI to be deleted
+      * @return true if the POI was found and could be deleted
+      *         false otherwise
+      */
+    bool deletePointOfInterest( std::string name );
+
+    /**
+     * @brief place the current poi list
+     * @param poilist new poi list
+     */
+    void replacePOIList(std::vector<homer_mapnav_msgs::PointOfInterest> poilist);
+
+    /**
+     * @brief Publishes a PointsOfInterest Message with current POIs
+
+     */
+    void broadcastPoiList();
+
+    /** Returns current POI list
+      * @return the POI list
+      */
+    std::vector< homer_mapnav_msgs::PointOfInterest > getList();
+
+  private:
+
+    /** Looks for POI with name in the list
+      * @param name Name of the POI
+      */
+    bool poiExists( std::string name );
+
+
+    /** The copy constructor of the class.
+      * It's kept private, because it will never be used.
+      */
+    PoiManager( const PoiManager& instance );
+
+    /** Holds the POI vector */
+    std::vector< homer_mapnav_msgs::PointOfInterest > m_Pois;
+
+    /** publisher that publishes the current poi list */
+    ros::Publisher m_POIsPublisher;
+
+};
+#endif

homer_map_manager/include/homer_map_manager/Managers/RoiManager.h

+#ifndef ROI_MANAGER_H
+#define ROI_MANAGER_H
+
+#include <ros/ros.h>
+#include <homer_mapnav_msgs/RegionOfInterest.h>
+#include <homer_mapnav_msgs/RegionsOfInterest.h>
+#include <geometry_msgs/PointStamped.h>
+#include <sstream>
+#include <Eigen/Geometry>
+#include <tf/transform_listener.h>
+
+
+/**
+ * @class RoiManager (Roi = Region of interest)
+ * @author Viktor Seib (R23)
+ * @brief This class manages the List of regions of interest (ROIs)
+  *
+  * This class keeps a list of all ROIs within the current map. It provides the usual functions
+  * to edit the list.
+  */
+class RoiManager
+{
+
+  public:
+
+    /** The constructor of the class. */
+    RoiManager(ros::NodeHandle* nh);
+
+    /** constructor initializing the roi list */
+    RoiManager( std::vector< homer_mapnav_msgs::RegionOfInterest > rois );
+
+    /** Does nothing. */
+    ~RoiManager() {}
+
+    /** Adds a new ROI to the list, in contrast to POIs, several ROIs with the same name are allowed
+      * @param roi RegionOfInterest message with the ROI to be added
+      * @return true if successful, false otherwise
+      */
+    bool addRegionOfInterest( const homer_mapnav_msgs::RegionOfInterest::ConstPtr& roi );
+
+    /** Tests all Rois and returns a vector of ids in which the pose is
+      * @param pose Pose which is tested to be inside the Rois
+      * @return vector of ids in which the pose is
+      */
+	std::vector<homer_mapnav_msgs::RegionOfInterest> pointInsideRegionOfInterest( const geometry_msgs::PointStamped point );
+
+    /** Replaces a ROI with a new one
+      * @param roi RegionOfInterest to be inserted
+      *            the ROI with the same ID as the new one is first deleted
+      * @return true if the old ROI was found and could be deleted
+      *         false otherwise
+      */
+    bool modifyRegionOfInterest( const homer_mapnav_msgs::RegionOfInterest::ConstPtr& roi );
+
+    /** Deletes all ROIs with a certain name from the list
+      * @param name Name of the ROIs to be deleted
+      * @return true if the ROI was found and could be deleted
+      *         false otherwise
+      */
+    bool deleteRegionOfInterest( std::string name );
+
+    /** Deltes ROI with the given id
+     * @param id ID of ROI to be deleted
+     * @return true if the ROI was found and could be deleted
+     *         false otherwise
+     */
+    bool deleteRegionOfInterest( int id );
+
+    /**
+     * @brief place the current roi list
+     * @param roilist new roi list
+     */
+    void replaceROIList(std::vector<homer_mapnav_msgs::RegionOfInterest> roilist);
+
+
+    /** Returns current ROI list
+      * @return the ROI list
+      */
+    std::vector< homer_mapnav_msgs::RegionOfInterest > getList();
+    
+    /**
+     * @brief Publishes a RegionsOfInterest Message with current ROIs
+
+     */
+    void broadcastRoiList();
+
+    /** Returns name of ROI 
+      * @param id of ROI
+      * @return ROI name
+      */
+    std::string getROIName(int id);
+
+  private:
+
+    /** Looks for ROI with name in the list
+      * @param id ID of the ROI
+      */
+    bool roiExists(int id );
+
+    /** Looks for ROI with name in the list
+      * @param name Name of the ROI
+      */
+    bool roiExists( std::string name );
+
+
+    /**
+      * @brief gets the highest id of all ROIs and save it into m_highest_id
+      */
+    void setHighestId();
+
+    /** The copy constructor of the class.
+      * It's kept private, because it will never be used.
+      */
+    RoiManager( const RoiManager& instance );
+
+    /** Holds the ROI vector */
+    std::vector< homer_mapnav_msgs::RegionOfInterest > m_Rois;
+
+    /** publisher that publishes the current roi list */
+    ros::Publisher m_ROIsPublisher;
+
+    /** to set a new id, the current highest id is needed */
+    int m_highest_id;
+    
+	tf::TransformListener tf_listener;
+};
+#endif

homer_map_manager/include/homer_map_manager/MapIO/image_loader.h

+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef MAP_SERVER_MAP_SERVER_H
+#define MAP_SERVER_MAP_SERVER_H
+
+/*
+ * Author: Brian Gerkey
+ */
+
+#include <nav_msgs/GetMap.h>
+
+namespace map_server
+{
+
+/** Read the image from file and fill out the resp object, for later 
+ * use when our services are requested. 
+ *
+ * @param resp The map wil be written into here
+ * @param fname The image file to read from
+ * @param res The resolution of the map (gets stored in resp)
+ * @param negate If true, then whiter pixels are occupied, and blacker
+ *               pixels are free
+ * @param occ_th Threshold above which pixels are occupied
+ * @param free_th Threshold below which pixels are free
+ * @param origin Triple specifying 2-D pose of lower-left corner of image
+ *
+ * @throws std::runtime_error If the image file can't be loaded
+ * */
+void loadMapFromFile(nav_msgs::OccupancyGrid* map,
+                     const char* fname, double res, bool negate,
+                     double occ_th, double free_th, double* origin);
+}
+
+#endif

homer_map_manager/include/homer_map_manager/MapIO/map_loader.h

+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Author: Brian Gerkey */
+
+#define USAGE "\nUSAGE: map_server <map.yaml>\n" \
+              "  map.yaml: map description file\n" \
+              "DEPRECATED USAGE: map_server <map> <resolution>\n" \
+              "  map: image file to load\n"\
+              "  resolution: map resolution [meters/pixel]"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <libgen.h>
+#include <fstream>
+
+#include "ros/ros.h"
+#include "ros/console.h"
+#include <homer_map_manager/MapIO/image_loader.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include "yaml-cpp/yaml.h"
+
+#include <homer_mapnav_msgs/PointOfInterest.h>
+#include <homer_mapnav_msgs/RegionOfInterest.h>
+
+class MapServer
+{
+  public:
+    /** Trivial constructor */
+    MapServer(const std::string fname, bool &success);
+
+    nav_msgs::OccupancyGrid getSLAMMap();
+
+    nav_msgs::OccupancyGrid getMaskingMap();
+
+    std::vector<homer_mapnav_msgs::PointOfInterest> getPois();
+
+    std::vector<homer_mapnav_msgs::RegionOfInterest> getRois();
+
+  
+  private:
+
+    /** The map data is cached here
+     */
+    nav_msgs::GetMap::Response map_resp_;
+    nav_msgs::OccupancyGrid m_SLAMMap;
+    nav_msgs::OccupancyGrid m_MaskingMap;
+
+
+    std::vector<homer_mapnav_msgs::PointOfInterest> poiList;
+    std::vector<homer_mapnav_msgs::RegionOfInterest> roiList;
+
+};

homer_map_manager/include/homer_map_manager/MapIO/map_saver.h

+/*
+ * map_saver
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the <ORGANIZATION> nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <cstdio>
+#include "ros/ros.h"
+#include "ros/console.h"
+#include "nav_msgs/GetMap.h"
+//#include "LinearMath/btMatrix3x3.h"
+#include "geometry_msgs/Quaternion.h"
+
+#include <homer_mapnav_msgs/PointOfInterest.h>
+#include <homer_mapnav_msgs/RegionOfInterest.h>
+
+using namespace std;
+ 
+/**
+ * @brief Map generation node.
+ */
+class MapGenerator 
+{
+
+  public:
+    MapGenerator(const std::string mapname);
+
+    void save(const nav_msgs::OccupancyGridConstPtr& SLAMMap,
+              const nav_msgs::OccupancyGridConstPtr& maskingMap,
+              std::vector<homer_mapnav_msgs::PointOfInterest> poiList,
+              std::vector<homer_mapnav_msgs::RegionOfInterest> roiList );
+
+  private:
+    std::string m_Mapname;
+
+    void saveMapLayer(const nav_msgs::OccupancyGridConstPtr& map, std::string fileName);
+
+};
+
+
+

homer_map_manager/include/homer_map_manager/Workers/PointOfInterest/PointOfInterest.h

+#ifndef POINTOFINTEREST_H
+#define POINTOFINTEREST_H
+
+#include <string>
+#include <map>
+
+#include <homer_nav_libs/Math/Pose.h>
+
+using namespace std;
+
+typedef std::map< std::string, std::string > StringMapT;
+typedef std::map< std::string, float > FloatMapT;
+typedef std::map< std::string, int > IntMapT;
+
+/**
+ * @class PointOfInterest
+ *
+ * @author Robert Hoffmann (RX), David Gossow (RX), Simon Graeser (RX),
+ *         Nicolai Wojke (R14)
+ *
+ * @brief This class represents a point of interest (POI)
+ *
+ * This class represents a point of interest (POI). It is derived from
+ * Point2D and thus inherits its methods x(), y(), and theta() to query
+ * the world position and orientation.
+ */
+
+class PointOfInterest: public Pose
+{
+
+  public:
+
+    enum PoiType {  DEFAULT           =100,
+                    VICTIM            =200,
+                    OBJECT            =300,
+                    GRIPPABLE_OBJECT  =400,
+                    PERSON            =600,
+                    ROOMBA            =700,
+                    HAZARD_MATERIAL   =800,
+                    START_POSITION    =900,
+                    START_ORIENTATION =1000
+               };
+
+    PointOfInterest( );
+
+    /**
+      * @brief The constructor
+      *
+      * Creates a point of interest with orientation 0.0.
+      *
+      * @param id unique identification number
+      * @param name name of the poi
+      * @param type type of the poi (victim, etc.)
+      * @param posX,posY position within the map
+      * @param remarks additional information associated with the poi
+      */
+    PointOfInterest ( int id, string name, PoiType type, float posX, float posY, string remarks,
+                      StringMapT stringMap=StringMapT(), FloatMapT floatMap=FloatMapT() ,
+                      IntMapT intMap=IntMapT() )
+        : Pose ( posX, posY, 0.0f )
+    {
+      init ( id, name, type , remarks, stringMap, floatMap, intMap );
+    }
+
+    /** @brief The constructor
+      * @param id unique identification number
+      * @param name name of the poi
+      * @param type type of the poi (victim, etc.)
+      * @param posX,posY position within the map
+      * @param theta orientation of the poi
+      * @param remarks additional information associated with the poi
+      */
+    PointOfInterest ( int id, string name, PoiType type, float posX, float posY, float theta, string remarks,
+                      StringMapT stringMap=StringMapT(), FloatMapT floatMap=FloatMapT(), IntMapT intMap=IntMapT() )
+        : Pose ( posX, posY, theta )
+    {
+      init ( id, name, type , remarks, stringMap, floatMap, intMap );
+    }
+
+    /** @brief Alternative constructor omitting the id parameter */
+    PointOfInterest ( string name, PoiType type, float posX, float posY , string remarks )
+        : Pose ( posX, posY, 0.0f )
+    {
+      init ( -1, name, type , remarks, StringMapT(), FloatMapT(), IntMapT() );
+    }
+
+    /** @brief Alternative constructor omitting the id parameter */
+    PointOfInterest ( string name, PoiType type, float posX, float posY , float theta, string remarks )
+        : Pose ( posX, posY, theta )
+    {
+      init ( -1, name, type , remarks, StringMapT(), FloatMapT(), IntMapT() );
+    }
+
+    /** @brief Copy constructor, assigns a new id */
+    PointOfInterest ( int id, const PointOfInterest* poi );
+
+    /** @brief The standard destructor */
+    virtual ~PointOfInterest() {}
+
+    /** @brief Tests whether this PointOfInterest has the specified name or not, ignoring case.
+      * @param name The name to test as a string.
+      * @return true if the given name matches the PointOfInterest's name (not case-sensitive), false if not.
+      */
+    bool hasName ( string name ) const;
+
+    /** @brief Tests whether this PointOfInterest has the specified string in its name or not, ignoring case.
+     * @param part The string to test.
+     * @return true if the given string is contained in the PointOfInterest's name (not case-sensitive), false if not.
+     */
+    bool hasInName ( string part ) const;
+
+    // SETTER FUNCTIONS /////////////////////////////
+
+    /** @param remarks Changes the contents of remarks attribute. */
+    void setRemarks ( string remarks ) { m_Remarks = remarks; }
+
+    void setName ( string name ) { m_Name = name; }
+
+    /** @brief Add a new String to the StringMap */
+    void addString ( string key, string data ) { m_StringMap.insert ( pair< string, string > ( key, data ) ); }
+
+    /** @brief Add a new Float to the FloatMap */
+    void addFloat ( string key, float data ) { m_FloatMap.insert ( pair< string, float > ( key, data ) ); }
+
+    /** @brief Add a new Int to the IntMap */
+    void addInt ( string key, int data ) { m_IntMap.insert ( pair< string, int > ( key, data ) ); }
+
+    // GETTER FUNCTIONS //////////////////////
+
+    /** @return id attribute */
+    int getId() const { return m_Id; }
+
+    /** @return name attribute */
+    string getName() const { return m_Name; }
+
+    /** @return type attribute */
+    PoiType getType() const { return m_Type; }
+
+    /** @return remarks attribute */
+    string getRemarks() const { return m_Remarks; }
+
+    /** Get the string to the matching string */
+    string getString ( string key ) const { return m_StringMap.find ( key )->second; }
+
+    /** Get the float to the matching string */
+    float getFloat ( string key ) const { return m_FloatMap.find ( key )->second; }
+
+    /** Get the Int to the matching string */
+    int getInt ( string key ) const { return m_IntMap.find ( key )->second; }
+
+    /** Get the string map */
+    map<string, string> getStringMap() const { return m_StringMap; }
+
+    /** Get the float map */
+    map<string, float> getFloatMap() const { return m_FloatMap; }
+
+    /** Get the int map */
+    map<string, int> getIntMap() const { return m_IntMap; }
+
+
+    // (DE)SERIALIZATION ///////////////////////////////////
+    //TODO
+    //    void storer ( ExtendedOutStream& ) const ;
+//    PointOfInterest ( ExtendedInStream& );
+
+    /** @brief print description */
+    void printOn ( ostream& strm ) const;
+
+  private:
+
+    void init ( int id, string name, PoiType type, string remarks, StringMapT stringMap, FloatMapT floatMap, IntMapT intMap );
+
+    int m_Id;
+    string m_Name;
+    PoiType m_Type;
+    string m_Remarks;
+
+    StringMapT m_StringMap;
+    IntMapT m_IntMap;
+    FloatMapT m_FloatMap;
+
+};
+
+#endif

homer_map_manager/include/homer_map_manager/map_manager_node.h

+#ifndef MAP_MANAGER_NODE_H
+#define MAP_MANAGER_NODE_H
+
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+
+#include <homer_map_manager/Managers/MapManager.h>
+#include <homer_map_manager/Managers/PoiManager.h>
+#include <homer_map_manager/Managers/RoiManager.h>
+#include <homer_map_manager/Managers/MaskingManager.h>
+
+#include "std_msgs/String.h"
+#include "std_msgs/Int32.h"
+#include "std_msgs/Empty.h"
+#include "std_srvs/Empty.h"
+#include "nav_msgs/OccupancyGrid.h"
+#include <homer_mapnav_msgs/GetPointsOfInterest.h>
+#include <homer_mapnav_msgs/GetRegionsOfInterest.h>
+#include <homer_mapnav_msgs/GetRegionOfInterestName.h>
+#include <homer_mapnav_msgs/PointInsideRois.h>
+#include <homer_mapnav_msgs/ModifyPOI.h>
+#include <homer_mapnav_msgs/ModifyMap.h>
+#include <homer_mapnav_msgs/MapLayers.h>
+#include <homer_mapnav_msgs/SaveMap.h>
+#include <homer_mapnav_msgs/LoadMap.h>
+#include <homer_mapnav_msgs/RoiChange.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <homer_map_manager/MapIO/map_saver.h>
+#include <homer_map_manager/MapIO/map_loader.h>
+#include <homer_nav_libs/tools.h>
+
+/** @class MapManagerNode
+  * @author Malte Knauf, Viktor Seib
+  * @brief This class handles incoming and outgoing messages and redirects them to the according modules
+  */
+class MapManagerNode
+{
+public:
+    MapManagerNode(ros::NodeHandle* nh);
+    ~MapManagerNode();
+
+private:
+
+    /** callback functions */
+
+    /** callbacks of MapManagerModule */
+    void callbackSLAMMap( const nav_msgs::OccupancyGrid::ConstPtr& msg );
+    void callbackSaveMap( const std_msgs::String::ConstPtr& msg );
+    void callbackLoadMap( const std_msgs::String::ConstPtr& msg );
+    void callbackMapVisibility( const homer_mapnav_msgs::MapLayers::ConstPtr& msg );
+    void callbackResetMaps( const std_msgs::Empty::ConstPtr& msg );
+
+    /** laser scan callback */
+    void callbackLaserScan( const sensor_msgs::LaserScan::ConstPtr& msg );
+    void callbackBackLaser( const sensor_msgs::LaserScan::ConstPtr& msg );
+    void callbackFrontLaser(const sensor_msgs::LaserScan::ConstPtr& msg );
+
+    /** callbacks of PointOfInterestManagerModule */
+    void callbackAddPOI( const homer_mapnav_msgs::PointOfInterest::ConstPtr& msg );
+    void callbackModifyPOI( const homer_mapnav_msgs::ModifyPOI::ConstPtr& msg );
+    void callbackDeletePOI( const std_msgs::String::ConstPtr& msg );
+
+    /** callbacks of RegionOfInterestManagerModule */
+    void callbackAddROI( const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg );
+    void callbackModifyROI( const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg );
+    void callbackDeleteROIbyName( const std_msgs::String::ConstPtr& msg );
+    void callbackDeleteROIbyID( const std_msgs::Int32::ConstPtr& msg );
+
+    
+    void callbackOctomapMap( const nav_msgs::OccupancyGrid::ConstPtr& msg);
+    void callbackRapidMap( const nav_msgs::OccupancyGrid::ConstPtr& msg);
+
+    /** callback of MaskingMapModule */
+    void callbackModifyMap( const homer_mapnav_msgs::ModifyMap::ConstPtr& msg );
+
+    /** service of PointOfInterestModule to get a list with all Points Of Interest */
+    bool getPOIsService(homer_mapnav_msgs::GetPointsOfInterest::Request& req,
+                        homer_mapnav_msgs::GetPointsOfInterest::Response& res);
+
+    /** service of RegionOfInterestModule to get a list with all Regions Of Interest */
+    bool getROIsService(homer_mapnav_msgs::GetRegionsOfInterest::Request& req,
+                        homer_mapnav_msgs::GetRegionsOfInterest::Response& res);
+                        
+    bool pointInsideRoisService(homer_mapnav_msgs::PointInsideRois::Request& req,
+						homer_mapnav_msgs::PointInsideRois::Response& res);
+						
+    bool getROINameService(homer_mapnav_msgs::GetRegionOfInterestName::Request& req,
+						homer_mapnav_msgs::GetRegionOfInterestName::Response& res);
+
+    void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);    
+
+	/** Service for saving a map**/
+    bool saveMapService(homer_mapnav_msgs::SaveMap::Request& req,
+						homer_mapnav_msgs::SaveMap::Response& res);
+
+	/** Service for loading a map**/
+    bool loadMapService(homer_mapnav_msgs::LoadMap::Request& req,
+						homer_mapnav_msgs::LoadMap::Response& res);
+
+	/** Service resetting the current map**/
+    bool resetMapService(std_srvs::Empty::Request& req,
+						std_srvs::Empty::Response& res);
+    /** modules that are included in this node */
+    MapManager* m_MapManager;
+    PoiManager* m_POIManager;
+    RoiManager* m_ROIManager;
+    MaskingManager* m_MaskingManager;
+
+    //subscriptions of MapManagerModule here
+    ros::Subscriber m_SLAMMapSubscriber;
+    ros::Subscriber m_SaveMapSubscriber;
+    ros::Subscriber m_LoadMapSubscriber;
+    ros::Subscriber m_MapVisibilitySubscriber;
+    ros::Subscriber m_LaserScanSubscriber;
+    ros::Subscriber m_BackLaserScanSubscriber;
+    ros::Subscriber m_FrontLaserScanSubscriber;
+
+    //subscriptions of PointOfInterestManagerModule
+    ros::Subscriber m_AddPOISubscriber;
+    ros::Subscriber m_ModifyPOISubscriber;
+    ros::Subscriber m_DeletePOISubscriber;
+
+    //subscriptions of RegionOfInterestManagerModule
+    ros::Subscriber m_AddROISubscriber;
+    ros::Subscriber m_ModifyROISubscriber;
+    ros::Subscriber m_DeleteROIByIDSubscriber;
+    ros::Subscriber m_DeleteROIByNameSubscriber;
+    ros::Subscriber m_PoiInsideROISubscriber;
+    ros::Subscriber m_PoseSubscriber;
+    ros::Publisher  m_RoiPollPublisher;
+
+    //subscriptions of MaskingMapModule
+    ros::Subscriber m_ModifyMapSubscriber;
+    ros::Subscriber m_ResetMapsSubscriber;
+
+    /** publisher for loaded maps */
+    ros::Publisher m_LoadedMapPublisher;
+
+    /** publisher to mask slam map */
+    ros::Publisher m_MaskSlamMapPublisher;
+
+    /** service of PointOfInterstModule */
+    ros::ServiceServer m_GetPOIsService;
+
+    /** service of RegionOfInterstModule */
+    ros::ServiceServer m_GetROIsService;
+	ros::ServiceServer m_GetROINameService;
+	ros::ServiceServer m_PoiInsideROISService;
+
+	/** Service for saving the current Map to a file **/
+	ros::ServiceServer m_SaveMapService;
+	/** Resetting the current map**/
+	ros::ServiceServer m_ResetMapService;
+	/** Service for loading a previously saved map **/
+	ros::ServiceServer m_LoadMapService;
+	
+
+	tf::TransformListener m_TransformListener;
+
+    /** timestamp of last incoming laser scan message */
+    ros::Time m_LastLaserTime;
+    
+    ros::Subscriber m_OctomapSubscriber;
+    ros::Subscriber m_RapidMapSubscriber;
+    
+    nav_msgs::OccupancyGrid::ConstPtr m_highSensitiveMap;
+    
+    bool m_roi_polling;
+    std::vector<int> m_ids;
+    ros::Time m_lastROIPoll;
+    float m_roi_polling_time;
+};
+
+#endif // MAP_MANAGER_NODE_H

homer_map_manager/mainpage.dox

+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b map_manager 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/

homer_map_manager/manifest.xml

+<package>
+  <description brief="map_manager">
+
+     map_manager
+
+  </description>
+  <author>Malte Knauf</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/map_manager</url>
+
+  <depend package="roscpp"/>
+  <depend package="roslib"/>
+  <depend package="tf"/>
+
+  <depend package="robbie_architecture"/>
+  <depend package="BaseLib"/>
+  <depend package="map_messages"/>
+
+</package>
+
+

homer_map_manager/package.xml

+<package>
+  <name>homer_map_manager</name>
+  <version>0.1.1</version>
+  <description>
+     map_manager
+  </description>
+  <maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
+  <author email="mknauf@uni-koblenz.de">Malte Knauf</author>
+  <license>GPLv3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>roslib</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>homer_mapnav_msgs</build_depend>
+  <build_depend>homer_nav_libs</build_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>eigen</build_depend>
+  <build_depend>sdl</build_depend>
+  <build_depend>sdl-image</build_depend>
+  <build_depend>yaml-cpp</build_depend>
+  <build_depend>std_srvs</build_depend>
+  
+  <run_depend>roscpp</run_depend>
+  <run_depend>roslib</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>homer_mapnav_msgs</run_depend>
+  <run_depend>homer_nav_libs</run_depend>
+  <run_depend>eigen</run_depend>
+  <run_depend>sdl</run_depend>
+  <run_depend>sdl-image</run_depend>
+  <run_depend>yaml-cpp</run_depend>
+  <run_depend>std_srvs</run_depend>
+
+</package>
+
+

homer_map_manager/src/Managers/MapManager.cpp

+#include <homer_map_manager/Managers/MapManager.h>
+#include <omp.h>
+
+MapManager::MapManager(ros::NodeHandle* nh)
+{
+    m_MapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map", 1);
+
+	m_map_layers.push_back( homer_mapnav_msgs::MapLayers::SLAM_LAYER);
+	m_map_layers.push_back(	homer_mapnav_msgs::MapLayers::KINECT_LAYER);
+	m_map_layers.push_back(	homer_mapnav_msgs::MapLayers::RAPID_MAP);
+	m_map_layers.push_back(	homer_mapnav_msgs::MapLayers::MASKING_LAYER);
+	m_laser_layers.push_back( homer_mapnav_msgs::MapLayers::HOKUYO_FRONT); 
+	m_laser_layers.push_back( homer_mapnav_msgs::MapLayers::SICK_LAYER);
+							   
+    //enable all map layers
+    
+    for(int i = 0; i< m_map_layers.size(); i++)
+    {
+    	m_MapVisibility[m_map_layers[i]] = true;
+    }
+    
+    for(int i = 0; i< m_laser_layers.size(); i++)
+    {
+    	m_MapVisibility[m_laser_layers[i]] = true;
+    }
+
+    m_Height 			= -1;
+    m_Width 			= -1;
+    m_Resolution 		= -1;
+    
+	try
+	{
+		m_TransformListener.waitForTransform("/base_link", "/laser", ros::Time(0), ros::Duration(3));
+		m_TransformListener.lookupTransform	("/base_link", "/laser", ros::Time(0), m_sick_transform);	
+		m_TransformListener.lookupTransform	("/base_link", "/hokuyo_front", ros::Time(0), m_hokuyo_transform);	
+        m_got_transform = true;
+    }
+    catch (tf::TransformException ex)
+    {
+		ROS_ERROR("%s",ex.what());
+		m_got_transform = false;
+    }
+    geometry_msgs::PoseStamped pose; 
+    pose.pose.position.x = 0;
+    pose.pose.position.y = 0;
+    pose.pose.position.z = 0;
+    pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);    
+    
+	m_pose = boost::make_shared<geometry_msgs::PoseStamped>(pose) ;
+    
+}
+
+MapManager::~MapManager()
+{
+}
+
+void MapManager::updateMapLayer(int type, nav_msgs::OccupancyGrid::ConstPtr layer)
+{
+    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();
+    }
+}
+
+void MapManager::clearMapLayers()
+{
+    m_MapLayers.clear();
+}
+
+void MapManager::toggleMapVisibility(int type, bool state)
+{
+    ROS_INFO_STREAM("MapManager: " << type << ": " << state);
+    m_MapVisibility[type] = state;
+}
+    
+void MapManager::updateLaser(int layer, const sensor_msgs::LaserScan::ConstPtr &msg)
+{
+	m_laserLayers[layer] = msg;
+}
+
+nav_msgs::OccupancyGrid::ConstPtr MapManager::getMapLayer(int type)
+{
+    if(m_MapLayers.find(type) == m_MapLayers.end())
+        return nav_msgs::OccupancyGrid::ConstPtr();
+    return m_MapLayers[type];
+}
+
+/**
+ * Sends the SLAM map (OccupancyGrid) and (if available and enabled) other merged map layers to the gui node
+ *
+ */
+void MapManager::sendMergedMap()
+{
+
+    if ( m_MapLayers.find(homer_mapnav_msgs::MapLayers::SLAM_LAYER) == m_MapLayers.end() )
+    {
+      ROS_DEBUG_STREAM( "SLAM map is missing!" );
+      return;
+    }
+    int k;
+    nav_msgs::OccupancyGrid mergedMap( *(m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]));
+    //bake maps
+    for(int j = 1; j < m_map_layers.size(); j++)
+    {
+      if ( m_MapLayers.find(m_map_layers[j]) != m_MapLayers.end()
+         && m_MapVisibility[m_map_layers[j]])
+	   	{
+            //calculating parallel on 8 threads
+            omp_set_num_threads(8);
+            size_t mapsize = m_MapLayers[m_map_layers[j]]->info.height * m_MapLayers[m_map_layers[j]]->info.width;
+            const std::vector<signed char> *tempdata = &m_MapLayers[m_map_layers[j]]->data;
+            const int frei = homer_mapnav_msgs::ModifyMap::FREE;
+            signed char currentvalue = 0;
+            #pragma omp parallel for private(currentvalue) shared(mapsize, tempdata, mergedMap)
+            for (int i = 0; i < mapsize; i++ )
+			{
+                currentvalue = tempdata->at(i);
+                if(currentvalue > 50 || currentvalue == frei)
+				{
+                    mergedMap.data[i] = currentvalue;
+				}
+			}	
+		}
+    }
+    //bake Lasers
+
+	try
+	{
+		int data ;
+		if(m_got_transform)
+		{
+			for(int j = 0; j < m_laser_layers.size(); j++)
+			{
+				if ( m_laserLayers.find(m_laser_layers[j]) != m_laserLayers.end()
+						&& m_MapVisibility[m_laser_layers[j]])
+				{
+					if(m_laser_layers[j] == homer_mapnav_msgs::MapLayers::SICK_LAYER)
+					{
+						data = homer_mapnav_msgs::ModifyMap::BLOCKED;
+					}
+					else if(m_laser_layers[j] == homer_mapnav_msgs::MapLayers::HOKUYO_BACK)
+					{
+						data = homer_mapnav_msgs::ModifyMap::HOKUYO;
+					}
+					else if(m_laser_layers[j] == homer_mapnav_msgs::MapLayers::HOKUYO_FRONT)
+					{
+						data = homer_mapnav_msgs::ModifyMap::HOKUYO;
+					}
+					tf::StampedTransform pose_transform;
+					m_TransformListener.waitForTransform("/map", "/base_link", m_laserLayers[m_laser_layers[j]]->header.stamp, ros::Duration(0.09));
+					m_TransformListener.lookupTransform("/map", "/base_link", m_laserLayers[m_laser_layers[j]]->header.stamp , pose_transform);
+
+				//	pose_transform.setTransform(Transform);		
+
+					for ( int i = 0; i < m_laserLayers[m_laser_layers[j]]->ranges.size(); i++ )
+					{  
+						if(m_laserLayers[m_laser_layers[j]]->ranges[i] < m_laserLayers[m_laser_layers[j]]->range_max &&
+								m_laserLayers[m_laser_layers[j]]->ranges[i] > m_laserLayers[m_laser_layers[j]]->range_min)
+						{
+							float alpha = m_laserLayers[m_laser_layers[j]]->angle_min + i * m_laserLayers[m_laser_layers[j]]->angle_increment;
+							geometry_msgs::Point point;
+							tf::Vector3 pin;
+							tf::Vector3 pout;
+							pin.setX( cos(alpha) * m_laserLayers[m_laser_layers[j]]->ranges[i]);
+							pin.setY( sin(alpha) * m_laserLayers[m_laser_layers[j]]->ranges[i]);
+							pin.setZ(0);
+
+							if(m_laser_layers[j] == homer_mapnav_msgs::MapLayers::SICK_LAYER)
+							{
+								pout = pose_transform * m_sick_transform * pin;
+							}
+							else if(m_laser_layers[j] == homer_mapnav_msgs::MapLayers::HOKUYO_FRONT)
+							{
+								pout = pose_transform * m_hokuyo_transform * pin;
+							}
+							point.x = pout.x();
+							point.y = pout.y();
+							point.z = 0;
+
+							Eigen::Vector2i map_point = map_tools::toMapCoords(point, m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]->info.origin, m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]->info.resolution);
+							k = map_point.y() * m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]->info.width + map_point.x();
+							if(k < 0 || k > m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]->data.size())
+							{
+								continue;
+							}
+							else
+							{
+								mergedMap.data[k] = data;
+							}
+						}
+					}
+				}
+			}
+		}
+		else
+		{
+			try
+			{
+				if(m_TransformListener.waitForTransform("/base_link", "/laser", ros::Time(0), ros::Duration(0.1)))
+				{
+					m_TransformListener.lookupTransform	("/base_link", "/laser", ros::Time(0), m_sick_transform);	
+					m_TransformListener.lookupTransform	("/base_link", "/hokuyo_front", ros::Time(0), m_hokuyo_transform);	
+					m_got_transform = true;
+				}
+			}
+			catch (tf::TransformException ex)
+			{
+				ROS_ERROR("%s",ex.what());
+				m_got_transform = false;
+			}
+		}
+	}
+	catch(tf::TransformException ex)
+	{
+		ROS_ERROR_STREAM(ex.what());
+	}
+    mergedMap.header.stamp = ros::Time::now();
+    mergedMap.header.frame_id = "map";
+    
+    m_MapPublisher.publish(mergedMap);
+    ROS_DEBUG_STREAM("Publishing map");
+}