Commit 8e600b7a authored by Niklas Yann Wettengel's avatar Niklas Yann Wettengel
Browse files

Trimmed the branch to only the homer_mapnav_msgs sub directory

parent fb66f874
# mapping
# map_messages
See:
* [mapping]
* [navigation]
* [map_manager]
* [map_msgs]
* [nav_libs]
## 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]: 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
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package homer_map_manager
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.1 (2016-11-03)
------------------
* fixes
* initial commit
* Contributors: Niklas Yann Wettengel
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}
)
# 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
This diff is collapsed.
#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
#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
#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: