Commit 7821b2d6 authored by Niklas Yann Wettengel's avatar Niklas Yann Wettengel
Browse files

Trimmed the branch to only the homer_navigation sub directory

parent 33563858
# mapping
# homer_navigation
See:
## Introduction
* [mapping]
* [navigation]
* [map_manager]
* [map_msgs]
* [nav_libs]
Das Package "homer_navigation" mit seiner gleichnamigen Node ist für die Navigation des Roboters zuständig. Es hält stets die aktuelle Karte des Roboters vor, die auf Topic /map empfangen wurde und berechnet anhand von dieser Karte einen Pfad vom Roboter zu einem gegebenen Zielpunkt. Dafür verwendet die Navigation die sogenannte Pfad-Transformation - eine Kombination aus Distanz-Transformation und Hindernistransformation -, in Verbindung mit A-Stern, um einen optimalen Pfad zu finden. Das Verhältnis zwischen Distanz- und Hindernis-Transformation kann über einen Parameter angepasst werden.
Nach der Pfadberechnung ist das Package außerdem dafür zuständig den Roboter diesen Pfad zum Ziel abfahren zu lassen.
[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
## Topics
#### Publisher
* `/homer_navigation/target_reached (std_msgs/String)`: Wenn der Roboter sein Ziel erreicht hat, wird eine Message über dieses Topic veschickt.
* `/homer_navigation/target_unreachable (mapnav_msgs/TargetUnreachable)`: Über dieses Topic wird mitgeteilt, wenn der Roboter sein Ziel nicht erreichen kann und die Navigation abgebrochen wurde. Ein Statuscode wird mitgeliefert (siehe Package map_messages).
* `/homer_navigation/path (nav_msgs/Path)`: Hierüber wird der aktuelle Pfad zur Visualisierung an die GUI geschickt.
* `/robot_platform/cmd_vel (geometry_msgs/Twist)`: Über dieses Topic werden die aktuellen Fahrbefehle zum Roboter geschickt.
* `/ptu/set_pan_tilt (ptu/SetPanTilt)`: Über dieses Topic wird die PTU nach navigation auf 0, 0 gefahren.
* `/ptu/center_world_point (ptu/CenterWorldPoint)`: Über dieses Topic wird die PTU auf den nächsten Wegpunkt centriert.
#### Subscriber
* `/map (nav_msgs/OccupancyGrid)`: Die jeweils aktuelle Map wird empfangen, um sie für die Pfadplanung und Hindernisvermeidung während der Navigation zu verwenden.
* `/pose (geometry_msgs/PoseStamped)`: Die aktuelle Pose des Roboters wird als Startpunkt der Pfadplanung verwendet.
* `/scan (sensor_msgs/LaserScan)`: Der aktuelle LaserScan wird zur Hindernisvermeidung verwendet.
* `/front_scan (sensor_msgs/LaserScan)`: Der aktuelle LaserScan wird zur Hindernisvermeidung verwendet.
* `/homer_navigation/start_navigation (mapnav_msgs/StartNavigation)`: Startet die Pfadplanung und anschließend die Navigation zur mitgelieferten Zielpose.
* `/move_base_simple/goal (geometry_msgs/PoseStamped)`: Started die Pfadplanung und anschließende Navigation zur Zielpose.
* `/homer_navigation/stop_navigation (std_msgs/Empty)`: Stoppt die aktuelle Navigation.
* `/homer_navigation/navigate_to_POI (mapnav_msgs/NavigateToPOI)`: Startet die Pfadplanung und anschließend die Navigation zum POI mit angegebenen Namen. Der entsprechende POI wird anschließend vom map_manager erfragt.
* `/homer_navigation/unknown_threshold (std_msgs/Int8)`: Über dieses Topic kann der Schwellwert verändert werden, ab dem ein Zellwahrscheinlichkeit als belegt und damit unbefahrbar markiert wird. Standard ist 50 (%).
* `/homer_navigation/refresh_params (std_msgs/Empty)`: Über dieses Topic lassen sich die Parameter neu auslesen.
* `/homer_navigation/max_depth_move_distance (std_msgs/Float32)`: Über dieses Topic lässt sich die Hindernissdistance der Tiefendaten übermitteln.
## Launch Files
* `homer_navigation.launch:` Startet die Navigation und lädt alle Navigations-Paramter in den Parameterserver.
## Parameter
### homer_navigation
* `/homer_navigation/safe_path_weight:` 1.2 # factor weight for safer path in relation to shortest path
* `/homer_navigation/waypoint_sampling_threshold:` 1.5 # factor of how dense the path waypoints are sampled regarding the obstacle_distance of the last or next waypoint
* `/homer_navigation/frontier_safeness_factor:` 1.4 # factor of min_allowed_obstacle_distance to an obstacle of a cell which is considered safe
### cost calculation parameters
* `/homer_navigation/allowed_obstacle_distance/min:` 0.3 # m robot must stay further away than this from obstacles
* `/homer_navigation/allowed_obstacle_distance/max:` 5.0 # m not used at the moment
* `/homer_navigation/safe_obstacle_distance/min:` 0.7 # m if possible robot should move further away than this from obstacles
* `/homer_navigation/safe_obstacle_distance/max:` 1.5 # m further away than this from obstacles doesn't give a lesser cost addition
### collision Avoidance parameters
* `/homer_navigation/collision_distance:` 0.3 # m distance to obstacle from robotFront in which the obstacle avoidance will be executed
* `/homer_navigation/collision_distance_near_target:` 0.2 # m distance to obstacle from robotFront where obstacle avoidance won't be executed when near the target
* `/homer_navigation/backward_collision_distance:` 0.5 # m distance behind robot in which the robot won't back up into while doing collision avoidance
* `/homer_navigation/min_y:` 0.27 # m half robot width for max_move_distance calculation
* `/homer_navigation/min_x:` 0.3 # m distance from base_link to robot front for max_move_distance calculation
### check path on map update
* `/homer_navigation/check_path:` true # bool toggles if the calculated path will be checked for obstacles while navigating
* `/homer_navigation/check_path_max_distance:` 2 # m maximal distance from robot position in which the path is being checked for obstacles
### speed parameters
* `/homer_navigation/min_turn_angle:` 0.15 # rad values lower than this angle will let the navigation assume reaching the designated position
* `/homer_navigation/max_turn_speed:` 0.6 # rad/s max turn velocity the navigation can send
* `/homer_navigation/min_turn_speed:` 0.3 # rad/s min turn speed for Final Turn so the Robot doesn't stop turning
* `/homer_navigation/max_drive_angle:` 0.6 # rad threshold for driving and turning - if above that value only turn
* `/homer_navigation/max_move_speed:` 0.4 # m/s max move speed the navigation can send
### caution factors values near 0 mean high caution values greater values mean less caution
### if any factor equals 0 the robot can't follow paths !!
* `/homer_navigation/map_speed_factor:` 1.2 # factor for the max speed calculation of the obstacleDistancemap
* `/homer_navigation/waypoint_speed_factor:` 1.2 # factor for the max speed calculation with the distance to the next waypoint
* `/homer_navigation/obstacle_speed_factor:` 1.0 # factor for the max speed calculation with the last laser may movement distance
* `/homer_navigation/callback_error_duration:` 0.3 # s max duration between pose and laser callbacks before error handling is executed
* `/homer_navigation/use_ptu:` false# bool toggles if the ptu is being used to look at the next Waypoint during navigation
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
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:
/** 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
#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;