Skip to content
Snippets Groups Projects
Commit c42291a5 authored by Niklas Yann Wettengel's avatar Niklas Yann Wettengel
Browse files

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

This reverts commit d5e27568.
parent d5e27568
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 5132 additions and 12 deletions
README.md
+ 2
− 12
View file @ c42291a5
# nav_libs
# mapping and navigation
## Known Issues / Todo's
Im Package nav_libs ist eine Kopie der Robbie-Bibliothek Math, umbenannt in MappingMath, die nur von homer_mapping benutzt werden soll, solange es keine vernünftige Alternative für die Pose gibt.
## Introduction
Das Package nav_libs enthält einige Bibliotheken, die vom Package homer_mapping und nav_libs verwendet werden. Außerdem enthält es im Ordner tools die Header-Datei tools.h, die Funktionen zum Transformieren in verschiedene Koordinatenframes enthält. All diese Funktionen befinden sich im namespace "map_tools".
* Die Bibliothek `Explorer` wird von homer_mapping und homer_navigation verwendet und enthält die Pfadplanungsalgorithmen A-Stern sowie die dafür benötigte Datenstruktur der GridMap.
* Die Bibliothek `SpeedControl` wird von homer_navigation verwendet und ist dafür zuständig abhängig von den aktuellen Laserdaten die höchstzulässige Geschwindigkeit zu berechnen.
* Die Bibliothek `MappingMath` wird von homer_mapping verwendet und enthält die Datenstruktur Pose, in der die aktuelle Roboterposition innerhalb der Node gespeichert wird.
Here you can find the homer mapping and navigation packages
homer_map_manager/CHANGELOG.rst 0 → 100644
+ 8
− 0
View file @ c42291a5
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package homer_map_manager
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.0.1 (2015-09-08)
------------------
* init
* Contributors: Raphael Memmesheimer
homer_map_manager/CMakeLists.txt 0 → 100644
+ 35
− 0
View file @ c42291a5
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)
find_package( Eigen REQUIRED )
include_directories(
${catkin_INCLUDE_DIRS}
${EIGEN_INCLUDE_DIRS}
$ENV{HOMER_DIR}
)
add_subdirectory(${PROJECT_SOURCE_DIR}/src/Managers)
add_subdirectory(${PROJECT_SOURCE_DIR}/src/MapIO)
catkin_package(
CATKIN_DEPENDS
roscpp
)
add_executable(map_manager src/map_manager_node.cpp)
target_link_libraries(
map_manager
${catkin_LIBRARIES}
Managers
image_io
)
add_dependencies(
map_manager
${catkin_EXPORTED_TARGETS}
homer_mapnav_msgs_gencpp
)
homer_map_manager/README.md 0 → 100644
+ 42
− 0
View file @ c42291a5
# 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/config/custom.xml 0 → 100644
+ 3
− 0
View file @ c42291a5
<map_manager>
</map_manager>
homer_map_manager/config/default.xml 0 → 100644
+ 327
− 0
View file @ c42291a5
<default>
<!-- MAPPING SETTINGS -->
<Map>
<!-- Path to a default map file that is loaded at start (e.g. maps/test.map). If value is not set, no map is loaded -->
<value name="sDefaultMapFile" value=""/>
<!-- Size of the map in x and y direction. -->
<!-- Note: the robot's initial pose is in the middle
of the map, so choose an adequate size.
i.e. if you set it to 2000, the map size will be
2000 * 2000 mm and the robot's start position is at (1000, 1000) -->
<!-- Note: values > 50000 may cause segmentation faults -->
<value name="iSize" value="35000"/>
<!-- Size of one cell of the map in mm.
The grid map will be of size iSize/iCellSize+1 -->
<value name="iCellSize" value="50"/>
<!-- Minimum occupancy probability for treating a map pixel as obstacle -->
<value name="fMinObstacleOccupancy" value="0.5"/>
<!-- This variabe is true, if dynamic obstacles are integrated in the map. -->
<value name="bDynamicMapping" value="1"/>
<!-- This variabe is true, if a loaded map shall be updated by the current laserdata.
If it is false, the map is just used for localization. -->
<value name="bUpdateLoadedMap" value="0"/>
<!-- Minimum distance (in mm) between two samples for probability calcualation
Values above 1.0 speed things up, but can result in lower accuracy -->
<value name="fMeasureSamplingStep" value="150.0"/>
<!-- Enable checking to avoid matching front- and backside of obstacles, e.g. walls.
Useful when creating high resolution maps. -->
<value name="bBacksideChecking" value="0"/>
<!-- Leaves a small border around obstacles unchanged when inserting a laser scan.
Useful when creating high resolution maps. -->
<value name="bObstacleBorders" value="1"/>
<!-- Options for map generation based on the sonar sensors -->
<Sonar>
<!-- If the sonar sensors detect an obstacle at a larger distance than this value,
only a free beam is inserted into the map, the position of the detected obstacle
is left blank. -->
<value name="fMaxTrustedObstacleDistance" value="800.0"/>
<!-- Minimal movement of the robot [deg/mm] when inserting a new measurement -->
<value name="fMinMoveAngle" value="2.0"/>
<value name="fMinMoveDistance" value="5.0"/>
<!-- Minimal difference between last and current sonar measurement when inserting -->
<value name="iMinSonarDiff" value="50"/>
</Sonar>
<Geotiff>
<!-- Standard file name for geotiff export
As the filename is printed on the geotiff map, a reasonable filename has to be choosen
During a competition this value should be set for providing backup savings-->
<value name="sStandardFileName" value="Resko.tiff"/>
<!-- Number of necessary observations of a cell to be displayed without transparency.
Transparency is approached via different gray level.-->
<value name="iTrancparencyThreshold" value="10"/>
<!-- Threshold to mark an occupied cell as an obstacle in the geotiff map.
In percentage of reliability. -->
<value name="iObstacleThreshold" value="90"/>
<Visibility>
<!-- 1 if the inaccessible regions shall be displayed in geotiff. -->
<value name="bShowInaccessibleRegions" value="1"/>
<!-- 1 if the navigation map shall be displayed in geotiff. -->
<value name="bShowNavigationMap" value="1"/>
<!-- 1 if the robot path shall be displayed in geotiff. -->
<value name="bShowRobotPath" value="1"/>
<!-- Sizes of the geotiff map.
Attention: all this values influence the map resolution -->
</Visibility>
<Sizes>
<!-- The width (in mm) of one forground grid cell in an exported map. -->
<value name="iGridWidth" value="500"/>
<!-- Thickness of the grid in the background of the explored area in mm -->
<value name="iExploredAreaGrid" value="10"/>
<!-- Size of the victim sign, radius in mm -->
<value name="iVictimLocation" value="350"/>
<!-- Size of the hazard sign, radius in mm -->
<value name="iHazardLocation" value="400"/>
<!-- Thickness of the robot path in mm -->
<value name="iRobotPath" value="20"/>
</Sizes>
<!-- Used color schemas for geotiff export. -->
<ColorSchema>
<NavigationMap>
<value name="iRed" value="160"/>
<value name="iGreen" value="230"/>
<value name="iBlue" value="160"/>
</NavigationMap>
<VictimLocation>
<value name="iRed" value="240"/>
<value name="iGreen" value="10"/>
<value name="iBlue" value="10"/>
</VictimLocation>
<HazardLocation>
<value name="iRed" value="255"/>
<value name="iGreen" value="100"/>
<value name="iBlue" value="30"/>
</HazardLocation>
<InitialRobotPosition>
<value name="iRed" value="250"/>
<value name="iGreen" value="200"/>
<value name="iBlue" value="0"/>
</InitialRobotPosition>
<RobotPath>
<value name="iRed" value="120"/>
<value name="iGreen" value="0"/>
<value name="iBlue" value="140"/>
</RobotPath>
<Obstacle>
<value name="iRed" value="0"/>
<value name="iGreen" value="40"/>
<value name="iBlue" value="120"/>
</Obstacle>
<FileInfo>
<value name="iRed" value="0"/>
<value name="iGreen" value="50"/>
<value name="iBlue" value="140"/>
</FileInfo>
<!-- Checkerbord pattern of two colors in the background of the unexplored region -->
<UnexploredAreaGrid>
<Light>
<value name="iRed" value="170"/>
<value name="iGreen" value="170"/>
<value name="iBlue" value="171"/>
</Light>
<Dark>
<value name="iRed" value="160"/>
<value name="iGreen" value="160"/>
<value name="iBlue" value="161"/>
</Dark>
</UnexploredAreaGrid>
<!-- Grid lines painted in the background in the explored region -->
<ExploredAreaGrid>
<value name="iRed" value="0"/>
<value name="iGreen" value="0"/>
<value name="iBlue" value="1"/>
</ExploredAreaGrid>
</ColorSchema>
</Geotiff>
</Map>
<!-- PARTICLE FILTER SETTINGS -->
<ParticleFilter>
<HyperSlamFilter>
<!-- Number of particle filters to use (hyper particles) -->
<value name="iParticleFilterNum" value="1"/>
<!-- Threshold for deleting particle filters (quotient between best/worst contrast) -->
<value name="fDeletionThreshold" value="0.98"/>
</HyperSlamFilter>
<!-- Number of particles to use -->
<value name="iParticleNum" value="1000"/>
<!-- The maximal rotation if mapping is performed. If the rotation is
bigger, mapping is interrupted.
This value may depend on the computing power, because it is influenced
by the size of time intervals of mapping. 0.35-->
<value name="fMaxRotationPerSecond" value="1.0"/> <!-- 0.55-->
<!-- Minimum time to wait between two mapping/localization steps -->
<value name="iWaitTime" value="500"/>
<!-- The map is only updated when the robot has turned a minimal angle (iUpdateMinMoveAngle),
has moved a minimal distance (iUpdateMinMoveDist) or a maximal time has passed( iMaxUpdateInterval ) -->
<value name="iUpdateMinMoveAngle" value="5"/>
<value name="iUpdateMinMoveDist" value="100"/>
<value name="iMaxUpdateInterval" value="2000"/>
<!-- Handle all incoming sensordata messages (AlwaysDeliver - value="1" - for SLAM benchmark) or only newest messages (keeponlynewest - value="0" - default) -->
<value name="bProcessAllMeasurements" value="0"/>
<!-- ASSUMED ERROR VALUES OF MOTION CALCULATION -->
<ErrorValues>
<!-- Rotation error while rotating in percent - old value: 50 20-->
<value name="fRotationErrorRotating" value="5.0"/>
<!-- Rotation error while translating in degrees per meter - old value: 5 2.0-->
<value name="fRotationErrorTranslating" value="5.0"/>
<!-- Translation error while translating in percent - old value: 15-25-->
<value name="fTranslationErrorTranslating" value="15.0"/>
<!-- Translation error while rotating in mm per degree - old value: 0.0-->
<value name="fTranslationErrorRotating" value="0.0"/>
<!-- Move jitter while turning in mm per degree - old value: 45800-->
<value name="fMoveJitterWhileTurning" value="30000.0"/>
</ErrorValues>
<!-- SETTINGS FOR SCAN MATCHING-->
<!-- SETTINGS FOR SCAN MATCHING-->
<ScanMatching>
<!-- If this variabe is true, scan matching is performed to improve the odometry data.
In a dynamic environment scan matching causes grave errors, especially if the
moving objects are close to the robot -->
<value name="bActivated" value="0"/>
<!-- Ignore odometry if true -->
<value name="bTest" value="0"/>
<!-- Used algorithm for scan matching.
1 = ICP, 2 = IDC, 3 = MbICP -->
<value name="iUsedAlgorithm" value="3"/>
<!-- Minimal point distance after reducing the number of scan points for
scan matching.
Value in mm. Value 0.0 means it is switched off. Standard: 200.0-->
<value name="fReducedPointDist" value="100.0"/>
<!-- Maximal distance of corresponding scan points, which are used
for scan matching. Value in mm. Standard: 300.0-->
<value name="fMaxCorrespondenceDiff" value="300.0"/>
<!-- Difference of scan angles which is considered to find
corresponding scan points for scan matching.
Value in degree. Standard: 90.0-->
<value name="fMaxAngleDiff" value="40.0"/>
<!-- Maximal distance of points in a cluster which is considered in
scan matching. Value in mm. 100.0-->
<value name="fMaxClusterDist" value="100.0"/>
<!-- Minimal diameter of a cluster of scan points which is considered
in scan matching.
Value in mm. Value 1.0 means it is switched of. Standard: 200.0-->
<value name="fMinClusterSize" value="200.0"/> <!--!!!!!!-->
<LineExtraction>
<!-- If this variable is true, the scan matcher works line-based.
If it is false, it works on the raw scan data.-->
<value name="bActivated" value="0"/>
<!-- Maximal distance of two consecutive points of a line -->
<value name="fMaxPointDistance" value="50.0"/>
<!-- Minimal number of points on a line -->
<value name="iMinPointsOnLine" value="10"/>
<!-- Minimal length of a line -->
<value name="fMinLength" value="100.0"/>
<!-- Maximal standard deviation of all points of a line-->
<value name="fMaxSigma" value="15.0"/>
</LineExtraction>
<MbICP>
<!--Factor which influences the behavior of the applied metric distance.
It describes the relation between rotation and translation.
If it is set to inifity, the behaviour of the MbIcpScanMatcher draws near to the IcpScanMatcher.
The value must be > 0. -->
<value name="fMetricFactor" value="1800.0"/>
</MbICP>
</ScanMatching>
</ParticleFilter>
<Kinect>
<!-- Settings for the kinect rgb camera -->
<Camera>
<!-- Note: sColorFormat can be RGB8 or Y8UV8 -->
<value name="sColorFormat" value="RGB8"/>
</Camera>
<!-- Determines whether to use Pointcloud(0) or Image(1) messages to compute our RGBDepthM -->
<value name="bUseImageTransport" value="1" />
<!-- Determines whether to use Image compression for transport of Images
If true(1) be sure to add a compression value (in percent) -->
<value name="bCompressImages" value="0" />
<value name="iCompressionValue" value="100" />
<value name="bSendContinuousDepthData" value="1" />
<!-- Determines how often RGBDepthDataM is updated -->
<value name="iIdleInterval" value="250" />
<value name="iTofIdleInterval" value="50" />
<!-- If set to true, RGBDepthM will be kept for painting and a 3DMap will be created
Always set to false, because of memory usage. -->
<value name="bCreate3DMap" value="0" />
<!-- 1: We are using the kinect instead of Laser Scanner data, affects PeopleTrackingModule (list may be incomplete)-->
<value name="bUseKinect" value="1" />
<value name="bUseKinectTracking" value="0" />
<value name="fTiltAngle" value="0"/>
<!-- If bMaskObstacles is true, then 3D Scan points with height greater
fMinObstacleHeight are marked inaccessible in the map -->
<value name="bMaskObstacles" value="1"/>
<!--value name="fMinObstacleHeight" value="140"/-->
<value name="fMinObstacleHeight" value="70"/>
<!-- Threshold for the maximum distance of points that are candidates as obstacles.
Needed because accuracy of the Kinect is decreasing by distance -->
<value name="fMaxObstacleFromRobotDistance" value="2000"/>
<value name="fMinObstacleFromRobotDistance" value="300"/>
<!-- How long is an obstcle saved after his last appearance in the kinect data -->
<value name="iObstacleSaveTime" value="10"/>
<value name="bDoKinectMappingWithTemporaryObstacles" value="1"/>
<!-- How often must an obstacle appear before it is known as an real obstacle -->
<value name="iObstacleMinAppearance" value="5"/>
</Kinect>
<SelfLocalization>
<value name="fScatterVarXY" value="50"/>
<!-- scattering variance for pose orientation in radiants -->
<value name="fScatterVarTheta" value="0.2"/>
</SelfLocalization>
<LaserScanners>
<SickLMS100>
<!-- Use this device in ActiveLaserScannerModule? -->
<value name="bActiveLaserScanner" value="0"/>
<!-- Is the device used for navigation? -->
<value name="bPassiveScan" value="1"/>
<!-- the ip and port the scanner is providing -->
<value name="sIP" value="169.254.97.144"/>
<value name="iPort" value="2111"/>
<!-- position of laserscanner in robot coordinates in mm -->
<value name="fPositionX" value="240"/>
<value name="fPositionY" value="0.0"/>
<value name="fPositionZ" value="40"/>
<!-- rotation radiuses of laserscanner beam in mm -->
<value name="fRotateRadiusY" value="0"/>
<value name="fRotateRadiusX" value="0"/>
<!-- orientation of laserscanner in robot coordinates
in degrees (counter clockwise) -->
<value name="fOrientation" value="-0.5"/>
<!-- valid maximum range (mm) -->
<value name="iValidMaxRange" value="30000"/>
<!-- valid minimum range (mm) -->
<value name="iValidMinRange" value="150"/>
<!-- minimum distance to be classified as free in case of errorneous measurement -->
<value name="iFreeReadingDistance" value="800"/>
<!-- number of scan points old 541-->
<value name="iScanSteps" value="1081"/>
<!-- scan angle in degrees -->
<value name="fScanAngle" value="270.0"/>
<!-- starting angle of one scan in degrees -->
<value name="fStartAngle" value="-135.0"/>
<value name="fObstacleThreshold" value="10"/>
<value name="iNumOffsetSteps" value="110" />
<value name="iNumBottomOffsetSteps" value="0"/>
<value name="iNumTopOffsetSteps" value="100"/>
<!-- iNumClearSlots for a 270Degree scanner. Only 180 Degree are used. Left and right 45 degree are deaktivated. For 0.5 resolution with 541 scanpoints we will need 90 points at each side masked. For security the Value for 0.5Degree Resolution is 100. If we use a 0.25 resulution we need 45Degree to masked out that means 180 Points + 10 Points security = 190 Points -->
<value name="iNumClearSlots" value="190"/>
<value name="sId" value="FrontLRF"/>
<!-- Set to 1 if scanner is mounted upside down -->
<value name="bMirror" value="1"/>
</SickLMS100>
</LaserScanners>
</default>
homer_map_manager/config/merged.xml 0 → 100644
+ 339
− 0
View file @ c42291a5
<?xml version="1.0" standalone="no" ?>
<MergedConfig>
<default>
<!-- MAPPING SETTINGS -->
<Map>
<!-- Path to a default map file that is loaded at start (e.g. maps/test.map). If value is not set, no map is loaded -->
<value name="sDefaultMapFile" value=""/>
<!-- Size of the map in x and y direction. -->
<!-- Note: the robot's initial pose is in the middle
of the map, so choose an adequate size.
i.e. if you set it to 2000, the map size will be
2000 * 2000 mm and the robot's start position is at (1000, 1000) -->
<!-- Note: values > 50000 may cause segmentation faults -->
<value name="iSize" value="35000"/>
<!-- Size of one cell of the map in mm.
The grid map will be of size iSize/iCellSize+1 -->
<value name="iCellSize" value="50"/>
<!-- Minimum occupancy probability for treating a map pixel as obstacle -->
<value name="fMinObstacleOccupancy" value="0.5"/>
<!-- This variabe is true, if dynamic obstacles are integrated in the map. -->
<value name="bDynamicMapping" value="1"/>
<!-- This variabe is true, if a loaded map shall be updated by the current laserdata.
If it is false, the map is just used for localization. -->
<value name="bUpdateLoadedMap" value="0"/>
<!-- Minimum distance (in mm) between two samples for probability calcualation
Values above 1.0 speed things up, but can result in lower accuracy -->
<value name="fMeasureSamplingStep" value="150.0"/>
<!-- Enable checking to avoid matching front- and backside of obstacles, e.g. walls.
Useful when creating high resolution maps. -->
<value name="bBacksideChecking" value="0"/>
<!-- Leaves a small border around obstacles unchanged when inserting a laser scan.
Useful when creating high resolution maps. -->
<value name="bObstacleBorders" value="1"/>
<!-- Options for map generation based on the sonar sensors -->
<Sonar>
<!-- If the sonar sensors detect an obstacle at a larger distance than this value,
only a free beam is inserted into the map, the position of the detected obstacle
is left blank. -->
<value name="fMaxTrustedObstacleDistance" value="800.0"/>
<!-- Minimal movement of the robot [deg/mm] when inserting a new measurement -->
<value name="fMinMoveAngle" value="2.0"/>
<value name="fMinMoveDistance" value="5.0"/>
<!-- Minimal difference between last and current sonar measurement when inserting -->
<value name="iMinSonarDiff" value="50"/>
</Sonar>
<Geotiff>
<!-- Standard file name for geotiff export
As the filename is printed on the geotiff map, a reasonable filename has to be choosen
During a competition this value should be set for providing backup savings-->
<value name="sStandardFileName" value="Resko.tiff"/>
<!-- Number of necessary observations of a cell to be displayed without transparency.
Transparency is approached via different gray level.-->
<value name="iTrancparencyThreshold" value="10"/>
<!-- Threshold to mark an occupied cell as an obstacle in the geotiff map.
In percentage of reliability. -->
<value name="iObstacleThreshold" value="90"/>
<Visibility>
<!-- 1 if the inaccessible regions shall be displayed in geotiff. -->
<value name="bShowInaccessibleRegions" value="1"/>
<!-- 1 if the navigation map shall be displayed in geotiff. -->
<value name="bShowNavigationMap" value="1"/>
<!-- 1 if the robot path shall be displayed in geotiff. -->
<value name="bShowRobotPath" value="1"/>
<!-- Sizes of the geotiff map.
Attention: all this values influence the map resolution -->
</Visibility>
<Sizes>
<!-- The width (in mm) of one forground grid cell in an exported map. -->
<value name="iGridWidth" value="500"/>
<!-- Thickness of the grid in the background of the explored area in mm -->
<value name="iExploredAreaGrid" value="10"/>
<!-- Size of the victim sign, radius in mm -->
<value name="iVictimLocation" value="350"/>
<!-- Size of the hazard sign, radius in mm -->
<value name="iHazardLocation" value="400"/>
<!-- Thickness of the robot path in mm -->
<value name="iRobotPath" value="20"/>
</Sizes>
<!-- Used color schemas for geotiff export. -->
<ColorSchema>
<NavigationMap>
<value name="iRed" value="160"/>
<value name="iGreen" value="230"/>
<value name="iBlue" value="160"/>
</NavigationMap>
<VictimLocation>
<value name="iRed" value="240"/>
<value name="iGreen" value="10"/>
<value name="iBlue" value="10"/>
</VictimLocation>
<HazardLocation>
<value name="iRed" value="255"/>
<value name="iGreen" value="100"/>
<value name="iBlue" value="30"/>
</HazardLocation>
<InitialRobotPosition>
<value name="iRed" value="250"/>
<value name="iGreen" value="200"/>
<value name="iBlue" value="0"/>
</InitialRobotPosition>
<RobotPath>
<value name="iRed" value="120"/>
<value name="iGreen" value="0"/>
<value name="iBlue" value="140"/>
</RobotPath>
<Obstacle>
<value name="iRed" value="0"/>
<value name="iGreen" value="40"/>
<value name="iBlue" value="120"/>
</Obstacle>
<FileInfo>
<value name="iRed" value="0"/>
<value name="iGreen" value="50"/>
<value name="iBlue" value="140"/>
</FileInfo>
<!-- Checkerbord pattern of two colors in the background of the unexplored region -->
<UnexploredAreaGrid>
<Light>
<value name="iRed" value="170"/>
<value name="iGreen" value="170"/>
<value name="iBlue" value="171"/>
</Light>
<Dark>
<value name="iRed" value="160"/>
<value name="iGreen" value="160"/>
<value name="iBlue" value="161"/>
</Dark>
</UnexploredAreaGrid>
<!-- Grid lines painted in the background in the explored region -->
<ExploredAreaGrid>
<value name="iRed" value="0"/>
<value name="iGreen" value="0"/>
<value name="iBlue" value="1"/>
</ExploredAreaGrid>
</ColorSchema>
</Geotiff>
</Map>
<!-- PARTICLE FILTER SETTINGS -->
<ParticleFilter>
<HyperSlamFilter>
<!-- Number of particle filters to use (hyper particles) -->
<value name="iParticleFilterNum" value="1"/>
<!-- Threshold for deleting particle filters (quotient between best/worst contrast) -->
<value name="fDeletionThreshold" value="0.98"/>
</HyperSlamFilter>
<!-- Number of particles to use -->
<value name="iParticleNum" value="1000"/>
<!-- The maximal rotation if mapping is performed. If the rotation is
bigger, mapping is interrupted.
This value may depend on the computing power, because it is influenced
by the size of time intervals of mapping. 0.35-->
<value name="fMaxRotationPerSecond" value="1.0"/> <!-- 0.55-->
<!-- Minimum time to wait between two mapping/localization steps -->
<value name="iWaitTime" value="500"/>
<!-- The map is only updated when the robot has turned a minimal angle (iUpdateMinMoveAngle),
has moved a minimal distance (iUpdateMinMoveDist) or a maximal time has passed( iMaxUpdateInterval ) -->
<value name="iUpdateMinMoveAngle" value="5"/>
<value name="iUpdateMinMoveDist" value="100"/>
<value name="iMaxUpdateInterval" value="2000"/>
<!-- Handle all incoming sensordata messages (AlwaysDeliver - value="1" - for SLAM benchmark) or only newest messages (keeponlynewest - value="0" - default) -->
<value name="bProcessAllMeasurements" value="0"/>
<!-- ASSUMED ERROR VALUES OF MOTION CALCULATION -->
<ErrorValues>
<!-- Rotation error while rotating in percent - old value: 50 20-->
<value name="fRotationErrorRotating" value="5.0"/>
<!-- Rotation error while translating in degrees per meter - old value: 5 2.0-->
<value name="fRotationErrorTranslating" value="5.0"/>
<!-- Translation error while translating in percent - old value: 15-25-->
<value name="fTranslationErrorTranslating" value="15.0"/>
<!-- Translation error while rotating in mm per degree - old value: 0.0-->
<value name="fTranslationErrorRotating" value="0.0"/>
<!-- Move jitter while turning in mm per degree - old value: 45800-->
<value name="fMoveJitterWhileTurning" value="30000.0"/>
</ErrorValues>
<!-- SETTINGS FOR SCAN MATCHING-->
<!-- SETTINGS FOR SCAN MATCHING-->
<ScanMatching>
<!-- If this variabe is true, scan matching is performed to improve the odometry data.
In a dynamic environment scan matching causes grave errors, especially if the
moving objects are close to the robot -->
<value name="bActivated" value="0"/>
<!-- Ignore odometry if true -->
<value name="bTest" value="0"/>
<!-- Used algorithm for scan matching.
1 = ICP, 2 = IDC, 3 = MbICP -->
<value name="iUsedAlgorithm" value="3"/>
<!-- Minimal point distance after reducing the number of scan points for
scan matching.
Value in mm. Value 0.0 means it is switched off. Standard: 200.0-->
<value name="fReducedPointDist" value="100.0"/>
<!-- Maximal distance of corresponding scan points, which are used
for scan matching. Value in mm. Standard: 300.0-->
<value name="fMaxCorrespondenceDiff" value="300.0"/>
<!-- Difference of scan angles which is considered to find
corresponding scan points for scan matching.
Value in degree. Standard: 90.0-->
<value name="fMaxAngleDiff" value="40.0"/>
<!-- Maximal distance of points in a cluster which is considered in
scan matching. Value in mm. 100.0-->
<value name="fMaxClusterDist" value="100.0"/>
<!-- Minimal diameter of a cluster of scan points which is considered
in scan matching.
Value in mm. Value 1.0 means it is switched of. Standard: 200.0-->
<value name="fMinClusterSize" value="200.0"/> <!--!!!!!!-->
<LineExtraction>
<!-- If this variable is true, the scan matcher works line-based.
If it is false, it works on the raw scan data.-->
<value name="bActivated" value="0"/>
<!-- Maximal distance of two consecutive points of a line -->
<value name="fMaxPointDistance" value="50.0"/>
<!-- Minimal number of points on a line -->
<value name="iMinPointsOnLine" value="10"/>
<!-- Minimal length of a line -->
<value name="fMinLength" value="100.0"/>
<!-- Maximal standard deviation of all points of a line-->
<value name="fMaxSigma" value="15.0"/>
</LineExtraction>
<MbICP>
<!--Factor which influences the behavior of the applied metric distance.
It describes the relation between rotation and translation.
If it is set to inifity, the behaviour of the MbIcpScanMatcher draws near to the IcpScanMatcher.
The value must be > 0. -->
<value name="fMetricFactor" value="1800.0"/>
</MbICP>
</ScanMatching>
</ParticleFilter>
<Kinect>
<!-- Settings for the kinect rgb camera -->
<Camera>
<!-- Note: sColorFormat can be RGB8 or Y8UV8 -->
<value name="sColorFormat" value="RGB8"/>
</Camera>
<!-- Determines whether to use Pointcloud(0) or Image(1) messages to compute our RGBDepthM -->
<value name="bUseImageTransport" value="1" />
<!-- Determines whether to use Image compression for transport of Images
If true(1) be sure to add a compression value (in percent) -->
<value name="bCompressImages" value="0" />
<value name="iCompressionValue" value="100" />
<value name="bSendContinuousDepthData" value="1" />
<!-- Determines how often RGBDepthDataM is updated -->
<value name="iIdleInterval" value="250" />
<value name="iTofIdleInterval" value="50" />
<!-- If set to true, RGBDepthM will be kept for painting and a 3DMap will be created
Always set to false, because of memory usage. -->
<value name="bCreate3DMap" value="0" />
<!-- 1: We are using the kinect instead of Laser Scanner data, affects PeopleTrackingModule (list may be incomplete)-->
<value name="bUseKinect" value="1" />
<value name="bUseKinectTracking" value="0" />
<value name="fTiltAngle" value="0"/>
<!-- If bMaskObstacles is true, then 3D Scan points with height greater
fMinObstacleHeight are marked inaccessible in the map -->
<value name="bMaskObstacles" value="1"/>
<!--value name="fMinObstacleHeight" value="140"/-->
<value name="fMinObstacleHeight" value="70"/>
<!-- Threshold for the maximum distance of points that are candidates as obstacles.
Needed because accuracy of the Kinect is decreasing by distance -->
<value name="fMaxObstacleFromRobotDistance" value="2000"/>
<value name="fMinObstacleFromRobotDistance" value="300"/>
<!-- How long is an obstcle saved after his last appearance in the kinect data -->
<value name="iObstacleSaveTime" value="10"/>
<value name="bDoKinectMappingWithTemporaryObstacles" value="1"/>
<!-- How often must an obstacle appear before it is known as an real obstacle -->
<value name="iObstacleMinAppearance" value="5"/>
</Kinect>
<SelfLocalization>
<value name="fScatterVarXY" value="50"/>
<!-- scattering variance for pose orientation in radiants -->
<value name="fScatterVarTheta" value="0.2"/>
</SelfLocalization>
<LaserScanners>
<SickLMS100>
<!-- Use this device in ActiveLaserScannerModule? -->
<value name="bActiveLaserScanner" value="0"/>
<!-- Is the device used for navigation? -->
<value name="bPassiveScan" value="1"/>
<!-- the ip and port the scanner is providing -->
<value name="sIP" value="169.254.97.144"/>
<value name="iPort" value="2111"/>
<!-- position of laserscanner in robot coordinates in mm -->
<value name="fPositionX" value="240"/>
<value name="fPositionY" value="0.0"/>
<value name="fPositionZ" value="40"/>
<!-- rotation radiuses of laserscanner beam in mm -->
<value name="fRotateRadiusY" value="0"/>
<value name="fRotateRadiusX" value="0"/>
<!-- orientation of laserscanner in robot coordinates
in degrees (counter clockwise) -->
<value name="fOrientation" value="-0.5"/>
<!-- valid maximum range (mm) -->
<value name="iValidMaxRange" value="30000"/>
<!-- valid minimum range (mm) -->
<value name="iValidMinRange" value="150"/>
<!-- minimum distance to be classified as free in case of errorneous measurement -->
<value name="iFreeReadingDistance" value="800"/>
<!-- number of scan points old 541-->
<value name="iScanSteps" value="1081"/>
<!-- scan angle in degrees -->
<value name="fScanAngle" value="270.0"/>
<!-- starting angle of one scan in degrees -->
<value name="fStartAngle" value="-135.0"/>
<value name="fObstacleThreshold" value="10"/>
<value name="iNumOffsetSteps" value="110" />
<value name="iNumBottomOffsetSteps" value="0"/>
<value name="iNumTopOffsetSteps" value="100"/>
<!-- iNumClearSlots for a 270Degree scanner. Only 180 Degree are used. Left and right 45 degree are deaktivated. For 0.5 resolution with 541 scanpoints we will need 90 points at each side masked. For security the Value for 0.5Degree Resolution is 100. If we use a 0.25 resulution we need 45Degree to masked out that means 180 Points + 10 Points security = 190 Points -->
<value name="iNumClearSlots" value="190"/>
<value name="sId" value="FrontLRF"/>
<!-- Set to 1 if scanner is mounted upside down -->
<value name="bMirror" value="1"/>
</SickLMS100>
</LaserScanners>
</default>
<map_manager>
</map_manager>
</MergedConfig>
\ No newline at end of file
homer_map_manager/images/rosgraph.png 0 → 100644
+ 0
− 0
View file @ c42291a5
homer_map_manager/images/rosgraph.png

660 KiB

homer_map_manager/images/rosgraph.svg 0 → 100644
+ 3607
− 0
View file @ c42291a5
This diff is collapsed.
homer_map_manager/mainpage.dox 0 → 100644
+ 14
− 0
View file @ c42291a5
/**
\mainpage
\htmlinclude manifest.html
\b map_manager
<!--
Provide an overview of your package.
-->
-->
*/
homer_map_manager/manifest.xml 0 → 100644
+ 22
− 0
View file @ c42291a5
<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 0 → 100644
+ 27
− 0
View file @ c42291a5
<package>
<name>homer_map_manager</name>
<version>1.0.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>
<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>
</package>
homer_map_manager/readme.pdf 0 → 100644
+ 0
− 0
View file @ c42291a5
File added
homer_map_manager/src/Managers/CMakeLists.txt 0 → 100644
+ 5
− 0
View file @ c42291a5
set(Managers_SRC MapManager.cpp MaskingManager.cpp PoiManager.cpp RoiManager.cpp)
add_library(Managers ${Managers_SRC})
target_link_libraries(Managers image_io)
add_dependencies(Managers homer_mapnav_msgs_gencpp)
homer_map_manager/src/Managers/MapManager.cpp 0 → 100644
+ 155
− 0
View file @ c42291a5
#include <stdio.h>
#include <stdlib.h>
#include <sstream>
#include <string>
#include <vector>
#include "MapManager.h"
#include <nav_msgs/OccupancyGrid.h>
#include <homer_mapnav_msgs/ModifyMap.h>
#include <homer_mapnav_msgs/MapLayers.h>
#include <tools/tools.h>
MapManager::MapManager(ros::NodeHandle* nh)
{
m_MapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map", 1);
//enable all map layers
m_MapVisibility[homer_mapnav_msgs::MapLayers::SLAM_LAYER] = true;
m_MapVisibility[homer_mapnav_msgs::MapLayers::KINECT_LAYER] = true;
m_MapVisibility[homer_mapnav_msgs::MapLayers::SICK_LAYER] = true;
m_MapVisibility[homer_mapnav_msgs::MapLayers::MASKING_LAYER] = true;
m_Height = -1;
m_Width = -1;
m_Resolution = -1;
}
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;
}
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_ERROR_STREAM( "SLAM map is missing!" );
return;
}
nav_msgs::OccupancyGrid mergedMap( *(m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]) );
//apply kinect map if enabled
if ( m_MapLayers.find(homer_mapnav_msgs::MapLayers::KINECT_LAYER) != m_MapLayers.end()
&& m_MapVisibility[homer_mapnav_msgs::MapLayers::KINECT_LAYER])
{
nav_msgs::OccupancyGridConstPtr &kinectMap = m_MapLayers[homer_mapnav_msgs::MapLayers::KINECT_LAYER];
for ( int y = 0; y < kinectMap->info.height; y++ )
{
for ( int x = 0; x < kinectMap->info.width; x++ )
{
int i = x + y * kinectMap->info.width;
//if cell is occupied by kinect obstacle merge cell with merged map
if(kinectMap->data[i] == homer_mapnav_msgs::ModifyMap::BLOCKED)
{
Eigen::Vector2i point(x,y);
geometry_msgs::Point tmp = map_tools::fromMapCoords( point ,kinectMap->info.origin, kinectMap->info.resolution);
point = map_tools::toMapCoords(tmp , mergedMap.info.origin, mergedMap.info.resolution);
int k = point.y() * mergedMap.info.width + point.x();
mergedMap.data[k] = homer_mapnav_msgs::ModifyMap::KINECT;
}
}
}
}
//apply SICK map if enabled
if(m_MapLayers.find(homer_mapnav_msgs::MapLayers::SICK_LAYER) != m_MapLayers.end()
&& m_MapVisibility[homer_mapnav_msgs::MapLayers::SICK_LAYER])
{
nav_msgs::OccupancyGridConstPtr &sickMap = m_MapLayers[homer_mapnav_msgs::MapLayers::SICK_LAYER];
if ( ( sickMap->info.width == mergedMap.info.width )
&& ( sickMap->info.height == mergedMap.info.height ) )
{
for ( int i=0; i<mergedMap.data.size(); i++ )
{
//if cell is occupied by kinect obstacle merge cell with merged map
if(sickMap->data[i] == homer_mapnav_msgs::ModifyMap::BLOCKED)
{
mergedMap.data[i] = homer_mapnav_msgs::ModifyMap::BLOCKED;
}
}
}
else
{
ROS_ERROR_STREAM( "Size mismatch between SLAM map and SICK map!" );
}
}
//apply masking map if enabled
if ( m_MapLayers.find(homer_mapnav_msgs::MapLayers::MASKING_LAYER) != m_MapLayers.end()
&& m_MapVisibility[homer_mapnav_msgs::MapLayers::MASKING_LAYER])
{
nav_msgs::OccupancyGridConstPtr &maskingMap = m_MapLayers[homer_mapnav_msgs::MapLayers::MASKING_LAYER];
if ( ( maskingMap->info.width == mergedMap.info.width )
&& ( maskingMap->info.height == mergedMap.info.height ) )
{
for ( int i=0; i<mergedMap.data.size(); i++ )
{
//if cell should be masked apply masking on merged map
if(maskingMap->data[i] != homer_mapnav_msgs::ModifyMap::NOT_MASKED)
{
mergedMap.data[i] = maskingMap->data[i];
}
}
}
else
{
ROS_ERROR_STREAM( "Size mismatch between SLAM map (" << mergedMap.info.width << "x" << mergedMap.info.height << ") and masking map (" << maskingMap->info.width << "x" << maskingMap->info.height << ")!" );
}
}
m_MapPublisher.publish(mergedMap);
ROS_DEBUG_STREAM("Publishing map");
}
homer_map_manager/src/Managers/MapManager.h 0 → 100644
+ 86
− 0
View file @ c42291a5
#ifndef MAPMANAGER_H
#define MAPMANAGER_H
#include <string>
#include <map>
#include <list>
#include "nav_msgs/OccupancyGrid.h"
#include "geometry_msgs/Pose.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; }
/** destructor */
virtual ~MapManager();
private:
/** merges all map layers and publishes the merged map */
void sendMergedMap();
/**
* The map data of each available map layer ist stored in this map
*/
std::map<int, nav_msgs::OccupancyGrid::ConstPtr > m_MapLayers;
/**
* 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;
geometry_msgs::Pose m_Origin;
/** map publisher */
ros::Publisher m_MapPublisher;
};
#endif
homer_map_manager/src/Managers/MaskingManager.cpp 0 → 100644
+ 187
− 0
View file @ c42291a5
#include "MaskingManager.h"
#include "ros/ros.h"
#include <homer_mapnav_msgs/ModifyMap.h>
#include <sstream>
using namespace std;
MaskingManager::MaskingManager(int mapSize, float resolution)
{
m_CellSize = resolution;
m_Width = mapSize / m_CellSize + 1;
m_Height = mapSize / m_CellSize + 1;
ROS_INFO_STREAM( "Creating " << m_Width << " x " << m_Height << " map." );
m_MaskingMap.info.resolution = m_CellSize;
m_MaskingMap.info.height = m_Height;
m_MaskingMap.info.width = m_Width;
m_MaskingMap.data.resize(m_Width * m_Height);
std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
m_SlamMap.info.resolution = m_CellSize;
m_SlamMap.info.height = m_Height;
m_SlamMap.info.width = m_Width;
m_SlamMap.data.resize(m_Width * m_Height);
std::fill( m_SlamMap.data.begin(), m_SlamMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
}
MaskingManager::~MaskingManager()
{}
nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg)
{
//reset SLAM mask map before each masking
std::fill( m_SlamMap.data.begin(), m_SlamMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
drawPolygon(msg->region, msg->maskAction, msg->mapLayer);
nav_msgs::OccupancyGrid::ConstPtr ret;
if(msg->mapLayer == 0)
{
ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_SlamMap);
}
else
{
ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_MaskingMap);
}
return ret;
}
nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap()
{
std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
nav_msgs::OccupancyGrid::ConstPtr ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_MaskingMap);
return ret;
}
void MaskingManager::replaceMap(nav_msgs::OccupancyGrid map)
{
if(map.data.size() != 0)
m_MaskingMap = map;
else
std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
}
void MaskingManager::drawPolygon ( vector< geometry_msgs::Point > vertices , int value , int mapLayer)
{
if ( vertices.size() == 0 )
{
ROS_INFO_STREAM( "No vertices given!" );
return;
}
//make temp. map
std::vector<int> data(m_Width * m_Height);
for ( int i = 0; i < data.size(); i++ )
{
data[i] = 0;
}
//draw the lines surrounding the polygon
for ( unsigned int i = 0; i < vertices.size(); i++ )
{
int i2 = ( i+1 ) % vertices.size();
drawLine ( data, vertices[i].x, vertices[i].y, vertices[i2].x, vertices[i2].y, 1);
}
//calculate a point in the middle of the polygon
float midX = 0;
float midY = 0;
for ( unsigned int i = 0; i < vertices.size(); i++ )
{
midX += vertices[i].x;
midY += vertices[i].y;
}
midX /= vertices.size();
midY /= vertices.size();
//fill polygon
fillPolygon ( data, (int)midX, (int)midY, 1 );
//copy polygon to masking map or slam map (according to parameter mapLayer)
for ( int i = 0; i < data.size(); i++ )
{
if ( data[i] != 0 )
{
switch(mapLayer)
{
case 0: //SLAM map
m_SlamMap.data[i] = value;
break;
case 1: //Kinect Map. apply masking to masking map
case 2: //masking map
m_MaskingMap.data[i] = value;
break;
}
}
}
}
void MaskingManager::drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value )
{
//bresenham algorithm
int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
// compute distances
dx = endX - startX;
dy = endY - startY;
// compute increment
if ( dx < 0 )
{
incx = -1;
dx = -dx;
}
else
{
incx = dx ? 1 : 0;
}
if ( dy < 0 )
{
incy = -1;
dy = -dy;
}
else
{
incy = dy ? 1 : 0;
}
// which distance is greater?
dist = ( dx > dy ) ? dx : dy;
// initializing
x = startX;
y = startY;
xerr = dx;
yerr = dy;
// compute cells
for ( t = 0; t < dist; t++ )
{
data[x + m_Width * y] = value;
xerr += dx;
yerr += dy;
if ( xerr > dist )
{
xerr -= dist;
x += incx;
}
if ( yerr > dist )
{
yerr -= dist;
y += incy;
}
}
}
void MaskingManager::fillPolygon ( std::vector<int> &data, int x, int y, int value )
{
int index = x + m_Width * y;
if ( value != data[index] )
{
data[index] = value;
fillPolygon ( data, x + 1, y, value );
fillPolygon ( data, x - 1, y, value );
fillPolygon ( data, x, y + 1, value );
fillPolygon ( data, x, y - 1, value );
}
}
homer_map_manager/src/Managers/MaskingManager.h 0 → 100644
+ 51
− 0
View file @ c42291a5
#ifndef MaskingManager_H
#define MaskingManager_H
#include <ros/ros.h>
#include "nav_msgs/OccupancyGrid.h"
#include "homer_mapnav_msgs/ModifyMap.h"
/**
* @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/src/Managers/PoiManager.cpp 0 → 100644
+ 133
− 0
View file @ c42291a5
#include "PoiManager.h"
#include <sstream>
#include "homer_mapnav_msgs/PointsOfInterest.h"
#include <ros/ros.h>
using namespace std;
PoiManager::PoiManager(ros::NodeHandle *nh)
{
m_POIsPublisher = nh->advertise<homer_mapnav_msgs::PointsOfInterest>("/map_manager/poi_list", 1);
}
PoiManager::PoiManager ( std::vector<homer_mapnav_msgs::PointOfInterest> pois )
{
//copy POIs
m_Pois = pois;
}
std::vector<homer_mapnav_msgs::PointOfInterest> PoiManager::getList()
{
return m_Pois;
}
bool PoiManager::addPointOfInterest (const homer_mapnav_msgs::PointOfInterest::ConstPtr &poi )
{
//make sure there's no POI with the same name
if ( poiExists ( poi->name ) )
{
ostringstream stream;
stream << "Poi with name " << poi->name << " already exists! Doing nothing.";
ROS_WARN_STREAM ( stream.str() );
return false;
}
//copy poi & assigning new id
homer_mapnav_msgs::PointOfInterest new_poi= *poi;
ROS_INFO_STREAM ("Adding POI '" << new_poi.name << "'.");
//insert into list
m_Pois.push_back ( new_poi );
broadcastPoiList();
return true;
}
bool PoiManager::modifyPointOfInterest (const homer_mapnav_msgs::ModifyPOI::ConstPtr &poi )
{
std::string name = poi->old_name;
std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
for ( it=m_Pois.begin() ; it != m_Pois.end(); it++ )
{
if ( it->name == name )
{
*it=poi->poi;
broadcastPoiList();
return true;
}
}
ROS_ERROR_STREAM ( "Cannot modify: POI does not exist!" );
return false;
}
void PoiManager::replacePOIList(std::vector<homer_mapnav_msgs::PointOfInterest> poilist)
{
m_Pois = poilist;
broadcastPoiList();
}
bool PoiManager::poiExists ( std::string name )
{
std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
for ( it=m_Pois.begin() ; it != m_Pois.end(); it++ )
{
if ( it->name == name )
{
return true;
}
}
return false;
}
bool PoiManager::deletePointOfInterest (std::string name )
{
std::vector< homer_mapnav_msgs::PointOfInterest >::iterator it;
for ( it=m_Pois.begin() ; it != m_Pois.end(); it++ )
{
if ( it->name == name )
{
ROS_INFO_STREAM ("Erasing POI " << name << ".");
it = m_Pois.erase ( it );
broadcastPoiList();
return true;
}
}
ROS_ERROR_STREAM ("POI " << name << " does not exist.");
return false;
}
void PoiManager::broadcastPoiList() {
ostringstream stream;
//print the current list
std::vector< homer_mapnav_msgs::PointOfInterest >::iterator it;
stream << "Contents of POI list:\n";
homer_mapnav_msgs::PointsOfInterest poiMsg;
for ( it = m_Pois.begin(); it != m_Pois.end(); it++ ) {
stream << " POI " << it->name << "', " << it->type
<< ", (" << it->pose.position.x << "," << it->pose.position.y << "), '" << it->remarks << "'\n";
}
poiMsg.pois = m_Pois;
ros::Rate poll_rate(10);
while(m_POIsPublisher.getNumSubscribers() == 0)
poll_rate.sleep();
m_POIsPublisher.publish(poiMsg);
ROS_DEBUG_STREAM( stream.str() );
}
homer_map_manager/src/Managers/PoiManager.h 0 → 100644
+ 89
− 0
View file @ c42291a5
#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);
/** 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 );
/**
* @brief Publishes a PointsOfInterest Message with current POIs
*/
void broadcastPoiList();
/** 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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment