diff --git a/homer_map_manager/CHANGELOG.rst b/CHANGELOG.rst
similarity index 100%
rename from homer_map_manager/CHANGELOG.rst
rename to CHANGELOG.rst
diff --git a/homer_map_manager/CMakeLists.txt b/CMakeLists.txt
similarity index 100%
rename from homer_map_manager/CMakeLists.txt
rename to CMakeLists.txt
diff --git a/README.md b/README.md
index 4443d3982a884066ea603ff9720566db7707c1db..39a46914d0f3b5b945ca0675ead28a7947da2370 100644
--- a/README.md
+++ b/README.md
@@ -1,16 +1,42 @@
-# mapping
-
-See:
-
-* [mapping]
-* [navigation]
-* [map_manager]
-* [map_msgs]
-* [nav_libs]
-
-[mapping]: homer_mapping/README.md
-[navigation]: homer_navigation/README.md
-[map_manager]: map_manager/README.md
-[map_msgs]: map_msgs/README.md
-[nav_libs]: nav_libs/README.md
-
+# 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.
+
+
+
+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
diff --git a/homer_map_manager/README.md b/homer_map_manager/README.md
deleted file mode 100644
index 39a46914d0f3b5b945ca0675ead28a7947da2370..0000000000000000000000000000000000000000
--- a/homer_map_manager/README.md
+++ /dev/null
@@ -1,42 +0,0 @@
-# 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.
-
-
-
-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
diff --git a/homer_mapnav_msgs/CHANGELOG.rst b/homer_mapnav_msgs/CHANGELOG.rst
deleted file mode 100644
index 324dcbdf2714fa4e3ed2430671009a0a4ea7b755..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/CHANGELOG.rst
+++ /dev/null
@@ -1,9 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package homer_mapnav_msgs
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.1.1 (2016-11-03)
-------------------
-* fixes
-* initial commit
-* Contributors: Niklas Yann Wettengel
diff --git a/homer_mapnav_msgs/CMakeLists.txt b/homer_mapnav_msgs/CMakeLists.txt
deleted file mode 100644
index 53485c0f6208a2ee02f0c7f471fee192e39f5960..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/CMakeLists.txt
+++ /dev/null
@@ -1,33 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-
-project(homer_mapnav_msgs)
-
-find_package(catkin REQUIRED
- COMPONENTS message_generation nav_msgs geometry_msgs)
-
-add_message_files(FILES
- MapLayers.msg
- ModifyMap.msg
- PointOfInterest.msg
- PointsOfInterest.msg
- RegionOfInterest.msg
- RegionsOfInterest.msg
- ModifyPOI.msg
- TargetUnreachable.msg
- StartNavigation.msg
- NavigateToPOI.msg
- RoiChange.msg
-)
-
-add_service_files(FILES
- GetPointsOfInterest.srv
- GetRegionsOfInterest.srv
- GetRegionOfInterestName.srv
- PointInsideRois.srv
- LoadMap.srv
- SaveMap.srv
-)
-
-generate_messages(DEPENDENCIES geometry_msgs nav_msgs)
-
-catkin_package(CATKIN_DEPENDS message_runtime geometry_msgs nav_msgs)
diff --git a/homer_mapnav_msgs/README.md b/homer_mapnav_msgs/README.md
deleted file mode 100644
index 47aeac049f53cbd30cc4bf36fe2986840d229471..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/README.md
+++ /dev/null
@@ -1,244 +0,0 @@
-# map_messages
-
-
-## 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.
-
-
diff --git a/homer_mapnav_msgs/msg/MapLayers.msg b/homer_mapnav_msgs/msg/MapLayers.msg
deleted file mode 100644
index f39bfdce21de88a0c14b6072855bbeb25acf9383..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/MapLayers.msg
+++ /dev/null
@@ -1,10 +0,0 @@
-int32 SLAM_LAYER=0
-int32 MASKING_LAYER=1
-int32 KINECT_LAYER=2
-int32 SICK_LAYER=3
-int32 HOKUYO_BACK=4
-int32 HOKUYO_FRONT=5
-int32 RAPID_MAP=6
-
-int32 layer # map layer name
-bool state # visibility state (on/off)
diff --git a/homer_mapnav_msgs/msg/ModifyMap.msg b/homer_mapnav_msgs/msg/ModifyMap.msg
deleted file mode 100644
index 0db7b6d88535ba5d2516691eb2191d6f86c086fc..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/ModifyMap.msg
+++ /dev/null
@@ -1,21 +0,0 @@
-int32 HIGH_SENSITIV = -2 # Area is highly sensitiv for Obstacles
-int32 NOT_MASKED = -1 # Area is not masked: use slam map
-int32 FREE = 0 # Area is free
-int32 OBSTACLE = 99 # Area is occupied by an obstacle (e.g. 3d scan)
-int32 BLOCKED = 100 # Area blocked by user (use UNBLOCK to .. )
-int32 HOKUYO = 101 # Area is blocked by Hokuyo Obstacle
-int32 KINECT = 102 # Area is blocked by Kinect Obstacle
-int32 DEPTH = 102 # Area is blocked by Depth Camera Obstacle
-int32 RAPID = 103 # Area is blocked by Rapid Mapping
-
-
-
-int32 SET_FREE = 0 # Mark area free, area must not be BLOCKED
-int32 SET_BLOCKED = 100 # Block area
-int32 SET_OBSTACLE = 99 # Mark area as occupied by an obstacle (e.g. 3d scan)
-int32 SET_HIGH_SENSITIV = -2 # Mark Area as highly sensitiv for Obstacles
-
-
-geometry_msgs/Point[] region
-int32 maskAction
-int32 mapLayer #currently not used. modifications only take place in the masking map layer
diff --git a/homer_mapnav_msgs/msg/ModifyPOI.msg b/homer_mapnav_msgs/msg/ModifyPOI.msg
deleted file mode 100644
index 59786f4b83d855a34bcfe4dc6c980c7c08916c53..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/ModifyPOI.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-PointOfInterest poi
-string old_name #necessary if name of poi is changed
diff --git a/homer_mapnav_msgs/msg/NavigateToPOI.msg b/homer_mapnav_msgs/msg/NavigateToPOI.msg
deleted file mode 100644
index 342f1d1fd3d0aaaa3af0e24c1b911b31934c6f57..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/NavigateToPOI.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-string poi_name
-float32 distance_to_target
-bool skip_final_turn
diff --git a/homer_mapnav_msgs/msg/PointOfInterest.msg b/homer_mapnav_msgs/msg/PointOfInterest.msg
deleted file mode 100644
index af6654c89cc82ee0ef703e6998bdcc986be90568..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/PointOfInterest.msg
+++ /dev/null
@@ -1,13 +0,0 @@
-#POI types
-int32 DEFAULT=100
-int32 OBJECT=300
-int32 GRIPPABLE_OBJECT=400
-int32 PERSON=600
-int32 START_POSITION=900
-int32 START_ORIENTATION=1000
-
-int32 type
-string name
-string remarks
-
-geometry_msgs/Pose pose
diff --git a/homer_mapnav_msgs/msg/PointsOfInterest.msg b/homer_mapnav_msgs/msg/PointsOfInterest.msg
deleted file mode 100644
index a2cfa93738a577b0ac78803f747eca511673510f..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/PointsOfInterest.msg
+++ /dev/null
@@ -1 +0,0 @@
-PointOfInterest[] pois
diff --git a/homer_mapnav_msgs/msg/RegionOfInterest.msg b/homer_mapnav_msgs/msg/RegionOfInterest.msg
deleted file mode 100644
index 6b4d30580029eb4d6ae649b308d5f0752afa8fc8..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/RegionOfInterest.msg
+++ /dev/null
@@ -1,12 +0,0 @@
-#ROI types
-int32 DEFAULT=100
-int32 ROOM=200
-int32 AREA=300
-int32 FORBIDDEN_AREA=400
-
-int32 type
-string name
-string remarks
-
-int32 id
-geometry_msgs/Point[4] points
diff --git a/homer_mapnav_msgs/msg/RegionsOfInterest.msg b/homer_mapnav_msgs/msg/RegionsOfInterest.msg
deleted file mode 100644
index 73c0b1b0d4a13d7984b88e0bb105543b6f6675ff..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/RegionsOfInterest.msg
+++ /dev/null
@@ -1 +0,0 @@
-RegionOfInterest[] rois
diff --git a/homer_mapnav_msgs/msg/RoiChange.msg b/homer_mapnav_msgs/msg/RoiChange.msg
deleted file mode 100644
index 7074ae5860b82ce8c0c93f422ed462d7804b6085..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/RoiChange.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-int32 id
-string name
-bool action # true-> enter, false-> left
diff --git a/homer_mapnav_msgs/msg/StartNavigation.msg b/homer_mapnav_msgs/msg/StartNavigation.msg
deleted file mode 100644
index cff7a6398c3619a107053d72616581954c446bb3..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/StartNavigation.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-geometry_msgs/Pose goal
-float32 distance_to_target #default should bei 0.01 [m]
-bool skip_final_turn
-bool fast_planning #path planning within bounding box containing robot pose and target
diff --git a/homer_mapnav_msgs/msg/TargetUnreachable.msg b/homer_mapnav_msgs/msg/TargetUnreachable.msg
deleted file mode 100644
index f69ddf67e6458613145c877e06ac6441645f00a7..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/TargetUnreachable.msg
+++ /dev/null
@@ -1,9 +0,0 @@
-int8 UNKNOWN=0
-int8 TILT_OCCURED=10
-int8 GRAVE_TILT_OCCURED=15
-int8 STALL_OCCURED=20
-int8 LASER_OBSTACLE=30
-int8 LASER_TIMEOUT=35
-int8 NO_PATH_FOUND=40
-
-int8 reason
diff --git a/homer_mapnav_msgs/package.xml b/homer_mapnav_msgs/package.xml
deleted file mode 100644
index f1c2ef8aba24d698f0c8139ebddd28525eb71298..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/package.xml
+++ /dev/null
@@ -1,22 +0,0 @@
-<package>
- <name>homer_mapnav_msgs</name>
- <version>0.1.1</version>
- <description> homer_mapnav_msgs contains the messages used for mapping and navigation
- </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>message_generation</build_depend>
- <build_depend>genmsg</build_depend>
- <build_depend>geometry_msgs</build_depend>
- <build_depend>nav_msgs</build_depend>
-
- <run_depend>geometry_msgs</run_depend>
- <run_depend>nav_msgs</run_depend>
- <run_depend>message_runtime</run_depend>
-</package>
-
-
diff --git a/homer_mapnav_msgs/readme.pdf b/homer_mapnav_msgs/readme.pdf
deleted file mode 100644
index bb76e2631376e2bda2c6ca741778247f62e4e6a2..0000000000000000000000000000000000000000
Binary files a/homer_mapnav_msgs/readme.pdf and /dev/null differ
diff --git a/homer_mapnav_msgs/srv/GetPointsOfInterest.srv b/homer_mapnav_msgs/srv/GetPointsOfInterest.srv
deleted file mode 100644
index e7a3c1a70ae02c33f819e04309c2020ee54fabf4..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/srv/GetPointsOfInterest.srv
+++ /dev/null
@@ -1,2 +0,0 @@
----
-PointsOfInterest poi_list
diff --git a/homer_mapnav_msgs/srv/GetRegionOfInterestName.srv b/homer_mapnav_msgs/srv/GetRegionOfInterestName.srv
deleted file mode 100644
index 9bf4c9a5672bd12e3278c229d845f91f4232a68d..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/srv/GetRegionOfInterestName.srv
+++ /dev/null
@@ -1,3 +0,0 @@
-int32 roi_id
----
-string name
diff --git a/homer_mapnav_msgs/srv/GetRegionsOfInterest.srv b/homer_mapnav_msgs/srv/GetRegionsOfInterest.srv
deleted file mode 100644
index f98be08203b1bb54c701c69822a512a5bc9d04fa..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/srv/GetRegionsOfInterest.srv
+++ /dev/null
@@ -1,2 +0,0 @@
----
-RegionsOfInterest roi_list
diff --git a/homer_mapnav_msgs/srv/LoadMap.srv b/homer_mapnav_msgs/srv/LoadMap.srv
deleted file mode 100644
index ef2e165e4a605e37a2a96692390cff33289de719..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/srv/LoadMap.srv
+++ /dev/null
@@ -1,2 +0,0 @@
-std_msgs/String filename
----
diff --git a/homer_mapnav_msgs/srv/PointInsideRois.srv b/homer_mapnav_msgs/srv/PointInsideRois.srv
deleted file mode 100644
index 2b37174193eb5be8ac1e969e68bb1214e76569d6..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/srv/PointInsideRois.srv
+++ /dev/null
@@ -1,3 +0,0 @@
-geometry_msgs/PointStamped point
----
-homer_mapnav_msgs/RegionOfInterest[] rois
diff --git a/homer_mapnav_msgs/srv/SaveMap.srv b/homer_mapnav_msgs/srv/SaveMap.srv
deleted file mode 100644
index 678315291b0c5ec89c16f836ead6eb3e83005301..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/srv/SaveMap.srv
+++ /dev/null
@@ -1,2 +0,0 @@
-std_msgs/String folder
----
diff --git a/homer_mapping/CHANGELOG.rst b/homer_mapping/CHANGELOG.rst
deleted file mode 100644
index 4b7831ec6188aaa2eb861fcaa7045074b8f65c77..0000000000000000000000000000000000000000
--- a/homer_mapping/CHANGELOG.rst
+++ /dev/null
@@ -1,9 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package homer_mapping
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.1.1 (2016-11-03)
-------------------
-* fixes
-* initial commit
-* Contributors: Niklas Yann Wettengel
diff --git a/homer_mapping/CMakeLists.txt b/homer_mapping/CMakeLists.txt
deleted file mode 100644
index fee377dbe23c1855cd1eeb04aca4b04917147b73..0000000000000000000000000000000000000000
--- a/homer_mapping/CMakeLists.txt
+++ /dev/null
@@ -1,91 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(homer_mapping)
-
-find_package(
- catkin REQUIRED COMPONENTS
- roscpp
- homer_mapnav_msgs
- sensor_msgs
- nav_msgs
- homer_nav_libs
- tf
- roslib
-)
-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()
-find_package(tf REQUIRED)
-
-set(CMAKE_BUILD_TYPE Release)
-
-
-include_directories(
- include
- ${catkin_INCLUDE_DIRS}
-)
-
-catkin_package(
- INCLUDE_DIRS include
- CATKIN_DEPENDS
- roscpp
- homer_mapnav_msgs
- homer_nav_libs
- nav_msgs
- tf
- roslib
- LIBRARIES homerOccupancyMap homerParticleFilter
-)
-
-add_library(homerOccupancyMap
- src/OccupancyMap/OccupancyMap.cpp
-
-)
-
-target_link_libraries(
- homerOccupancyMap
- ${catkin_LIBRARIES}
- ${tf_LIBRARIES}
-)
-set(
- ParticleFilter_SRC
- src/ParticleFilter/HyperSlamFilter.cpp
- src/ParticleFilter/SlamParticle.cpp
- src/ParticleFilter/SlamFilter.cpp
- src/ParticleFilter/Particle.cpp
-)
-
-add_library(
- homerParticleFilter
- ${ParticleFilter_SRC}
-)
-
-target_link_libraries(
- homerParticleFilter
- ${catkin_LIBRARIES}
-)
-
-add_executable(homer_mapping src/slam_node.cpp)
-
-target_link_libraries(
- homer_mapping
- ${catkin_LIBRARIES}
- homerParticleFilter
- homerOccupancyMap
-)
-
-add_dependencies(
- homer_mapping
- ${catkin_EXPORTED_TARGETS}
-)
-
-install(DIRECTORY include/${PROJECT_NAME}/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-
-install(TARGETS homerOccupancyMap homerParticleFilter homer_mapping
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
diff --git a/homer_mapping/README.md b/homer_mapping/README.md
deleted file mode 100644
index e27819068b37da060d45b8ed38f5c626a1085fd7..0000000000000000000000000000000000000000
--- a/homer_mapping/README.md
+++ /dev/null
@@ -1,67 +0,0 @@
-# homer_mapping
-
-## Introduction
-
-Das Package homer_mapping besteht aus einer gleichnamigen Node. Diese ist für die Lokalisierung und Kartierung des Roboters mit Hilfe der Odometrie des Roboters und eines Laserscanners zuständig.
-Das SLAM-Problem wird durch den Partikelfilter-Algorithmus gelöst.
-Die Node erwartet kontiniuierlich Odometrie-Werte und Laserdaten und verschickt in konstanten Abständen korrigierte Poseschätzungen über das Topic /pose und tf-Transformation /map -> /base_link.
-Zudem kann der Roboter sich auf einer vorher geladenen Karte lokalisieren, sowie eine aktuell erstellte Karte abgespeichert werden.
-Es besteht die Option, die Kartierung ein- oder auszuschalten. Beim Laden einer Karte wird die Kartierung automatisch ausgeschaltet.
-
-## Topics
-
-
-
-
-
-
-#### Publisher
-* `/pose (geometry_msgs/PoseStamped)`: Die aktuell ermittelte Pose relativ zu Karte (im Frame /map) des Roboters aus dem Partikelfilter.
-* `/homer_mapping/slam_map (nav_msgs/OccupancyGrid)`: Das aktuelle Karte des Roboters.
-
-
-
-#### Subscriber
-
-* `/odom (nav_msgs/Odometry)`: Die aktuellen Odometrie-Werte vom Roboter. Diese werden für die Partikelfilter benötigt.
- `/scan (sensor_msgs/LaserScan)`: Die aktuelle Lasermessung, die vom Partikelfilter benötigt wird.
-* `/homer_mapping/userdef_pose (geometry_msgs/Pose)`: Mit diesem Topic kann die die aktuell vom Partikelfilter berechnete Pose auf eine benutzerdefinierte gesetzt werden. Der Partikelfilter arbeitet nun mit dieser weiter.
-* `/homer_mapping/do_mapping (map_messages/DoMapping)`: Mit diesem Topic kann die Kartierung ein- oder ausgeschaltet werden.
-* `/map_manager/reset_maps (std_msgs/Empty)`: Hiermit kann die aktuelle Karte zurückgesetzt werden.
-* `/map_manager/loaded_map (nav_msgs/OccupancyGrid)`: Mit diesem Topic kann die aktuelle Karte durch eine andere (geladene) Karte ausgetauscht werden.
-* `/map_manager/mask_slam (nav_msgs/OccupancyGrid)`: Im OccupancyGrid dieses Topics stehen Informationen, welche Teile der aktuellen Karte durch andere Werte (frei oder belegt) ersetzt werden sollen.
-
-## Launch Files
-
-* `homer_mapping.launch`: Dieses Launchfile lädt die Parameterdatei `homer_mapping.yaml` und startet die Node homer_mapping sowie die Node map_manager im gleichnamigen Package.
-* `homer_mapping_rviz.launch`:`Dieses Launchfile hat die gleiche Funktionalität wie das obige, wobei es zusätzlich rviz startet.
-
-## Parameter
-
-
-### homer_mapping.yaml
-
-
-
-* `/homer_mapping/size:` Size beschreibt die Größe einer Seite der Karte in Metern. Die Karte ist quadratisch
-* `/homer_mapping/resolution:` Resolution ist die Länge einer (quadratischen) Zelle der Karte in Metern
-* `/homer_mapping/backside_checking:` Wenn auf "true" gesetzt, wird verhindet, dass Vorder- und Rückseite einer dickeren Wand auf die gematcht werden.
-* `/homer_mapping/obstacle_borders:` Wenn auf "true" gesetzt, wird um eingetragene Hindernisse ein kleiner Rand unbekanntes Gebiet gelassen.
-* `/homer_mapping/measure_sampling_step:` Minimale Distanz in Metern, die zwischen zwei aufeinanderfolgenden Messpunkten im Laserscan vorhanden sein muss, um sie für die Poseberechnung zu verwenden
-* `/homer_mapping/laser_scanner/free_reading_distance:` Minimale Distanz in Metern, die als hindernissfrei angenommen wird, wenn der aktuelle Messpunkt fehlerhaft ist
-* `/particlefilter/error_values/rotation_error_rotating:` Rotationsfehler in Prozent, der beim Rotieren des Roboters auftritt
-* `/particlefilter/error_values/rotation_error_translating:` Rotationsfehler in Grad, der beim Fahren von einem Meter auftritt
-* `/particlefilter/error_values/translation_error_translating:` Translationsfehler in Prozent, der beim Geradeausfahren auftritt
-* `/particlefilter/error_values/translation_error_rotating:` Translationsfehler in Metern, der beim Rotieren von einem Grad auftritt
-* `/particlefilter/error_values/move_jitter_while_turning:` Streuung der neu berechneten Pose in Meter pro Grad Drehung
-* `/particlefilter/hyper_slamfilter/particlefilter_num:` Anzahl der Partikelfilter im Hyperpartikelfilter (ist standardmäßig auf 1 gesetzt)
-* `/particlefilter/particle_num:` Anzahl der Partikel in jedem Partikelfilter
-* `/particlefilter/max_rotation_per_second:` Maximale Rotation in Radiant pro Sekunde, die der Roboter sich drehen darf, ohne dass das Mapping ausgesetzt wird
-* `/particlefilter/wait_time:` Minimale Zeit, die zwischen zwei Mapping-Schritten verstrichen sein muss
-* `/particlefilter/update_min_move_angle:` Minimale Rotation in Grad, die durchgeführt werden muss, damit ein Partikelfilterschritt ausgeführt wird...
-* `/particlefilter/update_min_move_dist:` ...oder minimale Distanz in Metern, die der Roboter geradeaus fährt...
-* `/particlefilter/max_update_interval:` ...oder minimale Wartezeit, in der der Roboter still steht.
-* `/selflocalization/scatter_var_xy:` Streuung der Partikel in x/y-Richtung in Metern beim Setzen einer benutzerdefinierten Pose
-* `/selflocalization/scatter_var_theta:` Streuung der Ausrichtung der Partikel in Radiant beim Setzen einer benutzerdefinierten Pose
-
-
diff --git a/homer_mapping/config/homer_mapping.yaml b/homer_mapping/config/homer_mapping.yaml
deleted file mode 100644
index a7e2bf69bc4aef3b76f4219b5d5069596db6195f..0000000000000000000000000000000000000000
--- a/homer_mapping/config/homer_mapping.yaml
+++ /dev/null
@@ -1,34 +0,0 @@
-/homer_mapping/size: 40 # size of one edge of the map in m. map is quadratic
-/homer_mapping/resolution: 0.05 # m meter per cell
-/homer_mapping/max_laser: 20.0 # m max range for including range into map
-
-#map config values
-/homer_mapping/backside_checking: false # Enable checking to avoid matching front- and backside of obstacles, e.g. walls. Useful when creating high resolution maps
-/homer_mapping/obstacle_borders: true # Leaves a small border around obstacles unchanged when inserting a laser scan. Improves stability of generated map
-/homer_mapping/measure_sampling_step: 0.1 # m Minimum distance in m between two samples for probability calculation
-
-/homer_mapping/laser_scanner/free_reading_distance: 0.8 # Minimum distance in m to be classified as free in case of errorneous measurement
-
-/particlefilter/error_values/rotation_error_rotating: 10.0 # percent
-/particlefilter/error_values/rotation_error_translating: 2.0 # degrees per meter
-/particlefilter/error_values/translation_error_translating: 10.0 # percent
-/particlefilter/error_values/translation_error_rotating: 0.05 # m per degree
-/particlefilter/error_values/move_jitter_while_turning: 0.1 # m per degree
-
-/particlefilter/hyper_slamfilter/particlefilter_num: 1
-
-/particlefilter/particle_num: 1000
-/particlefilter/max_rotation_per_second: 0.4 # maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted
-/particlefilter/wait_time: 0.05 # minimum time to wait between two slam steps in seconds
-
-#the map is only updated when the robot has turned a minimal angle, has moved a minimal distance or a maximal time has passed
-/particlefilter/update_min_move_angle: 2 # degrees
-/particlefilter/update_min_move_dist: 0.05 # m
-/particlefilter/max_update_interval: 0.5 # sec
-
-/selflocalization/scatter_var_xy: 0.1 # m
-/selflocalization/scatter_var_theta: 0.2 # radiants
-
-
-/map_manager/roi_updates: true # toggles if a entering or exiting of a roi is beeing published
-map_manager/roi_polling_time: 0.5 # sec timeinterval after which the roi status is checked
diff --git a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
deleted file mode 100644
index 0235e8604a89f3ea651bd3a1815eb393e0213cc3..0000000000000000000000000000000000000000
--- a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
+++ /dev/null
@@ -1,385 +0,0 @@
-#ifndef OCCUPANCYMAP_H
-#define OCCUPANCYMAP_H
-
-#include <vector>
-#include <list>
-#include <string>
-#include <iostream>
-
-#include <Eigen/Geometry>
-
-#include <homer_nav_libs/Math/Pose.h>
-#include <homer_nav_libs/Math/Point2D.h>
-#include <homer_nav_libs/Math/Box2D.h>
-
-#include <nav_msgs/OccupancyGrid.h>
-#include <tf/transform_listener.h>
-
-#include <sensor_msgs/LaserScan.h>
-
-class QImage;
-
-using namespace std;
-
-/**
- * Structure to store the start and end point of each laser range in the current scan
- * @param sensorPos position of the laser in the current scan (in base_link frame)
- * @param endPos position of the end point of the laser frame in the current scan (in base_link frame)
- * @param free indicates if the laser range hit an obstacle (false) or not (true)
- */
-struct RangeMeasurement
-{
- geometry_msgs::Point sensorPos;
- geometry_msgs::Point endPos;
- bool free;
-};
-
-/**
- * Used in struct MeasurePoint to specify if a measurement point is at the border of a scan segment
- */
-enum BorderType
-{
- NoBorder,
- LeftBorder,
- RightBorder
-};
-
-/**
- * Structure to store a measurement point for computeLaserScanProbability()
- * @param hitPos Position of measured obstacle (robot coordinates)
- * @param frontPos Position to check for NOT_KNOWN terrain
- * This is needed to assure that front- and backside of obstacles can be distinguished
- * @param border specifies if the measurement point is at the border of a scan segment
- */
-struct MeasurePoint
-{
- Point2D hitPos;
- Point2D frontPos;
- BorderType borderType;
-};
-
-/**
- * @class OccupancyMap
- *
- * @author Malte Knauf, Stephan Wirth, Susanne Maur (RX), David Gossow (RX), Susanne Thierfelder (R16)
- *
- * @brief This class holds and manages an occupancy map.
- *
- * An occupancy map is a map where free space and occupied space are marked. This map stores values
- * for free and occupied space in an (2D-)unsigned char array. This array can be seen as a graylevel image.
- * The darker a cell, the higher the probability that this cell is occupied by an obstacle.
- * The size of the map and the size of one cell can be defined in the setup file with the values
- * MAP_SIZE and MAP_CELL_SIZE. The origin of the coordinate system of the map is the center of the array.
- * The x-axis is heading front, the y-axis points to the left (like the robot's coordinate system).
- * The mapping data has to be inserted via the method insertLaserData().
- */
-class OccupancyMap {
-
- public:
- static const int8_t INACCESSIBLE = 100;
- static const int8_t OBSTACLE = 99;
- static const int8_t OCCUPIED = 98;
- static const int8_t UNKNOWN = 50;
- static const int8_t NOT_SEEN_YET = -1;
- static const int8_t FREE = 0;
-
- /**
- * The default constructor calls initMembers().
- */
- OccupancyMap();
-
- /**
- * Constructor for a loaded map.
- */
- OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin, float resolution, int pixelSize, Box2D<int> exploredRegion);
-
- /**
- * Copy constructor, copies all members inclusive the arrays that lie behind the pointers.
- * @param occupancyMap Source for copying
- */
- OccupancyMap(const OccupancyMap& occupancyMap);
-
- /**
- * Method to init all members with default values from the configuration file. All arrays are initialized.
- */
- void initMembers();
-
- /**
- * Assignment operator, copies all members (deep copy!)
- * @param source Source to copy from
- * @return Reference to copied OccupancyMap
- */
- OccupancyMap& operator=(const OccupancyMap& source);
-
- /**
- * Deletes all dynamically allocated memory.
- */
- ~OccupancyMap();
-
- /*
- /**
- * @return The resolution of the map in m.
- */
-// int resolution() const;
-
- geometry_msgs::Pose origin() const;
-
- /**
- * @return Width of the map.
- */
- int width() const;
-
- /**
- * @return Height of the map.
- */
- int height() const;
-
- /**
- * This method is used to reset all HighSensitive areas
- */
- void resetHighSensitive();
-
- /**
- * @return Probability of pixel p being occupied.
- */
- float getOccupancyProbability(Eigen::Vector2i p);
-
- /**
- * @brief This function inserts the data of a laserscan into the map.
- *
- * With the given data, start and end cells of a laser beam are computed and given to the
- * method markLineFree().
- * If the measurement is smaller than VALID_MAX_RANGE, markOccupied() is called for the endpoint.
- * @param laserData The laser data msg.
- */
- void insertLaserData( sensor_msgs::LaserScan::ConstPtr laserData, tf::Transform transform);
-
- void insertRanges( vector<RangeMeasurement> ranges , ros::Time stamp = ros::Time::now() );
-
- /**
- * @brief gives a list specially processed coordinates to be used for computeLaserScanProbability
- */
- std::vector<MeasurePoint> getMeasurePoints( sensor_msgs::LaserScanConstPtr laserData );
-
- /**
- * This method computes a score that describes how good the given hypothesis matches with the map
- * @param robotPose The pose of the robot
- * @return The "fitting factor". The higher the factor, the better the fitting.
- * This factor is NOT normalized, it is a positive float between 0 and FLOAT_MAX
- */
- float computeScore( Pose robotPose, std::vector<MeasurePoint> measurePoints );
-
- /**
- * @return QImage of size m_PixelSize, m_PixelSize with values of m_OccupancyProbability scaled to 0-254
- */
- QImage getProbabilityQImage(int trancparencyThreshold, bool showInaccessible) const;
-
- //puma2::ColorImageRGB8* getUpdateImage( bool withMap=true ); TODO
-
- /**
- * Returns an "image" of the obstacles e.g. seen in the 3D scans
- * @returns image with dark red dots in areas where the obstacles were seen
- */
- //puma2::ColorImageRGB8* getObstacleImage ( ); TODO
-
- /**
- * Returns an "image" of occupancy probability image.
- * @param[out] data vector containing occupancy probabilities. 0 = free, 100 = occupied, -1 = NOT_KNOWN
- * @param[out] width Width of data array
- * @param[out] height Height of data array
- * @param[out] resolution Resolution of the map (m_Resolution)
- */
- void getOccupancyProbabilityImage(vector<int8_t> &data, int& width, int& height, float &resolution);
-
- /**
- * This method marks free the position of the robot (according to its dimensions).
- */
- void markRobotPositionFree();
-
- /**
- * @brief Computes the contrast of a single pixel.
- * @param prob probability value (100=occupied, 50=NOT_KNOWN, 0=free) of a pixel.
- * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of this pixel
- */
- double contrastFromProbability (int8_t prob);
-
- /**
- * @brief This method computes the sharpness of the occupancy grid
- * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the map
- */
- double evaluateByContrast();
-
- /// GETTERS
-
- Box2D<int> getExploredRegion() { return m_ExploredRegion; }
- Box2D<int> getChangedRegion() { return m_ChangedRegion; }
-
- /**
- * Sets cells of this map to free or occupied according to maskMap
- */
- void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg);
-
-
- protected:
-
- /**
- * This method increments m_MeasurementCount for pixel p.
- * @param p Pixel that has been measured.
- */
- void incrementMeasurementCount(Eigen::Vector2i p);
-
- /**
- * This method increments the occupancy count int m_OccupancyCount for pixel p.
- * @param p Occupied pixel.
- */
- void incrementOccupancyCount(Eigen::Vector2i p);
-
- /**
- * This method increments m_MeasurementCount and if neccessary m_OccupancyCount for all pixels.
- */
- void applyChanges();
-
- void clearChanges();
-
- /**
- * This method scales the counts of all pixels down to the given value.
- * @param maxCount Maximum value to which all counts are set.
- */
- void scaleDownCounts(int maxCount);
-
- /**
- * This function paints a line from a start pixel to an end pixel.
- * The computation is made with the Bresenham algorithm.
- * @param data array on which the line shall be painted
- * @param startPixel starting coordinates of the beam
- * @param endPixel ending coordinates of the beam
- * @param value The value with which the lines are marked.
- */
- template<class DataT>
- void drawLine(DataT* data, Eigen::Vector2i& startPixel, Eigen::Vector2i& endPixel, char value);
-
- /**
- * This method computes the values for m_OccupancyProbabilities from m_MeasurementCount and m_OccupancyCount.
- */
- void computeOccupancyProbabilities();
-
- /**
- * This method sets all values of m_CurrentChanges to NO_CHANGE.
- */
- void clearCurrentChanges();
-
- /**
- * This method resets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY and m_MaxChangeY.
- * This means that no current changes are assumed.
- */
- void resetChangedRegion();
-
- /**
- * This method updates the values of m_MinChangeX, m_MaxChangeX, m_MinChangeY and m_MaxChangeY to current changes.
- * The area around the current robot pose will be included to the changed region.
- * @param robotPose The current pose of the robot.
- */
- void updateChangedRegion(Pose robotPose);
-
- /**
- * This method sets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY and m_MaxChangeY
- * to initial values so that the complete map will be processed.
- */
- void maximizeChangedRegion();
-
- /**
- * This method resets all values of m_ExploredX, m_MaxExploredX, m_MinExploredY and m_MaxExploredY.
- */
- void resetExploredRegion();
-
- /**
- * Deletes all allocated members.
- */
- void cleanUp();
-
- /**
- * Stores the size of one map pixel in m.
- */
- float m_Resolution;
-
- /**
- * Stores the origin of the map
- */
- geometry_msgs::Pose m_Origin;
- /**
- * Stores the width of the map in cell numbers.
- */
- int m_PixelSize;
-
- /**
- * Stores the size of the map arrays, i.e. m_PixelSize * m_PixelSize
- * for faster computation.
- */
- unsigned m_ByteSize;
-
- /**
- * Array to store occupancy probability values.
- * Values between 0 and 1.
- */
- float* m_OccupancyProbability;
-
- // Counts how often a pixel is hit by a measurement.
- unsigned short* m_MeasurementCount;
-
- // Counts how often a pixel is hit by a measurement that says the pixel is occupied.
- unsigned short* m_OccupancyCount;
-
- // Counts how often a cell is marked as inaccessible via markInaccessible()
- unsigned char* m_InaccessibleCount;
-
- // Used for setting flags for cells, that have been modified during the current update.
- unsigned char* m_CurrentChanges;
-
- // Used for high Sensitive areas
- unsigned short* m_HighSensitive;
-
- /**
- * Store values from config files.
- */
- // maximum valid range of one laser measurement
- float m_LaserMaxRange;
- //minimum valid range of one laser measurement
- float m_LaserMinRange;
- //minimum range classified as free in case of errorneous laser measurement
- float m_FreeReadingDistance;
- //enables checking to avoid matching front- and backside of an obstacle, e.g. wall
- bool m_BacksideChecking;
- //leaves a small border around obstacles unchanged when inserting a laser scan
- bool m_ObstacleBorders;
- //minimum distance in m between two samples for probability calculation
- float m_MeasureSamplingStep;
-
- //bool to reset high_sensitive Areas on the next iteration
- bool m_reset_high;
-
- //position of the laser scaner in base_link frame
- geometry_msgs::Point m_LaserPos;
-
- /**
- * Defines a bounding box around the changes in the current map.
- */
- Box2D<int> m_ChangedRegion;
-
- /**
- * Defines a bounding box around the area in the map, which is already explored.
- */
- Box2D<int> m_ExploredRegion;
-
- /**
- * ros transform listener
- */
- tf::TransformListener m_tfListener;
-
- /**
- * ros transformation laser to base_link
- */
- tf::StampedTransform m_laserTransform;
- tf::Transform m_latestMapTransform;
-
-};
-#endif
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
deleted file mode 100755
index c85da226e7e2db741d950381f3f887a46b1b163a..0000000000000000000000000000000000000000
--- a/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
+++ /dev/null
@@ -1,163 +0,0 @@
-#ifndef HYPERSLAMFILTER_H
-#define HYPERSLAMFILTER_H
-
-#include <vector>
-#include <homer_mapping/ParticleFilter/ParticleFilter.h>
-#include <homer_mapping/ParticleFilter/SlamParticle.h>
-#include <homer_mapping/ParticleFilter/SlamFilter.h>
-#include <homer_nav_libs/Math/Pose.h>
-#include <homer_mapping/OccupancyMap/OccupancyMap.h>
-
-#include <sensor_msgs/LaserScan.h>
-
-class OccupancyMap;
-
-/**
- * @class HyperSlamFilter
- *
- * @author Malte Knauf, Stephan Wirth, Susanne Maur
- *
- * @brief This class is used to determine the robot's most likely pose with given map and given laser data.
- *
- * A particle filter is a descrete method to describe and compute with a probability distribution.
- * This particle filter uses an occupancy map to determine the probability of robot states.
- * The robot states are stored in a particle together with their weight @see SlamParticle.
- *
- * @see SlamParticle
- * @see ParticleFilter
- * @see OccupancyMap
- */
-class HyperSlamFilter {
-
- public:
-
- /**
- * This constructor initializes the random number generator and sets the member variables to the given values.
- * @param particleNum Number of particleFilters to use.
- */
- HyperSlamFilter(int particleFilterNum, int particleNum);
-
- /**
- * The destructor releases the OccupancyMap and the particles.
- */
- ~HyperSlamFilter();
-
- /**
- * This method runs the filter routine.
- * The given odometry measurement is used as movement hypothesis, the laserData-vector is used
- * as measurement and is used to weight the particles.
- * @param currentPoseOdometry Odometry data of time t.
- * @param laserData msg containing the laser measurement.
- * @param measurementTime Time stamp of the measurement.
- * @param filterDurationTime Returns the time in ms that the filtering needed
- */
- void filter(Pose currentPoseOdometry, sensor_msgs::LaserScanConstPtr laserData, ros::Time measurementTime,
- ros::Duration &filterDuration);
-
- /**
- * Computes and sets the new value for m_Alpha1.
- * @param percent Rotation error while rotating (see class constructor for details)
- */
- void setRotationErrorRotating(float percent);
-
- /**
- * Computes and sets the new value for m_Alpha2.
- * @param degreesPerMeter Rotation error while translating (see class constructor for details)
- */
- void setRotationErrorTranslating(float degreesPerMeter);
-
- /**
- * Computes and sets the new value for m_Alpha3.
- * @param percent Translation error while translating (see class constructor for details)
- */
- void setTranslationErrorTranslating(float percent);
-
- /**
- * Computes and sets the new value for m_Alpha4.
- * @param mPerDegree Translation error while rotating (see class constructor for details)
- */
- void setTranslationErrorRotating(float mPerDegree);
-
- /**
- * Computes and sets the new value for m_Alpha5.
- * @param mPerDegree Move jitter while turning (see class constructor for details)
- */
- void setMoveJitterWhileTurning(float mPerDegree);
-
- /**
- * Sets a new minimal size of a cluster of scan points which is considered in scan matching.
- * @param clusterSize Minimal size of a cluster in mm of scan points which is considered in scan matching.
- */
- void setScanMatchingClusterSize(float clusterSize);
-
- /**
- * Sets whether the map is updated or just used for self-localization.
- * @param doMapping True if robot shall do mapping, false otherwise.
- */
- void setMapping(bool doMapping);
-
- /**
- * Deletes the current occupancy map and copies a new one into the system.
- * @param occupancyMap The occupancy map to load into the system (copies are being made)
- */
- void setOccupancyMap(OccupancyMap* occupancyMap);
-
- /**
- * Sets the robot pose in the current occupancy map.
- * @param Robot pose.
- * @param scatterVariance if not equal to 0 the poses are equally scattered around the pose
- */
- void setRobotPose(Pose pose, double scatterVarXY=0.0, double scatterVarTheta=0.0);
-
- /**
- *Returns the best SlamFilter
- */
- SlamFilter* getBestSlamFilter();
-
- void resetHigh();
-
- /**
- * Factor (default 0.98) of the contrast of the best particle.
- * If the contrast of the worst particle is below this threshold
- * it will be replaced by the best particle
- * @param deletionThreshold see above
- */
- void setDeletionThreshold(double deletionThreshold);
-
- /**
- * applies masking to map of slam filter set in GUI
- * @param msg masking message received from GUI
- */
- void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg)
- {
- for(unsigned i = 0; i < m_ParticleFilterNum; ++i)
- {
- m_SlamFilters[i]->applyMasking(msg);
- }
- }
-
- private:
-
- /** Used SlamFilters */
- std::vector <SlamFilter*> m_SlamFilters;
-
- /** Number of SlamFilters */
- unsigned m_ParticleFilterNum;
-
- /** Number of Particles of SlamFilter */
- unsigned m_ParticleNum;
-
- /** */
- double m_DeletionThreshold;
-
- /** Best SLAM Filter */
- SlamFilter* m_BestSlamFilter;
-
- /** Worst SlamFilter */
- SlamFilter* m_WorstSlamFilter;
-
- bool m_DoMapping;
-
-};
-#endif
-
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/Particle.h b/homer_mapping/include/homer_mapping/ParticleFilter/Particle.h
deleted file mode 100644
index bd96aa857da906c16c47914e38c7bbe000178e2b..0000000000000000000000000000000000000000
--- a/homer_mapping/include/homer_mapping/ParticleFilter/Particle.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#ifndef PARTICLE_H
-#define PARTICLE_H
-
-#include <iostream>
-#include <fstream>
-
-/**
- * @class Particle
- *
- * @author Malte Knauf, Stephan Wirth
- *
- * @brief This class is an implementation of a "particle".
- *
- * A particle as it is used in particle filters is a set of one state and one importance factor (=weight).
- * A set of Particles is a discrete representation of a probability distribution.
- *
- * @see ParticleFilter
- */
-class Particle {
-
- public:
- /**
- * This constructor assigns the given weight to the member m_Weight.
- * @param weight The weight of the particle.
- */
- Particle(float weight = 0.0, int id = 0);
-
- /**
- * The destructor does nothing so far.
- */
- virtual ~Particle();
-
- /**
- * This method returns the importance factor of the particle.
- * @return The importance factor (=weight) of the particle.
- */
- inline float getWeight() const { return m_Weight; }
-
- /**
- * Method to set the weight of the particle.
- * @param newWeight New weight for the particle.
- */
- inline void setWeight(float newWeight) { m_Weight=newWeight; }
-
- /**
- * @return id of the particle that is stored in m_Id
- */
- inline int getId() { return m_Id; }
-
- private:
- /**
- * Stores the importance factor (=weight) of the particle. This should be a value between 0 and 1.
- */
- float m_Weight;
-
- /**
- * Stores the id of the particle (for testing purpose)
- */
- int m_Id;
-
-};
-
-#endif
-
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
deleted file mode 100644
index 651f758714e8b1b2ca8a03338b2f46beea05c195..0000000000000000000000000000000000000000
--- a/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
+++ /dev/null
@@ -1,320 +0,0 @@
-#ifndef PARTICLEFILTER_H
-#define PARTICLEFILTER_H
-
-#include <iostream>
-#include <cmath>
-#include <limits.h>
-#include <omp.h>
-
-#include <ros/ros.h>
-
-class Particle;
-
-const float MIN_EFFECTIVE_PARTICLE_WEIGHT = 0.2;
-
-/**
- * @class ParticleFilter
- *
- * @author Malte Knauf, Stephan Wirth
- *
- * @brief This class is a template class for a particle filter.
- *
- * A particle filter is a descrete method to describe and compute with a probability distribution.
- * This template class implements the basic methods for a particle filter: sort() and resample().
- * Use this class do derivate your custom particle filter from it. Use a self-defined subclass of
- * Particle as ParticleType.
- *
- * @see Particle
- */
-template <class ParticleType>
-class ParticleFilter {
-
- public:
- /**
- * The constructor initializes the random number generator and allocates the memory for the particle lists.
- * The lists will have particleNum elements.
- * @param particleNum Number of particles for the filter.
- */
- ParticleFilter<ParticleType>(int particleNum);
-
- /**
- * The destructor releases the particle lists.
- */
- virtual ~ParticleFilter();
-
- /**
- * @return Number of particles used in this filter
- */
- int getParticleNum();
-
- /**
- * @return The number of effective particles (according to "Improving Grid-based SLAM with Rao-Blackwellized Particle
- * Filters by Adaptive Proposals and Selective Resampling (2005)" by Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
- */
- int getEffectiveParticleNum() const;
- int getEffectiveParticleNum2() const;
-
- /**
- * @return Pointer to the particle that has the highest weight.
- */
- ParticleType* getBestParticle() const;
-
-
- protected:
-
- /**
- * This method generates a random variable in the interval [0,1].
- * @param init The initial value for the static random base number. When running the constructor of this
- * class, this method is run once with the C-function time() as parameter to initialize it.
- * Then you should use it without parameter.
- * @return Random value between 0 and 1
- */
- double random01(unsigned long init = 0) const;
-
- /**
- * This method sorts the particles in m_CurrentList from leftIndex to rightIndex according to their weight.
- * The particle with the highest weight is at position 0 after calling this function. The algorithm used here is
- * known as quicksort and works recursively.
- * @param leftIndex Left index of area to sort
- * @param rightIndex Right index of area to sort
- */
- void sort(int leftIndex, int rightIndex);
-
- /**
- * This method normalizes the weights of the particles. After calling this function, the sum of the weights of
- * all particles in m_CurrentList equals 1.0.
- * In this function the sum of all weights of the particles of m_CurrentList is computed and each weight of each
- * particle is devided through this sum.
- */
- void normalize();
-
- /**
- * This method selects a new set of particles out of an old set according to their weight
- * (importance resampling). The particles from the list m_CurrentList points to are used as source,
- * m_LastList points to the destination list. The pointers m_CurrentList and m_LastList are switched.
- * The higher the weight of a particle, the more particles are drawn (copied) from this particle.
- * The weight remains untouched, because measure() will be called afterwards.
- * This method only works on a sorted m_CurrentList, therefore sort() is called first.
- */
- void resample();
-
- /**
- * This method drifts the particles (second step of a filter process).
- * Has to be implemented in sub-classes (pure virtual function).
- */
- virtual void drift() = 0;
-
- /**
- * This method has to be implemented in sub-classes. It is used to determine the weight of each particle.
- */
- virtual void measure() = 0;
-
- /**
- * These two pointers point to m_ParticleListOne and to m_ParticleListTwo.
- * The particles are drawn from m_LastList to m_CurrentList to avoid new and delete commands.
- * In each run, the pointers are switched in resample().
- */
- ParticleType** m_CurrentList;
- ParticleType** m_LastList;
-
- /**
- * Stores the number of particles.
- */
- int m_ParticleNum;
-
- /**
- * Stores the number of effective particles.
- */
- int m_EffectiveParticleNum;
-};
-
-template <class ParticleType>
-ParticleFilter<ParticleType>::ParticleFilter(int particleNum) {
- // initialize particle lists
- m_CurrentList = new ParticleType*[particleNum];
- m_LastList = new ParticleType*[particleNum];
-
- // initialize random number generator
- random01(time(0));
-
- m_ParticleNum = particleNum;
-}
-
-
-template <class ParticleType>
-ParticleFilter<ParticleType>::~ParticleFilter() {
- if (m_CurrentList) {
- delete[] m_CurrentList;
- m_CurrentList = 0;
- }
- if (m_LastList) {
- delete[] m_LastList;
- m_LastList = 0;
- }
-}
-
-template <class ParticleType>
-int ParticleFilter<ParticleType>::getParticleNum() {
- return m_ParticleNum;
-}
-
-template <class ParticleType>
-double ParticleFilter<ParticleType>::random01(unsigned long init) const {
- static unsigned long n;
- if (init > 0) {
- n = init;
- }
- n = 1664525 * n + 1013904223;
- // create double from unsigned long
- return (double)(n/2) / (double)LONG_MAX;
-}
-
-
-template <class ParticleType>
-void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight) {
-
- // SOMETHING LEFT TO SORT?
- if (indexLeft >= indexRight) {
- // ready!
- return;
- }
-
- // CREATE PARTITION
- int le = indexLeft;
- int ri = indexRight;
- int first = le;
- int pivot = ri--;
- while(le <= ri) {
- // skip from left
- while(m_CurrentList[le]->getWeight() > m_CurrentList[pivot]->getWeight()) {
- le++;
- }
- // skip from right
- while((ri >= first) && (m_CurrentList[ri]->getWeight() <= m_CurrentList[pivot]->getWeight())) {
- ri--;
- }
- // now we have two elements to swap
- if(le < ri) {
- // swap
- ParticleType* temp = m_CurrentList[le];
- m_CurrentList[le] = m_CurrentList[ri];
- m_CurrentList[ri] = temp;
- le++;
- }
- }
-
- if(le != pivot) {
- // swap
- ParticleType* temp = m_CurrentList[le];
- m_CurrentList[le] = m_CurrentList[pivot];
- m_CurrentList[pivot] = temp;
- }
-
- // sort left side
- sort(indexLeft, le - 1);
- // sort right side
- sort(le + 1, indexRight);
-
-}
-
-template <class ParticleType>
-void ParticleFilter<ParticleType>::normalize() {
-
- float weightSum = 0.0;
- for (int i = 0; i < m_ParticleNum; i++) {
- weightSum += m_CurrentList[i]->getWeight();
- }
- // only normalize if weightSum is big enough to divide
- if (weightSum > 0.000001)
- {
-
- //calculating parallel on 8 threats
- omp_set_num_threads(8);
- int i = 0;
- // #pragma omp parallel for
- for ( i = 0; i < m_ParticleNum; i++)
- {
- float newWeight = m_CurrentList[i]->getWeight() / weightSum;
- m_CurrentList[i]->setWeight(newWeight);
- }
- }
- else
- {
- ROS_WARN_STREAM( "Particle weights VERY small: " << weightSum << ". Got "<< m_ParticleNum << " particles.");
- }
-}
-
-template <class ParticleType>
-void ParticleFilter<ParticleType>::resample()
-{
- // swap pointers
- ParticleType** help = m_LastList;
- m_LastList = m_CurrentList;
- m_CurrentList = help;
- // now we copy from m_LastList to m_CurrentList
-
- int drawIndex = 0;
- // index of the particle where we are drawing to
- int targetIndex = 0;
-
- int numToDraw = 0;
- do {
- numToDraw = static_cast<int>(round((m_ParticleNum * m_LastList[drawIndex]->getWeight()) + 0.5));
- for (int i = 0; i < numToDraw; i++) {
- *m_CurrentList[targetIndex++] = *m_LastList[drawIndex];
- // don't draw too much
- if (targetIndex >= m_ParticleNum) {
- break;
- }
- }
- drawIndex++;
- } while (numToDraw > 0 && targetIndex < m_ParticleNum);
-
- // fill the rest of the particle list
- for (int i = targetIndex; i < m_ParticleNum; i++) {
- float particlePos = random01();
- float weightSum = 0.0;
- drawIndex = 0;
- weightSum += m_LastList[drawIndex]->getWeight();
- while (weightSum < particlePos) {
- weightSum += m_LastList[++drawIndex]->getWeight();
- }
- *m_CurrentList[i] = *m_LastList[drawIndex];
- }
-}
-
-
-template <class ParticleType>
-int ParticleFilter<ParticleType>::getEffectiveParticleNum() const {
- // does not work with normalized particle weights
- // does not work with our weights at all (algorithm of Grisetti)
- float squareSum = 0;
- for (int i = 0; i < m_ParticleNum; i++) {
- float weight = m_CurrentList[i]->getWeight();
- squareSum += weight * weight;
- }
- return static_cast<int>(1.0f / squareSum);
-}
-
-
-template <class ParticleType>
-int ParticleFilter<ParticleType>::getEffectiveParticleNum2() const {
- // does not work with normalized particle weights
- int effectiveParticleNum = 0;
- for (int i = 0; i < m_ParticleNum; i++) {
- if (m_CurrentList[i]->getWeight() > MIN_EFFECTIVE_PARTICLE_WEIGHT) {
- effectiveParticleNum ++;
- }
- }
- return effectiveParticleNum;
-}
-
-
-template <class ParticleType>
-ParticleType* ParticleFilter<ParticleType>::getBestParticle() const {
- return m_CurrentList[0];
-}
-
-
-#endif
-
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
deleted file mode 100644
index a712fb390b06c70a7a311674f4ca6492fdb32a6e..0000000000000000000000000000000000000000
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
+++ /dev/null
@@ -1,327 +0,0 @@
-#ifndef SLAMFILTER_H
-#define SLAMFILTER_H
-
-#include <vector>
-#include <homer_mapping/ParticleFilter/ParticleFilter.h>
-#include <homer_mapping/ParticleFilter/SlamParticle.h>
-#include <homer_nav_libs/Math/Pose.h>
-#include <homer_mapping/OccupancyMap/OccupancyMap.h>
-
-#include <sensor_msgs/LaserScan.h>
-#include <homer_nav_libs/Math/Transformation2D.h>
-#include <homer_nav_libs/Math/Math.h>
-
-#include <tf/transform_broadcaster.h>
-
-#include <cmath>
-#include <fstream>
-#include <sstream>
-
-#include "ros/ros.h"
-
-
-class OccupancyMap;
-
-/**
- * @class SlamFilter
- *
- * @author Malte Knauf, Stephan Wirth, Susanne Maur
- *
- * @brief This class is used to determine the robot's most likely pose with given map and given laser data.
- *
- * A particle filter is a descrete method to describe and compute with a probability distribution.
- * This particle filter uses an occupancy map to determine the probability of robot states.
- * The robot states are stored in a particle together with their weight @see SlamParticle.
- *
- * @see SlamParticle
- * @see ParticleFilter
- * @see OccupancyMap
- */
-class SlamFilter : public ParticleFilter<SlamParticle> {
-
- public:
-
- /**
- * This constructor initializes the random number generator and sets the member variables to the given values.
- * @param particleNum Number of particles to use.
- */
- SlamFilter(int particleNum);
-
- /// @brief copy constructor
- SlamFilter( SlamFilter& slamFilter );
-
- /**
- * The destructor releases the OccupancyMap and the particles.
- */
- ~SlamFilter();
-
- /**
- * This method runs the filter routine.
- * The given odometry measurement is used as movement hypothesis, the laserData-vector is used
- * as measurement and is used to weight the particles.
- * @param currentPoseOdometry Odometry data of time t.
- * @param laserData msg containing the laser measurement.
- * @param measurementTime Time stamp of the measurement.
- * @param filterDurationTime Returns the time that the filtering needed
- */
- void filter(Pose currentPoseOdometry, sensor_msgs::LaserScanConstPtr laserData, ros::Time measurementTime,
- ros::Duration &filterDuration);
-
- /**
- * @return The Pose of the most important particle (particle with highest weight).
- */
- Pose getLikeliestPose(ros::Time poseTime = ros::Time::now());
-
- /**
- * This method can be used to retrieve the most likely map that is stored by the particle filter.
- * @return Pointer to the most likely occupancy map.
- */
- OccupancyMap* getLikeliestMap() const;
-
- /**
- * This function prints out the list of particles to stdout via cout.
- */
- void printParticles() const;
-
- void resetHigh();
-
- /**
- * Computes and sets the new value for m_Alpha1.
- * @param percent Rotation error while rotating (see class constructor for details)
- */
- void setRotationErrorRotating(float percent);
-
- /**
- * Computes and sets the new value for m_Alpha2.
- * @param degreesPerMeter Rotation error while translating (see class constructor for details)
- */
- void setRotationErrorTranslating(float degreesPerMeter);
-
- /**
- * Computes and sets the new value for m_Alpha3.
- * @param percent Translation error while translating (see class constructor for details)
- */
- void setTranslationErrorTranslating(float percent);
-
- /**
- * Computes and sets the new value for m_Alpha4.
- * @param mPerDegree Translation error while rotating (see class constructor for details)
- */
- void setTranslationErrorRotating(float mPerDegree);
-
- /**
- * Computes and sets the new value for m_Alpha5.
- * @param mPerDegree Move jitter while turning (see class constructor for details)
- */
- void setMoveJitterWhileTurning(float mPerDegree);
-
- /**
- * Sets a new minimal size of a cluster of scan points which is considered in scan matching.
- * @param clusterSize Minimal size of a cluster in mm of scan points which is considered in scan matching.
- */
- void setScanMatchingClusterSize(float clusterSize);
-
- /**
- * Sets whether the map is updated or just used for self-localization.
- * @param doMapping True if robot shall do mapping, false otherwise.
- */
- void setMapping(bool doMapping);
-
- /**
- * Deletes the current occupancy map and copies a new one into the system.
- * @param occupancyMap The occupancy map to load into the system (is being copied)
- */
- void setOccupancyMap(OccupancyMap* occupancyMap);
-
- /**
- * Sets the robot pose in the current occupancy map.
- * @param Robot pose.
- * @param scatterVariance if not equal to 0 the poses are equally scattered around the pose
- */
- void setRobotPose(Pose pose, double scatterVarXY=0.0, double scatterVarTheta=0.0);
-
- /**
- * @return Vector of current particle poses. The vector is sorted according to the weights of the
- * particles. The pose of the particle with the highest value is the first element of the vector.
- */
- std::vector<Pose>* getParticlePoses() const;
-
- /**
- * @return vector of all particles
- */
- std::vector<SlamParticle*>* getParticles() const;
-
- /**
- * @return Vector of current particle weights. The vector is sorted by weight, highest weight first.
- */
- std::vector<float> getParticleWeights() const;
-
- /**
- * Calculates and returns the variance of the current likeliest particle poses.
- * The orientation of the particle is neglected.
- * @param The number of treated particles.
- * @param[out] poseVarianceX The variance of particle poses in x direction.
- * @param[out] poseVarianceY The variance of particle poses in y direction.
- */
- void getPoseVariances(int particleNum, float& poseVarianceX, float& poseVarianceY);
-
- /**
- * This method reduces the number of particles used in this SlamFilter to the given value.
- * @param newParticleNumber The new number of particles
- */
- void reduceParticleNumber(int newParticleNumber);
-
- /**
- * This method returns the contrast of the occupancy grid
- * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the map
- */
- double evaluateByContrast();
-
- /**
- * This method passes a masking map to to the underlying occupancy map
- */
- void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg);
-
-
- private:
-
- /**
- * This method filter outliers in the given laser scan
- * @param rawData the laser scan to check
- * @param maxDiff maximal difference between two adjacent ranges
- * @return filtered scan without outliers
- */
- vector<float> filterOutliers(sensor_msgs::LaserScanConstPtr rawData, float maxDiff );
-
- /**
- * This method generates Gauss-distributed random variables with the given variance. The computation
- * method is the Polar Method that is described in the book "A First Course on Probability" by Sheldon Ross.
- * @param variance The variance for the Gauss-distribution that is used to generate the random variable.
- * @return A random variable that is N(0, variance) distributed.
- */
- double randomGauss(float variance = 1.0) const;
-
- /**
- * This method drifts the particles according to the last two odometry readings (time t-1 and time t).
- */
- void drift();
-
- /**
- * This method weightens each particle according to the given laser measurement in m_LaserData.
- */
- void measure();
-
- /**
- * This method updates the map by inserting the current laser measurement at the pose of the likeliest particle.
- */
- void updateMap();
-
- /**
- * For weightening the particles, the filter needs a map.
- * This variable holds a pointer to a map.
- * @see OccupancyMap
- */
- OccupancyMap* m_OccupancyMap;
-
- /**
- * threshold values for when the map will be updated.
- * The map is only updated when the robot has turned a minimal angle (m_UpdateMinMoveAngle in radiants),
- * has moved a minimal distance (m_UpdateMinMoveDistance in m) or a maximal time has passed (m_MaxUpdateInterval)
- */
- float m_UpdateMinMoveAngle;
- float m_UpdateMinMoveDistance;
- ros::Duration m_MaxUpdateInterval;
-
- /**
- * This variable holds the rotation error that the robot makes while it is rotating.
- * Has to be given in percent. Example: robot makes errors of 3 degrees while making a 60 degrees
- * move -> error is 5% -> rotationErrorRotating = 5)
- */
- float m_Alpha1;
-
- /**
- * This variable holds the rotation error that the robot makes while it is translating
- * (moving forward or backwards). Has to be given in degrees per meter.
- */
- float m_Alpha2;
-
- /**
- * This variable holds the translation error that the robot makes while it is translating.
- * Has to be given in percent.
- */
- float m_Alpha3;
-
- /**
- * This variable holds the translation error that the robot makes while it is rotating.
- * This error only carries weight, if a translation es performed at the same time.
- * See also m_Alpha5.
- * Has to be given in milimeters per degree. Example: Robot makes a turn of 10 degrees and moves its
- * center unintentional 15 mm. -> translationErrorRotating = 15.0 / 10.0 = 1.5
- */
- float m_Alpha4;
-
- /**
- * This variable holds a move jitter that is considered if the robot is turning.
- * Has to be given in milimeters per degree.
- */
- float m_Alpha5;
-
- /**
- * 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.
- */
- float m_MaxRotationPerSecond;
-
- /**
- * Last laser data.
- */
- sensor_msgs::LaserScanPtr m_CurrentLaserData;
-
- /**
- * Last two odometry measurements.
- */
- Pose m_ReferencePoseOdometry;
- Pose m_CurrentPoseOdometry;
-
- /**
- * Time stamp of the last sensor measurement.
- */
- ros::Time m_ReferenceMeasurementTime;
-
- /**
- * True if it is the first run of SlamFilter, false otherwise.
- */
- bool m_FirstRun;
-
- /**
- * This variabe is true, if the SlamFilter is used for mapping and updates the map,
- * false if it is just used for self-localization.
- */
- bool m_DoMapping;
-
- /** Points used in last measure() step */
- vector<MeasurePoint> m_MeasurePoints;
-
- /// Pose of robot during last map update
- Pose m_LastUpdatePose;
-
- tf::Transform m_latestTransform;
-
- /**
- * Time stamp of the last particle filter step
- */
- ros::Time m_LastUpdateTime;
-
- /**
- * Calculates the square of given input f
- * @param f input
- * @return square of input
- */
- template<class T>
- T sqr(T f)
- {
- return f * f;
- }
-};
-#endif
-
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h
deleted file mode 100644
index dc05b056bcabf524b5a9132a665315beddd2dd03..0000000000000000000000000000000000000000
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h
+++ /dev/null
@@ -1,72 +0,0 @@
-#ifndef SLAMPARTICLE_H
-#define SLAMPARTICLE_H
-
-#include <iostream>
-#include <fstream>
-
-#include <homer_mapping/ParticleFilter/Particle.h>
-
-/**
- * @class SlamParticle
- *
- * @author Malte Knauf, Stephan Wirth
- *
- * @brief This class defines a particle for the SlamFilter.
- *
- * This particle contains a weight (inherited from base class) and a Pose (position + orientation).
- * The Pose describes a possible position and orientation of the robot.
- *
- * @see SlamFilter
- * @see Particle
- */
-class SlamParticle : public Particle
-{
-
- public:
- /**
- * This constructor assigns the given weight to the member m_Weight.
- * @param weight The weight of the particle.
- * @param robotX X-Position of the robot (world coordinates in m).
- * @param robotY Y-Position of the robot (world coordinates in m).
- * @param robotTheta Orientation of the robot (radiants).
- */
- SlamParticle ( float weight = 1.0, float robotX = 0.0, float robotY = 0.0, float robotTheta = 0.0 );
-
- ///@brief copy contructor
- SlamParticle ( SlamParticle& slamParticle );
-
- /**
- * The destructor does nothing so far.
- */
- ~SlamParticle();
-
- /**
- * Sets the three members m_RobotPositionX, m_RobotPositionY, m_RobotOrientation.
- * @param robotX X-Position of the robot (world coordinates in m).
- * @param robotY Y-Position of the robot (world coordinates in m).
- * @param robotTheta Orientation of the robot (radiants).
- */
- void setRobotPose ( float robotX, float robotY, float robotTheta );
-
- /**
- * Returns the content of the three members m_RobotPositionX, m_RobotPositionY, m_RobotOrientation.
- * @param[out] robotX X-Position of the robot (world coordinates in m).
- * @param[out] robotY Y-Position of the robot (world coordinates in m).
- * @param[out] robotTheta Orientation of the robot (radiants).
- */
- void getRobotPose ( float& robotX, float& robotY, float& robotTheta );
-
-
- private:
-
- /**
- * These members store the pose of the robot.
- */
- float m_RobotPositionX;
- float m_RobotPositionY;
- float m_RobotOrientation;
-
-};
-
-#endif
-
diff --git a/homer_mapping/include/homer_mapping/slam_node.h b/homer_mapping/include/homer_mapping/slam_node.h
deleted file mode 100644
index 5e4081a4a76ad6282cdbf92b55cbe2d40260fdec..0000000000000000000000000000000000000000
--- a/homer_mapping/include/homer_mapping/slam_node.h
+++ /dev/null
@@ -1,186 +0,0 @@
-#ifndef SLAM_NODE_H
-#define SLAM_NODE_H
-
-#include <vector>
-#include <map>
-#include <sstream>
-#include <vector>
-#include <iostream>
-#include <fstream>
-#include <sstream>
-#include <cmath>
-#include <stdlib.h>
-
-#include <homer_nav_libs/Math/Pose.h>
-
-#include <tf/transform_broadcaster.h>
-
-#include <sensor_msgs/LaserScan.h>
-#include <nav_msgs/Odometry.h>
-#include <nav_msgs/OccupancyGrid.h>
-#include <geometry_msgs/Pose.h>
-#include <geometry_msgs/PoseWithCovariance.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <std_msgs/Empty.h>
-#include <std_msgs/Bool.h>
-#include <nav_msgs/Odometry.h>
-#include <nav_msgs/OccupancyGrid.h>
-#include <tf/tf.h>
-
-#include <homer_mapping/ParticleFilter/SlamFilter.h>
-#include <homer_mapping/ParticleFilter/HyperSlamFilter.h>
-#include <homer_nav_libs/Math/Box2D.h>
-#include <homer_mapping/OccupancyMap/OccupancyMap.h>
-
-class OccupancyMap;
-class SlamFilter;
-class HyperSlamFilter;
-
-/**
- * @class SlamNode
- *
- * @author Malte Knauf, Stephan Wirth, Susanne Maur (RX), David Gossow (RX),
- * Christian Fuchs (R12), Nicolai Wojke (R14), Susanne Thierfelder (R16)
- *
- *
- * @brief The Simultaneous localization and mapping module
- *
- * This module receives odometry and laser data and computes the
- * robot's position and a map out of this data. Then it sends a
- * geometry_msgs/PoseStamped and nav_msgs/OccupancyGrid message.
- */
-class SlamNode
-{
-
-public:
-
- /**
- * The constructor adds the message types and prepares the module for receiving them.
- */
- SlamNode(ros::NodeHandle *nh);
-
- /**
- * This method initializes the member variables.
- */
- virtual void init();
-
- /**
- * The destructor deletes the filter thread instance.
- */
- virtual ~SlamNode();
-
-private:
-
- /**
- * Callback methods for all incoming messages
- */
- void callbackLaserScan( const sensor_msgs::LaserScan::ConstPtr& msg );
- void callbackOdometry( const nav_msgs::Odometry::ConstPtr& msg );
- void callbackUserDefPose( const geometry_msgs::Pose::ConstPtr& msg );
- void callbackDoMapping( const std_msgs::Bool::ConstPtr& msg );
- void callbackResetMap( const std_msgs::Empty::ConstPtr& msg );
- void callbackLoadedMap( const nav_msgs::OccupancyGrid::ConstPtr& msg );
- void callbackMasking( const nav_msgs::OccupancyGrid::ConstPtr& msg );
- void callbackResetHigh(const std_msgs::Empty::ConstPtr& msg);
- void callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
-
- /**
- * This function resets the current maps to the initial state.
- */
- void resetMaps();
-
- /**
- * This function processes the current odometry data in combination with the
- * last send odometry and laser informations to pass on corresponding data
- * to the filter threads.
-
- /**
- * This method retrieves the current map of the slam filter and sends a map
- * data message containing the map.
- */
- void sendMapDataMessage(ros::Time mapTime = ros::Time::now());
-
- /**
- * This variables stores the last odometry measurement as reference that is used
- * to compute the pose of the robot during a specific laserscan.
- */
- Pose m_ReferenceOdometryPose;
-
- Pose m_LastLikeliestPose;
-
- /**
- * This variable stores the time of the last odometry measurement as reference
- * which is used to compute the pose of the robot during a specific laserscan.
- */
- ros::Time m_ReferenceOdometryTime;
-
- /**
- * This variable stores the time the last map message was sent to be able to
- * compute the time for the next map send.
- */
- ros::Time m_LastMapSendTime;
- ros::Time m_LastPositionSendTime;
-
- /**
- * This variable stores the last laser measurement.
- */
- sensor_msgs::LaserScan::ConstPtr m_LastLaserData;
-
- /**
- * time stamp of last particle filter step
- */
- ros::Time m_LastMappingTime;
-
-
- /**
- * This variable stores a pointer to the hyper slam filter
- */
- HyperSlamFilter* m_HyperSlamFilter;
-
- /**
- * Scatter variances in self localization.
- */
- double m_ScatterVarXY;
- double m_ScatterVarTheta;
-
- /**
- * This variabe is true, if the slam algorithm is used for mapping and
- * keeps updating the map, false otherwise.
- */
- bool m_DoMapping;
-
- /**
- * Vectors used to queue laser and odom messages to find a fit
- */
- std::vector<sensor_msgs::LaserScan::ConstPtr> m_laser_queue;
- std::vector<nav_msgs::Odometry::ConstPtr> m_odom_queue;
-
- /**
- * duration to wait between two particle filter steps
- */
- ros::Duration m_WaitDuration;
-
- /**
- * Broadcasts the transform map -> base_link
- */
- tf::TransformBroadcaster m_tfBroadcaster;
-
- /**
- * subscribers and publishers
- */
- ros::Subscriber m_LaserScanSubscriber;
- ros::Subscriber m_OdometrySubscriber;
- ros::Subscriber m_UserDefPoseSubscriber;
- ros::Subscriber m_DoMappingSubscriber;
- ros::Subscriber m_ResetMapSubscriber;
- ros::Subscriber m_LoadMapSubscriber;
- ros::Subscriber m_MaskingSubscriber;
- ros::Subscriber m_ResetHighSubscriber;
- ros::Subscriber m_InitialPoseSubscriber;
-
- ros::Publisher m_PoseStampedPublisher;
- ros::Publisher m_SLAMMapPublisher;
-
-};
-
-#endif
diff --git a/homer_mapping/launch/homer_mapping.launch b/homer_mapping/launch/homer_mapping.launch
deleted file mode 100644
index d268a3362c9d9f8c77a11e0edf56358d6fe301eb..0000000000000000000000000000000000000000
--- a/homer_mapping/launch/homer_mapping.launch
+++ /dev/null
@@ -1,5 +0,0 @@
-<launch>
- <rosparam command="load" file="$(find homer_mapping)/config/homer_mapping.yaml"/>
- <node name="homer_mapping" pkg="homer_mapping" type="homer_mapping" output="screen"/>
- <node name="map_manager" pkg="homer_map_manager" type="map_manager" output="screen"/>
-</launch>
diff --git a/homer_mapping/mainpage.dox b/homer_mapping/mainpage.dox
deleted file mode 100644
index 054b63cfe4f5a61597be9ebd14e7358d2a7ffdd9..0000000000000000000000000000000000000000
--- a/homer_mapping/mainpage.dox
+++ /dev/null
@@ -1,26 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b homer_mapping is ...
-
-<!--
-Provide an overview of your package.
--->
-
-
-\section codeapi Code API
-
-<!--
-Provide links to specific auto-generated API documentation within your
-package that is of particular interest to a reader. Doxygen will
-document pretty much every part of your code, so do your best here to
-point the reader to the actual API.
-
-If your codebase is fairly large or has different sets of APIs, you
-should use the doxygen 'group' tag to keep these APIs together. For
-example, the roscpp documentation has 'libros' group.
--->
-
-
-*/
diff --git a/homer_mapping/manifest.xml b/homer_mapping/manifest.xml
deleted file mode 100644
index 2c9977b60f6c46c0bcbca4e6eb2d36bc17167dd9..0000000000000000000000000000000000000000
--- a/homer_mapping/manifest.xml
+++ /dev/null
@@ -1,23 +0,0 @@
-<package>
- <description brief="homer_mapping">
-
- homer_mapping
-
- </description>
- <author>Malte</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://ros.org/wiki/homer_mapping</url>
- <depend package="std_msgs"/>
- <depend package="roscpp"/>
- <depend package="sensor_msgs"/>
- <depend package="nav_msgs"/>
- <depend package="tf"/>
- <depend package="roslib"/>
-
- <depend package="robbie_architecture"/>
- <depend package="BaseLib"/>
- <depend package="map_messages"/>
-</package>
-
-
diff --git a/homer_mapping/package.xml b/homer_mapping/package.xml
deleted file mode 100644
index bb664fa5d58089740cac9d27d06e7815150b4093..0000000000000000000000000000000000000000
--- a/homer_mapping/package.xml
+++ /dev/null
@@ -1,34 +0,0 @@
-<package>
- <name>homer_mapping</name>
- <version>0.1.1</version>
- <description>
-
- homer_mapping
-
- </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>sensor_msgs</build_depend>
- <build_depend>nav_msgs</build_depend>
- <build_depend>tf</build_depend>
- <build_depend>roslib</build_depend>
- <build_depend>homer_nav_libs</build_depend>
- <build_depend>homer_mapnav_msgs</build_depend>
- <build_depend>cmake_modules</build_depend>
-
-
- <run_depend>std_msgs</run_depend>
- <run_depend>roscpp</run_depend>
- <run_depend>sensor_msgs</run_depend>
- <run_depend>nav_msgs</run_depend>
- <run_depend>tf</run_depend>
- <run_depend>roslib</run_depend>
- <run_depend>homer_nav_libs</run_depend>
- <run_depend>homer_mapnav_msgs</run_depend>
-
-</package>
diff --git a/homer_mapping/readme.pdf b/homer_mapping/readme.pdf
deleted file mode 100644
index 07f93c8ff0f8b7c478d0e93269eb5df0febdd4ad..0000000000000000000000000000000000000000
Binary files a/homer_mapping/readme.pdf and /dev/null differ
diff --git a/homer_mapping/src/CMakeLists.txt b/homer_mapping/src/CMakeLists.txt
deleted file mode 100644
index e5e17710f279e7e2c9be4e2a0ce235ad7a070efd..0000000000000000000000000000000000000000
--- a/homer_mapping/src/CMakeLists.txt
+++ /dev/null
@@ -1,2 +0,0 @@
-add_subdirectory(Workers)
-add_subdirectory(Modules)
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
deleted file mode 100644
index 5ab3b94abed4e9300c1a11f610ba049548303356..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ /dev/null
@@ -1,977 +0,0 @@
-#include <homer_mapping/OccupancyMap/OccupancyMap.h>
-
-#include <homer_nav_libs/Math/Math.h>
-
-#include <cmath>
-#include <vector>
-#include <fstream>
-#include <sstream>
-#include <QImage>
-
-#include <Eigen/Geometry>
-
-#include <ros/ros.h>
-#include <tf/transform_listener.h>
-
-#include <homer_mapnav_msgs/ModifyMap.h>
-#include <homer_nav_libs/tools.h>
-
-//uncomment this to get extended information on the tracer
-//#define TRACER_OUTPUT
-
-using namespace std;
-
-const float UNKNOWN_LIKELIHOOD = 0.3;
-
-// Flags of current changes //
-const char NO_CHANGE = 0;
-const char OCCUPIED = 1;
-const char FREE = 2;
-//the safety border around occupied pixels which is left unchanged
-const char SAFETY_BORDER = 3;
-///////////////////////////////
-
-//assumed laser measure count for loaded maps
-const int LOADED_MEASURECOUNT = 10;
-
-
-OccupancyMap::OccupancyMap()
-{
- initMembers();
-}
-
-OccupancyMap::OccupancyMap(float *&occupancyProbability, geometry_msgs::Pose origin, float resolution, int pixelSize, Box2D<int> exploredRegion)
-{
- initMembers();
-
-
- m_Origin = origin;
- m_Resolution = resolution;
- m_PixelSize = pixelSize;
- m_ByteSize = pixelSize * pixelSize;
- m_ExploredRegion = exploredRegion;
- m_ChangedRegion = exploredRegion;
-
- if ( m_OccupancyProbability )
- {
- delete[] m_OccupancyProbability;
- }
- m_OccupancyProbability = occupancyProbability;
- for(unsigned i = 0; i < m_ByteSize; i++)
- {
- if(m_OccupancyProbability[i] != 0.5)
- {
- m_MeasurementCount[i] = LOADED_MEASURECOUNT;
- m_OccupancyCount[i] = m_OccupancyProbability[i] * (float)LOADED_MEASURECOUNT;
- }
- }
-}
-
-
-OccupancyMap::OccupancyMap ( const OccupancyMap& occupancyMap )
-{
- m_OccupancyProbability = 0;
- m_MeasurementCount = 0;
- m_OccupancyCount = 0;
- m_CurrentChanges = 0;
- m_InaccessibleCount = 0;
- m_HighSensitive = 0;
- m_LaserMaxRange = 0;
- m_LaserMinRange = 0;
-
- *this = occupancyMap;
-}
-
-OccupancyMap::~OccupancyMap()
-{
- cleanUp();
-}
-
-void OccupancyMap::initMembers()
-{
- float mapSize;
- ros::param::get("/homer_mapping/size", mapSize);
- ros::param::get("/homer_mapping/resolution", m_Resolution);
- ros::param::param("/homer_mapping/max_laser", m_LaserMaxRange, (float) 8.0);
-
- //add one safety pixel
- m_PixelSize = mapSize / m_Resolution + 1;
- m_ByteSize = m_PixelSize * m_PixelSize;
-
- m_Origin.position.x = -m_PixelSize*m_Resolution/2;
- m_Origin.position.y = -m_PixelSize*m_Resolution/2;
- m_Origin.orientation.w = 1.0;
- m_Origin.orientation.x = 0.0;
- m_Origin.orientation.y = 0.0;
- m_Origin.orientation.z = 0.0;
-
- ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
- ros::param::get("/homer_mapping/obstacle_borders", m_ObstacleBorders);
- ros::param::get("/homer_mapping/measure_sampling_step", m_MeasureSamplingStep);
- ros::param::get("/homer_mapping/laser_scanner/free_reading_distance", m_FreeReadingDistance);
-
- m_OccupancyProbability = new float[m_ByteSize];
- m_MeasurementCount = new unsigned short[m_ByteSize];
- m_OccupancyCount = new unsigned short[m_ByteSize];
- m_CurrentChanges = new unsigned char[m_ByteSize];
- m_InaccessibleCount = new unsigned char[m_ByteSize];
- m_HighSensitive = new unsigned short[m_ByteSize];
- for ( unsigned i=0; i<m_ByteSize; i++ )
- {
- m_OccupancyProbability[i]=UNKNOWN_LIKELIHOOD;
- m_OccupancyCount[i]=0;
- m_MeasurementCount[i]=0;
- m_CurrentChanges[i]=NO_CHANGE;
- m_InaccessibleCount[i]=0;
- m_HighSensitive[i] = 0;
- }
-
- m_ExploredRegion=Box2D<int> ( m_PixelSize/2.1, m_PixelSize/2.1, m_PixelSize/1.9, m_PixelSize/1.9 );
- maximizeChangedRegion();
-
- try
- {
- bool got_transform = m_tfListener.waitForTransform("/base_link","/laser", ros::Time(0), ros::Duration(1));
- while(!got_transform);
- {
- ROS_ERROR_STREAM("need transformation from base_link to laser!");
- got_transform = m_tfListener.waitForTransform("/base_link","/laser", ros::Time(0), ros::Duration(1));
- }
-
- m_tfListener.lookupTransform("/base_link", "/laser", ros::Time(0), m_laserTransform);
- }
- catch (tf::TransformException ex) {
- ROS_ERROR_STREAM(ex.what());
- }
-
-}
-
-
-OccupancyMap& OccupancyMap::operator= ( const OccupancyMap & occupancyMap )
-{
- // free allocated memory
- cleanUp();
-
- m_Resolution = occupancyMap.m_Resolution;
- m_ExploredRegion = occupancyMap.m_ExploredRegion;
- m_PixelSize = occupancyMap.m_PixelSize;
- m_ByteSize = occupancyMap.m_ByteSize;
-
- ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
-
- // re-allocate all arrays
- m_OccupancyProbability = new float[m_ByteSize];
- m_MeasurementCount = new unsigned short[m_ByteSize];
- m_OccupancyCount = new unsigned short[m_ByteSize];
- m_CurrentChanges = new unsigned char[m_ByteSize];
- m_InaccessibleCount = new unsigned char[m_ByteSize];
- m_HighSensitive = new unsigned short[m_ByteSize];
-
- // copy array data
- memcpy ( m_OccupancyProbability, occupancyMap.m_OccupancyProbability, m_ByteSize * sizeof ( *m_OccupancyProbability ) );
- memcpy ( m_MeasurementCount, occupancyMap.m_MeasurementCount, m_ByteSize * sizeof ( *m_MeasurementCount ) );
- memcpy ( m_OccupancyCount, occupancyMap.m_OccupancyCount, m_ByteSize * sizeof ( *m_OccupancyCount ) );
- memcpy ( m_CurrentChanges, occupancyMap.m_CurrentChanges, m_ByteSize * sizeof ( *m_CurrentChanges ) );
- memcpy ( m_InaccessibleCount, occupancyMap.m_InaccessibleCount, m_ByteSize * sizeof ( *m_InaccessibleCount ) );
- memcpy ( m_HighSensitive, occupancyMap.m_HighSensitive, m_ByteSize * sizeof ( *m_HighSensitive) );
-
-
- return *this;
-}
-
-int OccupancyMap::width() const
-{
- return m_PixelSize;
-}
-
-int OccupancyMap::height() const
-{
- return m_PixelSize;
-}
-
-float OccupancyMap::getOccupancyProbability ( Eigen::Vector2i p )
-{
- unsigned offset = m_PixelSize * p.y() + p.x();
- if ( offset > unsigned ( m_ByteSize ) )
- {
- return UNKNOWN_LIKELIHOOD;
- }
- return m_OccupancyProbability[ offset ];
-}
-
-void OccupancyMap::resetHighSensitive()
-{
- ROS_INFO_STREAM("High sensitive Areas reseted");
- m_reset_high = true;
-}
-
-void OccupancyMap::computeOccupancyProbabilities()
-{
- for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ )
- {
- int yOffset = m_PixelSize * y;
- for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ )
- {
- int i = x + yOffset;
- if ( m_MeasurementCount[i] > 0 )
- {
- m_OccupancyProbability[i] = m_OccupancyCount[i] / static_cast<float> ( m_MeasurementCount[i] );
- if (m_HighSensitive[i] == 1)
- {
- if(m_reset_high == true)
- {
- m_OccupancyCount[i] = 0;
- m_OccupancyProbability[i] = 0;
- }
- if(m_MeasurementCount[i] > 20 )
- {
- m_MeasurementCount[i] = 10;
- m_OccupancyCount[i] = 10 * m_OccupancyProbability[i];
- }
- if(m_OccupancyProbability[i] > 0.3)
- {
- m_OccupancyProbability[i] = 1 ;
- }
- }
- }
- else
- {
- m_OccupancyProbability[i] = UNKNOWN_LIKELIHOOD;
- }
- }
- }
- if(m_reset_high == true)
- {
- m_reset_high = false;
- }
-}
-
-void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData, tf::Transform transform)
-{
- m_latestMapTransform = transform;
- markRobotPositionFree();
- ros::Time stamp = laserData->header.stamp;
-
- //m_LaserMaxRange = laserData->range_max;
- m_LaserMinRange = laserData->range_min;
-
-
- m_LaserPos.x = m_laserTransform.getOrigin().getX();
- m_LaserPos.y = m_laserTransform.getOrigin().getY();
-
- std::vector<RangeMeasurement> ranges;
- ranges.reserve ( laserData->ranges.size() );
-
- bool errorFound=false;
- int lastValidIndex=-1;
- float lastValidRange=m_FreeReadingDistance;
-
- RangeMeasurement rangeMeasurement;
- rangeMeasurement.sensorPos = m_LaserPos;
-
- for ( unsigned int i = 0; i < laserData->ranges.size(); i++ )
- {
- if ( ( laserData->ranges[i] >= m_LaserMinRange ) && ( laserData->ranges[i] <= m_LaserMaxRange ) )
- {
- //if we're at the end of an errorneous segment, interpolate
- //between last valid point and current point
- if ( errorFound )
- {
- //smaller of the two ranges belonging to end points
- float range = Math::min ( lastValidRange, laserData->ranges[i] );
- range -= m_Resolution * 2;
-
- if ( range < m_FreeReadingDistance )
- {
- range = m_FreeReadingDistance;
- }
- else
- if ( range > m_LaserMaxRange )
- {
- range = m_LaserMaxRange;
- }
-
- //choose smaller range
- for ( unsigned j=lastValidIndex+1; j<i; j++ )
- {
- float alpha = laserData->angle_min + j * laserData->angle_increment;
- geometry_msgs::Point point;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX( cos(alpha) * range);
- pin.setY( sin(alpha) * range);
- pin.setZ(0);
-
- pout = m_laserTransform * pin;
-
- point.x = pout.x();
- point.y = pout.y();
-
- rangeMeasurement.endPos = point;
- rangeMeasurement.free = true;
- ranges.push_back ( rangeMeasurement );
- }
- }
- float alpha = laserData->angle_min + i * laserData->angle_increment;
- geometry_msgs::Point point;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX( cos(alpha) * laserData->ranges[i]);
- pin.setY( sin(alpha) * laserData->ranges[i]);
- pin.setZ(0);
-
- pout = m_laserTransform * pin;
-
- point.x = pout.x();
- point.y = pout.y();
-
- rangeMeasurement.endPos = point;
- rangeMeasurement.free = false;
- ranges.push_back ( rangeMeasurement );
-
- errorFound=false;
- lastValidIndex=i;
- lastValidRange=laserData->ranges[i];
- }
- else
- {
- errorFound=true;
- }
- }
-
-// if ( errorFound )
-// {
-// for ( unsigned j=lastValidIndex+1; j<laserData->ranges.size(); j++ )
-// {
-// rangeMeasurement.endPos = map_tools::laser_range_to_point(m_FreeReadingDistance, j, laserData->angle_min, laserData->angle_increment, m_tfListener, laserData->header.frame_id, "/base_link"); //
-
-// rangeMeasurement.free = true;
-// ranges.push_back ( rangeMeasurement );
-// }
-// }
-
- insertRanges ( ranges , laserData->header.stamp);
-}
-
-
-void OccupancyMap::insertRanges ( vector<RangeMeasurement> ranges, ros::Time stamp )
-{
- clearChanges();
-
- Eigen::Vector2i lastEndPixel;
-
- //paint safety borders
- if ( m_ObstacleBorders )
- {
- for ( unsigned i=0; i<ranges.size(); i++ )
- {
- geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
- Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution);
-
- for ( int y=endPixel.y()-1; y <= endPixel.y() +1; y++ )
- {
- for ( int x=endPixel.x()-1; x <= endPixel.x() +1; x++ )
- {
- unsigned offset=x+m_PixelSize*y;
- if ( offset < unsigned ( m_ByteSize ) )
- {
- m_CurrentChanges[ offset ] = SAFETY_BORDER;
- }
- }
- }
- }
- }
- //paint safety ranges
- for ( unsigned i=0; i<ranges.size(); i++ )
- {
- geometry_msgs::Point startPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
- Eigen::Vector2i startPixel = map_tools::toMapCoords(startPosWorld, m_Origin, m_Resolution);
- geometry_msgs::Point endPos;
- endPos.x = ranges[i].endPos.x * 4;
- endPos.y = ranges[i].endPos.y * 4;
-
- geometry_msgs::Point endPosWorld = map_tools::transformPoint(endPos, m_latestMapTransform);
- Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution);
-
-
- if(endPixel.x() < 0) endPixel.x() = 0;
- if(endPixel.y() < 0) endPixel.y() = 0;
- if(endPixel.x() >= m_PixelSize) endPixel.x() = m_PixelSize - 1;
- if(endPixel.y() >= m_PixelSize) endPixel.y() = m_PixelSize - 1;
-
- drawLine ( m_CurrentChanges, startPixel, endPixel, SAFETY_BORDER );
- }
-
- //paint end pixels
- for ( unsigned i=0; i<ranges.size(); i++ )
- {
- if ( !ranges[i].free )
- {
- geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
- Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution);
-
- if ( endPixel != lastEndPixel )
- {
- unsigned offset = endPixel.x() + m_PixelSize * endPixel.y();
- if ( offset < m_ByteSize )
- {
- m_CurrentChanges[ offset ] = ::OCCUPIED;
- }
- }
- lastEndPixel=endPixel;
- }
- }
-
- //paint free ranges
- geometry_msgs::Point sensorPosWorld = map_tools::transformPoint(ranges[0].sensorPos, m_latestMapTransform);
- Eigen::Vector2i sensorPixel = map_tools::toMapCoords(sensorPosWorld, m_Origin, m_Resolution);
-
- for ( unsigned i=0; i<ranges.size(); i++ )
- {
- geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
- Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution);
-
- m_ChangedRegion.enclose ( sensorPixel.x(), sensorPixel.y() );
- m_ChangedRegion.enclose ( endPixel.x(), endPixel.y() );
-
- if ( endPixel != lastEndPixel )
- {
- drawLine ( m_CurrentChanges, sensorPixel, endPixel, ::FREE );
- }
-
- lastEndPixel=endPixel;
- }
-
- m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_PixelSize-1,m_PixelSize-1 ) );
- m_ExploredRegion.enclose ( m_ChangedRegion );
- applyChanges();
- computeOccupancyProbabilities();
-}
-
-double OccupancyMap::contrastFromProbability ( int8_t prob )
-{
- // range from 0..126 (=127 values) and 128..255 (=128 values)
- double diff = ( ( double ) prob - UNKNOWN );
- double contrast;
- if ( prob <= UNKNOWN )
- {
- contrast = ( diff / UNKNOWN ); // 0..1
- }
- else
- {
- contrast = ( diff / ( UNKNOWN+1 ) ); // 0..1
- }
- return ( contrast * contrast );
-}
-
-double OccupancyMap::evaluateByContrast()
-{
- double contrastSum = 0.0;
- unsigned int contrastCnt = 0;
-
- for ( int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++ )
- {
- for ( int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++ )
- {
- int i = x + y * m_PixelSize;
- if ( m_MeasurementCount [i] > 1 )
- {
- int prob = m_OccupancyProbability[i] * 100;
- if ( prob != NOT_SEEN_YET ) // ignore not yet seen cells
- {
- contrastSum += contrastFromProbability ( prob );
- contrastCnt++;
- }
- }
- }
- }
- if ( ( contrastCnt ) > 0 )
- {
- return ( ( contrastSum / contrastCnt ) * 100 );
- }
- return ( 0 );
-}
-
-
-
-vector<MeasurePoint> OccupancyMap::getMeasurePoints (sensor_msgs::LaserScanConstPtr laserData)
-{
- vector<MeasurePoint> result;
- result.reserve ( laserData->ranges.size() );
-
- double minDist = m_MeasureSamplingStep;
-
- //m_LaserMaxRange = laserData->range_max;
- m_LaserMinRange = laserData->range_min;
-
- Point2D lastHitPos;
- Point2D lastUsedHitPos;
-
- //extract points for measuring
- for ( unsigned int i=0; i < laserData->ranges.size(); i++ )
- {
- if ( laserData->ranges[i] <= m_LaserMaxRange && laserData->ranges[i] >= m_LaserMinRange )
- {
- float alpha = laserData->angle_min + i * laserData->angle_increment;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX( cos(alpha) * laserData->ranges[i]);
- pin.setY( sin(alpha) * laserData->ranges[i]);
- pin.setZ(0);
-
- pout = m_laserTransform * pin;
-
- Point2D hitPos(pout.x(), pout.y());
-
- if ( hitPos.distance ( lastUsedHitPos ) >= minDist )
- {
- MeasurePoint p;
- //preserve borders of segments
- if ( ( i!=0 ) &&
- ( lastUsedHitPos.distance ( lastHitPos ) > m_Resolution*0.5 ) &&
- ( hitPos.distance ( lastHitPos ) >= minDist*1.5 ) )
- {
- p.hitPos = lastHitPos;
- p.borderType = RightBorder;
- result.push_back ( p );
- p.hitPos = hitPos;
- p.borderType = LeftBorder;
- result.push_back ( p );
- lastUsedHitPos = hitPos;
- }
- else
- {
- //save current point
- p.hitPos = hitPos;
- p.borderType = NoBorder;
- result.push_back ( p );
- lastUsedHitPos = hitPos;
- }
- }
- lastHitPos = hitPos;
- }
- }
-
- //the first and last one are border pixels
- if ( result.size() > 0 )
- {
- result[0].borderType = LeftBorder;
- result[result.size()-1].borderType = RightBorder;
- }
-
- //calculate front check points
- for ( unsigned i=0; i < result.size(); i++ )
- {
- CVec2 diff;
-
- switch ( result[i].borderType )
- {
- case NoBorder:
- diff = result[i-1].hitPos - result[i+1].hitPos;
- break;
- case LeftBorder:
- diff = result[i].hitPos - result[i+1].hitPos;
- break;
- case RightBorder:
- diff = result[i-1].hitPos - result[i].hitPos;
- break;
- }
-
- CVec2 normal = diff.rotate ( Math::Pi * 0.5 );
- normal.normalize();
- normal *= m_Resolution * sqrt ( 2.0 ) * 10.0;
-
- result[i].frontPos = result[i].hitPos + normal;
- }
-
- return result;
-}
-
-
-float OccupancyMap::computeScore ( Pose robotPose, std::vector<MeasurePoint> measurePoints )
-{
- // This is a very simple implementation, using only the end point of the beam.
- // For every beam the end cell is computed and tested if the cell is occupied.
- unsigned lastOffset=0;
- unsigned hitOffset=0;
- unsigned frontOffset=0;
- float fittingFactor = 0.0;
-
- float sinTheta = sin ( robotPose.theta() );
- float cosTheta = cos ( robotPose.theta() );
-
- for ( unsigned int i = 0; i < measurePoints.size(); i++ )
- {
- //fast variant:
- float x = cosTheta * measurePoints[i].hitPos.x() - sinTheta * measurePoints[i].hitPos.y() + robotPose.x();
- float y = sinTheta * measurePoints[i].hitPos.x() + cosTheta * measurePoints[i].hitPos.y() + robotPose.y();
- geometry_msgs::Point hitPos;
- hitPos.x = x;
- hitPos.y = y;
-
- Eigen::Vector2i hitPixel = map_tools::toMapCoords(hitPos, m_Origin, m_Resolution);
- hitOffset = hitPixel.x() + m_PixelSize*hitPixel.y();
-
- //avoid multiple measuring of same pixel or unknown pixel
- if ( ( hitOffset == lastOffset ) || ( hitOffset >= unsigned ( m_ByteSize ) ) || ( m_MeasurementCount[hitOffset] == 0 ) )
- {
- continue;
- }
-
- if ( m_BacksideChecking )
- {
- //avoid matching of back and front pixels of obstacles
- x = cosTheta * measurePoints[i].frontPos.x() - sinTheta * measurePoints[i].frontPos.y() + robotPose.x();
- y = sinTheta * measurePoints[i].frontPos.x() + cosTheta * measurePoints[i].frontPos.y() + robotPose.y();
- geometry_msgs::Point frontPos;
- frontPos.x = x;
- frontPos.y = y;
-
- Eigen::Vector2i frontPixel = map_tools::toMapCoords(frontPos, m_Origin, m_Resolution);
- frontOffset = frontPixel.x() + m_PixelSize*frontPixel.y();
-
- if ( ( frontOffset >= unsigned ( m_ByteSize ) ) || ( m_MeasurementCount[frontOffset] == 0 ) )
- {
- continue;
- }
- }
-
- lastOffset=hitOffset;
- //fittingFactor += m_SmoothOccupancyProbability[ offset ];
- fittingFactor += m_OccupancyProbability[ hitOffset ];
- }
- return fittingFactor;
-}
-
-
-template<class DataT>
-void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::Vector2i& endPixel, char value )
-{
-
- //bresenham algorithm
- int xstart = startPixel.x();
- int ystart = startPixel.y();
- int xend = endPixel.x();
- int yend = endPixel.y();
-
- int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
- // compute distances
- dx = xend - xstart;
- dy = yend - ystart;
-
- // 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 = xstart;
- y = ystart;
- xerr = dx;
- yerr = dy;
-
- // compute cells
- for ( t = 0; t < dist; t++ )
- {
- int index = x + m_PixelSize * y;
- // set flag to free if no flag is set
- // (do not overwrite occupied cells)
- if(index < 0) continue;
- if ( data[index] == NO_CHANGE )
- {
- data[index] = value;
- }
-/* if ( data[index] == OCCUPIED || data[index] == SAFETY_BORDER )
- {
- return;
- }*/
- xerr += dx;
- yerr += dy;
- if ( xerr > dist )
- {
- xerr -= dist;
- x += incx;
- }
- if ( yerr > dist )
- {
- yerr -= dist;
- y += incy;
- }
- }
-}
-
-
-void OccupancyMap::applyChanges()
-{
- for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ )
- {
- int yOffset = m_PixelSize * y;
- for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ )
- {
- int i = x + yOffset;
- if ( ( m_CurrentChanges[i] == ::FREE || m_CurrentChanges[i] == ::OCCUPIED ) && m_MeasurementCount[i] < SHRT_MAX )
- {
- m_MeasurementCount[i]++;
- }
- if ( m_CurrentChanges[i] == ::OCCUPIED && m_OccupancyCount[i] < USHRT_MAX )
- {
- m_OccupancyCount[i]++;
- }
- }
- }
-}
-
-void OccupancyMap::clearChanges()
-{
- m_ChangedRegion.expand ( 2 );
- m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_PixelSize-1,m_PixelSize-1 ) );
- for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ )
- {
- int yOffset = m_PixelSize * y;
- for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ )
- {
- int i = x + yOffset;
- m_CurrentChanges[i] = NO_CHANGE;
- }
- }
- m_ChangedRegion=Box2D<int> ( m_PixelSize - 1, m_PixelSize - 1, 0, 0 );
-}
-
-void OccupancyMap::incrementMeasurementCount ( Eigen::Vector2i p )
-{
- unsigned index = p.x() + m_PixelSize * p.y();
- if ( index < m_ByteSize )
- {
- if ( m_CurrentChanges[index] == NO_CHANGE && m_MeasurementCount[index] < USHRT_MAX )
- {
- m_CurrentChanges[index] = ::FREE;
- m_MeasurementCount[index]++;
- }
- }
- else
- {
- ROS_ERROR( "Index out of bounds: x = %d, y = %d", p.x(), p.y() );
- }
-}
-
-void OccupancyMap::incrementOccupancyCount ( Eigen::Vector2i p )
-{
- int index = p.x() + m_PixelSize * p.y();
- if ( ( m_CurrentChanges[index] == NO_CHANGE || m_CurrentChanges[index] == ::FREE ) && m_MeasurementCount[index] < USHRT_MAX )
- {
- m_CurrentChanges[index] = ::OCCUPIED;
- m_OccupancyCount[index]++;
- }
-}
-
-void OccupancyMap::scaleDownCounts ( int maxCount )
-{
- clearChanges();
- if ( maxCount <= 0 )
- {
- ROS_WARN("WARNING: argument maxCount is choosen to small, resetting map.");
- memset ( m_MeasurementCount, 0, m_ByteSize );
- memset ( m_OccupancyCount, 0, m_ByteSize );
- memset ( m_InaccessibleCount, 0, m_ByteSize );
- }
- else
- {
- for ( unsigned i = 0; i < m_ByteSize; i++ )
- {
- int scalingFactor = m_MeasurementCount[i] / maxCount;
- if ( scalingFactor != 0 )
- {
- m_MeasurementCount[i] /= scalingFactor;
- m_OccupancyCount[i] /= scalingFactor;
- m_InaccessibleCount[i] /= scalingFactor;
- }
- if ( m_InaccessibleCount[i] > maxCount )
- {
- m_InaccessibleCount[i] = maxCount;
- }
- }
- }
- maximizeChangedRegion();
- applyChanges();
- computeOccupancyProbabilities();
-}
-
-
-void OccupancyMap::markRobotPositionFree()
-{
- geometry_msgs::Point point;
- point.x = 0;
- point.y = 0;
- point.z = 0;
- geometry_msgs::Point endPosWorld = map_tools::transformPoint(point, m_latestMapTransform);
- Eigen::Vector2i robotPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution);
-
- int width = 0.4 / m_Resolution;
- for ( int i = robotPixel.y() - width; i <= robotPixel.y() + width; i++ )
- {
- for ( int j = robotPixel.x() - width; j <= robotPixel.x() + width; j++ )
- {
- incrementMeasurementCount ( Eigen::Vector2i ( i, j ) );
- }
- }
- Box2D<int> robotBox ( robotPixel.x()-width, robotPixel.y()-width, robotPixel.x() +width, robotPixel.y() +width );
- m_ChangedRegion.enclose ( robotBox );
- m_ExploredRegion.enclose ( robotBox );
-}
-
-
-QImage OccupancyMap::getProbabilityQImage ( int trancparencyThreshold, bool showInaccessible ) const
-{
- QImage retImage ( m_PixelSize, m_PixelSize, QImage::Format_RGB32 );
- for ( int y = 0; y < m_PixelSize; y++ )
- {
- for ( int x = 0; x < m_PixelSize; x++ )
- {
- int index = x + y * m_PixelSize;
- int value = UNKNOWN;
- if ( m_MeasurementCount[index] > 0 )
- {
- value = static_cast<int> ( ( 1.0 - m_OccupancyProbability[index] ) * 255 );
- if ( m_MeasurementCount[index] < trancparencyThreshold )
- {
- value = static_cast<int> ( ( 0.75 + 0.025 * m_MeasurementCount[index] ) * ( 1.0 - m_OccupancyProbability[index] ) * 255 );
- }
- }
- if ( showInaccessible && m_InaccessibleCount[index] >= 2 )
- {
- value = 0;
- }
- retImage.setPixel ( x, y, qRgb ( value, value, value ) );
- }
- }
- return retImage;
-}
-
-void OccupancyMap::getOccupancyProbabilityImage ( vector<int8_t>& data, int& width, int& height, float& resolution )
-{
- width = m_PixelSize;
- height = m_PixelSize;
- resolution = m_Resolution;
- data.resize(m_PixelSize * m_PixelSize);
- std::fill(data.begin(), data.end(), (int8_t)NOT_SEEN_YET); //note: linker error without strange cast from int8_t to int8_t
- for ( int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++ )
- {
- int yOffset = m_PixelSize * y;
- for ( int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++ )
- {
- int i = x + yOffset;
- if ( m_MeasurementCount[i] < 1 )
- {
- continue;
- }
- // set inaccessible points to black
- if ( m_InaccessibleCount[i] >= 2 )
- {
- data[i] = 99;
- continue;
- }
- if(m_OccupancyProbability[i] == UNKNOWN_LIKELIHOOD)
- {
- data[i] = NOT_SEEN_YET;
- }
- else
- {
- data[i] = (int)(m_OccupancyProbability[i] * 99); //TODO maybe - 2 (or *0.99 or smth)
- }
- }
- }
-}
-
-void OccupancyMap::maximizeChangedRegion()
-{
- m_ChangedRegion=m_ExploredRegion;
-}
-
-void OccupancyMap::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg)
-{
- if(msg->data.size() != m_ByteSize)
- {
- ROS_ERROR_STREAM("Size Mismatch between SLAM map (" << m_ByteSize << ") and masking map (" << msg->data.size() << ")");
- return;
- }
- for(size_t y = 0; y < msg->info.height; y++)
- {
- int yOffset = msg->info.width * y;
- for(size_t x = 0; x < msg->info.width; x++)
- {
- int i = yOffset + x;
-
- switch(msg->data[i])
- {
- case homer_mapnav_msgs::ModifyMap::BLOCKED:
- case homer_mapnav_msgs::ModifyMap::OBSTACLE:
- //increase measure count of cells which were not yet visible to be able to modify unknown areas
- if(m_MeasurementCount[i] == 0)
- m_MeasurementCount[i] = 10;
-
- m_OccupancyCount[i] = m_MeasurementCount[i];
- m_OccupancyProbability[i] = 1.0;
- m_ExploredRegion.enclose(x, y);
- break;
- case homer_mapnav_msgs::ModifyMap::FREE:
- //see comment above
- if(m_MeasurementCount[i] == 0)
- m_MeasurementCount[i] = 10;
-
- m_OccupancyCount[i] = 0;
- m_OccupancyProbability[i] = 0.0;
- m_ExploredRegion.enclose(x, y);
- break;
- case homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV:
- m_HighSensitive[i] = 1;
- break;
- }
- }
- }
-}
-
-void OccupancyMap::cleanUp()
-{
- if ( m_OccupancyProbability )
- {
- delete[] m_OccupancyProbability;
- }
- if ( m_MeasurementCount )
- {
- delete[] m_MeasurementCount;
- }
- if ( m_OccupancyCount )
- {
- delete[] m_OccupancyCount;
- }
- if ( m_CurrentChanges )
- {
- delete[] m_CurrentChanges;
- }
- if ( m_InaccessibleCount )
- {
- delete[] m_InaccessibleCount;
- }
- if ( m_HighSensitive )
- {
- delete[] m_HighSensitive;
- }
-}
diff --git a/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp b/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
deleted file mode 100755
index 7265b0ead89933bb6abfc422dd448d0c7c5a4889..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
+++ /dev/null
@@ -1,202 +0,0 @@
-#include <homer_mapping/ParticleFilter/HyperSlamFilter.h>
-
-#include <vector>
-#include <cmath>
-#include <fstream>
-#include <sstream>
-#include <stdlib.h>
-
-#include "ros/ros.h"
-
-using namespace std;
-
-HyperSlamFilter::HyperSlamFilter (int particleFilterNum, int particleNum )
-{
- m_ParticleFilterNum = particleFilterNum;
- if ( m_ParticleFilterNum < 1 )
- {
- m_ParticleFilterNum = 1;
- }
- ROS_DEBUG( "Using %d Hyper Particles.", particleFilterNum);
-
- m_ParticleNum = particleNum;
-
- m_DoMapping = true;
-
- m_DeletionThreshold = 0.98;
-
- for ( unsigned i=0; i < m_ParticleFilterNum; i++ )
- {
- ostringstream stream;
- stream << "SlamFilter " << i;
- SlamFilter *slamFilter = new SlamFilter ( particleNum );
- m_SlamFilters.push_back ( slamFilter );
- }
-
- m_BestSlamFilter = m_SlamFilters[0];
-}
-
-HyperSlamFilter::~HyperSlamFilter()
-{
- for (unsigned i = 0; i < m_ParticleFilterNum; i++)
- {
- if( m_SlamFilters[i] )
- {
- delete m_SlamFilters[i];
- m_SlamFilters[i] = 0;
- }
- }
-}
-
-void HyperSlamFilter::setRotationErrorRotating ( float percent )
-{
- for ( unsigned i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->setRotationErrorRotating(percent / 100.0);
- }
-}
-
-void HyperSlamFilter::setRotationErrorTranslating ( float degreePerMeter )
-{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->setRotationErrorTranslating(degreePerMeter / 180.0 * M_PI);
- }
-}
-
-void HyperSlamFilter::setTranslationErrorTranslating ( float percent )
-{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->setTranslationErrorTranslating(percent / 100.0);
- }
-}
-
-void HyperSlamFilter::setTranslationErrorRotating ( float mPerDegree )
-{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->setTranslationErrorRotating( mPerDegree / 180.0 * M_PI );
- }
-}
-
-void HyperSlamFilter::setMoveJitterWhileTurning ( float mPerDegree )
-{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->setMoveJitterWhileTurning( mPerDegree / 180.0 * M_PI );
- }
-}
-
-void HyperSlamFilter::setScanMatchingClusterSize ( float minClusterSize )
-{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->setScanMatchingClusterSize( minClusterSize );
- }
-}
-
-void HyperSlamFilter::resetHigh()
-{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->resetHigh();
- }
-}
-
-void HyperSlamFilter::setMapping ( bool doMapping )
-{
- m_DoMapping = doMapping;
-}
-
-void HyperSlamFilter:: setOccupancyMap ( OccupancyMap* occupancyMap )
-{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->setOccupancyMap( occupancyMap );
- }
-}
-
-void HyperSlamFilter::setRobotPose ( Pose pose, double scatterVarXY, double scatterVarTheta )
-{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->setRobotPose(pose, scatterVarXY, scatterVarTheta);
- }
-}
-
-void HyperSlamFilter::filter ( Pose currentPose, sensor_msgs::LaserScanConstPtr laserData, ros::Time measurementTime, ros::Duration &filterDuration)
-{
- //call filter methods of all particle filters
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- bool randOnOff;
-
- if(m_SlamFilters.size() == 1)
- {
- randOnOff = true;
- }
- else
- {
- //if mapping is on, switch on with 80% probability to introduce some randomness in different particle filters
- randOnOff = (rand() % 100) < 80;
- }
- m_SlamFilters[i]->setMapping( m_DoMapping && randOnOff );
- m_SlamFilters[i]->filter(currentPose, laserData, measurementTime, filterDuration);
-
- }
- if(m_SlamFilters.size() != 1)
- {
- //determine which map has the best/worst contrast
- double bestContrast = 0.0;
- static unsigned int bestFilter = 0;
- double worstContrast = 100.0;
- static unsigned int worstFilter = 0;
-
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- double contrast = m_SlamFilters[i]->evaluateByContrast();
- {
- if( contrast > bestContrast )
- {
- bestContrast = contrast;
- bestFilter = i;
- }
- if ( contrast < worstContrast )
- {
- worstContrast = contrast;
- worstFilter = i;
- }
- }
- }
-
- // set best filter
- SlamFilter* lastBestFilter = m_BestSlamFilter;
- m_BestSlamFilter = m_SlamFilters[bestFilter];
-
- if ( m_BestSlamFilter != lastBestFilter )
- {
- ROS_INFO( "Switched to best filter %d (bestContrast: %f) -- the worst filter is %d (worstContrast: %f)", bestFilter, bestContrast, worstFilter, worstContrast); //TODO
- }
-
- if ( bestFilter != worstFilter )
- {
- if ( worstContrast < ( bestContrast * m_DeletionThreshold ) )
- {
- // replace the worst filter by the one with the best contrast
- delete m_SlamFilters[worstFilter];
- m_SlamFilters[worstFilter] = new SlamFilter ( * m_SlamFilters [bestFilter] );
- }
- }
- }
-}
-
-SlamFilter* HyperSlamFilter::getBestSlamFilter()
-{
- return m_BestSlamFilter;
-}
-
-void HyperSlamFilter::setDeletionThreshold(double deletionThreshold)
-{
- m_DeletionThreshold = deletionThreshold;
-}
diff --git a/homer_mapping/src/ParticleFilter/Particle.cpp b/homer_mapping/src/ParticleFilter/Particle.cpp
deleted file mode 100644
index 1d64cf733efb608db16e4a43112fcd56f54485e2..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/Particle.cpp
+++ /dev/null
@@ -1,10 +0,0 @@
-#include <homer_mapping/ParticleFilter/Particle.h>
-
-Particle::Particle(float weight, int id) {
- m_Weight = weight;
- m_Id = id;
-}
-
-Particle::~Particle() {
-}
-
diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
deleted file mode 100644
index 628ae187df2b2ea8d1bb131a3ac8c12083de87b6..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp
+++ /dev/null
@@ -1,662 +0,0 @@
-#include <homer_mapping/ParticleFilter/SlamFilter.h>
-#include <omp.h>
-
-// minimum move for translation in m
-const float MIN_MOVE_DISTANCE2 = 0.001 * 0.001;
-// minimum turn in radiants
-const float MIN_TURN_DISTANCE2 = 0.001 * 0.001;
-
-const float M_2PI = 2 * M_PI;
-
-SlamFilter::SlamFilter ( int particleNum ) : ParticleFilter<SlamParticle> ( particleNum )
-{
-
- m_OccupancyMap = new OccupancyMap();
- // generate initial particles
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- m_CurrentList[i] = new SlamParticle();
- m_LastList[i] = new SlamParticle();
- }
-
- float rotationErrorRotating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_rotating", rotationErrorRotating);
- float rotationErrorTranslating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_translating", rotationErrorTranslating);
- float translationErrorTranslating = 0.0;
- ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorTranslating);
- float translationErrorRotating = 0.0;
- ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorRotating);
- float moveJitterWhileTurning = 0.0;
- ros::param::get("/particlefilter/error_values/move_jitter_while_turning", moveJitterWhileTurning);
- ros::param::get("/particlefilter/max_rotation_per_second", m_MaxRotationPerSecond);
-
- int updateMinMoveAngleDegrees;
- ros::param::get("/particlefilter/update_min_move_angle", updateMinMoveAngleDegrees);
- m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
- ros::param::get("/particlefilter/update_min_move_dist", m_UpdateMinMoveDistance);
- double maxUpdateInterval;
- ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
- m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
-
- setRotationErrorRotating ( rotationErrorRotating );
- setRotationErrorTranslating ( rotationErrorTranslating );
- setTranslationErrorTranslating ( translationErrorTranslating );
- setTranslationErrorRotating ( translationErrorRotating );
- setMoveJitterWhileTurning ( moveJitterWhileTurning );
-
- m_FirstRun = true;
- m_DoMapping = true;
-
- m_EffectiveParticleNum = m_ParticleNum;
- m_LastUpdateTime = ros::Time(0);
-}
-
-SlamFilter::SlamFilter ( SlamFilter& slamFilter ) : ParticleFilter<SlamParticle> ( slamFilter.m_ParticleNum )
-{
- m_OccupancyMap = new OccupancyMap ( * ( slamFilter.m_OccupancyMap ) );
- // generate initial particles
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- if ( slamFilter.m_CurrentList[i] == 0 )
- {
- m_CurrentList[i]=0;
- }
- else
- {
- m_CurrentList[i] = new SlamParticle ( * ( slamFilter.m_CurrentList[i] ) );
- }
- if ( slamFilter.m_LastList[i] == 0 )
- {
- m_LastList[i]=0;
- }
- else
- {
- m_LastList[i] = new SlamParticle ( * ( slamFilter.m_LastList[i] ) );
- }
- }
-
- float rotationErrorRotating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_rotating", rotationErrorRotating);
- float rotationErrorTranslating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_translating", rotationErrorTranslating);
- float translationErrorTranslating = 0.0;
- ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorTranslating);
- float translationErrorRotating = 0.0;
- ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorRotating);
- float moveJitterWhileTurning = 0.0;
- ros::param::get("/particlefilter/error_values/move_jitter_while_turning", moveJitterWhileTurning);
- ros::param::get("/particlefilter/max_rotation_per_second", m_MaxRotationPerSecond);
-
- int updateMinMoveAngleDegrees;
- ros::param::get("/particlefilter/update_min_move_angle", updateMinMoveAngleDegrees);
- m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
- ros::param::get("/particlefilter/update_min_move_dist", m_UpdateMinMoveDistance);
- double maxUpdateInterval;
- ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
- m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
-
- setRotationErrorRotating ( rotationErrorRotating );
- setRotationErrorTranslating ( rotationErrorTranslating );
- setTranslationErrorTranslating ( translationErrorTranslating );
- setTranslationErrorRotating ( translationErrorRotating );
- setMoveJitterWhileTurning ( moveJitterWhileTurning );
-
- m_FirstRun = slamFilter.m_FirstRun;
- m_DoMapping = slamFilter.m_DoMapping;
-
- m_EffectiveParticleNum = slamFilter.m_EffectiveParticleNum;
-
- m_LastUpdateTime = slamFilter.m_LastUpdateTime;
-
- m_ReferencePoseOdometry = slamFilter.m_ReferencePoseOdometry;
- m_ReferenceMeasurementTime = slamFilter.m_ReferenceMeasurementTime;
-}
-
-
-SlamFilter::~SlamFilter()
-{
- if ( m_OccupancyMap )
- {
- delete m_OccupancyMap;
- }
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- if ( m_CurrentList[i] )
- {
- delete m_CurrentList[i];
- m_CurrentList[i] = 0;
- }
- if ( m_LastList[i] )
- {
- delete m_LastList[i];
- m_LastList[i] = 0;
- }
- }
-}
-
-
-void SlamFilter::setRotationErrorRotating ( float percent )
-{
- m_Alpha1 = percent / 100.0;
-}
-
-void SlamFilter::resetHigh()
-{
- m_OccupancyMap->resetHighSensitive();
-}
-
-void SlamFilter::setRotationErrorTranslating ( float degreePerMeter )
-{
- m_Alpha2 = degreePerMeter / 180.0 * M_PI;
-}
-
-void SlamFilter::setTranslationErrorTranslating ( float percent )
-{
- m_Alpha3 = percent / 100.0;
-}
-
-void SlamFilter::setTranslationErrorRotating ( float mPerDegree )
-{
- m_Alpha4 = mPerDegree / 180.0 * M_PI;
-}
-
-void SlamFilter::setMoveJitterWhileTurning ( float mPerDegree )
-{
- m_Alpha5 = mPerDegree / 180.0 * M_PI;
-}
-
-void SlamFilter::setScanMatchingClusterSize ( float minClusterSize )
-{
- minClusterSize = minClusterSize;
-}
-
-void SlamFilter::setMapping ( bool doMapping )
-{
- m_DoMapping = doMapping;
-}
-
-void SlamFilter:: setOccupancyMap ( OccupancyMap* occupancyMap )
-{
- //delete old
- if ( m_OccupancyMap )
- {
- delete m_OccupancyMap;
- }
- //copy
- m_OccupancyMap = occupancyMap;
-}
-
-
-vector<Pose>* SlamFilter::getParticlePoses() const
-{
- vector<Pose>* particlePoses = new vector<Pose>();
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- float robotX, robotY, robotTheta;
- SlamParticle* particle = m_CurrentList[i];
- particle->getRobotPose ( robotX, robotY, robotTheta );
- particlePoses->push_back ( Pose ( robotX, robotY, robotTheta ) );
- }
- return particlePoses;
-}
-
-vector<SlamParticle*>* SlamFilter::getParticles() const
-{
- vector<SlamParticle*>* particles = new vector<SlamParticle*>();
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- SlamParticle* particle = m_CurrentList[i];
- particles->push_back ( particle );
- }
- return particles;
-}
-
-void SlamFilter::setRobotPose ( Pose pose, double scatterVarXY, double scatterVarTheta )
-{
- // set first particle to exact position
- m_CurrentList[0]->setRobotPose ( pose.x(), pose.y(), pose.theta() );
- m_LastList[0]->setRobotPose ( pose.x(), pose.y(), pose.theta() );
- // scatter remaining particles
- for ( int i = 1; i < m_ParticleNum; ++i )
- {
- const double scatterX = randomGauss() * scatterVarXY;
- const double scatterY = randomGauss() * scatterVarXY;
- const double scatterTheta = randomGauss() * scatterVarTheta;
-
- m_CurrentList[i]->setRobotPose ( pose.x()+scatterX, pose.y()+scatterY, pose.theta()+scatterTheta );
- m_LastList[i]->setRobotPose ( pose.x()+scatterX, pose.y()+scatterY, pose.theta()+scatterTheta );
- }
-}
-
-vector<float> SlamFilter::getParticleWeights() const
-{
- vector<float> particleWeights ( m_ParticleNum );
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- particleWeights[i] = m_CurrentList[i]->getWeight();
- }
- return particleWeights;
-}
-
-double SlamFilter::randomGauss ( float variance ) const
-{
- if ( variance < 0 )
- {
- variance = -variance;
- }
- double x1, x2, w, y1;
- do
- {
- x1 = 2.0 * random01() - 1.0;
- x2 = 2.0 * random01() - 1.0;
- w = x1 * x1 + x2 * x2;
- }
- while ( w >= 1.0 );
-
- w = sqrt ( ( -2.0 * log ( w ) ) / w );
- y1 = x1 * w;
- // now y1 is uniformly distributed
- return sqrt ( variance ) * y1;
-}
-
-vector<float> SlamFilter::filterOutliers (sensor_msgs::LaserScanConstPtr rawData, float maxDiff )
-{
- if ( rawData->ranges.size() < 2 )
- {
- return rawData->ranges;
- }
- vector<float> filteredData = rawData->ranges;
- for ( unsigned int i = 1; i < filteredData.size() - 1; i++ )
- {
- if ( abs ( ( float ) ( rawData->ranges[i-1] - rawData->ranges[i]*2 + rawData->ranges[i+1] ) ) > maxDiff*2 )
- {
- filteredData[i] = 0;
- }
- }
- if ( fabs ( rawData->ranges[0] - rawData->ranges[1] ) > maxDiff )
- {
- filteredData[0] = 0;
- }
- if ( fabs ( rawData->ranges[ rawData->ranges.size()-1 ] - rawData->ranges[ rawData->ranges.size()-2 ] ) > maxDiff )
- {
- filteredData[ rawData->ranges.size()-1 ] = 0;
- }
-
- return filteredData;
-}
-
-void SlamFilter::filter (Pose currentPose, sensor_msgs::LaserScanConstPtr laserData, ros::Time measurementTime, ros::Duration &FilterDuration)
-{
- // if first run, initialize data
- if ( m_FirstRun )
- {
- m_FirstRun = false;
- // only do mapping, save first pose as reference
- if ( m_DoMapping )
- {
- tf::Transform transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(0.0, 0.0, 0.0));
- m_OccupancyMap->insertLaserData ( laserData , transform);
- }
- m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(*laserData); //copy const ptr to be able to change values; //test
- m_ReferencePoseOdometry = currentPose;
- m_ReferenceMeasurementTime = measurementTime;
-
- measure();
- ROS_INFO_STREAM("first run!");
- normalize();
- sort ( 0, m_ParticleNum - 1 );
- return;
- }
- //m_CurrentLaserConfig = laserConf;
- m_CurrentPoseOdometry = currentPose;
- m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(*laserData); //copy const ptr to be able to change values
- m_CurrentLaserData->ranges = filterOutliers ( laserData, 0.3 );
-
- Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
-
- // do not resample if move to small
- if ( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 )
- {
- ROS_DEBUG_STREAM( "Move too small, will not resample." );
- if ( m_EffectiveParticleNum < m_ParticleNum / 10 )
- {
- resample();
- ROS_INFO_STREAM( "Particles too scattered, resampling." );
- // filter steps
- drift();
- measure();
- normalize();
-
- sort ( 0, m_ParticleNum - 1 );
- }
- }
- else
- {
- resample();
- // filter steps
- drift();
- measure();
- normalize();
-
- sort ( 0, m_ParticleNum - 1 );
- }
-
- Pose likeliestPose = getLikeliestPose(measurementTime); //test
- Transformation2D transSinceLastUpdate = likeliestPose - m_LastUpdatePose;
-
- ostringstream stream;
- stream.precision ( 2 );
- stream << "Transformation since last update: angle=" << Math::rad2Deg ( transSinceLastUpdate.theta() ) << " dist=" << transSinceLastUpdate.magnitude() << "m" << endl;
-
- bool update = ( fabs ( transSinceLastUpdate.theta() ) > m_UpdateMinMoveAngle ) ||
- ( transSinceLastUpdate.magnitude() > m_UpdateMinMoveDistance ) ||
- ( ( measurementTime - m_LastUpdateTime ) > m_MaxUpdateInterval );
-
- if ( m_DoMapping && update )
- {
- stream << "Updating map.";
- double elapsedSeconds = ( measurementTime - m_ReferenceMeasurementTime ).toSec();
- double thetaPerSecond;
- if(elapsedSeconds == 0.0)
- {
- thetaPerSecond = trans.theta();
- }
- else
- {
- thetaPerSecond = trans.theta() / elapsedSeconds;
- }
-
- m_LastUpdatePose = likeliestPose;
- m_LastUpdateTime = measurementTime;
- if ( std::fabs(thetaPerSecond) < m_MaxRotationPerSecond )
- {
- updateMap();
- }
- else
- {
- ROS_DEBUG_STREAM( "No mapping performed, rotation angle too big." );
- }
- }
- else
- {
- stream << "No map update performed.";
- }
- ROS_DEBUG_STREAM( stream.str() );
- // safe last used pose and laserdata as reference
-
- m_ReferencePoseOdometry = m_CurrentPoseOdometry;
- m_ReferenceMeasurementTime = measurementTime;
-}
-
-/**
- * For the probabilistic motion model of the robot we use the following three parameters:
- * - When the robot starts, the initial orientation may have errors (a few degrees). (m_InitialOrientationError)
- * - The distance of the robot movement may be wrong (a percentage of the moved distance). (m_TranslationError)
- * - The orientation of the robot when the motion was finished may be wrong (a percentage of the rotation) (m_RotationError).
- * [cf. "An Efficient FastSLAM Algorithm for Generating Maps of Large-Scale Cyclic Environments
- * from Raw Laser Range Measurements", Dirk Haenelt et. al.]
- * We use Gaussian-Distributions to estimate the error.
- * The expected value of the errors are zero.
- */
-
-void SlamFilter::drift()
-{
-
- float rx = m_ReferencePoseOdometry.x();
- float ry = m_ReferencePoseOdometry.y();
- float rt = m_ReferencePoseOdometry.theta();
- float cx = m_CurrentPoseOdometry.x();
- float cy = m_CurrentPoseOdometry.y();
- float ct = m_CurrentPoseOdometry.theta();
-
- Transformation2D odoTrans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
-
- // find out if driving forward or backward
- bool backwardMove = false;
- float scalar = odoTrans.x() * cosf ( rt ) + odoTrans.y() * sinf ( rt );
- if ( scalar <= 0 )
- {
- backwardMove = true;
- }
- float distance = sqrt ( sqr ( odoTrans.x() ) + sqr ( odoTrans.y() ) );
- float deltaRot1, deltaTrans, deltaRot2;
- if ( distance < sqrt ( MIN_MOVE_DISTANCE2 ) )
- {
- deltaRot1 = odoTrans.theta();
- deltaTrans = 0.0;
- deltaRot2 = 0.0;
- }
- else if ( backwardMove )
- {
- deltaRot1 = atan2 ( ry - cy, rx - cx ) - rt;
- deltaTrans = - distance;
- deltaRot2 = ct - rt - deltaRot1;
- }
- else
- {
- deltaRot1 = atan2 ( odoTrans.y(), odoTrans.x() ) - rt;
- deltaTrans = distance;
- deltaRot2 = ct - rt - deltaRot1;
- }
-
- while ( deltaRot1 >= M_PI ) deltaRot1 -= M_2PI;
- while ( deltaRot1 < -M_PI ) deltaRot1 += M_2PI;
- while ( deltaRot2 >= M_PI ) deltaRot2 -= M_2PI;
- while ( deltaRot2 < -M_PI ) deltaRot2 += M_2PI;
-
- // always leave one particle with pure displacement
- SlamParticle* particle = m_CurrentList[0];
- // get stored pose
- float robotX, robotY, robotTheta;
- particle->getRobotPose ( robotX, robotY, robotTheta );
- Pose pose ( robotX, robotY, robotTheta );
- // move pose
- float posX = pose.x() + deltaTrans * cos ( pose.theta() + deltaRot1 );
- float posY = pose.y() + deltaTrans * sin ( pose.theta() + deltaRot1 );
- float theta = pose.theta() + deltaRot1 + deltaRot2;
- while ( theta > M_PI ) theta -= M_2PI;
- while ( theta <= -M_PI ) theta += M_2PI;
- // save new pose
- particle->setRobotPose ( posX, posY, theta );
- int i = 1;
-
- //calculating parallel on 8 threats
- //TODO: ERROR ON RESET MAPS
-// omp_set_num_threads(4);
-// #pragma omp parallel for
- for ( i = 1; i < m_ParticleNum; i++ )
- {
- SlamParticle* particle = m_CurrentList[i];
- // get stored pose
- float robotX, robotY, robotTheta;
- particle->getRobotPose ( robotX, robotY, robotTheta );
- Pose pose ( robotX, robotY, robotTheta );
- // move pose
- float estDeltaRot1 = deltaRot1 - randomGauss ( m_Alpha1 * fabs ( deltaRot1 ) + m_Alpha2 * deltaTrans );
- float estDeltaTrans = deltaTrans - randomGauss ( m_Alpha3 * deltaTrans + m_Alpha4 * ( fabs ( deltaRot1 ) + fabs ( deltaRot2 ) ) );
- float estDeltaRot2 = deltaRot2 - randomGauss ( m_Alpha1 * fabs ( deltaRot2 ) + m_Alpha2 * deltaTrans );
-
- float posX = pose.x() + estDeltaTrans * cos ( pose.theta() + estDeltaRot1 ) + randomGauss ( m_Alpha5 * fabs ( estDeltaRot1 + estDeltaRot2 ) );
- float posY = pose.y() + estDeltaTrans * sin ( pose.theta() + estDeltaRot1 ) + randomGauss ( m_Alpha5 * fabs ( estDeltaRot1 + estDeltaRot2 ) );
- float theta = pose.theta() + estDeltaRot1 + estDeltaRot2;
-
- // save new pose
- while ( theta > M_PI ) theta -= M_2PI;
- while ( theta <= -M_PI ) theta += M_2PI;
-
- particle->setRobotPose ( posX, posY, theta );
- }
-
-
-}
-
-
-void SlamFilter::measure()
-{
- if ( m_OccupancyMap )
- {
- m_MeasurePoints = m_OccupancyMap->getMeasurePoints ( m_CurrentLaserData );
-
- //calculating parallel on 8 threats
- omp_set_num_threads(8);
- int i = 0;
- //#pragma omp parallel for
- for (i = 0; i < m_ParticleNum; i++ )
- {
- SlamParticle* particle = m_CurrentList[i];
- if ( !particle )
- {
- ROS_ERROR_STREAM("ERROR: Particle is NULL-pointer!");
- }
- else
- {
- // calculate weights
- float robotX, robotY, robotTheta;
- particle->getRobotPose ( robotX, robotY, robotTheta );
- Pose pose ( robotX, robotY, robotTheta );
- float weight = m_OccupancyMap->computeScore ( pose, m_MeasurePoints );
- particle->setWeight ( weight );
- }
- }
- }
- m_EffectiveParticleNum = getEffectiveParticleNum2();
-
-}
-
-void SlamFilter::updateMap()
-{
- m_OccupancyMap->insertLaserData ( m_CurrentLaserData, m_latestTransform);
-}
-
-void SlamFilter::printParticles() const
-{
- cout << endl << "### PARTICLE LIST ###" << endl;
- cout << right << fixed;
- cout.width ( 5 );
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- SlamParticle* p_particle = m_CurrentList[i];
- if ( p_particle )
- {
- float robotX, robotY, robotTheta;
- p_particle->getRobotPose ( robotX, robotY, robotTheta );
- cout << "Particle " << i << ": (" << robotX << "," << robotY << "," << robotTheta * 180.0 / M_PI << "), weight:\t" << p_particle->getWeight() << endl;
- }
- }
- cout << "### END OF LIST ###" << endl;
-}
-
-
-void SlamFilter::reduceParticleNumber ( int newParticleNum )
-{
- if ( newParticleNum < m_ParticleNum )
- {
-
- SlamParticle** newCurrentList = new SlamParticle*[newParticleNum];
- SlamParticle** newLastList = new SlamParticle*[newParticleNum];
-
- for ( int i = 0; i < newParticleNum; i++ )
- {
- newCurrentList[i] = m_CurrentList[i];
- newLastList[i] = m_LastList[i];
- }
-
- for ( int i = newParticleNum + 1; i < m_ParticleNum; i++ )
- {
- delete m_CurrentList[i];
- delete m_LastList[i];
- }
- delete[] m_CurrentList;
- delete[] m_LastList;
-
- m_CurrentList = newCurrentList;
- m_LastList = newLastList;
-
- m_ParticleNum = newParticleNum;
- normalize();
- }
-}
-
-Pose SlamFilter::getLikeliestPose(ros::Time poseTime)
-{
- float percentage = 0.4; //TODO param? //test
- float sumX = 0, sumY = 0, sumDirX = 0, sumDirY = 0;
- int numParticles = static_cast<int> ( percentage / 100 * m_ParticleNum );
- if ( 0 == numParticles )
- {
- numParticles = 1;
- }
- for ( int i = 0; i < numParticles; i++ )
- {
- float robotX, robotY, robotTheta;
- m_CurrentList[i]->getRobotPose ( robotX, robotY, robotTheta );
- sumX += robotX;
- sumY += robotY;
- // calculate sum of vectors in unit circle
- sumDirX += cos ( robotTheta );
- sumDirY += sin ( robotTheta );
- }
- float meanTheta = atan2 ( sumDirY, sumDirX );
- float meanX = sumX / numParticles;
- float meanY = sumY / numParticles;
- //broadcast transform map -> base_link
- tf::Transform transform(tf::createQuaternionFromYaw(meanTheta),
- tf::Vector3(sumX / numParticles, sumY / numParticles, 0.0));
- tf::TransformBroadcaster tfBroadcaster;
- m_latestTransform = transform;
-
- tfBroadcaster.sendTransform(tf::StampedTransform(m_latestTransform, poseTime, "/map", "/base_link"));
- return Pose ( meanX, meanY, meanTheta );
-}
-
-OccupancyMap* SlamFilter::getLikeliestMap() const
-{
- return m_OccupancyMap;
-}
-
-void SlamFilter::getPoseVariances ( int particleNum, float& poseVarianceX, float& poseVarianceY )
-{
-
- // the particles of m_CurrentList are sorted by their weights
- if ( particleNum > m_ParticleNum || particleNum <= 0 )
- {
- particleNum = m_ParticleNum;
- }
- // calculate average pose
- float averagePoseX = 0;
- float averagePoseY = 0;
- float robotX = 0.0;
- float robotY = 0.0;
- float robotTheta = 0.0;
- for ( int i = 0; i < particleNum; i++ )
- {
- m_CurrentList[i]->getRobotPose ( robotX, robotY, robotTheta );
- averagePoseX += robotX;
- averagePoseY += robotY;
- }
- averagePoseX /= particleNum;
- averagePoseY /= particleNum;
-
- // calculate standard deviation of pose
- poseVarianceX = 0.0;
- poseVarianceY = 0.0;
- for ( int i = 0; i < particleNum; i++ )
- {
- m_CurrentList[i]->getRobotPose ( robotX, robotY, robotTheta );
- poseVarianceX += ( averagePoseX - robotX ) * ( averagePoseX - robotX );
- poseVarianceY += ( averagePoseY - robotY ) * ( averagePoseY - robotY );
- }
- poseVarianceX /= particleNum;
- poseVarianceY /= particleNum;
-}
-
-double SlamFilter::evaluateByContrast()
-{
- return m_OccupancyMap->evaluateByContrast();
-}
-
-void SlamFilter::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg)
-{
- m_OccupancyMap->applyMasking(msg);
-}
diff --git a/homer_mapping/src/ParticleFilter/SlamParticle.cpp b/homer_mapping/src/ParticleFilter/SlamParticle.cpp
deleted file mode 100644
index c0024dec39ff14883cc39bb0c962a652313c0629..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/SlamParticle.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-#include <homer_mapping/ParticleFilter/SlamParticle.h>
-
-SlamParticle::SlamParticle(float weight, float robotX, float robotY, float robotTheta) : Particle(weight) {
- m_RobotPositionX = robotX;
- m_RobotPositionY = robotY;
- m_RobotOrientation = robotTheta;
-}
-
-SlamParticle::SlamParticle( SlamParticle& slamParticle )
-{
- m_RobotPositionX = slamParticle.m_RobotPositionX;
- m_RobotPositionY = slamParticle.m_RobotPositionY;
- m_RobotOrientation = slamParticle.m_RobotOrientation;
-}
-
-SlamParticle::~SlamParticle() {
-}
-
-void SlamParticle::setRobotPose(float robotX, float robotY, float robotTheta) {
- m_RobotPositionX = robotX;
- m_RobotPositionY = robotY;
- m_RobotOrientation = robotTheta;
-}
-
-void SlamParticle::getRobotPose(float& robotX, float& robotY, float& robotTheta) {
- robotX = m_RobotPositionX;
- robotY = m_RobotPositionY;
- robotTheta = m_RobotOrientation;
-}
-
diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp
deleted file mode 100644
index c80692b22b0c147b9c4fbad1a0407681a3589ff4..0000000000000000000000000000000000000000
--- a/homer_mapping/src/slam_node.cpp
+++ /dev/null
@@ -1,387 +0,0 @@
-#include <homer_mapping/slam_node.h>
-
-SlamNode::SlamNode(ros::NodeHandle* nh)
- : m_HyperSlamFilter( 0 )
-{
- init();
-
- // subscribe to topics
- m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>("/scan", 1, &SlamNode::callbackLaserScan, this);
- m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>("/odom", 1, &SlamNode::callbackOdometry, this);
- m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>("/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this);
- m_InitialPoseSubscriber = nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, &SlamNode::callbackInitialPose, this);
- m_DoMappingSubscriber = nh->subscribe<std_msgs::Bool>("/homer_mapping/do_mapping", 1, &SlamNode::callbackDoMapping, this);
- m_ResetMapSubscriber = nh->subscribe<std_msgs::Empty>("/map_manager/reset_maps", 1, &SlamNode::callbackResetMap, this);
- m_LoadMapSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1, &SlamNode::callbackLoadedMap, this);
- m_MaskingSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1, &SlamNode::callbackMasking, this);
- m_ResetHighSubscriber = nh->subscribe<std_msgs::Empty>("/map_manager/reset_high", 1, &SlamNode::callbackResetHigh, this);
-
- // advertise topics
- m_PoseStampedPublisher = nh->advertise<geometry_msgs::PoseStamped>("/pose", 2);
- m_SLAMMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
-
-
- geometry_msgs::PoseStamped poseMsg;
- poseMsg.header.stamp = ros::Time(0);
- poseMsg.header.frame_id = "map";
- poseMsg.pose.position.x = 0.0;
- poseMsg.pose.position.y = 0.0;
- poseMsg.pose.position.z = 0.0;
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
- geometry_msgs::Quaternion quatMsg;
- tf::quaternionTFToMsg(quatTF, quatMsg); //conversion from tf::Quaternion to
- poseMsg.pose.orientation = quatMsg;
- m_PoseStampedPublisher.publish(poseMsg);
-
-//// //broadcast transform map -> base_link
- tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
- m_tfBroadcaster.sendTransform(tf::StampedTransform(transform, poseMsg.header.stamp, "map", "base_link"));
- Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y, tf::getYaw(poseMsg.pose.orientation));
- m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta );
-
-}
-
-void SlamNode::init()
-{
- double waitTime;
- ros::param::get("/particlefilter/wait_time", waitTime);
- m_WaitDuration = ros::Duration(waitTime);
- ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY);
- ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
-
- m_DoMapping = true;
-
- int particleNum;
- ros::param::get("/particlefilter/particle_num", particleNum);
- int particleFilterNum;
- ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num", particleFilterNum);
- m_HyperSlamFilter = new HyperSlamFilter ( particleFilterNum, particleNum );
-
- m_ReferenceOdometryTime = ros::Time(0);
- m_LastMapSendTime = ros::Time(0);
- m_LastPositionSendTime = ros::Time(0);
- m_LastMappingTime = ros::Time(0);
- m_ReferenceOdometryTime = ros::Time(0);
-
- m_laser_queue.clear();
- m_odom_queue.clear();
-}
-
-SlamNode::~SlamNode()
-{
- delete m_HyperSlamFilter;
-}
-
-void SlamNode::resetMaps()
-{
- ROS_INFO( "Resetting maps.." );
-
- delete m_HyperSlamFilter;
- m_HyperSlamFilter = 0;
-
- init();
- geometry_msgs::PoseStamped poseMsg;
- poseMsg.header.stamp = ros::Time::now();
- poseMsg.header.frame_id = "map";
- poseMsg.pose.position.x = 0.0;
- poseMsg.pose.position.y = 0.0;
- poseMsg.pose.position.z = 0.0;
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
- geometry_msgs::Quaternion quatMsg;
- tf::quaternionTFToMsg(quatTF, quatMsg); //conversion from tf::Quaternion to
- poseMsg.pose.orientation = quatMsg;
- m_PoseStampedPublisher.publish(poseMsg);
-
- m_LastLikeliestPose.set(0.0, 0.0);
- m_LastLikeliestPose.setTheta(0.0f);
-
-//// //broadcast transform map -> base_link
- tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
- m_tfBroadcaster.sendTransform(tf::StampedTransform(transform, poseMsg.header.stamp, "map", "base_link"));
- Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y, tf::getYaw(poseMsg.pose.orientation));
- m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta );
-
-// sendMapDataMessage();
-}
-
-void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg)
-{
- m_HyperSlamFilter->resetHigh();
-}
-
-
-void SlamNode::sendMapDataMessage(ros::Time mapTime)
-{
- std::vector<int8_t> mapData;
- int width, height;
- float resolution;
-
- OccupancyMap* occMap = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
- occMap->getOccupancyProbabilityImage (mapData, width, height, resolution);
-
- if ( width != height )
- {
- ROS_ERROR_STREAM("ERROR: Map is not quadratic! can not send map!");
- }
- else
- {
- nav_msgs::OccupancyGrid mapMsg;
- std_msgs::Header header;
- header.stamp = mapTime;
- header.frame_id = "map";
- mapMsg.header = header;
- nav_msgs::MapMetaData mapMetaData;
- mapMetaData.width = width;
- mapMetaData.height = height;
- mapMetaData.resolution = resolution;
- mapMetaData.origin.position.x = -height*resolution/2;
- mapMetaData.origin.position.y = -width*resolution/2;
- mapMetaData.origin.orientation.w = 1.0;
- mapMetaData.origin.orientation.x = 0.0;
- mapMetaData.origin.orientation.y = 0.0;
- mapMetaData.origin.orientation.z = 0.0;
- mapMsg.info = mapMetaData;
- mapMsg.data = mapData;
-
- m_SLAMMapPublisher.publish(mapMsg);
- }
-}
-
-void SlamNode::callbackUserDefPose( const geometry_msgs::Pose::ConstPtr& msg )
-{
- Pose userdef_pose(msg->position.x, msg->position.y, tf::getYaw(msg->orientation));
- m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta );
-}
-
-void SlamNode::callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
-{
- Pose userdef_pose(msg->pose.pose.position.x, msg->pose.pose.position.y, tf::getYaw(msg->pose.pose.orientation));
- m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta );
-}
-
-void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg)
-{
- m_laser_queue.push_back(msg);
- if(m_laser_queue.size() > 5) //Todo param
- {
- m_laser_queue.erase(m_laser_queue.begin());
- }
-}
-
-void SlamNode::callbackOdometry( const nav_msgs::Odometry::ConstPtr& msg)
-{
- m_odom_queue.push_back(msg);
- static int last_i = 0;
- if(m_odom_queue.size() > 5) //Todo param
- {
- last_i--;
- if(last_i < 0)
- {
- last_i = 0;
- }
- m_odom_queue.erase(m_odom_queue.begin());
- }
-
- static Pose last_interpolatedPose(0.0, 0.0, 0.0);
- ros::Time currentOdometryTime = msg->header.stamp;
- if( ( ros::Time::now() - m_LastMappingTime > m_WaitDuration ) && m_odom_queue.size() > 1 && m_laser_queue.size() > 0)
- {
- int i, j;
- bool got_match = false;
-
- for( i = m_odom_queue.size()-1; i > 0 ; i--)
- {
- //Check if we would repeat a calculation which is already done but still in the queue
- if(m_odom_queue.at(i-1)->header.stamp == m_ReferenceOdometryTime)
- {
- break;
- }
-
- for(j = m_laser_queue.size()-1; j > -1; j--)
- {
- // find a laserscan in between two odometry readings (or at the same time)
- if(
- (m_laser_queue.at(j)->header.stamp >= m_odom_queue.at(i-1)->header.stamp)
- &&
- (m_odom_queue.at(i)->header.stamp >= m_laser_queue.at(j)->header.stamp)
- )
- {
- got_match = true;
- break;
- }
- }
- if(got_match)
- {
- break;
- }
- }
-
- if(got_match)
- {
- last_i = i;
- m_LastLaserData = m_laser_queue.at(j);
- m_LastMappingTime = m_LastLaserData->header.stamp;
-
- float odoX = m_odom_queue.at(i)->pose.pose.position.x;
- float odoY = m_odom_queue.at(i)->pose.pose.position.y;
- float odoTheta = tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation);
- Pose currentOdometryPose ( odoX, odoY, odoTheta );
- currentOdometryTime = m_odom_queue.at(i)->header.stamp;
-
- odoX = m_odom_queue.at(i-1)->pose.pose.position.x;
- odoY = m_odom_queue.at(i-1)->pose.pose.position.y;
- odoTheta = tf::getYaw(m_odom_queue.at(i-1)->pose.pose.orientation);
- Pose lastOdometryPose ( odoX, odoY, odoTheta );
- m_ReferenceOdometryPose = lastOdometryPose;
- m_ReferenceOdometryTime = m_odom_queue.at(i-1)->header.stamp;
-
-
- ros::Time startTime = ros::Time::now();
- // laserscan in between current odometry reading and m_ReferenceOdometry
- // -> calculate pose of robot during laser scan
- ros::Duration d1 = m_LastLaserData->header.stamp - m_ReferenceOdometryTime;
- ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime;
-
- float timeFactor;
- if(d1.toSec() == 0.0)
- {
- timeFactor = 0.0f;
- }
- else if(d2.toSec() == 0.0)
- {
- timeFactor = 1.0f;
- }
- else
- {
- timeFactor = d1.toSec() / d2.toSec();
- }
- ros::Duration duration = ros::Duration(0);
-
- last_interpolatedPose = m_ReferenceOdometryPose.interpolate ( currentOdometryPose, timeFactor );
- m_HyperSlamFilter->filter( last_interpolatedPose, m_LastLaserData, m_LastLaserData->header.stamp, duration);
- ros::Time finishTime = ros::Time::now();
-
- m_LastLikeliestPose = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose(m_LastLaserData->header.stamp);
-
- //TODO löschen
- m_LastPositionSendTime = m_LastLaserData->header.stamp;
- // send map max. every 500 ms
- if ( (m_LastLaserData->header.stamp - m_LastMapSendTime).toSec() > 0.5 )
- {
- sendMapDataMessage(m_LastLaserData->header.stamp);
- m_LastMapSendTime = m_LastLaserData->header.stamp;
- }
-
- ROS_DEBUG_STREAM("Pos. data calc time: " << (finishTime - startTime).toSec() << "s" );
- ROS_DEBUG_STREAM("Map send Interval: " << ( finishTime - m_LastPositionSendTime ).toSec() << "s" );
- }
- }
- Pose currentPose = m_LastLikeliestPose;
- //currentPose.setX(currentPose.x() - last_interpolatedPose.x());
- //currentPose.setY(currentPose.y() - last_interpolatedPose.y());
- //currentPose.setTheta(currentPose.theta() - last_interpolatedPose.theta());
- for(int i = (last_i-1<0?0:last_i-1); i < m_odom_queue.size(); i++)
- {
- currentPose.setX(currentPose.x() + m_odom_queue.at(i)->twist.twist.linear.x);
- currentPose.setY(currentPose.y() + m_odom_queue.at(i)->twist.twist.linear.y);
- currentPose.setTheta(currentPose.theta() + m_odom_queue.at(i)->twist.twist.angular.z);
- }
-
- geometry_msgs::PoseStamped poseMsg;
- //header
- poseMsg.header.stamp = msg->header.stamp;
- poseMsg.header.frame_id = "map";
-
- //position and orientation
- poseMsg.pose.position.x = currentPose.x();
- poseMsg.pose.position.y = currentPose.y();
- poseMsg.pose.position.z = 0.0;
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta());
- geometry_msgs::Quaternion quatMsg;
- tf::quaternionTFToMsg(quatTF, quatMsg); //conversion from tf::Quaternion to geometry_msgs::Quaternion
- poseMsg.pose.orientation = quatMsg;
- m_PoseStampedPublisher.publish(poseMsg);
- //broadcast transform map -> base_link
- //tf::Transform transform(quatTF,tf::Vector3(currentPose.x(), currentPose.y(), 0.0));
- //m_tfBroadcaster.sendTransform(tf::StampedTransform(transform, msg->header.stamp, "map", "base_link"));
-}
-
-void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr &msg)
-{
- m_DoMapping = msg->data;
- m_HyperSlamFilter->setMapping ( m_DoMapping );
- ROS_INFO_STREAM( "Do mapping is set to " << ( m_DoMapping ) );
-}
-
-void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr &msg)
-{
- resetMaps();
-}
-
-void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
-{
- float res = msg->info.resolution;
- int height = msg->info.height; // cell size
- int width = msg->info.width; //cell size
- if(height!=width) {
- ROS_ERROR("Height != width in loaded map");
- return;
- }
-
- //convert map vector from ros format to robbie probability array
- float* map = new float[msg->data.size()];
- //generate exploredRegion
- int minX = INT_MIN;
- int minY = INT_MIN;
- int maxX = INT_MAX;
- int maxY = INT_MAX;
- for(size_t y = 0; y < msg->info.height; y++)
- {
- int yOffset = msg->info.width * y;
- for(size_t x = 0; x < msg->info.width; x++)
- {
- int i = yOffset + x;
- if(msg->data[i] == -1 )
- map[i] = 0.5;
- else
- map[i] = msg->data[i]/100.0;
-
- if(map[i]!=0.5) {
- if(minX==INT_MIN || minX > (int)x)
- minX = (int)x;
- if(minY==INT_MIN || minY > (int)y)
- minY = (int)y;
- if(maxX==INT_MAX || maxX < (int)x)
- maxX = (int)x;
- if(maxY==INT_MAX || maxY < (int)y)
- maxY = (int)y;
- }
- }
- }
- Box2D<int> exploredRegion = Box2D<int> ( minX, minY, maxX, maxY );
- OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width, exploredRegion);
- m_HyperSlamFilter->setOccupancyMap( occMap );
- m_HyperSlamFilter->setMapping( false ); //is this already done by gui message?
- ROS_INFO_STREAM( "Replacing occupancy map" );
-}
-
-void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg)
-{
- m_HyperSlamFilter->applyMasking(msg);
-}
-
-/**
- * @brief main function
- */
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "homer_mapping");
- ros::NodeHandle nh;
-
- SlamNode slamNode(&nh);
-
- ros::Rate loop_rate(50);
- ros::spin();
- return 0;
-}
-
diff --git a/homer_nav_libs/CHANGELOG.rst b/homer_nav_libs/CHANGELOG.rst
deleted file mode 100644
index df486cd25f123f680e7d183e52a81d0d7ba4481b..0000000000000000000000000000000000000000
--- a/homer_nav_libs/CHANGELOG.rst
+++ /dev/null
@@ -1,9 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package homer_nav_libs
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.1.1 (2016-11-03)
-------------------
-* fixes
-* initial commit
-* Contributors: Niklas Yann Wettengel
diff --git a/homer_nav_libs/CMakeLists.txt b/homer_nav_libs/CMakeLists.txt
deleted file mode 100644
index c368217131a707cfd9ab2e79a104523cbea855d1..0000000000000000000000000000000000000000
--- a/homer_nav_libs/CMakeLists.txt
+++ /dev/null
@@ -1,44 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(homer_nav_libs)
-
-find_package(catkin REQUIRED COMPONENTS
- roscpp
- geometry_msgs
- sensor_msgs
- tf
- cmake_modules
-)
-
-find_package(Eigen3 REQUIRED)
-
-catkin_package(
- INCLUDE_DIRS include
- LIBRARIES homerExplorer homerMappingMath
- CATKIN_DEPENDS roscpp geometry_msgs tf
- DEPENDS Eigen3)
-
-
-include_directories(
- include
- ${catkin_INCLUDE_DIRS}
- ${EIGEN3_INCLUDE_DIRS}
-)
-
-add_library(homerExplorer src/Explorer/Explorer.cpp)
-add_library(homerMappingMath
- src/Math/Line2D.cpp
- src/Math/Transformation2D.cpp
- src/Math/Pose.cpp
- src/Math/Math.cpp
- src/Math/Point2D.cpp
-)
-
-install(DIRECTORY include/${PROJECT_NAME}/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-
-install(TARGETS homerExplorer homerMappingMath
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
diff --git a/homer_nav_libs/README.md b/homer_nav_libs/README.md
deleted file mode 100644
index 4c31e40eedeadeec620fe3756cad0ab1c5849b4c..0000000000000000000000000000000000000000
--- a/homer_nav_libs/README.md
+++ /dev/null
@@ -1,13 +0,0 @@
-# nav_libs
-
-## 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.
diff --git a/homer_nav_libs/include/homer_nav_libs/Explorer/Explorer.h b/homer_nav_libs/include/homer_nav_libs/Explorer/Explorer.h
deleted file mode 100644
index 2d34991270d1f94d5fcac6e24417744cd1ed25de..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Explorer/Explorer.h
+++ /dev/null
@@ -1,359 +0,0 @@
-#ifndef EXPLORER_H
-#define EXPLORER_H
-
-#include <vector>
-#include <cmath>
-#include <iostream>
-#include <queue>
-#include <sstream>
-#include <geometry_msgs/Pose.h>
-
-#include <homer_nav_libs/Explorer/GridMap.h>
-#include <homer_nav_libs/tools.h>
-
-namespace ExplorerConstants
-{
- static int8_t UNKNOWN;
- static const int8_t NOT_SEEN_YET = -1;
- static const double MAX_DISTANCE = DBL_MAX;
- static const double MAX_COST = DBL_MAX;
- static const int OBSTACLE = INT_MAX;
-}
-
-/**
- * @class Explorer
- * @author Malte Knauf, Stephan Wirth, David Gossow (RX)
- * @brief Path planning & exploration class
- *
- * Usage:
- *
- * - Call setOccupancyMap() to set the base map for path finding.
- * - Set a start point by calling setStart()
- *
- * - For path planning:
- * +Choose a target by calling setTarget()
- * +To correct a target to the nearest approachable position,
- * call getNearestAccessibleTarget
- * +Call getPathTransformPath()
- *
- * -For exploration:
- * +Call resetExploration()
- * +Call getExplorationTransformPath()
- * +The calculated target is the last element in the returned path
- *
- * - Call sampleWaypointsFromPath() to extract waypoints from a calculated path
- *
- * This class uses a couple of "maps" for computation and storing data:
- *
- * - m_OccupancyMap stores the occupancy probabilities in double values. A value of 100 means
- * totally occupied, 0 totally free.
- * - m_ObstacleDistanceMap stores in each cell the distance (one unit = one cell) to the nearest obstacle.
- * This map is computed by an eucledian distance transformation from m_OccupancyMap.
- * - m_FrontierMap is a bool map which has 1 in frontier cells and 0 in all others. A frontier
- * is defined as a free cell that has one of its four direct neighbours in unknown space and is "safe" for
- * the robot (m_ObstacleDistanceMap is used for that).
- * - m_DrivingDistanceMap is a double map that stores for each cell the distance to m_Start. It is computed
- * by a flood-fill (seed-fill) algorithm. The values are therefor only an approximation and not exact.
- * m_DrivingDistanceMap is used to search the nearest frontier when requesting an auto target.
- * - m_TargetMap is a double map that stores for each cell the distance to m_Target. It is computed
- * like m_DrivingDistanceMap. This map is used as heuristic for the A*-Pathfinding algorithm.
- * - m_NavigationMap is used to mark the cells that are touched by the A*-Pathfinding algorithm.
- *
- *
- * The coordinate system and units that are used in this class are based on map cells.
- * @see GridMap
- *
- */
-class Explorer
-{
-
- public:
-
- /**
- * @brief Default constructor.
- * @param minAllowedObstacleDistance,maxAllowedObstacleDistance Range of allowed distances to next obstacle [Pixels]
- * @param minSafeObstacleDistance,maxSafeObstacleDistance Range of distances to next obstacle considered as safe [Pixels]
- * @param safePathWeight Weight for safer path
- */
- Explorer ( double minAllowedObstacleDistance, double maxAllowedObstacleDistance,
- double minSafeObstacleDistance, double maxSafeObstacleDistance,
- double safePathWeight, double frontierSafenessFactor=1.0, int unknownThreshold=50 );
-
- /**
- * @brief Destructor deletes all dynamically allocated memory used by the maps
- */
- ~Explorer();
-
- void setUnknownThreshold(int unknownTresh);
- void setAllowedObstacleDistance ( double min, double max );
- void setSafeObstacleDistance ( double min, double max );
- void setFrontierSafenessFactor ( double frontierSafenessFactor );
- void setSafePathWeight ( double weight );
- /**
- * @brief Copies and sets the occupancy map.
- * @param width Width of the map
- * @param height Height of the map
- * @param origin Real-world pose of the cell (0,0) in the map
- * @param data GridMap-data (occupancy probabilities: 0 = free, 100 = occupied) of size width * height
- */
- void setOccupancyMap ( int width, int height, geometry_msgs::Pose origin, int8_t* mapData);
-
- /** only update occupied areas in current occupancy map */
- void updateObstacles ( int width, int height, geometry_msgs::Pose origin, int8_t* mapData );
-
- /**
- * @brief Sets the start position for the path finding algorithm.
- * m_Start is set to the given value.
- * If startPixel lies outside the map, m_Start remains untouched.
- * @param startPixel Start position for path finding in pixel (map-) coordinates.
- */
- void setStart ( Eigen::Vector2i start );
-
- /**
- * @brief Resets the internal state of the exploration mode.
- * Sets m_DesiredDistance to 0, such that getExplorationTransformPath() triggers
- * a frontier exploration if there is no prior call of setTarget(point, distance).
- * Call this method once before every exploration.
- */
- void resetExploration();
-
- /**
- * Sets the target position for path finding. m_Target is set to the given value.
- * If endPixel lies outside of the map, m_Target remains untouched.
- * computeTargetDistanceMap() is called at the end of this method. m
- * @param targetPixel Target to reach from startPixel
- */
- void setTarget ( Eigen::Vector2i targetPixel );
-
- /**
- * Sets the target region for path finding. m_ExplorationMap is set to the given region.
- * If targetPixel lies outside of the map, the exploration map is set empty.
- * @param targetPixel Center of the target region to reach from startPixel
- * @param radius Radius of the target region in pixels
- */
- void setTarget( Eigen::Vector2i targetPixel, int radius );
-
- /**
- * @brief find the nearest position to target that is approachble from the start position
- */
- Eigen::Vector2i getNearestAccessibleTarget ( Eigen::Vector2i target );
-
- /**
- * @brief find the nearest position to target surpassing the minimum obstacle distance
- */
- Eigen::Vector2i getNearestWalkablePoint ( Eigen::Vector2i target );
-
- /**
- * @brief Returns the map-coordinates of the nearest frontier to m_Start.
- * Uses m_DrivingDistanceMap and m_ObstacleDistanceMap. If there is no frontier left,
- * nextFrontier remains untouched.
- * @param[out] nextFrontier Nearest frontier in map-coordinates.
- * @return true if frontier found and stored in nextFrontier, false if no frontier found (nextFrontier
- * remains untouched).
- */
- bool getNearestFrontier ( Eigen::Vector2i& nextFrontier );
-
- /**
- * Computes the path from m_Start to m_Target with path transform.
- * The result is returned. If the returned vector contains no elements, there is no path.
- * @return vector with path points
- */
- std::vector<Eigen::Vector2i> getPath( bool &success );
-
- /**
- * Computes the path from m_Start to the next frontier using exploration transform.
- * The result is returned. If the returned vector contains no elements, there is no path.
- * @return vector with path points
- */
- std::vector<Eigen::Vector2i> getExplorationTransformPath( bool &success );
-
- /**
- * @brief Returns a version of the path that contains less vertices.
- * @note The nearer the next obstacle, the more waypoints are created.
- * @param path List of vertices to be simplified
- * @param treshold[0..1] a lower threshold results in more waypoints (default:1.0)
- * @return Vector of (sampled) waypoints.
- */
- std::vector<Eigen::Vector2i> sampleWaypointsFromPath ( std::vector<Eigen::Vector2i> path, float threshold=1.0 );
-
- /**
- * Getters for the different transforms (see constructor for description)
- */
- GridMap<int8_t>* getOccupancyMap();
- GridMap<double>* getObstacleTransform();
- GridMap<double>* getCostTransform();
- GridMap<bool>* getTargetMap();
- GridMap<double>* getDrivingDistanceTransform();
- GridMap<double>* getTargetDistanceTransform();
- GridMap<double>* getPathTransform();
- GridMap<double>* getExplorationTransform();
-
- /**
- * @return Start position
- */
- Eigen::Vector2i getStart() const;
-
- /**
- * @return Target position
- */
- Eigen::Vector2i getTarget() const;
-
- private:
-
- /** @brief Delete the given map and set pointer to 0 */
- template <class T>
- void releaseMap ( GridMap<T>*& map )
- {
- if ( map )
- {
- delete map;
- map=0;
- }
- }
-
- /** @brief Delete and re-create given map */
- template <class T>
- void resetMap ( GridMap<T>*& map )
- {
- if ( !m_OccupancyMap )
- {
- ROS_ERROR ( "Occupancy map is missing." );
- return;
- }
- releaseMap ( map );
- map = new GridMap<T> ( m_OccupancyMap->width(), m_OccupancyMap->height() );
- }
-
- /**
- * @return true if the robot can stand on the given position without touching an obstacle, false otherwise
- * @warning Call computeWalkableMaps before
- */
- inline bool isWalkable ( int x, int y ) const
- {
- return ( ( m_OccupancyMap->getValue ( x, y ) <= ExplorerConstants::UNKNOWN ) &&
- ( m_ObstacleTransform->getValue ( x, y ) > m_MinAllowedObstacleDistance ) );
- }
-
- /**
- * @return true if point is approachable from the current start position, false otherwise.
- * @warning m_OccupancyMap, m_ObstacleTransform and m_DrivingDistanceTransform have to be present!
- * @warning Call computeApproachableMaps before
- */
- inline bool isApproachable ( int x, int y ) const
- {
- return ( m_DrivingDistanceTransform->getValue ( x, y ) < ExplorerConstants::MAX_DISTANCE );
- }
-
- /** @brief Releases all memory of the member maps */
- void releaseMaps();
-
- /**
- * @brief Helper function for computeDistanceTransformation.
- * @param f 1D-Array for distance transformation
- * @param n Number of elements in f
- * @return Distance transformation of f
- */
- double* distanceTransform1D ( double *f, int n );
-
- /**
- * @brief Fills the given map from given start point with distance values to this point.
- * The filling will only be performed on cells that are marked as free in m_OccupancyMap and
- * that have an obstacle distance value between m_MinimumObstacleDistance and m_MaximumObstacleDistance.
- * The map that is passed as argument will be fully overwritten by this function.
- * @param map GridMap to fill
- * @param start Start point for the fill algorithm
- */
- void distanceFloodFill ( GridMap<double>* map, Eigen::Vector2i start );
-
- /** @brief Compute map needed for path calculation */
- void computePathTransform();
-
- /** @brief Compute map needed for exploration path calculation */
- void computeExplorationTransform();
-
- /** @brief Compute the distances to the next obstacle with eucledian distance transform from m_OccupancyMap. */
- void computeObstacleTransform();
-
- /** @brief Compute cost function based on obstacle transform */
- void computeCostTransform();
-
- /** @brief Compute the frontiers between free and unknown space. Depends on OccupancyMap and ObstacleTransform. */
- void computeFrontierMap();
-
- /** @brief Compute the target region (a circle of radius m_DesiredDistance around m_Target). */
- void computeRegionMap();
-
- /** @brief Compute the target map, which is either a frontier map or a region map. */
- void computeTargetMap();
-
- /** @brief Compute a map of driving distances from the start point */
- void computeDrivingDistanceTransform();
-
- /** @brief Compute a map of driving distances to the target point */
- void computeTargetDistanceTransform();
-
- /** @brief Compute maps needed for isWalkable */
- void computeWalkableMaps();
-
- /** @brief Compute maps needed for isApproachable */
- void computeApproachableMaps();
-
- /** @brief Start point for the way search algorithm. */
- Eigen::Vector2i m_Start;
-
- /** @brief Target for the way search algorithm */
- Eigen::Vector2i m_Target;
-
- /** @brief Desired distance to target in pixels */
- int m_DesiredDistance;
-
- /** @brief Occupancy map */
- GridMap<int8_t>* m_OccupancyMap;
-
- /** @see computeObstacleTransform */
- GridMap<double>* m_ObstacleTransform;
-
- /** @see computeCostTransform */
- GridMap<double>* m_CostTransform;
-
- /** @see computeTargetMap */
- GridMap<bool>* m_TargetMap;
-
- /** computeDrivingDistanceTransform */
- GridMap<double>* m_DrivingDistanceTransform;
-
- /** @see computeTargetDistanceTransform */
- GridMap<double>* m_TargetDistanceTransform;
-
- /** @see computePathTransform */
- GridMap<double>* m_PathTransform;
-
- /** @see computeExplorationTransform */
- GridMap<double>* m_ExplorationTransform;
-
- /** @see constructor */
- double m_MinAllowedObstacleDistance;
- double m_MaxAllowedObstacleDistance;
-
- double m_MinSafeObstacleDistance;
- double m_MaxSafeObstacleDistance;
-
- /**
- * Weight for safer path
- */
- double m_SafePathWeight;
-
- /**
- * Factor for minObstacleDistance that determines if a frontier pixel is valid
- */
- double m_FrontierSafenessFactor;
-
- /**
- * Real-world pose of the point (0,0) in the map
- */
- geometry_msgs::Pose m_Origin;
-
-};
-
-#endif
-
diff --git a/homer_nav_libs/include/homer_nav_libs/Explorer/GridMap.h b/homer_nav_libs/include/homer_nav_libs/Explorer/GridMap.h
deleted file mode 100644
index 64b3469eb331afcb9ff24067564fa1ff479ce234..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Explorer/GridMap.h
+++ /dev/null
@@ -1,575 +0,0 @@
-#ifndef GridMap_H
-#define GridMap_H
-
-#include <float.h>
-#include <iostream>
-#include <sstream>
-
-#include "ros/ros.h"
-
-#include <Eigen/Geometry>
-
-#define GRIDMAP_SAFE_ACCESS
-
-/**
- * @class GridMap
- * @author Malte Knauf, Stephan Wirth, David Gossow (RX)
- * @brief GridMap data structure. Implemeted as template class. The template type
- * defines the data type of each map cell.
- */
-
-template<class DataT>
-class GridMap
-{
-
- public:
-
- /// Initialize empty map
- GridMap();
-
- /**
- * @param width Width of the map.
- * @param height Height of the map.
- * @param data Pointer to map data, must be of size width*height.
- * @param copyData if true, the map data will be copied
- * if false, GridMap takes ownership of the pointer
- * @param cellSize physical size of each map cell [m]
- * @param centerX,centerY center of the map in world coordinates
- */
- GridMap ( int width, int height, DataT* data = 0, bool copyData = true, float cellSize = 1, float centerX = 0, float centerY = 0 );
-
- /// Copy data from given region
- GridMap ( int width, int height, DataT* data, Eigen::AlignedBox2i extractRegion );
-
- /// Copy data from given map
- GridMap<DataT> ( const GridMap<DataT>& other ) { m_Data=0; *this = other; }
-
- /// Copy data from given map
- GridMap<DataT>& operator= ( const GridMap<DataT>& other );
-
- ~GridMap();
-
- /// Convert map coordinates to world coordinates
- void mapToWorld ( int mapX, int mapY, float& worldX, float& worldY );
-
- /// Convert world coordinates to map coordinates
- void worldToMap ( float worldX, float worldY, int& mapX, int& mapY );
-
- /// @brief set value at given position
- inline void setValue ( int x, int y, DataT val );
-
- /// @brief replace content with given value
- void fill ( DataT val );
-
- /// @brief Draw a filled polygon into the map (world coords)
- void drawPolygon ( std::vector<Eigen::Vector2d> vertices, DataT value );
-
- /// @brief Draw a filled circle into the map (world coords)
- void drawCircle( Eigen::Vector2d center, float radius, DataT value );
-
- /// @return Value at the given position.
- inline DataT getValue ( int x, int y ) const;
-
- /// @return Pointer to given pixel
- inline DataT* getDirectAccess ( int x, int y );
-
- /// @return width in grid cells
- int width() const { return m_Width; }
-
- /// @return height in grid cells
- int height() const { return m_Height; }
-
- /// @return center of the map in world coordinates
- Eigen::Vector2d center() const {return Eigen::Vector2d(m_CenterX,m_CenterY);}
-
- /// @return side length of one cell in mm
- float cellSize() { return m_CellSize; }
-
- private:
-
- void drawLine ( DataT *data, int startX, int startY, int endX, int endY, DataT value );
- void fillPolygon ( DataT* data, int x, int y, char value );
-
- int m_Width;
- int m_Height;
- int m_DataSize;
- DataT* m_Data;
- float m_CellSize;
- float m_CenterX;
- float m_CenterY;
-};
-
-
-template<class DataT>
-GridMap<DataT>::GridMap()
-{
- m_Width = 0;
- m_Height = 0;
- m_DataSize = 0;
- m_Data = 0;
- m_CellSize = 0;
- m_CenterX = 0;
- m_CenterY = 0;
-}
-
-template<class DataT>
-GridMap<DataT>::GridMap ( int width, int height, DataT* data, bool copyData, float cellSize, float centerX, float centerY )
-{
- m_Width = width;
- m_Height = height;
- m_CellSize = cellSize;
- m_DataSize = width * height;
- m_CenterX = centerX;
- m_CenterY = centerY;
- m_Data = 0;
-
- if ( data )
- {
- if ( copyData )
- {
- m_Data = new DataT[m_DataSize];
-
- for ( int i = 0; i < m_DataSize; i++ )
- {
- m_Data[i] = data[i];
- }
- }
- else
- {
- m_Data = data;
- }
- }
- else
- {
- m_Data = new DataT[m_DataSize];
-
- for ( int i = 0; i < m_DataSize; i++ )
- {
- m_Data[i] = 0;
- }
- }
-}
-
-template<class DataT>
-GridMap<DataT>::GridMap (int width, int height, DataT* data, Eigen::AlignedBox2i extractRegion )
-{
- m_Width = extractRegion.sizes().x();
- m_Height = extractRegion.sizes().y();
- m_DataSize = m_Width * m_Height;
- m_Data = new DataT[m_DataSize];
- m_CellSize = 1;
- m_CenterX = 0;
- m_CenterY = 0;
-
- for ( int y = extractRegion.min().y(); y <= extractRegion.max().y(); y++ )
- {
- int yOffset = m_Width * y;
-
- for ( int x = extractRegion.min().x(); x <= extractRegion.max().x(); x++ )
- {
- int i = x + yOffset;
- m_Data[i] = data[i];
- }
- }
-}
-
-
-template<class DataT>
-inline DataT* GridMap<DataT>::getDirectAccess ( int x, int y )
-{
-#ifdef GRIDMAP_SAFE_ACCESS
- if ( x >= 0 && x < m_Width && y >= 0 && y < m_Height )
- {
- return &m_Data[y * m_Width + x];
- }
- else
- {
- throw;
- }
-#else
- return &m_Data[y * m_Width + x];
-#endif
-}
-
-
-template<class DataT>
-GridMap<DataT>& GridMap<DataT>::operator= ( const GridMap<DataT>& other )
-{
- delete[] m_Data;
- m_Width = other.m_Width;
- m_Height = other.m_Height;
- m_DataSize = other.m_DataSize;
- m_Data = new DataT[m_DataSize];
- memcpy ( m_Data, other.m_Data, sizeof ( DataT ) *m_DataSize );
- m_CellSize = other.m_CellSize;
- m_CenterX = other.m_CenterX;
- m_CenterY = other.m_CenterY;
- return *this;
-}
-/* TODO
-template<class DataT>
-GridMap<DataT>::GridMap ( ExtendedInStream& strm )
-{
- short version;
- strm >> version;
- strm >> m_Width;
- strm >> m_Height;
- strm >> m_CellSize;
- strm >> m_CenterX;
- strm >> m_CenterY;
- m_DataSize = m_Width * m_Height;
- m_Data = new DataT[m_DataSize];
- strm.get ( m_Data, m_DataSize );
-}
-*/
-template<class DataT>
-GridMap<DataT>::~GridMap()
-{
- if ( m_Data )
- {
- delete m_Data;
- m_Data = 0;
- }
-}
-/*
-template<class DataT>
-void GridMap<DataT>::storer ( ExtendedOutStream& strm ) const
-{
- strm << short ( 12 );
- strm << m_Width;
- strm << m_Height;
- strm << m_CellSize;
- strm << m_CenterX;
- strm << m_CenterY;
- strm.put ( m_Data, m_DataSize );
-}
-*/
-
-template<class DataT>
-void GridMap<DataT>::mapToWorld ( int mapX, int mapY, float& worldX, float& worldY )
-{
- worldX = m_CenterX + m_CellSize * ( mapX - m_Width / 2 );
- worldY = m_CenterY + m_CellSize * ( mapY - m_Height / 2 );
-}
-
-template<class DataT>
-void GridMap<DataT>::worldToMap ( float worldX, float worldY, int& mapX, int& mapY )
-{
- mapX = float ( m_Width ) / 2.0 - ( ( worldY - m_CenterY ) / m_CellSize + 0.5 );
- mapY = float ( m_Height ) / 2.0 - ( ( worldX - m_CenterX ) / m_CellSize + 0.5 );
-
- if ( mapX < 0 || mapX >= m_Width || mapY < 0 || mapY >= m_Height )
- {
- //ROS_WARN_STREAM ( "Index out of bounds: " << mapX << "," << mapY ); //TODO
-
- if ( mapX < 0 )
- {
- mapX = 0;
- }
-
- if ( mapX >= m_Width )
- {
- mapX = m_Width - 1;
- }
-
- if ( mapY < 0 )
- {
- mapY = 0;
- }
-
- if ( mapY >= m_Height )
- {
- mapY = m_Height - 1;
- }
- }
-}
-
-
-template<class DataT>
-inline void GridMap<DataT>::setValue ( int x, int y, DataT val )
-{
-#ifdef GRIDMAP_SAFE_ACCESS
- if ( x >= 0 && x < m_Width && y >= 0 && y < m_Height )
- {
- m_Data[y * m_Width + x] = val;
- }
- else
- {
- throw;
- }
-#else
- m_Data[y * m_Width + x] = val;
-#endif
-}
-
-template<class DataT>
-inline DataT GridMap<DataT>::getValue ( int x, int y ) const
-{
-#ifdef GRIDMAP_SAFE_ACCESS
- if ( x >= 0 && x < m_Width && y >= 0 && y < m_Height )
- {
- return m_Data[y * m_Width + x];
- }
- else
- {
- ROS_ERROR_STREAM( "Accessing map pixels " << x << "," << y << ": out of bounds (0,0," << m_Width-1 << "," << m_Height-1 << ")" ); //TODO
- throw;
- }
-#else
- return m_Data[y * m_Width + x];
-#endif
-}
-
-template<class DataT>
-void GridMap<DataT>::fill ( DataT val )
-{
- for ( int i = 0; i < m_DataSize; i++ )
- {
- m_Data[i] = val;
- }
-}
-
-/* TODO do we need image representation?
-template<class DataT>
-puma2::ColorImageRGB8* GridMap<DataT>::getImage ( DataT specialValue, DataT clipRangeLow, DataT clipRangeHigh )
-{
- puma2::ColorImageRGB8* image = new puma2::ColorImageRGB8 ( m_Width, m_Height );
- double maxVal = 0.0001;
- double minVal = 0.0;
-
- for ( int i = 0; i < m_DataSize; i++ )
- {
- if ( ( m_Data[i] < minVal ) && ( m_Data[i] != specialValue ) )
- {
- minVal = m_Data[i];
- }
-
- if ( ( m_Data[i] > maxVal ) && ( m_Data[i] != specialValue ) )
- {
- maxVal = m_Data[i];
- }
- }
-
- std::ostringstream stream;
-
- stream << " Min: " << minVal << "Max: " << maxVal;
- stream << " ClipMin: " << double ( clipRangeLow ) << " ClipMax: " << double ( clipRangeHigh );
- ROS_DEBUG_STREAM ( stream.str() ); //TODO: was TRACE_SYSTEMINFO
-
- if ( maxVal > clipRangeHigh )
- {
- maxVal = clipRangeHigh;
- }
-
- if ( minVal < clipRangeLow )
- {
- minVal = clipRangeLow;
- }
-
- double range = maxVal - minVal;
-
- puma2::ColorImageRGB8::PixelType* imageData;
- imageData = image->unsafeRowPointerArray() [0];
-
- for ( int i = 0; i < m_DataSize; i++ )
- {
- DataT currentValue = m_Data[i];
-
- if ( currentValue == specialValue )
- {
- imageData[i][0] = 40;
- imageData[i][1] = 220;
- imageData[i][2] = 120;
- continue;
- }
-
- if ( currentValue > clipRangeHigh )
- {
- imageData[i][0] = 200;
- imageData[i][1] = 200;
- imageData[i][2] = 128;
- continue;
- }
-
- if ( currentValue < clipRangeLow )
- {
- imageData[i][0] = 40;
- imageData[i][1] = 40;
- imageData[i][2] = 180;
- continue;
- }
-
- double valueDouble = ( ( double ) ( currentValue - minVal ) ) / range;
-
- unsigned char value = ( unsigned char ) ( valueDouble * 255 );
-
- imageData[i][0] = value;
- imageData[i][1] = value;
- imageData[i][2] = value;
- }
-
- return image;
-}
-*/
-
-
-
-template<class DataT>
-void GridMap<DataT>::drawCircle(Eigen::Vector2d center, float radius, DataT value )
-{
- int centerMapX,centerMapY;
- worldToMap( center.x(), center.y(), centerMapX, centerMapY );
-
- int radiusCells = radius / m_CellSize;
- int radiusCells2 = radiusCells*radiusCells;
-
- Eigen::AlignedBox2i bBox( Eigen::Vector2i(centerMapX - radiusCells, centerMapY - radiusCells), Eigen::Vector2i(centerMapX + radiusCells, centerMapY + radiusCells) );
- Eigen::AlignedBox2i bBoxGrid( Eigen::Vector2i(0,0), Eigen::Vector2i(m_Width-1,m_Height-1) );
- bBox.clamp( bBoxGrid );
-
- for ( int y = bBox.min().y(); y <= bBox.max().y(); y++ )
- {
- for ( int x = bBox.min().x(); x <= bBox.max().x(); x++ )
- {
- int xC = x-centerMapX;
- int yC = y-centerMapY;
- if ( xC*xC+yC*yC <= radiusCells2 )
- {
- setValue( x, y, value );
- }
- }
- }
-}
-
-
-template<class DataT>
-void GridMap<DataT>::drawPolygon (std::vector<Eigen::Vector2d> vertices, DataT value )
-{
- if ( vertices.size() == 0 )
- {
- ROS_INFO( "No vertices given!" );
- return;
- }
- //make temp. map
- DataT* data = new DataT[ m_DataSize ];
- for ( int i = 0; i < m_DataSize; 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();
- int startX,startY,endX,endY;
- worldToMap( vertices[i].x(), vertices[i].y(), startX, startY );
- worldToMap( vertices[i2].x(), vertices[i2].y(), endX, endY );
- drawLine ( data, startX, startY, endX, endY, 1 );
- }
- //claculate 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();
- int midMapX,midMapY;
- worldToMap( midX, midY, midMapX, midMapY );
- //fill polygon
- fillPolygon ( data, midMapX, midMapY, 1 );
-
- //copy polygon to map
- for ( int i = 0; i < m_DataSize; i++ )
- {
- if ( data[i] != 0 )
- {
- m_Data[i] = value;
- }
- }
-
- delete[] data;
-}
-
-template<class DataT>
-void GridMap<DataT>::fillPolygon ( DataT* data, int x, int y, char 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 );
- }
-}
-
-
-template<class DataT>
-void GridMap<DataT>::drawLine ( DataT *data, int startX, int startY, int endX, int endY, DataT 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;
- }
- }
-}
-
-
-
-#endif
-
-#ifdef GRIDMAP_SAFE_ACCESS
-#undef GRIDMAP_SAFE_ACCESS
-#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Box2D.h b/homer_nav_libs/include/homer_nav_libs/Math/Box2D.h
deleted file mode 100644
index fc6a5478977767c7274dbc6851ec87bc3cf24c59..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Math/Box2D.h
+++ /dev/null
@@ -1,184 +0,0 @@
-/*******************************************************************************
- * Box2D.h
- *
- * (C) 2008 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Additional information:
- * $Id: Box2D.h 44313 2011-04-06 22:46:28Z agas $
- *******************************************************************************/
-
-#ifndef Box2D_H
-#define Box2D_H
-
-#include <homer_nav_libs/Math/Point2D.h>
-#include <vector>
-
-/**
- * @class Box2D
- * @author David Gossow (RX)
- * @brief Represents a box given by the upper-left and lower-right corner
- */
-template<class T=float>
-class Box2D
-{
-
- public:
-
- /** @brief Creates a box given by top-left (minX,minY) and lower-right (maxX,maxY) coordinates */
- Box2D(T minX=0, T minY=0, T maxX=0, T maxY=0);
-
- /** @brief The destructor */
- ~Box2D() {};
-
- inline void setMinX(T value) { m_MinX=value; }
- inline void setMaxX(T value) { m_MaxX=value; }
- inline void setMinY(T value) { m_MinY=value; }
- inline void setMaxY(T value) { m_MaxY=value; }
-
- inline T minX() const { return m_MinX; }
- inline T maxX() const { return m_MaxX; }
- inline T minY() const { return m_MinY; }
- inline T maxY() const { return m_MaxY; }
-
- inline T width() const { return m_MaxX-m_MinX; }
- inline T height() const { return m_MaxY-m_MinY; }
-
- std::vector< Point2D > vertices();
-
- /** @brief Clip the box to fit into clipArea */
- void clip( Box2D<T> clipArea );
-
- /** @return true if the given point is inside the box */
- bool contains( T x, T y );
-
- /** @brief enlarge the box by 'size' units in all directions */
- void expand( T size );
-
- /** @brief shrink the box by 'size' units in all directions */
- void shrink( T size );
-
- /** @brief expand the box so that it contains the given point */
- void enclose( Point2D point );
- void enclose( T x, T y );
-
- template<class OtherT>
- void enclose( Box2D<OtherT> box );
-
- Point2D centerPoint()
- {
- Point2D center;
- center.setX(m_MinX + ( (m_MaxX - m_MinX) / 2 ) );
- center.setY(m_MinY + ( (m_MaxY - m_MinY) / 2 ) );
- return center;
- }
-
- /** @brief area covered by the box */
- T area();
-
- Box2D<T>& operator/= ( T div ) { m_MinX/=div; m_MinY/=div; m_MaxX/=div; m_MaxY/=div; return *this; }
- Box2D<T>& operator*= ( T div ) { m_MinX*=div; m_MinY*=div; m_MaxX*=div; m_MaxY*=div; return *this; }
-
- private:
-
- T m_MinX;
- T m_MaxX;
- T m_MinY;
- T m_MaxY;
-
-};
-
-template<class T>
-Box2D<T>::Box2D(T minX, T minY, T maxX, T maxY)
-{
- m_MinX=minX;
- m_MinY=minY;
- m_MaxX=maxX;
- m_MaxY=maxY;
-}
-
-template<class T>
-void Box2D<T>::clip( Box2D<T> clipArea )
-{
- if (m_MinX < clipArea.minX()) { m_MinX=clipArea.minX(); }
- if (m_MinY < clipArea.minY()) { m_MinY=clipArea.minY(); }
- if (m_MaxX > clipArea.maxX()) { m_MaxX=clipArea.maxX(); }
- if (m_MaxY > clipArea.maxY()) { m_MaxY=clipArea.maxY(); }
-}
-
-template<class T>
-bool Box2D<T>::contains( T x, T y )
-{
- return ( (x>=m_MinX) && (x<=m_MaxX) && (y>=m_MinY) && (y<=m_MaxY) );
-}
-
-template<class T>
- void Box2D<T>::expand( T size )
-{
- m_MinX-=size;
- m_MaxX+=size;
- m_MinY-=size;
- m_MaxY+=size;
-}
-
-template<class T>
-void Box2D<T>::shrink( T size )
-{
- m_MinX+=size;
- m_MaxX-=size;
- m_MinY+=size;
- m_MaxY-=size;
-}
-
-template<class T>
-T Box2D<T>::area()
-{
- T width = m_MaxX - m_MinX;
- T height = m_MaxY - m_MinY;
-
- T capacity = width * height;
-
- return (T) capacity;
-}
-
-template<class T>
-void Box2D<T>::enclose( Point2D point )
-{
- if ( m_MinX > point.x() ) { m_MinX=point.x(); }
- if ( m_MinY > point.y() ) { m_MinY=point.y(); }
- if ( m_MaxX < point.x() ) { m_MaxX=point.x(); }
- if ( m_MaxY < point.y() ) { m_MaxY=point.y(); }
-}
-
-template<class T>
-void Box2D<T>::enclose( T x, T y )
-{
- if ( m_MinX > x ) { m_MinX=x; }
- if ( m_MinY > y ) { m_MinY=y; }
- if ( m_MaxX < x ) { m_MaxX=x; }
- if ( m_MaxY < y ) { m_MaxY=y; }
-}
-
-template<class T>
-template<class OtherT>
-void Box2D<T>::enclose( Box2D<OtherT> box )
-{
- enclose( box.minX(), box.minY() );
- enclose( box.maxX(), box.maxY() );
-}
-
-
-template<class T>
-std::vector< Point2D > Box2D<T>::vertices()
-{
- std::vector<Point2D> myVertices(5);
- myVertices[0]=Point2D( m_MinX-0.5, m_MinY-0.5 );
- myVertices[1]=Point2D( m_MinX-0.5, m_MaxY+0.5 );
- myVertices[2]=Point2D( m_MaxX+0.5, m_MaxY+0.5 );
- myVertices[3]=Point2D( m_MaxX+0.5, m_MinY-0.5 );
- myVertices[4]=myVertices[0];
- return myVertices;
-}
-
-
-#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Line2D.h b/homer_nav_libs/include/homer_nav_libs/Math/Line2D.h
deleted file mode 100644
index 9761be66439a30872edcce7a1f91535971a7f179..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Math/Line2D.h
+++ /dev/null
@@ -1,223 +0,0 @@
-/*******************************************************************************
- * Line2D.h
- *
- * (C) 2008 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Information on Code Review state:
- * Author: SM; DevelTest: Date; Reviewer: Initials; Review: Date; State: NOK
- *
- * Additional information:
- * $Id: Line2D.h 44313 2011-04-06 22:46:28Z agas $
- *******************************************************************************/
-
-#ifndef LINE2D_H
-#define LINE2D_H
-
-#include <homer_nav_libs/Math/vec2.h>
-#include <homer_nav_libs/Math/mat2.h>
-#include <homer_nav_libs/Math/Point2D.h>
-#include <vector>
-
-/**
- * @class Line2D
- *
- * @author Susanne Maur
- *
- */
-class Line2D
-{
-
- public:
-
- /**
- * Creates a new line.
- * @param start Start point of the line.
- * @param end End point of the line.
- */
- inline Line2D ( Point2D start, Point2D end )
- {
- m_Start = start;
- m_Vec = end-m_Start;
- }
-
-
- /**
- * Creates a new line.
- * @param start Start point of the line.
- * @param vec The vector from start to end point of the line.
- */
- inline Line2D ( Point2D start, CVec2 vec )
- {
- m_Start = start;
- m_Vec = vec;
- }
-
- /**
- * Destructor does nothing.
- */
- inline ~Line2D() {}
-
- /**
- * Sets the start point of the line to a new value.
- * @param start Start point of the line.
- */
- inline void setStart ( const Point2D start )
- {
- Point2D end = m_Start + m_Vec;
- m_Start = start;
- m_Vec = end-m_Start;
- }
-
- /**
- * Sets the end point of the line to a new value.
- * @param end End point of the line.
- */
-
- inline void setEnd ( const Point2D end )
- {
- m_Vec = end - m_Start;
- }
-
- /**
- * Returns the start point of the line.
- * @return Start point of the line.
- */
- inline Point2D start() const
- {
- return m_Start;
- }
-
- /**
- * Returns the end point of the line.
- * @return End point of the line.
- */
- inline Point2D end() const
- {
- return m_Start + m_Vec;
- }
-
- /**
- * Returns a vector from the start to the end of the line.
- * @return Vector to the end point of the line.
- */
- inline CVec2 vec() const
- {
- return m_Vec;
- }
-
- inline bool operator== ( const Line2D& line ) const
- {
- return ( m_Start == line.start() && end() == line.end() );
- }
-
- /**
- * Returns the gradient of the line.
- * @return Gradient of the line.
- */
- float gradient() const ;
-
- /**
- * Returns the length of the line.
- * @return Length of the line.
- */
- inline float length() const {
- return m_Vec.magnitude();
- }
-
- /**
- * Returns the minimum euclidean distance of the given point to the line.
- * @param point Point of which the distance to the line will be calculated.
- * @return Distance of point to line.
- */
- inline float distance ( Point2D point ) const
- {
- Point2D pointOnLine = getRootPoint ( point );
- return ( point - pointOnLine ).magnitude();
- }
-
- /**
- * Rotates the line round the origin.
- * @param angle The angle of rotation in radiants.
- */
- inline void rotate ( float angle )
- {
- CMat2 rotMat = CMat2 ( angle );
- m_Start = rotMat * m_Start;
- m_Vec = rotMat * m_Vec;
- }
-
- /**
- * Returns the point of the line with the minimal distance to a given point.
- * This algorithm may return a point which lies on the extension of the line and not on itself.
- * See also: getClosestPoint.
- * @param point Point to which the distance is calculated.
- * @return Root point of the line.
- */
- inline Point2D getRootPoint ( Point2D point ) const
- {
- float t = ( point-m_Start ) * m_Vec;
- t /= m_Vec * m_Vec;
- Point2D pointOnLine = m_Start + ( t * m_Vec );
- return pointOnLine;
- }
-
- /**
- * Returns the normal of the line.
- * @return Normal of the line.
- */
- inline CVec2 getNormal() const {
- return m_Vec.getNormal()/m_Vec.magnitude();
- }
-
- /**
- * Returns the point of the line with the minimal distance to a given point.
- * This algorithm returns always a point which lies on the line.
- * Therefor it is not always the root point.
- * See also: getRootPoint.
- * @param point Point to which the distance is calculated.
- * @return Root point of the line.
- */
- Point2D getClosestPoint ( Point2D point ) const;
-
- /**
- * Returns the intersection point of this line with a second line.
- * The intersection point is element of this line.
- * @param line The line with which the intersection is calculated.
- * @return Intersection point.
- */
- Point2D getIntersectionPoint ( Line2D line ) const;
-
- /**
- * Returns the parameter t which identifies the intersection point of this line with a second line.
- * The intersection point is element of this line.
- * @param line The line with which the intersection is calculated.
- * @return Parameter t which identifies the intersection point on the line.
- */
- float getIntersectionPointParameter ( Line2D line ) const;
-
- /**
- * @return Vertices, e.g. for use in a VectorObject
- * @param substeps number of linear interpolation steps between start and end
- */
- std::vector< Point2D > vertices ( unsigned substeps=0 );
-
- /**
- * @overwrite
- */
- std::string toString() const;
-
- private:
-
- /**
- * Start point of the line.
- */
- Point2D m_Start;
-
- /**
- * Vector from the start to the end of the line.
- */
- CVec2 m_Vec;
-};
-
-#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Math.h b/homer_nav_libs/include/homer_nav_libs/Math/Math.h
deleted file mode 100644
index d1ce8e65db79a0fdef434e8e1480ac2f295f7d13..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Math/Math.h
+++ /dev/null
@@ -1,129 +0,0 @@
-/*******************************************************************************
- * Math.h
- *
- * (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Additional information:
- * $Id: $
- *******************************************************************************/
-
-#ifndef Math_H
-#define Math_H
-
-#include <vector>
-#include <homer_nav_libs/Math/Point2D.h>
-
-/**
- * @class Math
- * @brief Generic math and statistics functions
- * @author David Gossow (RX)
- */
-class Math
-{
- public:
-
- struct WeightedValue
- {
- float value;
- float weight;
- };
-
- static const double Pi = 3.14159265358979323846;
-
- /** @return mean value */
- template<class ContainerT>
- static double mean ( const ContainerT& values );
-
- /** @return variance of given values */
- template<class ContainerT>
- static double variance ( const ContainerT& values );
-
- /** @return mean angle of given values
- * @note there are always two possible choices for the mean angle. This function returns the one with the smallest deviation
- * @note Works for angles in [-Pi..Pi], negative angles are treated
- */
- static float meanAngle ( const std::vector<float>& angles );
-
- static float meanAngleWeighted ( const std::vector< WeightedValue >& weightedAngles );
-
- /** @return variance for given mean */
- static float angleVariance ( float meanAngle, const std::vector<float>& angles );
-
- /** @return minimal angle needed to turn from angle 1 to angle 2 [-Pi..Pi] */
- static float minTurnAngle ( float angle1, float angle2 );
-
- static Point2D center ( std::vector<Point2D>& points );
-
- static float deg2Rad ( float deg ) { return deg / 180.0*Pi; }
-
- static float rad2Deg ( float rad ) { return rad / Pi*180.0; }
-
- static double randomGauss ( float variance = 1.0 );
-
- static double random01 ( unsigned long init = 0 );
-
- /** @return ratio between one dimension seen under old viewangle and dimension under new viewangle*/
- static double angleToPercent ( double newAngle, double oldAngle ) { return tan ( ( Pi / 180.0 ) * newAngle / 2 ) / tan ( ( Pi / 180.0 ) * oldAngle / 2 ); };
-
- /** @return angle under which the ratio between dimension seen under old viewangle and new viewangle equals percent*/
- static double percentToAngle ( double percent, double angle ) { return 2* atan ( tan ( ( Pi / 180.0 ) * angle / 2 ) * percent ) * ( 180 / Pi ); };
-
- /** @return horizontal view angle corresponding to diagonal view angle and aspect ratio (e.g. 4.0/3.0)*/
- static double horizontalViewAngle ( double diagonalAngle, double aspectRatio ) { return verticalViewAngle ( diagonalAngle, 1.0 / aspectRatio ); };
-
- /** @return vertical view angle corresponding to diagonal view angle and aspect ratio (e.g. 4.0/3.0)*/
- static double verticalViewAngle ( double diagonalAngle, double aspectRatio )
- {
- return percentToAngle ( 1.0 / sqrt ( pow ( aspectRatio, 2 ) + 1.0 ), diagonalAngle );
- };
-
- template<class ValueT>
- static inline ValueT min ( ValueT a, ValueT b ) { return a < b ? a : b; }
-
- template<class ValueT>
- static inline ValueT max ( ValueT a, ValueT b ) { return a > b ? a : b; }
-
- private:
-
- /** @brief The constructor */
- Math();
-
- /** @brief The destructor */
- ~Math();
-
-};
-
-template<class ContainerT>
-double Math::mean ( const ContainerT& values )
-{
- typename ContainerT::const_iterator it;
- it = values.begin();
- double sum = 0;
- while ( it != values.end() )
- {
- sum += *it;
- it++;
- }
- return sum / double ( values.size() );
-}
-
-
-template<class ContainerT>
-double Math::variance ( const ContainerT& values )
-{
- double mean = mean ( values );
- typename ContainerT::const_iterator it;
- it = values.begin();
- double sum = 0;
- while ( it != values.end() )
- {
- double diff = *it - mean;
- sum += diff * diff;
- it++;
- }
- return sum / double ( values.size() );
-}
-
-
-#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Pixel.h b/homer_nav_libs/include/homer_nav_libs/Math/Pixel.h
deleted file mode 100644
index cc58f229a0cfc4edc11eced1275f42f209a361ed..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Math/Pixel.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*******************************************************************************
- * Pixel.h
- *
- * (C) 2006 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * $Id: Pixel.h 44313 2011-04-06 22:46:28Z agas $
- *******************************************************************************/
-
-#ifndef PIXEL_H
-#define PIXEL_H
-
-#include <homer_nav_libs/Math/Point2D.h>
-#include <vector>
-
-/** @class Pixel
- * @brief Stores discrete pixel coordinates
- * @author Stephan Wirth, David Gossow (RX)
- */
-class Pixel {
-
- public:
-
- inline Pixel( int x = 0, int y = 0) { m_X = x; m_Y = y; };
- inline ~Pixel() {};
-
- inline int x() const { return m_X; }
- inline int y() const { return m_Y; }
-
- inline void setX( int x ) { m_X = x; }
- inline void setY( int y ) { m_Y = y; }
-
- inline bool operator ==( Pixel& rhs ) { return ( m_X == rhs.m_X ) && ( m_Y == rhs.m_Y ); }
- inline bool operator !=( Pixel& rhs ) { return ( m_X != rhs.m_X ) || ( m_Y != rhs.m_Y ); }
- inline Pixel operator *( float rhs ) { return Pixel( m_X * rhs, m_Y * rhs ); }
- inline Pixel operator /( float rhs ) { return Pixel( m_X / rhs, m_Y / rhs ); }
-
- inline Point2D toPoint2D() { return Point2D( m_X, m_Y ); }
-
- inline std::vector<Point2D> vertices()
- {
- std::vector<Point2D> result(5);
- result[0]=Point2D( m_X-0.5, m_Y-0.5 );
- result[1]=Point2D( m_X+0.5, m_Y-0.5 );
- result[2]=Point2D( m_X+0.5, m_Y+0.5 );
- result[3]=Point2D( m_X-0.5, m_Y+0.5 );
- result[4]=result[0];
- return result;
- }
-
- private:
-
- int m_X;
- int m_Y;
-
-};
-
-
-#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Point2D.h b/homer_nav_libs/include/homer_nav_libs/Math/Point2D.h
deleted file mode 100644
index fdbf6fd0564580b686f501d678ad429fddd1722c..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Math/Point2D.h
+++ /dev/null
@@ -1,312 +0,0 @@
-/*******************************************************************************
- * Point2D.h
- *
- * (C) 2008 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * $Id: Point2D.h 44313 2011-04-06 22:46:28Z agas $
- *******************************************************************************/
-
-#include <iostream>
-#include <sstream>
-
-#include <homer_nav_libs/Math/vec2.h>
-#include <float.h>
-
-#ifndef POINT2D_H
-#define POINT2D_H
-
-/**
- * @class Point2D
- *
- * @author Susanne Maur
- *
- */
-class Point2D
-{
-
- public:
-
- /**
- * Creates a new point in 2D with x- and y-coordinat set to zero.
- */
- inline Point2D()
- {
- m_X = 0.0;
- m_Y = 0.0;
- }
-
- /**
- * Creates a new point in 2D.
- * @param x x-coordinate of the point.
- * @param y y-coordinate of the point.
- */
- inline Point2D ( double x, double y )
- {
- m_X = x;
- m_Y = y;
- }
-
-
-
- /**
- * Copy construcor
- */
- inline Point2D (const Point2D& p){
- m_X = p.x();
- m_Y = p.y();
- }
-
- /**
- * Creates a new point in 2D.
- * @param v Vector form origin to the point.
- */
- inline Point2D ( const CVec2& v )
- {
- m_X = v[0];
- m_Y = v[1];
- }
-
- /**
- * Destructor, does nothing.
- */
- inline ~Point2D()
- {
- }
-
- /**
- * Returns the x-coordinate of the point.
- * @return the x-coordinate of the point.
- */
- inline double x() const
- {
- return m_X;
- }
-
- /**
- * Returns the y-coordinate of the point.
- * @return the y-coordinate of the point.
- */
- inline double y() const
- {
- return m_Y;
- }
-
- /**
- * Sets the x- and y-coordinate of the point to new values.
- * @param x the new value of the x coordinate.
- * @param y the new value of the x coordinate.
- */
- inline void set ( double x, double y )
- {
- m_X = x;
- m_Y = y;
- }
-
- /**
- * Sets the x-coordinate of the point to a new value.
- * @param x the new value of the x coordinate.
- */
- inline void setX ( double x )
- {
- m_X = x;
- }
-
- /**
- * Sets the y-coordinate of the point to a new value.
- * @param y the new value of the x coordinate.
- */
- inline void setY ( double y )
- {
- m_Y = y;
- }
-
- /**
- * Overloaded operators.
- */
-
- inline Point2D& operator= ( const Point2D& p) {
- m_X = p.x();
- m_Y = p.y();
- return *this;
- }
-
- inline Point2D operator+ ( const CVec2& v ) const
- {
- return Point2D ( m_X + v[0], m_Y + v[1] );
- }
-
- inline Point2D operator+ ( const Point2D& p ) const
- {
- return Point2D ( m_X + p.x(), m_Y + p.y() );
- }
-
- inline CVec2 operator- ( const Point2D& p ) const
- {
- return CVec2 ( m_X - p.x(), m_Y - p.y() );
- }
-
- inline Point2D operator- ( const CVec2& v ) const
- {
- return Point2D ( m_X - v[0], m_Y - v[1] );
- }
-
- inline Point2D operator* ( double scalar ) const
- {
- return Point2D ( m_X * scalar, m_Y * scalar );
- }
-
- inline Point2D operator/ ( double scalar ) const
- {
- return Point2D ( m_X / scalar, m_Y / scalar );
- }
-
- inline Point2D& operator+= ( const CVec2& v )
- {
- m_X += v[0];
- m_Y += v[1];
- return ( *this );
- }
-
- inline Point2D& operator-= ( const CVec2& v )
- {
- m_X -= v[0];
- m_Y -= v[1];
- return ( *this );
- }
-
- inline Point2D& operator*= ( double scalar )
- {
- m_X *= scalar;
- m_Y *= scalar;
- return ( *this );
- }
-
- inline Point2D& operator/= ( double scalar )
- {
- m_X /= scalar;
- m_Y /= scalar;
- return ( *this );
- }
-
- inline double operator [] ( unsigned int i ) const
- {
- return ( ( double* ) this ) [i];
- }
-
- inline double& operator [] ( unsigned int i )
- {
- return ( ( double* ) this ) [i];
- }
-
- inline bool operator== ( const Point2D& point ) const
- {
- return ( fabs(m_X - point.x()) < 0.001 && fabs(m_Y - point.y()) < 0.001 );
- }
-
- inline bool operator!= ( const Point2D& point ) const
- {
- return !((*this)== point);
- }
-
- /**
- * Returns the distance to a given point.
- * @param point The point to calculate the distance to.
- * @return the distance between point the two points.
- */
- inline double distance ( const Point2D& point ) const
- {
- return sqrt ( ( m_X-point.x() ) * ( m_X-point.x() ) + ( m_Y-point.y() ) * ( m_Y-point.y() ) );
- }
-
- /**
- * Returns the distance to origin.
- * @return the distance between point the two points.
- */
- inline double distance ( ) const
- {
- return sqrt ( m_X * m_X + m_Y * m_Y );
- }
-
- /**
- * Checks whether two points are equal.
- * @param p The point to check equality.
- * @return true if points are equal, false otherwise.
- */
- inline bool equal ( const Point2D& point ) const
- {
- if ( ( *this - point ).magnitude() < 0.0001 )
- {
- return true;
- }
- else
- {
- return false;
- }
- }
-
- /**
- * Returns the vector which represents the point in 2D.
- * @return vector which represents the point in 2D.
- */
- inline CVec2 toVector() const
- {
- return CVec2 ( m_X, m_Y );
- }
-
- /**
- * Returns the angle of the corresponding polar coordinates.
- * @return polar angle.
- */
- float getPolarAngle () const;
-
- /**
- * Rotate by angle (in radiants) around center.
- * @param center Center of rotation
- * @param angle Angle in radiants
- */
- inline void rotate ( const Point2D& center, float angle )
- {
- double x0=m_X-center.m_X;
- double y0=m_Y-center.m_Y;
- double xRot = x0*cos ( angle ) - y0*sin ( angle );
- double yRot = x0*sin ( angle ) + y0*cos ( angle );
- m_X = xRot+center.m_X;
- m_Y = yRot+center.m_Y;
- }
-
- /**
- * Rotate by angle (in radiants) around (0,0).
- * @param angle Angle in radiants
- */
- inline void rotate ( float angle )
- {
- double xRot = m_X*cos ( angle ) - m_Y*sin ( angle );
- double yRot = m_X*sin ( angle ) + m_Y*cos ( angle );
- m_X = xRot;
- m_Y = yRot;
- }
-
- /**
- * Returns the string representation of the point.
- * @return string representation of the point.
- */
- inline std::string toString() const
- {
- std::ostringstream str;
- str << m_X << " " << m_Y;
- return str.str();
- }
-
- /** @return "invalid" Point (used as end marker in vector drawings) **/
- static Point2D invalidPoint() { return Point2D( DBL_MAX, DBL_MAX ); }
-
- bool isValid() { return ( ( m_X != DBL_MAX ) || ( m_Y != DBL_MAX ) ); }
-
- protected:
-
- double m_X;
- double m_Y;
-};
-
-#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Pose.h b/homer_nav_libs/include/homer_nav_libs/Math/Pose.h
deleted file mode 100644
index a4b47b9ad72304d903b8506556804dfe1746e9e3..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Math/Pose.h
+++ /dev/null
@@ -1,72 +0,0 @@
-/*******************************************************************************
- * Pose.h
- *
- * (C) 2006 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * $Id: Pose.h 44313 2011-04-06 22:46:28Z agas $
- *******************************************************************************/
-
-#ifndef POSE_H
-#define POSE_H
-
-#include <homer_nav_libs/Math/Point2D.h>
-
-class Transformation2D;
-
-/**
- * @class Pose
- *
- * @author Stephan Wirth, Susanne Maur (RX), David Gossow (RX)
- * @brief Class to describe and hold a pose of the robot (x, y)-Position + Orientation
- * in world-coordinates
- */
-class Pose : public Point2D {
-
-public:
-
- /**
- * Constructor which initializes the members with the given values.
- * @param x x-position
- * @param y y-position
- * @param theta orientation in radiants
- */
- Pose(float x, float y, float theta);
-
- /**
- * Default constructor, initializes members to 0.
- */
- Pose();
-
- /**
- * The destructor is empty.
- */
- ~Pose();
-
- float theta() const;
-
- void setTheta(float theta);
-
- Pose operator+ ( const Transformation2D& transformation ) const;
- Pose operator- ( const Transformation2D& transformation ) const;
- Transformation2D operator- ( const Pose& pose ) const;
-
- /**
- * Interpolates between two poses and returns a pose which correlates with
- * current pose + t * (reference pose - current pose)
- * @param referencePose The second pose to interpolate between.
- * @param t The factor of interpolation.
- * @return Interpolated pose
- */
- Pose interpolate(const Pose& referencePose, float t) const;
-
-// Pose( ExtendedInStream& extStrm );
-
-// void storer( ExtendedOutStream& extStrm ) const;
-
-protected:
-
- float m_Theta;
-};
-
-#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Transformation2D.h b/homer_nav_libs/include/homer_nav_libs/Math/Transformation2D.h
deleted file mode 100644
index a2ba0d67c093a667f36d340a3bf5fa54d6678df0..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Math/Transformation2D.h
+++ /dev/null
@@ -1,143 +0,0 @@
-/*******************************************************************************
- * Transformation2D.h
- *
- * (C) 2008 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * $Id: Transformation2D.h 44313 2011-04-06 22:46:28Z agas $
- *******************************************************************************/
-
-#ifndef TRANSFORMATION2D_H
-#define TRANSFORMATION2D_H
-
-#include <cmath>
-#include <vector>
-#include <homer_nav_libs/Math/Point2D.h>
-#include <homer_nav_libs/Math/Line2D.h>
-
-
-/**
- * @class Transformation2D
- *
- * @author Susanne Maur
- *
- * @brief Class to describe a transformation of poses in 2D.
- * This inplies a translation in x and y direction each and a rotation.
- */
-class Transformation2D : public CVec2
-{
-
- public:
-
- /**
- * Constructor that initializes the members.
- * @param x translation in x direction in m
- * @param y translation in y direction in m
- * @param theta rotation in radiants
- */
- Transformation2D ( double x, double y, double theta );
-
- /**
- * Constructor that initializes the members.
- * @param vec a vector which represents the translation in x and y direction
- * @param theta rotation in radiants
- */
- Transformation2D ( const CVec2& vec, double theta );
-
- /**
- * Default constructor sets all members to 0.0.
- */
- Transformation2D();
-
- /**
- * Default destructor.
- */
- ~Transformation2D();
-
- /**
- * Sets the values of transformation.
- * @param x translation in x direction in mm
- * @param y translation in y direction in mm
- * @param theta rotation in radiants
- */
- void set ( double x, double y, double theta );
-
- /**
- * Returns the rotation in radiants.
- * @return rotation in radiants
- */
- double theta() const;
-
- /**
- * Adds two transformations.
- */
- Transformation2D operator+ ( Transformation2D t ) const;
- Transformation2D& operator+= ( Transformation2D t );
-
- /**
- * Subtracts two transformations.
- */
- Transformation2D operator- ( Transformation2D t ) const;
- Transformation2D& operator-= ( Transformation2D t );
-
- /**
- * Scales a transformation by a factor
- */
- Transformation2D operator* ( float factor ) const;
- Transformation2D& operator*= ( float factor );
-
- /**
- * Scales a transformation by a factor
- */
- Transformation2D operator/ ( float factor ) const;
- Transformation2D& operator/= ( float factor );
-
- /**
- * Test equality of transformations.
- */
- bool operator== ( Transformation2D t ) const;
- bool operator!= ( Transformation2D t ) const;
-
- /**
- * Compare transformations.
- * (attention: algebraic signs are taken into account, if necessary use fabs())
- */
- bool operator<= ( Transformation2D t ) const;
- bool operator>= ( Transformation2D t ) const;
- bool operator< ( Transformation2D t ) const;
- bool operator> ( Transformation2D t ) const;
-
- /**
- * Applies abs() on every attribute.
- */
- Transformation2D abs() const;
-
- /**
- * Inverts the transformation, scales every attribute with -1.
- */
- Transformation2D inverse() const;
-
- /**
- * Transformes points by first rotation, then translating.
- */
- Point2D transform ( const Point2D& point ) const;
- std::vector<Point2D> transform ( const std::vector<Point2D>& points ) const;
-
- /**
- * Transformes lines by first rotation, then translating.
- */
- Line2D transform ( const Line2D& line ) const;
- std::vector<Line2D> transform ( const std::vector<Line2D>& lines ) const;
-
- /**
- * Returns the string representation of the transformation.
- * @return string representation of the transformation.
- */
- std::string toString() const;
-
- private:
- double m_Theta;
-};
-
-#endif
-
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/mat2.h b/homer_nav_libs/include/homer_nav_libs/Math/mat2.h
deleted file mode 100644
index 45365d81907ee2b5fb78662b6039c01798c1588a..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Math/mat2.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*******************************************************************************
- * mat2.h
- *
- * (C) 2006 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Author: Frank Neuhaus
- *******************************************************************************/
-
-#ifndef MAT2_H
-#define MAT2_H
-
-#include <math.h>
-#include <homer_nav_libs/Math/Point2D.h>
-#include <homer_nav_libs/Math/vec2.h>
-
-class CMat2
-{
- public:
- CMat2();
- CMat2(float rot);
- ~CMat2();
-
- CMat2 operator *(const CMat2 &mat) const;
-
- CVec2 operator *(const CVec2& v) const;
-
- Point2D operator *(const Point2D& p) const;
-
- float& operator [] (unsigned int position) {
- return fMatrix[position];
- }
-
- CMat2 operator +(const CMat2 rhs) const {
- CMat2 newMatrix;
- for (unsigned int i = 0; i < 4; i++) {
- newMatrix[i] = valueAt(i) + rhs.valueAt(i);
- }
- return newMatrix;
- }
-
- CMat2 operator -(const CMat2 rhs) const {
- CMat2 newMatrix;
- for (unsigned int i = 0; i < 4; i++) {
- newMatrix[i] = valueAt(i) - rhs.valueAt(i);
- }
- return newMatrix;
- }
-
- float valueAt(unsigned int position) const{
- return fMatrix[position];
- }
-
- union
- {
- float fMatrix[4];
- float m[2][2];
- struct
- {
- float xx, xy;
- float yx, yy;
- };
- };
-
- void transpose();
- void loadIdentity();
-
- void makeRotation(float fA);
-
- bool invert();
-};
-
-
-
-#include "mat2_inl.h"
-
-#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/mat2_inl.h b/homer_nav_libs/include/homer_nav_libs/Math/mat2_inl.h
deleted file mode 100644
index e3f77e161151e98c7df7f06b43db1d40fd9c4753..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Math/mat2_inl.h
+++ /dev/null
@@ -1,86 +0,0 @@
-/*******************************************************************************
- * mat2_inl.h
- *
- * (C) 2006 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Author: Frank Neuhaus
- *******************************************************************************/
-
-#include <assert.h>
-
-inline CMat2::CMat2(){
- for (unsigned int i = 0; i < 4; i++) {
- fMatrix[i] = 0;
- }
-}
-
-inline CMat2::~CMat2()
-{}
-
-inline CMat2::CMat2(float rot)
-{
- makeRotation(rot);
-}
-
-inline CMat2 CMat2::operator * ( const CMat2 & mat ) const
-{
- CMat2 retValue;
- for (unsigned int line = 0; line < 2; line++) {
- for (unsigned int column = 0; column < 2; column++) {
- retValue[line*2 + column] = valueAt(line*2 + column) + mat.valueAt(column*2 + line);
- }
- }
- return retValue;
-}
-
-inline CVec2 CMat2::operator * ( const CVec2& v ) const
-{
- return CVec2(xx*v[0] + xy*v[1],yx*v[0] + yy*v[1]);
-}
-
-inline Point2D CMat2::operator * ( const Point2D& p ) const
-{
- return Point2D(xx*p.x() + xy*p.y(), yx*p.x() + yy*p.y());
-}
-
-inline void CMat2::transpose()
-{
- float t=xy;
- xy=yx;
- yx=t;
-}
-
-inline void CMat2::loadIdentity()
-{
- xx=1.0f; xy=0.0f;
- yx=0.0f; yy=1.0f;
-}
-
-inline void CMat2::makeRotation ( float fA )
-{
- xx=yy=cosf(fA);
- yx=sinf(fA);
- xy=-yx;
-}
-
-inline bool CMat2::invert()
-{
- CMat2 tmp;
- float det = fMatrix[0]*fMatrix[3] - fMatrix[1]*fMatrix[2];
-
- if(fabs(det) < 0.001f) return false;
-
- det = 1.0 / det;
- tmp.fMatrix[0] = fMatrix[3]*det;
- tmp.fMatrix[1] = -fMatrix[1]*det;
- tmp.fMatrix[2] = -fMatrix[2]*det;
- tmp.fMatrix[3] = fMatrix[0]*det;
-
- (*this)=tmp;
- return true;
-}
-
-
-
-
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/vec2.h b/homer_nav_libs/include/homer_nav_libs/Math/vec2.h
deleted file mode 100644
index 39a72c8eb60c6b6bf836dc7d7be932c376e25fd0..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/Math/vec2.h
+++ /dev/null
@@ -1,189 +0,0 @@
-/*******************************************************************************
- * vec2.h
- *
- * (C) 2006 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Author: Frank Neuhaus, Susanne Maur
- *******************************************************************************/
-
-#include <iostream>
-#include <sstream>
-
-#ifndef VEC2_H
-#define VEC2_H
-
-#include <math.h>
-
-class CVec2
-{
- public:
-
- inline CVec2()
- {
- m_X = 0; m_Y = 0;
- }
-
- inline CVec2 ( double x, double y )
- {
- m_X=x; m_Y=y;
- }
-
- inline CVec2 ( const CVec2& vec )
- {
- m_X=vec.x(); m_Y=vec.y();
- }
-
- inline CVec2 operator+ ( const CVec2& vVector ) const
- {
- return CVec2 ( vVector[0] + m_X, vVector[1] + m_Y );
- };
-
- inline CVec2 operator- ( const CVec2& vVector ) const
- {
- return CVec2 ( m_X - vVector[0], m_Y - vVector[1] );
- };
-
- inline CVec2 operator- ( ) const
- {
- return CVec2 ( - m_X, - m_Y );
- };
-
- inline CVec2 operator* ( double num ) const
- {
- return CVec2 ( m_X * num, m_Y * num );
- };
-
- inline double operator* ( const CVec2& vVector ) const
- {
- return m_X*vVector[0]+m_Y*vVector[1];
- }
-
- inline CVec2 operator/ ( double num ) const
- {
- return CVec2 ( m_X / num, m_Y / num );
- }
-
- inline void set ( double fx, double fy )
- {
- m_X=fx; m_Y=fy;
- }
-
- inline double x() const
- {
- return m_X;
- }
-
- inline double y() const
- {
- return m_Y;
- }
-
- inline double magnitude() const
- {
- double sumOfSquares = m_X*m_X + m_Y*m_Y;
- return sqrt ( sumOfSquares );
- }
-
- inline double operator [] ( unsigned int i ) const
- {
- return ( ( double* ) this ) [i];
- }
-
- inline double& operator [] ( unsigned int i )
- {
- return ( ( double* ) this ) [i];
- }
-
- inline CVec2& operator/= ( double num )
- {
- double inv=1.0f/num;
- m_X*=inv;
- m_Y*=inv;
- return ( *this );
- }
-
- inline CVec2& operator*= ( double num )
- {
- m_X*=num;
- m_Y*=num;
- return ( *this );
- }
-
- inline CVec2& normalize()
- {
- return ( *this/=magnitude() );
- }
-
- inline CVec2& makePerp()
- {
- double xn=m_X;
- m_X=-m_Y;
- m_Y=xn;
- return *this;
- }
-
- inline CVec2 getNormal() const
- {
- return CVec2 ( m_Y, -m_X ); //?
- }
-
- inline CVec2 getNormalized() const
- {
- return ( *this ) /magnitude();
- }
-
- inline double sqr() const
- {
- return ( *this ) * ( *this );
- }
-
- inline double dot ( const CVec2& vec ) const
- {
- return ( m_X*vec[0] ) + ( m_Y*vec[1] );
- }
-
- inline double getAngle ( const CVec2& vec ) const
- {
- return acos ( dot ( vec ) / ( magnitude() *vec.magnitude() ) );
- }
-
- /// @param angle Rotation angle in radiants
- inline CVec2 rotate ( float angle ) const
- {
- double xRot = m_X*cos ( angle ) - m_Y*sin ( angle );
- double yRot = m_X*sin ( angle ) + m_Y*cos ( angle );
- return CVec2 ( xRot, yRot );
- }
-
- inline bool equal ( CVec2 vec ) const
- {
- return ( m_X==vec.x() && m_Y==vec.y() );
- }
-
- /**
- * Returns the string representation of the vector.
- * @return string representation of the point.
- */
- inline std::string toString() const
- {
- std::ostringstream str;
- str << m_X << " " << m_Y;
- return str.str();
- }
-
- protected:
- double m_X, m_Y;
-};
-
-inline CVec2 operator* ( double f, const CVec2& v )
-{
- return v*f;
-}
-
-inline CVec2 normalize ( const CVec2& v )
-{
- return v/v.magnitude();
-}
-
-#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/tools.h b/homer_nav_libs/include/homer_nav_libs/tools.h
deleted file mode 100644
index 112c22f3fed568bc5b919311d777798ea2d36dbf..0000000000000000000000000000000000000000
--- a/homer_nav_libs/include/homer_nav_libs/tools.h
+++ /dev/null
@@ -1,396 +0,0 @@
-#ifndef TOOLS_H
-#define TOOLS_H
-
-#include <Eigen/Geometry>
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/Pose.h>
-#include <sensor_msgs/LaserScan.h>
-#include <tf/transform_listener.h>
-#include <ros/ros.h>
-#include <vector>
-
-/**
- * @author Malte Knauf (2014)
- * Convenience functions that are often used in the mapping and navigation process
- */
-namespace map_tools
-{
-
- /**
- * @brief Converts a point p in world frame /map to the respective cell position in the map
- * @param p Point in world frame
- * @param origin Origin of the map
- * @param resolution Resolution of the map
- * @return Cell position of the point
- */
- Eigen::Vector2i toMapCoords(geometry_msgs::Point p, geometry_msgs::Pose origin, float resolution)
- {
- int x_idx = (p.x - origin.position.x)/resolution + 0.51;
- int y_idx = (p.y - origin.position.y)/resolution + 0.51;
- Eigen::Vector2i ret(x_idx, y_idx);
- return ret;
- }
-
- /**
- * @brief Converts the cell position of a point to its respective position in the world frame
- * @param idx Cell position of the point
- * @param origin Origin of the map
- * @param resolution Resolution of the map
- * @return Point in world frame
- */
- geometry_msgs::Point fromMapCoords(Eigen::Vector2i idx, geometry_msgs::Pose origin, float resolution)
- {
- geometry_msgs::Point ret;
- ret.x = origin.position.x + (idx.x() - 0.5) * resolution;
- ret.y = origin.position.y + (idx.y() - 0.5) * resolution;
- return ret;
- }
-
- /**
- * @brief Converts the QT pixel position of a point to its respective position in the world frame
- * @param idx Cell position of the point
- * @param origin Origin of the map
- * @param resolution Resolution of the map
- * @return Point in world frame
- */
- geometry_msgs::Point qtFromMapCoords(Eigen::Vector2i idx, geometry_msgs::Pose origin, float resolution)
- {
- geometry_msgs::Point ret;
- ret.x = -(origin.position.x + idx.y()) * resolution;
- ret.y = -(origin.position.y + idx.x()) * resolution;
- return ret;
- }
-
- /**
- * @brief map_index returns for a given point in the map real-world frame the respective index in the map
- * @param p Point in the real-world frame (usually the frame /map or /world)
- * @param origin Pose of the point (0,0) of the map in the real-world frame
- * @param width Width of the map
- * @param resolution Resolution in meters/cell of the map
- * @return index of point in the map
- */
- int map_index(geometry_msgs::Point p, geometry_msgs::Pose origin, float width, float resolution)
- {
- return (int)(width *
- ((p.y - origin.position.y)/resolution + 0.51) +
- ((p.x - origin.position.x)/resolution + 0.51));
- }
-
- /**
- * @brief point_in_map returns true if given point is in the map. False otherwise
- * @param p Point in the real-world frame (usually the frame /map or /world)
- * @param origin Pose of the point (0,0) of the map in the real-world frame
- * @param width Width of the map
- * @param resolution Resolution in meters/cell of the map
- * @return true or false
- */
- bool point_in_map(geometry_msgs::Point p, geometry_msgs::Pose origin, float width, float resolution)
- {
- int x_idx = (p.x - origin.position.x)/resolution + 0.51;
- int y_idx = (p.y - origin.position.y)/resolution + 0.51;
- if(x_idx < 0 || y_idx < 0 || x_idx >= width || y_idx >= width) return false;
- return true;
- }
-
- /**
- * @brief transformPoint wrapper to transform points between coordinate frames
- * @param point input point in from_frame
- * @param listener transform listener
- * @param from_frame input frame
- * @param to_frame output frame
- * @return transformed point in to_frame
- */
- geometry_msgs::Point transformPoint(geometry_msgs::Point point, tf::TransformListener &listener,
- std::string from_frame, std::string to_frame, ros::Time stamp = ros::Time(0))
- {
- geometry_msgs::PointStamped pin;
- geometry_msgs::PointStamped pout;
- pin.header.frame_id = from_frame;
- pin.point = point;
- try
- {
- listener.transformPoint(to_frame, stamp, pin, "/map",pout);
- return pout.point;
- }
- catch (tf::TransformException ex){
- ROS_ERROR("%s",ex.what());
- }
- }
-
- geometry_msgs::Point transformPoint(const geometry_msgs::Point point, tf::StampedTransform transform)
- {
- geometry_msgs::Point point_out;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX(point.x);
- pin.setY(point.y);
- pin.setZ(point.z);
-
- pout = transform* pin;
-
- point_out.x = pout.x();
- point_out.y = pout.y();
-
- return point_out;
- }
- geometry_msgs::Point transformPoint(const geometry_msgs::Point point, tf::Transform transform)
- {
- geometry_msgs::Point point_out;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX(point.x);
- pin.setY(point.y);
- pin.setZ(point.z);
-
- pout = transform* pin;
-
- point_out.x = pout.x();
- point_out.y = pout.y();
- point_out.z = pout.z();
-
- return point_out;
- }
-
- /**
- * @brief transformPoint wrapper to transform points between coordinate frames at specific time
- * @param point input point in from_frame
- * @param listener transform listener
- * @param Time to look for transform
- * @param from_frame input frame
- * @param to_frame output frame
- * @return transformed point in to_frame
- */
- geometry_msgs::Point transformPoint(geometry_msgs::Point point, tf::TransformListener &listener, const ros::Time & time ,
- std::string from_frame, std::string to_frame)
- {
- geometry_msgs::PointStamped pin;
- geometry_msgs::PointStamped pout;
- pin.header.frame_id = from_frame;
- pin.point = point;
- try
- {
- listener.transformPoint(to_frame, time, pin, "/map" ,pout);
- return pout.point;
- }
- catch (tf::TransformException ex){
- ROS_ERROR("%s",ex.what());
- }
- }
-
- /**
- * @brief laser_range_to_point converts a single given laser scan range in polar coordinates
- * to the respective point in euclidean coordinates in the target frame
- * @param laser_range range of the laser point to convert
- * @param index
- * @param start_angle
- * @param angle_step
- * @param listener
- * @param from_frame
- * @param to_frame
- * @return
- */
- geometry_msgs::Point laser_range_to_point(float laser_range, int index, float start_angle,
- float angle_step, tf::TransformListener &listener,
- std::string from_frame, std::string to_frame, ros::Time stamp = ros::Time(0), float time_inc = 0)
- {
- float alpha = start_angle + index * angle_step;
- geometry_msgs::PointStamped pin;
- geometry_msgs::PointStamped pout;
- pin.header.frame_id = from_frame;
- pin.point.x = cos(alpha) * laser_range;
- pin.point.y = sin(alpha) * laser_range;
-
- try
- {
- listener.transformPoint(to_frame, stamp, pin, "/map", pout);
- return pout.point;
- }
- catch (tf::TransformException ex){
- //ROS_ERROR("%s",ex.what());
- }
- }
-
- /**
- * @brief laser_ranges_to_points converts a given laser scan in polar coordinates
- * to the respective points in euclidean coordinates in the target frame
- * @param laser_data laser data ranges
- * @param start_angle angle of the first measurement
- * @param angle_step angle increment between two consecutive laser measurements
- * @param range_min minimum valid range
- * @param range_max maximum valid range
- * @return vector containing the laser measurements in euclidean points
- */
- std::vector<geometry_msgs::Point> laser_ranges_to_points(const std::vector<float>& laser_data, float start_angle,
- float angle_step, float range_min, float range_max,
- tf::TransformListener &listener,
- std::string from_frame, std::string to_frame, ros::Time stamp = ros::Time(0), float time_inc = 0)
- {
- std::vector<geometry_msgs::Point> ret;
- float alpha = start_angle;
- for (int i = 0; i < laser_data.size(); i++) {
- if(laser_data[i] < range_min || laser_data[i] > range_max)
- {
- alpha += angle_step;
- continue;
- }
- geometry_msgs::Point point;
- point.x = cos(alpha) * laser_data.at(i);
- point.y = sin(alpha) * laser_data.at(i);
-
- geometry_msgs::PointStamped pin;
- pin.header.frame_id = from_frame;
- pin.point = point;
- geometry_msgs::PointStamped pout;
- try
- {
- listener.transformPoint(to_frame, stamp, pin, "/map", pout);
- ret.push_back(pout.point);
- }
- catch (tf::TransformException ex){
- //ROS_ERROR("%s",ex.what());
- }
-
- alpha += angle_step;
- }
- return ret;
- }
-
-
- /**
- * @brief laser_msg_to_points converts a given laser scan in polar coordinates
- * to the respective points in euclidean coordinates in the target frame
- * at a specific time
- * @param scan laser data msg
- * @param listener TransformListener
- * @param to_frame target frame
- * @return vector containing the laser measurements in euclidean points
- */
- std::vector<geometry_msgs::Point> laser_msg_to_points(const sensor_msgs::LaserScan::ConstPtr& scan,
- tf::TransformListener &listener, std::string to_frame, ros::Time stamp = ros::Time(0))
- {
- std::vector<geometry_msgs::Point> ret;
- float alpha = scan->angle_min;
- if(!listener.waitForTransform(scan->header.frame_id, to_frame, stamp, ros::Duration(0.3)))
- {
- return ret;
- }
- for (int i = 0; i < scan->ranges.size(); i++) {
- if(scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max)
- {
- alpha += scan->angle_increment;
- continue;
- }
- geometry_msgs::Point point;
- point.x = cos(alpha) * scan->ranges.at(i);
- point.y = sin(alpha) * scan->ranges.at(i);
-
- geometry_msgs::PointStamped pin;
- pin.header.frame_id = scan->header.frame_id;
- pin.point = point;
- geometry_msgs::PointStamped pout;
- try
- {
- //listener.transformPoint(to_frame, (stamp + ros::Duration( i * scan->time_increment)), pin, "/map" ,pout);
- listener.transformPoint(to_frame, stamp, pin, "/map" ,pout);
- ret.push_back(pout.point);
- }
- catch (tf::TransformException ex){
- ROS_ERROR("%s",ex.what());
- }
-
- alpha += scan->angle_increment;
- }
- return ret;
- }
-
- /**
- * @brief get_max_move_distance searches for the nearest values in an defined area
- * @param vector points: laserPoints in base_link
- * @return float distance to nearest Point
- */
-float get_max_move_distance(std::vector<geometry_msgs::Point> points, float min_x, float min_y)
-{
- float minDistance = 30;
- for (unsigned int i = 0; i < points.size(); i++)
- {
- if(std::fabs(points[i].y) < min_y && points[i].x > min_x)
- {
- float distance = sqrt((points[i].x * points[i].x) + (points[i].y * points[i].y));
- if (distance < minDistance)
- {
- minDistance = distance;
- }
- }
- }
- float maxMoveDist = minDistance - min_x;
- if (maxMoveDist < 0)
- {
- maxMoveDist = 0.0;
- }
- return maxMoveDist;
-}
-
-
- /**
- * @brief Calculates the euclidean distance (in cells) between to points in the map
- * @param a Point a
- * @param b point b
- * @return euclidean distance in cells
- */
- double distance(const Eigen::Vector2i& a, const Eigen::Vector2i& b)
- {
- return sqrt((a.x() - b.x()) * (a.x() - b.x()) + (a.y() - b.y()) * (a.y() - b.y()));
- }
-
- /**
- * @brief Calculates the euclidean distance (in m) between to points in the world
- * @param a Point a
- * @param b point b
- * @return euclidean distance in m
- */
- double distance(const geometry_msgs::Point& a, const geometry_msgs::Point& b)
- {
- return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
- }
-
-/**
- * @brief findValue
- * @param map Pointer to the map to search
- * @param width Width of the map
- * @param height Height of the map
- * @param center_x \__ Center point of circle to search within
- * @param center_y /
- * @param value Value to search for in given map
- * @param radius Radius of the circle
- * @return true if the given value could be found within the given radius around (x,y)
- */
- bool findValue(const std::vector<int8_t>* map, int width, int height, int center_x, int center_y, unsigned char value, float radius)
- {
-
- int start_x = int ( center_x - radius );
- int start_y = int ( center_y - radius );
- int end_x = int ( center_x + radius );
- int end_y = int ( center_y + radius );
-
- if ( start_x < 0 ) { start_x = 0; }
- if ( start_y < 0 ) { start_y = 0; }
- if ( end_x >= int ( width ) ) { end_x = width - 1; }
- if ( end_y >= int ( height ) ) { end_y = height - 1; }
-
- float sqr_radius = radius*radius;
-
- for ( int y = start_y; y <= end_y; y++ )
- for ( int x = start_x; x <= end_x; x++ )
- {
- if ( map->at(x+width*y) > value )
- {
- float sqr_dist = float ( x - center_x ) * float ( x - center_x ) + float ( y - center_y ) * float ( y - center_y );
- if ( sqr_dist <= sqr_radius ) { return true; }
- }
- }
-
- return false;
- }
-}
-
-#endif // TOOLS_H
diff --git a/homer_nav_libs/package.xml b/homer_nav_libs/package.xml
deleted file mode 100644
index 7f2a2eaa846304d2a4c60c57c0e54897a8fee63f..0000000000000000000000000000000000000000
--- a/homer_nav_libs/package.xml
+++ /dev/null
@@ -1,23 +0,0 @@
-<?xml version="1.0"?>
-<package>
- <name>homer_nav_libs</name>
- <version>0.1.1</version>
- <description>The nav_libs package</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>geometry_msgs</build_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>tf</build_depend>
- <build_depend>cmake_modules</build_depend>
- <build_depend>eigen</build_depend>
-
- <run_depend>roscpp</run_depend>
- <run_depend>geometry_msgs</run_depend>
- <run_depend>tf</run_depend>
- <run_depend>eigen</run_depend>
-
-</package>
diff --git a/homer_nav_libs/readme.pdf b/homer_nav_libs/readme.pdf
deleted file mode 100644
index 98fc92ccae14a8a818948ce4c73a8926f6ad2028..0000000000000000000000000000000000000000
Binary files a/homer_nav_libs/readme.pdf and /dev/null differ
diff --git a/homer_nav_libs/src/Explorer/Explorer.cpp b/homer_nav_libs/src/Explorer/Explorer.cpp
deleted file mode 100644
index 509964e423c1d73b88f62504bddfa3c7f200aa52..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Explorer/Explorer.cpp
+++ /dev/null
@@ -1,1408 +0,0 @@
-#include <homer_nav_libs/Explorer/Explorer.h>
-
-using namespace std;
-using namespace ExplorerConstants;
-
-Explorer::Explorer ( double minAllowedObstacleDistance, double maxAllowedObstacleDistance,
- double minSafeObstacleDistance, double maxSafeObstacleDistance,
- double safePathWeight, double frontierSafenessFactor, int unknownThreshold )
-{
- ExplorerConstants::UNKNOWN = unknownThreshold;
-
- m_MinAllowedObstacleDistance = minAllowedObstacleDistance;
- m_MaxAllowedObstacleDistance = maxAllowedObstacleDistance;
-
- m_MinSafeObstacleDistance = minSafeObstacleDistance;
- m_MaxSafeObstacleDistance = maxSafeObstacleDistance;
-
- m_SafePathWeight = safePathWeight;
- m_FrontierSafenessFactor = frontierSafenessFactor;
-
- m_OccupancyMap = 0;
- m_ObstacleTransform = 0;
- m_CostTransform = 0;
- m_TargetMap = 0;
- m_DrivingDistanceTransform = 0;
- m_TargetDistanceTransform = 0;
- m_PathTransform = 0;
- m_ExplorationTransform = 0;
- m_DesiredDistance = 0;
-}
-
-Explorer::~Explorer()
-{
- releaseMaps();
- releaseMap( m_OccupancyMap );
-}
-
-void Explorer::releaseMaps()
-{
- releaseMap( m_TargetMap );
- releaseMap( m_ObstacleTransform );
- releaseMap( m_CostTransform );
- releaseMap( m_DrivingDistanceTransform );
- releaseMap( m_TargetDistanceTransform );
- releaseMap( m_PathTransform );
- releaseMap( m_ExplorationTransform );
-}
-
-// SETTERS ////////////////////////////////////////////////////////////////////////////////////////////////
-
-void Explorer::setUnknownThreshold(int unknownTresh)
-{
- ExplorerConstants::UNKNOWN = unknownTresh;
-}
-
-void Explorer::setAllowedObstacleDistance ( double min, double max )
-{
- m_MinAllowedObstacleDistance = min;
- m_MaxAllowedObstacleDistance = max;
- releaseMaps();
-}
-
-void Explorer::setSafeObstacleDistance ( double min, double max )
-{
- m_MinSafeObstacleDistance = min;
- m_MaxSafeObstacleDistance = max;
- releaseMaps();
-}
-
-void Explorer::setSafePathWeight ( double weight )
-{
- m_SafePathWeight = weight;
- releaseMaps();
-}
-
-void Explorer::setFrontierSafenessFactor( double frontierSafenessFactor )
-{
- m_FrontierSafenessFactor = frontierSafenessFactor;
- releaseMaps();
-}
-
-void Explorer::setOccupancyMap ( int width, int height, geometry_msgs::Pose origin, int8_t* data )
-{
- if ( !data ) {
- ROS_ERROR( "Received 0-pointer." );
- return;
- }
- releaseMaps();
- releaseMap( m_OccupancyMap );
- //m_OccupancyMap = new GridMap<unsigned char> ( width, height, data, exploredRegion );
- m_OccupancyMap = new GridMap<int8_t> ( width, height, data );
- m_Origin = origin;
-}
-
-
-void Explorer::updateObstacles(int width, int height, geometry_msgs::Pose origin, int8_t* mapData)
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
- if ( (width != m_OccupancyMap->width()) || (height != m_OccupancyMap->height()) ) {
- ROS_ERROR_STREAM( "Wrong map size!" );
- return;
- }
- for ( unsigned i=0; i<m_OccupancyMap->width()*m_OccupancyMap->height(); i++ )
- {
- int8_t* myMapData=m_OccupancyMap->getDirectAccess(0,0);
- if ( myMapData[i] != UNKNOWN )
- {
- myMapData[i]=mapData[i];
- }
- }
- releaseMaps();
-}
-
-void Explorer::resetExploration()
-{
- m_DesiredDistance = 0;
-}
-
-void Explorer::setStart ( Eigen::Vector2i start )
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR_STREAM( "Occupancy map is missing." );
- return;
- }
- if (( start.x() <= 1 ) || ( start.y() <= 1 ) || ( start.x() >= m_OccupancyMap->width()-1 ) || ( start.y() >= m_OccupancyMap->height()-1 ) )
- {
- ROS_ERROR_STREAM( "Invalid position!" );
- return;
- }
- computeWalkableMaps();
-
- if ( !isWalkable( start.x(), start.y() ) )
- {
- Eigen::Vector2i correctedStart=getNearestWalkablePoint( start );
- if ( !isWalkable( correctedStart.x(), correctedStart.y() ) )
- {
- ROS_ERROR_STREAM( "No walkable position was found on the map!" );
- }
- else
- {
- ROS_INFO_STREAM("Start position " << start.x() << "," << start.y() << " was corrected to " << correctedStart.x() << "," << correctedStart.y());
- }
- m_Start = correctedStart;
- return;
- }
- m_Start = start;
-}
-
-
-Eigen::Vector2i Explorer::getNearestAccessibleTarget(Eigen::Vector2i target )
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return target;
- }
- if ( ( target.x() <= 1 ) || ( target.y() <= 1 ) || ( target.x() >= m_OccupancyMap->width()-1 ) || ( target.y() >= m_OccupancyMap->height()-1 ) )
- {
- ROS_ERROR( "Invalid position!" );
- return target;
- }
- computeApproachableMaps();
- computeWalkableMaps();
- Eigen::Vector2i correctTarget=target;
-
- if ( !isApproachable( target.x(), target.y() ) )
- {
- ROS_INFO_STREAM("target cell in drivingdistancetransform: " << m_DrivingDistanceTransform->getValue ( target.x(), target.y() ));
- ROS_INFO_STREAM("target " << target << " is not approachable. Correcting target...");
- int minSqrDist=INT_MAX;
- for ( int x = 0; x < m_ObstacleTransform->height(); x++ )
- {
- for ( int y = 0; y < m_ObstacleTransform->width(); y++ )
- {
- bool isSafe = m_ObstacleTransform->getValue ( x, y ) > m_FrontierSafenessFactor * m_MinAllowedObstacleDistance;
- if ( isApproachable ( x,y ) && isWalkable( x , y) && isSafe )
- {
- int xDiff = target.x() - x;
- int yDiff = target.y() - y;
- int sqrDist = xDiff*xDiff + yDiff*yDiff;
- if ( sqrDist < minSqrDist )
- {
- correctTarget.x() = x;
- correctTarget.y() = y;
- minSqrDist = sqrDist;
- }
- }
- }
- }
- }
- ROS_DEBUG_STREAM("Target position " << target.x() << "," << target.y() << " was corrected to " << correctTarget.x() << "," << correctTarget.y());
-
- return correctTarget;
-}
-
-
-Eigen::Vector2i Explorer::getNearestWalkablePoint( Eigen::Vector2i target )
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return target;
- }
- if (( target.x() <= 1 ) || ( target.y() <= 1 ) || ( target.x() >= m_OccupancyMap->width()-1 ) || ( target.y() >= m_OccupancyMap->height()-1 ) )
- {
- ROS_ERROR( "Invalid position!" );
- return target;
- }
-
- computeWalkableMaps();
- Eigen::Vector2i correctTarget=target;
-
-
- if ( !isWalkable( target.x(), target.y() ) )
- {
- int minSqrDist=INT_MAX;
- for ( int x = 0; x < m_ObstacleTransform->height(); x++ )
- {
- for ( int y = 0; y < m_ObstacleTransform->width(); y++ )
- {
- if ( isWalkable ( x,y ) )
- {
- int xDiff = target.x() - x;
- int yDiff = target.y() - y;
- int sqrDist = xDiff*xDiff + yDiff*yDiff;
- if ( sqrDist < minSqrDist )
- {
- correctTarget.x() = x;
- correctTarget.y() = y;
- minSqrDist = sqrDist;
- }
- }
- }
- }
- }
- ROS_DEBUG_STREAM("Position " << target.x() << "," << target.y() << " was corrected to " << correctTarget.x() << "," << correctTarget.y());
-
- return correctTarget;
-}
-
-
-void Explorer::setTarget (Eigen::Vector2i target )
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
- if ( ( target.x() <= 1 ) || ( target.y() <= 1 ) || ( target.x() >= m_OccupancyMap->width()-1 ) || ( target.y() >= m_OccupancyMap->height()-1 ) )
- {
- ROS_ERROR( "Invalid position!" );
- return;
- }
- computeApproachableMaps();
- if ( !isApproachable ( target.x(), target.y() ) )
- {
- ROS_WARN( "Target position is not approachable. Path computation will possibly fail." );
- }
- m_Target = target;
- m_DesiredDistance = 0;
-}
-
-
-void Explorer::setTarget (Eigen::Vector2i target, int desiredDistance )
-{
- ROS_ERROR_STREAM("setTarget still in use!!");
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
-
- if ( desiredDistance < 1 )
- {
- setTarget( target );
- return;
- }
-
- if ( target.x() + desiredDistance <= 1 || target.x() - desiredDistance >= m_OccupancyMap->width()-1 ||
- target.y() + desiredDistance <= 1 || target.y() - desiredDistance >= m_OccupancyMap->height()-1 )
- {
- ROS_ERROR( "Invalid position" );
- return;
- }
- computeApproachableMaps();
- // TODO: check if region is approachable
- m_Target = target;
- m_DesiredDistance = desiredDistance;
-}
-
-
-// GETTERS ////////////////////////////////////////////////////////////////////////////////////////////////
-
-Eigen::Vector2i Explorer::getStart() const
-{
- return m_Start;
-}
-
-Eigen::Vector2i Explorer::getTarget() const
-{
- return m_Target;
-}
-
-GridMap<int8_t>* Explorer::getOccupancyMap()
-{
- return m_OccupancyMap;
-}
-
-GridMap<double>* Explorer::getObstacleTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computeObstacleTransform();
- return m_ObstacleTransform;
-}
-
-GridMap<double>* Explorer::getCostTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computeCostTransform();
- return m_CostTransform;
-}
-
-GridMap<bool>* Explorer::getTargetMap()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
-
- computeTargetMap();
- return m_TargetMap;
-}
-
-GridMap<double>* Explorer::getDrivingDistanceTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computeDrivingDistanceTransform();
- return m_DrivingDistanceTransform;
-}
-
-GridMap<double>* Explorer::getTargetDistanceTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computeTargetDistanceTransform();
- return m_TargetDistanceTransform;
-}
-
-GridMap<double>* Explorer::getPathTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computePathTransform();
- return m_PathTransform;
-}
-
-GridMap<double>* Explorer::getExplorationTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computeExplorationTransform();
- return m_ExplorationTransform;
-}
-
-
-
-// MAP GENERATION ////////////////////////////////////////////////////////////////////////////////////////////////7
-
-
-void Explorer::computeApproachableMaps()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
- computeDrivingDistanceTransform();
-}
-
-
-void Explorer::computeWalkableMaps()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
- computeObstacleTransform();
-}
-
-
-void Explorer::computeDrivingDistanceTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
-
- if ( m_DrivingDistanceTransform ) { return; }
-
- ROS_DEBUG( "Computing drivingDistanceTransform..." );
- resetMap( m_DrivingDistanceTransform );
- distanceFloodFill ( m_DrivingDistanceTransform, m_Start );
-}
-
-
-void Explorer::computeTargetDistanceTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
-
- if ( m_TargetDistanceTransform ) { return; }
-
- ROS_DEBUG( "Computing targetDistanceTransform..." );
- resetMap( m_TargetDistanceTransform );
- distanceFloodFill ( m_TargetDistanceTransform, m_Target );
-}
-
-
-void Explorer::computeRegionMap()
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
-
- resetMap( m_TargetMap );
- ROS_DEBUG( "Computing target region map..." );
-
- m_TargetMap->fill( false );
- const int desiredDistanceSquared = m_DesiredDistance * m_DesiredDistance;
- int height = m_OccupancyMap->height();
- int width = m_OccupancyMap->width();
-
- // draw a circle onto the ExplorationMap
- const int firstX = m_Target.x() - m_DesiredDistance <= 1 ? 2 : m_Target.x() - m_DesiredDistance;
- const int firstY = m_Target.y() - m_DesiredDistance <= 1 ? 2 : m_Target.y() - m_DesiredDistance;
- const int lastX = m_Target.x() + m_DesiredDistance >= width-1 ? width-2 : m_Target.x() + m_DesiredDistance;
- const int lastY = m_Target.y() + m_DesiredDistance >= height-1 ? height-2 : m_Target.y() + m_DesiredDistance;
-
- for ( int y = firstY; y <= lastY; ++y )
- {
- for ( int x = firstX; x <= lastX; ++x )
- {
- const int dx = x - m_Target.x();
- const int dy = y - m_Target.y();
-
- if ( dx*dx + dy*dy <= desiredDistanceSquared )
- {
- m_TargetMap->setValue( x, y, true );
- }
- }
- }
-}
-
-void Explorer::computeFrontierMap()
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
-
- // if ( m_FrontierMap ) { return; }
-
- resetMap( m_TargetMap );
-
- ROS_DEBUG( "Computing frontier map..." );
- m_TargetMap->fill ( 0 );
- // extract borders
- for ( int y = 1; y < m_OccupancyMap->height() - 1; y++ )
- {
- for ( int x = 1; x < m_OccupancyMap->width() - 1; x++ )
- {
- int value = m_OccupancyMap->getValue ( x, y );
- int value_u = m_OccupancyMap->getValue ( x, y - 1 );
- int value_d = m_OccupancyMap->getValue ( x, y + 1 );
- int value_l = m_OccupancyMap->getValue ( x - 1, y );
- int value_r = m_OccupancyMap->getValue ( x + 1, y );
- bool isFree = value < UNKNOWN && value != NOT_SEEN_YET;
- bool upUnknown = (value_u == UNKNOWN || value_u == NOT_SEEN_YET);
- bool downUnknown = (value_d == UNKNOWN || value_u == NOT_SEEN_YET);
- bool leftUnknown = (value_l == UNKNOWN || value_u == NOT_SEEN_YET);
- bool rightUnknown = (value_r == UNKNOWN || value_u == NOT_SEEN_YET);
- bool hasUnknownNeighbour = upUnknown || downUnknown || leftUnknown || rightUnknown;
- bool isSafe = m_ObstacleTransform->getValue ( x, y ) > m_FrontierSafenessFactor * m_MinAllowedObstacleDistance;
- if ( isFree && hasUnknownNeighbour && isSafe )
- {
- m_TargetMap->setValue ( x, y, 1 );
- }
- else
- {
- m_TargetMap->setValue ( x, y, 0 );
- }
- }
- }
-}
-
-void Explorer::computeTargetMap()
-{
- ROS_ERROR_STREAM("target Map shouldn't be used anymore!");
- if ( m_DesiredDistance < 1 )
- {
- computeFrontierMap();
- }
- else
- {
- computeRegionMap();
- }
-}
-
-void Explorer::computeObstacleTransform()
-{
- if ( !m_OccupancyMap)
- {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return;
- }
-
- if ( m_ObstacleTransform )
- {
- return;
- }
-
- resetMap( m_ObstacleTransform );
-
- ROS_DEBUG( "Computing obstacle transform..." );
- for ( int x = 0; x < m_ObstacleTransform->width(); x++ )
- {
- for ( int y = 0; y < m_ObstacleTransform->height(); y++ )
- {
- if ( m_OccupancyMap->getValue(x, y) > UNKNOWN ||
- m_OccupancyMap->getValue(x, y) == NOT_SEEN_YET)
- {
- m_ObstacleTransform->setValue ( x, y, 0 ); // Obstacle
- }
- else
- {
- m_ObstacleTransform->setValue ( x, y, INT_MAX ); // Free
- }
- }
- }
-
- int width = m_ObstacleTransform->width();
- int height = m_ObstacleTransform->height();
- double* f = new double[width > height ? width : height];
-
- // transform along columns
- for ( int x = 0; x < width; x++ )
- {
- for ( int y = 0; y < height; y++ )
- {
- // copy column
- f[y] = m_ObstacleTransform->getValue ( x, y );
- }
- // 1-D transform of column
- double* d = distanceTransform1D ( f, height );
- // copy transformed 1-D to output image
- for ( int y = 0; y < height; y++ )
- {
- m_ObstacleTransform->setValue ( x, y, d[y] );
- }
- delete [] d;
- }
-
- // transform along rows
- for ( int y = 0; y < height; y++ )
- {
- for ( int x = 0; x < width; x++ )
- {
- f[x] = m_ObstacleTransform->getValue ( x, y );
- }
- double* d = distanceTransform1D ( f, width );
- for ( int x = 0; x < width; x++ )
- {
- m_ObstacleTransform->setValue ( x, y, d[x] );
- }
- delete [] d;
- }
- delete f;
-
- // take square roots
- for ( int y = 0; y < m_ObstacleTransform->height(); y++ )
- {
- for ( int x = 0; x < m_ObstacleTransform->width(); x++ )
- {
- if ( isWalkable( x,y ) )
- {
- float value = sqrt ( m_ObstacleTransform->getValue ( x, y ) );
- m_ObstacleTransform->setValue ( x, y, value );
- }
- }
- }
-}
-
-
-void Explorer::computeCostTransform()
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return;
- }
-
- if ( m_CostTransform ) { return; }
-
- computeObstacleTransform();
- computeApproachableMaps();
-
- resetMap( m_CostTransform );
- m_CostTransform->fill( ExplorerConstants::MAX_COST );
-
- for ( unsigned y=0; y<m_CostTransform->height(); y++)
- {
- for ( unsigned x=0; x<m_CostTransform->width(); x++)
- {
- if ( !isApproachable( x, y ) ) {
- continue;
- }
- double dist = m_ObstacleTransform->getValue(x, y);
- double cost = 0;
- if ( dist < m_MinSafeObstacleDistance ) {
- cost = m_MinSafeObstacleDistance - dist;
- }
-// if ( dist > m_MaxSafeObstacleDistance ) {
-// cost = dist - m_MaxSafeObstacleDistance;
-// }
- m_CostTransform->setValue( x, y, cost * cost );
- }
- }
-}
-
-
-void Explorer::computePathTransform()
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return;
- }
-
- if ( m_PathTransform ) { return; }
-
- computeObstacleTransform();
- computeCostTransform();
-
- resetMap( m_PathTransform );
-
- ROS_DEBUG( "Computing path transform..." );
- GridMap<double>* map = m_PathTransform;
- int width = map->width();
- int height = map->height();
- double maxDistance = MAX_DISTANCE;
- map->fill ( maxDistance );
-
- int fromX = m_Target.x();
- int fromY = m_Target.y();
- map->setValue ( fromX, fromY, 0 );
-
- queue<int> xQueue;
- queue<int> yQueue;
- xQueue.push ( fromX + 1 );
- yQueue.push ( fromY );
- xQueue.push ( fromX - 1 );
- yQueue.push ( fromY );
- xQueue.push ( fromX );
- yQueue.push ( fromY - 1 );
- xQueue.push ( fromX );
- yQueue.push ( fromY + 1 );
- int xVal, yVal;
- while ( !xQueue.empty() )
- {
- xVal = xQueue.front();
- yVal = yQueue.front();
- xQueue.pop();
- yQueue.pop();
- if ( xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 && isWalkable( xVal, yVal ) )
- {
- float value = map->getValue ( xVal, yVal );
- float value_u = map->getValue ( xVal, yVal - 1 ) + 1;
- float value_d = map->getValue ( xVal, yVal + 1 ) + 1;
- float value_l = map->getValue ( xVal - 1, yVal ) + 1;
- float value_r = map->getValue ( xVal + 1, yVal ) + 1;
-
- float value_ur = map->getValue ( xVal + 1, yVal - 1 ) + 1.4142;
- float value_ul = map->getValue ( xVal - 1, yVal - 1 ) + 1.4142;
- float value_ll = map->getValue ( xVal - 1, yVal + 1 ) + 1.4142;
- float value_lr = map->getValue ( xVal + 1, yVal + 1 ) + 1.4142;
-
- float min1 = value_u < value_d ? value_u : value_d;
- float min2 = value_l < value_r ? value_l : value_r;
- float min3 = value_ur < value_ul ? value_ur : value_ul;
- float min4 = value_ll < value_lr ? value_ll : value_lr;
- float min12 = min1 < min2 ? min1 : min2;
- float min34 = min3 < min4 ? min3 : min4;
- float min = min12 < min34 ? min12 : min34;
- float newVal = min + m_SafePathWeight * m_CostTransform->getValue( xVal, yVal );
- if ( value > newVal )
- {
- map->setValue ( xVal, yVal, newVal );
- if ( map->getValue ( xVal, yVal + 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal + 1 );
- }
- if ( map->getValue ( xVal, yVal - 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal );
- }
- if ( map->getValue ( xVal - 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal );
- }
- if ( map->getValue ( xVal + 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal - 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal + 1 );
- }
- if ( map->getValue ( xVal - 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal + 1 );
- }
- }
- }
- }
-}
-
-
-void Explorer::computeExplorationTransform()
-{
- ROS_ERROR_STREAM("Exploration Transform shouldn't be used!");
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return;
- }
-
- if ( m_ExplorationTransform ) { return; }
-
- ROS_DEBUG_STREAM("computeExplorationTransform: before obstacle transform");
- computeObstacleTransform();
- ROS_DEBUG_STREAM("computeExplorationTransform: before cost transform");
- computeCostTransform();
- ROS_DEBUG_STREAM("computeExplorationTransform: before target map");
- computeTargetMap();
- ROS_DEBUG_STREAM("computeExplorationTransform: before walkable maps");
- computeWalkableMaps();
- ROS_DEBUG_STREAM("computeExplorationTransform: before exploration transform");
- resetMap( m_ExplorationTransform );
-
- ROS_DEBUG( "Computing exploration transform..." );
- GridMap<double>* map = m_ExplorationTransform;
- int width = map->width();
- int height = map->height();
- double maxDistance = MAX_DISTANCE;
- map->fill ( maxDistance );
- queue<int> xQueue;
- queue<int> yQueue;
- // fill seeds: Mark the frontiers as targets
- ROS_DEBUG_STREAM("computeExplorationTransform: before first loop");
- for ( int y = 0; y < m_TargetMap->height(); y++ )
- {
- for ( int x = 0; x < m_TargetMap->width(); x++ )
- {
- if ( m_TargetMap->getValue ( x, y ) == 1 )
- {
- map->setValue ( x, y, 0 );
- xQueue.push ( x + 1 );
- yQueue.push ( y );
- xQueue.push ( x - 1 );
- yQueue.push ( y );
- xQueue.push ( x );
- yQueue.push ( y - 1 );
- xQueue.push ( x );
- yQueue.push ( y + 1 );
- }
- }
- }
- ROS_DEBUG_STREAM("computeExplorationTransform: After first looop");
- // Now go through the coordinates in the queue
- int xVal, yVal;
- ROS_DEBUG_STREAM("computeExplorationTransform: before while loop");
- while ( !xQueue.empty() )
- {
- xVal = xQueue.front();
- yVal = yQueue.front();
- xQueue.pop();
- yQueue.pop();
- if ( xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 && isWalkable ( xVal, yVal ) )
- {
- // Get own cost and the cost of the 8 neighbor cells (neighbors plus the cost to go there)
- float value = map->getValue ( xVal, yVal );
- float value_u = map->getValue ( xVal, yVal - 1 ) + 1;
- float value_d = map->getValue ( xVal, yVal + 1 ) + 1;
- float value_l = map->getValue ( xVal - 1, yVal ) + 1;
- float value_r = map->getValue ( xVal + 1, yVal ) + 1;
- float value_ur = map->getValue ( xVal + 1, yVal - 1 ) + 1.4142;
- float value_ul = map->getValue ( xVal - 1, yVal - 1 ) + 1.4142;
- float value_ll = map->getValue ( xVal - 1, yVal + 1 ) + 1.4142;
- float value_lr = map->getValue ( xVal + 1, yVal + 1 ) + 1.4142;
- float min1 = value_u < value_d ? value_u : value_d;
- float min2 = value_l < value_r ? value_l : value_r;
- float min3 = value_ur < value_ul ? value_ur : value_ul;
- float min4 = value_ll < value_lr ? value_ll : value_lr;
- float min12 = min1 < min2 ? min1 : min2;
- float min34 = min3 < min4 ? min3 : min4;
- float min = min12 < min34 ? min12 : min34;
- float newVal = min + m_SafePathWeight * m_CostTransform->getValue( xVal, yVal );
- if ( value > newVal )
- {
- // Cost is lower then the currently known cost: Reduce cost here
- map->setValue ( xVal, yVal, newVal );
- // Add the neighbours that might profit in the queue
- if ( map->getValue ( xVal, yVal + 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal + 1 );
- }
- if ( map->getValue ( xVal, yVal - 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal );
- }
- if ( map->getValue ( xVal - 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal );
- }
- if ( map->getValue ( xVal + 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal - 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal + 1 );
- }
- if ( map->getValue ( xVal - 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal + 1 );
- }
- }
- }
- }
- ROS_DEBUG_STREAM("computeExplorationTransform: after exploration transform");
-}
-
-
-vector<Eigen::Vector2i> Explorer::sampleWaypointsFromPath ( std::vector<Eigen::Vector2i> pathPoints, float threshold )
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return pathPoints;
- }
- if ( pathPoints.size() < 3 ) {
- return pathPoints;
- }
-
- computeObstacleTransform();
-
- vector<Eigen::Vector2i> simplifiedPath;
- simplifiedPath.reserve( pathPoints.size() );
-
- Eigen::Vector2i lastAddedPoint = pathPoints[0];
- simplifiedPath.push_back ( lastAddedPoint );
-
- for ( unsigned int i = 1; i < pathPoints.size() - 1; i++ )
- {
- double distanceToNextPoint = map_tools::distance(lastAddedPoint, pathPoints.at(i));
- double obstacleDistanceLastAddedPoint = m_ObstacleTransform->getValue ( lastAddedPoint.x(), lastAddedPoint.y() );
- double obstacleDistancePossibleNextPoint = m_ObstacleTransform->getValue ( pathPoints[i].x(), pathPoints[i].y() );
- if (( distanceToNextPoint >= obstacleDistanceLastAddedPoint*threshold ) ||
- ( distanceToNextPoint >= obstacleDistancePossibleNextPoint*threshold ) )
- {
- simplifiedPath.push_back ( pathPoints[i] );
- lastAddedPoint = pathPoints[i];
- }
- }
- simplifiedPath.push_back ( pathPoints[pathPoints.size() - 1] );
- return simplifiedPath;
-}
-
-
-std::vector<Eigen::Vector2i> Explorer::getPath(bool& success)
-{
- success = false;
-
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return vector<Eigen::Vector2i>();
- }
-
- if ( m_DesiredDistance > 0 ) {
- // we are actually performing an exploration since the target
- // is a region.
- ROS_ERROR_STREAM("Desired Distance > 0: Executing getExplorationTransformPath");
- return getExplorationTransformPath( success );
- }
- ROS_DEBUG_STREAM("Computing Path Transform");
- computePathTransform();
- ROS_DEBUG_STREAM("Finished Path Transform");
-
- vector<Eigen::Vector2i> path;
-
- int x = m_Start.x();
- int y = m_Start.y();
-
- int width = m_OccupancyMap->width();
- int height = m_OccupancyMap->height();
-
- //special case: start and end point are equal, return single waypoint
- if ( map_tools::distance( m_Start, m_Target ) < 2.0 )
- {
- success = true;
- path.push_back ( Eigen::Vector2i( m_Start.x(), m_Start.y() ) );
- return path;
- }
-
- while ( x != m_Target.x() || y != m_Target.y() )
- {
- path.push_back ( Eigen::Vector2i( x, y ) );
- int minPosX = x;
- int minPosY = y;
- double min = m_PathTransform->getValue ( x, y );
-
- if ( ( x <= 1 ) || ( y <= 1 ) || ( x >= width-1 ) || ( y >= height-1 ) )
- {
- ROS_ERROR( "Out of map bounds" );
- return vector<Eigen::Vector2i>();
- }
-
- for ( int i = -1; i <= 1; i++ )
- {
- for ( int j = -1; j <= 1; j++ )
- {
- double pt = m_PathTransform->getValue ( x + i, y + j );
- if ( pt < min )
- {
- min = pt;
- minPosX = x + i;
- minPosY = y + j;
- }
- }
- }
- if ( minPosX == x && minPosY == y )
- {
- ROS_WARN( "Target is unreachable!" );
- return vector<Eigen::Vector2i>();
- }
- else
- {
- x = minPosX;
- y = minPosY;
- }
- }
- success = true;
-
- return path;
-}
-
-vector<Eigen::Vector2i> Explorer::getExplorationTransformPath(bool& success)
-{
- success = false;
-
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return vector<Eigen::Vector2i>();
- }
-
- ROS_DEBUG_STREAM("Exploration Transform: Before obstacle transform");
- computeObstacleTransform();
- ROS_DEBUG_STREAM("Exploration Transform: Before exploration transform");
- computeExplorationTransform();
- ROS_DEBUG_STREAM("Exploration Transform: after obstacle transform");
-
- //check if we are already there
- if ( m_TargetMap->getValue ( m_Start.x(), m_Start.y() ) )
- {
- success = true;
- vector<Eigen::Vector2i> path;
- path.push_back ( Eigen::Vector2i ( m_Start.x(), m_Start.y() ) );
- return path;
- }
-
- int width = m_OccupancyMap->width();
- int height = m_OccupancyMap->height();
-
- vector<Eigen::Vector2i> path;
- int x = m_Start.x();
- int y = m_Start.y();
-
- if ( m_ObstacleTransform->getValue ( x, y ) < m_MinAllowedObstacleDistance )
- {
- // robot got stuck!
- // find way out using ObstacleTransform...
- int maxPosX = x;
- int maxPosY = y;
-
- if ( ( x <= 1 ) || ( y <= 1 ) || ( x >= width-1 ) || ( y >= height-1 ) )
- {
- ROS_ERROR( "Out of map bounds" );
- return vector<Eigen::Vector2i>();
- }
-
- while ( m_ObstacleTransform->getValue ( maxPosX, maxPosY ) < m_MinAllowedObstacleDistance )
- {
- double max = m_ObstacleTransform->getValue ( x, y );
- for ( int i = -1; i <= 1; i++ )
- {
- for ( int j = -1; j <= 1; j++ )
- {
- double pt = m_ObstacleTransform->getValue ( x + i, y + j );
- if ( pt > max )
- {
- max = pt;
- maxPosX = x + i;
- maxPosY = y + j;
- }
- }
- }
- if ( maxPosX == x && maxPosY == y ) // no ascentFound
- {
- break;
- }
- else
- {
- path.push_back ( Eigen::Vector2i ( maxPosX, maxPosY ) );
- x = maxPosX;
- y = maxPosY;
- }
- }
- }
- // now path is "free"
- bool descentFound = true;
- while ( descentFound )
- {
- descentFound = false;
- int minPosX = x;
- int minPosY = y;
- double min = m_ExplorationTransform->getValue ( x, y );
- if ( ( x <= 1 ) || ( y <= 1 ) || ( x >= width-1 ) || ( y >= height-1 ) )
- {
- ROS_ERROR( "Out of map bounds" );
- return vector<Eigen::Vector2i>();
- }
-
- for ( int i = -1; i <= 1; i++ )
- {
- for ( int j = -1; j <= 1; j++ )
- {
- double pt = m_ExplorationTransform->getValue ( x + i, y + j );
- if ( pt < min )
- {
- min = pt;
- minPosX = x + i;
- minPosY = y + j;
- }
- }
- }
- if ( minPosX == x && minPosY == y ) // no descentFound
- {
- descentFound = false;
- }
- else
- {
- descentFound = true;
- path.push_back ( Eigen::Vector2i ( minPosX, minPosY ) );
- x = minPosX;
- y = minPosY;
- }
- }
- success = true;
-
- ROS_INFO_STREAM("Exploration Transform: End of function");
- return path;
-
-#if 0
- // START P2AT HACK
- vector< Eigen::Vector2i > newPath;
- for ( unsigned start=0; start<path.size()-1; ++start )
- {
- int maxVal = start+1;
- for ( unsigned end=start+1; end<path.size(); ++end )
- {
- bool ok = true;
- // draw bresenham line and check wether an object is within maximum allowed distance
- // THANKS TO WIKIPEDIA
- int x, y, t, dx, dy, incx, incy, pdx, pdy, ddx, ddy, es, el, err;
- /* Entfernung in beiden Dimensionen berechnen */
- dx = path[end].x() - path[start].x();
- dy = path[end].y() - path[start].y();
- /* Vorzeichen des Inkrements bestimmen */
- incx = (dx > 0) ? 1 : (dx < 0) ? -1 : 0;
- incy = (dy > 0) ? 1 : (dy < 0) ? -1 : 0;
- if(dx<0) dx = -dx;
- if(dy<0) dy = -dy;
- /* feststellen, welche Entfernung größer ist */
- if (dx>dy)
- {
- /* x ist schnelle Richtung */
- pdx=incx; pdy=0; /* pd. ist Parallelschritt */
- ddx=incx; ddy=incy; /* dd. ist Diagonalschritt */
- es =dy; el =dx; /* Fehlerschritte schnell, langsam */
- } else
- {
- /* y ist schnelle Richtung */
- pdx=0; pdy=incy; /* pd. ist Parallelschritt */
- ddx=incx; ddy=incy; /* dd. ist Diagonalschritt */
- es =dx; el =dy; /* Fehlerschritte schnell, langsam */
- }
- /* Initialisierungen vor Schleifenbeginn */
- x = path[start].x();
- y = path[start].y();
- err = el/2;
- /* Pixel berechnen */
- for(t=0; t<el; ++t) /* t zaehlt die Pixel, el ist auch Anzahl */
- {
- /* Aktualisierung Fehlerterm */
- err -= es;
- if(err<0)
- {
- /* Fehlerterm wieder positiv (>=0) machen */
- err += el;
- /* Schritt in langsame Richtung, Diagonalschritt */
- x += ddx;
- y += ddy;
- } else
- {
- /* Schritt in schnelle Richtung, Parallelschritt */
- x += pdx;
- y += pdy;
- }
-
- // --- start: check if obstacle around
- if ( m_ObstacleTransform->getValue ( x, y ) < m_MinAllowedObstacleDistance )
- {
- ok = false;
- break;
- }
- // --- end : check if obstacle around
- } // Pixel berechnen
-
- if ( ok )
- {
- maxVal = end;
- }
- } // for: inner
- newPath.push_back( path[maxVal] );
- start = maxVal; // incremented by foor loop to max+1
- } // for: outer
- // END: P2AT HACK
-
- success = true;
- return newPath;
-#endif
-}
-
-
-bool Explorer::getNearestFrontier ( Eigen::Vector2i& nextFrontier )
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return false;
- }
-
- computeFrontierMap();
- computeDrivingDistanceTransform();
-
- bool found = false;
- int distXPos = -1;
- int distYPos = -1;
- double dist = 10000000;
- for ( int y = 0; y < m_TargetMap->height(); y++ )
- {
- for ( int x = 0; x < m_TargetMap->width(); x++ )
- {
- if ( m_TargetMap->getValue ( x, y ) == 1 && m_DrivingDistanceTransform->getValue ( x, y ) < 999999 )
- {
- if ( m_DrivingDistanceTransform->getValue ( x, y ) < dist )
- {
- found = true;
- dist = m_DrivingDistanceTransform->getValue ( x, y );
- distXPos = x;
- distYPos = y;
- }
- }
- }
- }
- if ( found )
- {
- nextFrontier.x() = distXPos;
- nextFrontier.y() = distYPos;
- return true;
- }
- else
- {
- return false;
- }
-}
-
-
-
-// HELPERS //////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-
-void Explorer::distanceFloodFill ( GridMap<double>* map, Eigen::Vector2i start )
-{
- if ( !map ) {
- ROS_ERROR( "Received 0-pointer!" );
- }
-
- computeObstacleTransform();
-
- int width = map->width();
- int height = map->height();
- map->fill ( MAX_DISTANCE );
-
- int fromX = start.x();
- int fromY = start.y();
- map->setValue ( fromX, fromY, 0 );
-
- queue<int> xQueue;
- queue<int> yQueue;
- xQueue.push ( fromX + 1 );
- yQueue.push ( fromY );
- xQueue.push ( fromX - 1 );
- yQueue.push ( fromY );
- xQueue.push ( fromX );
- yQueue.push ( fromY - 1 );
- xQueue.push ( fromX );
- yQueue.push ( fromY + 1 );
- int xVal, yVal;
- while ( !xQueue.empty() )
- {
- xVal = xQueue.front();
- yVal = yQueue.front();
- xQueue.pop();
- yQueue.pop();
- bool isFree = (m_OccupancyMap->getValue ( xVal, yVal ) < UNKNOWN ||
- m_OccupancyMap->getValue ( xVal, yVal ) != NOT_SEEN_YET); // only fill free cells
- bool isSafe = m_ObstacleTransform->getValue ( xVal, yVal ) > m_MinAllowedObstacleDistance;
- if ( xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 && isFree && isSafe )
- {
- float value = map->getValue ( xVal, yVal );
- float value_u = map->getValue ( xVal, yVal - 1 ) + 1;
- float value_d = map->getValue ( xVal, yVal + 1 ) + 1;
- float value_l = map->getValue ( xVal - 1, yVal ) + 1;
- float value_r = map->getValue ( xVal + 1, yVal ) + 1;
-
- float value_ur = map->getValue ( xVal + 1, yVal - 1 ) + 1.4142;
- float value_ul = map->getValue ( xVal - 1, yVal - 1 ) + 1.4142;
- float value_ll = map->getValue ( xVal - 1, yVal + 1 ) + 1.4142;
- float value_lr = map->getValue ( xVal + 1, yVal + 1 ) + 1.4142;
-
- float min1 = value_u < value_d ? value_u : value_d;
- float min2 = value_l < value_r ? value_l : value_r;
- float min3 = value_ur < value_ul ? value_ur : value_ul;
- float min4 = value_ll < value_lr ? value_ll : value_lr;
- float min12 = min1 < min2 ? min1 : min2;
- float min34 = min3 < min4 ? min3 : min4;
- float min = min12 < min34 ? min12 : min34;
- float newVal = min;
- if ( value > newVal )
- {
- map->setValue ( xVal, yVal, newVal );
- if ( map->getValue ( xVal, yVal + 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal + 1 );
- }
- if ( map->getValue ( xVal, yVal - 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal );
- }
- if ( map->getValue ( xVal - 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal );
- }
- if ( map->getValue ( xVal + 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal - 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal + 1 );
- }
- if ( map->getValue ( xVal - 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal + 1 );
- }
- }
- }
- }
-}
-
-
-// Implementation taken from http://www.cs.cmu.edu/~cil/vnew.html
-double* Explorer::distanceTransform1D ( double *f, int n )
-{
- //int width = m_OccupancyMap->width();
- //int height = m_OccupancyMap->height();
- //double maxDistance = height > width ? height : width;
-
- double *d = new double[n];
- int *v = new int[n];
- double *z = new double[n+1];
- int k = 0;
- v[0] = 0;
- z[0] = -INT_MAX;
- z[1] = INT_MAX;
- for ( int q = 1; q <= n-1; q++ )
- {
- double s = ( ( f[q]+ ( q*q ) )- ( f[v[k]]+ ( v[k]*v[k] ) ) ) / ( 2*q-2*v[k] );
- while ( s <= z[k] )
- {
- k--;
- s = ( ( f[q]+ ( q*q ) )- ( f[v[k]]+ ( v[k]*v[k] ) ) ) / ( 2*q-2*v[k] );
- }
- k++;
- v[k] = q;
- z[k] = s;
- z[k+1] = INT_MAX;
- }
-
- k = 0;
- for ( int q = 0; q <= n-1; q++ )
- {
- while ( z[k+1] < q )
- k++;
- d[q] = ( ( q-v[k] ) * ( q-v[k] ) ) + f[v[k]];
- }
-
- delete [] v;
- delete [] z;
- return d;
-}
diff --git a/homer_nav_libs/src/Math/Line2D.cpp b/homer_nav_libs/src/Math/Line2D.cpp
deleted file mode 100644
index 89cd5310cbb2a9db36bec894eeac2672756cecd4..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Math/Line2D.cpp
+++ /dev/null
@@ -1,99 +0,0 @@
-/*******************************************************************************
- * Line2D.cpp
- *
- * (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Information on Code Review state:
- * Author: SM; DevelTest: Date; Reviewer: Initials; Review: Date; State: NOK
- *
- * Additional information:
- * $Id: Line2D.cpp 44313 2011-04-06 22:46:28Z agas $
- *******************************************************************************/
-
-#include <iostream>
-#include <sstream>
-
-#include <homer_nav_libs/Math/Line2D.h>
-#include <homer_nav_libs/Math/vec2.h>
-
-#define THIS Line2D
-
-float THIS::gradient() const
-{
- float gradient = 10000000.0;
- if ( m_Vec[0] != 0.0 )
- {
- gradient = m_Vec[1]/m_Vec[0];
- }
- return gradient;
-}
-
-std::vector< Point2D > THIS::vertices ( unsigned substeps )
-{
- unsigned steps = substeps+2;
- std::vector<Point2D> myVertices ( steps );
- for ( unsigned i=0; i<steps; i++ )
- {
- float t= float ( i ) / float ( steps-1 );
- myVertices[i] = m_Start + t*m_Vec;
- }
- return myVertices;
-}
-
-Point2D THIS::getClosestPoint ( Point2D point ) const
-{
- float t = ( point-m_Start ) * m_Vec;
- t /= m_Vec * m_Vec;
- if ( t > 1.0 )
- {
- t = 1.0;
- }
- else if ( t < 0.0 )
- {
- t = 0.0;
- }
- Point2D pointOnLine = m_Start + ( t * m_Vec );
- return pointOnLine;
-}
-
-Point2D THIS::getIntersectionPoint ( Line2D line ) const
-{
- Point2D intersecPoint;
- double det1 = m_Vec.x() * ( -line.vec().y() ) - ( -line.vec().x() ) * m_Vec.y();
- // lines are not parallel
- if ( det1 != 0 )
- {
- CVec2 startToStart = line.start() -m_Start;
- // calculate intersection
- double lambda = ( startToStart.x() * ( -line.vec().y() ) - ( -line.vec().x() ) * startToStart.y() ) / det1;
- intersecPoint = m_Start + lambda* m_Vec;
- }
-
- return intersecPoint;
-}
-
-float THIS::getIntersectionPointParameter ( Line2D line ) const
-{
- double lambda = 0.0;
- double det1 = m_Vec.x() * ( -line.vec().y() ) - ( -line.vec().x() ) * m_Vec.y();
- // lines are not parallel
- if ( det1 != 0 )
- {
- CVec2 startToStart = line.start() -m_Start;
- // calculate intersection
- lambda = ( startToStart.x() * ( -line.vec().y() ) - ( -line.vec().x() ) * startToStart.y() ) / det1;
- }
-
- return lambda;
-}
-
-std::string THIS::toString() const
-{
- std::ostringstream str;
-// str << "Startpoint: " << m_Start.x() << " " << m_Start.y() << " Endpoint: " << end().x() << " " << end().y() <<
-// " Vector: " << m_Vec.x() << " " << m_Vec.y() << " ";
- str << m_Start.x() << " " << m_Start.y() << std::endl << end().x() << " " << end().y() << std::endl;
- return str.str();
-}
-
diff --git a/homer_nav_libs/src/Math/Math.cpp b/homer_nav_libs/src/Math/Math.cpp
deleted file mode 100644
index 5ad74fc3b39b77fd9f67276d78c4558b363485b0..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Math/Math.cpp
+++ /dev/null
@@ -1,135 +0,0 @@
-/*******************************************************************************
- * Math.cpp
- *
- * (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Additional information:
- * $Id: $
- *******************************************************************************/
-
-#include <limits.h>
-#include <homer_nav_libs/Math/Math.h>
-#include <math.h>
-
-#include <homer_nav_libs/Math/vec2.h>
-
-#define THIS Math
-
-THIS::THIS()
-{
-}
-
-THIS::~THIS()
-{
-}
-
-float THIS::meanAngle( const std::vector<float>& angles )
-{
- //calculate vectors from angles
- CVec2 vectorSum(0,0);
- for ( unsigned i=0; i<angles.size(); i++ )
- {
- vectorSum = vectorSum + CVec2( cos( angles[i] ), sin ( angles[i] ) );
- }
- //return vectorSum.getAngle( CVec2(1,0) );
- if ( vectorSum.magnitude() == 0 ) { return 0; }
- return atan2( vectorSum.y(), vectorSum.x() );
-}
-
-
-float THIS::meanAngleWeighted( const std::vector< WeightedValue >& weightedAngles )
-{
- //calculate vectors from angles
- CVec2 vectorSum(0,0);
- for ( unsigned i=0; i<weightedAngles.size(); i++ )
- {
- vectorSum = vectorSum + weightedAngles[i].weight * CVec2( cos( weightedAngles[i].value ), sin ( weightedAngles[i].value ) );
- }
- //return vectorSum.getAngle( CVec2(1,0) );
- if ( vectorSum.magnitude() == 0 ) { return 0; }
- return atan2( vectorSum.y(), vectorSum.x() );
-}
-
-
-float THIS::angleVariance( float meanAngle, const std::vector<float>& angles )
-{
- float quadSum=0;
- for( unsigned i=0; i < angles.size(); i++ )
- {
- float turnAngle=minTurnAngle( angles[i], meanAngle );
- quadSum += turnAngle*turnAngle;
- }
- return quadSum / float ( angles.size() );
-}
-
-
-float THIS::minTurnAngle( float angle1, float angle2 )
-{
-/* CVec2 vector1( cos( angle1 ), sin ( angle1 ) );
- CVec2 vector2( cos( angle2 ), sin ( angle2 ) );
- return vector1.getAngle( vector2 );
- */
- angle1 *= 180.0/M_PI;
- angle2 *= 180.0/M_PI;
- //if(angle1 < 0) angle1 += M_PI * 2;
- //if(angle2 < 0) angle2 += M_PI * 2;
- int diff= angle2 - angle1;
- diff = (diff + 180) % 360 - 180;
-
- //float sign=1;
- //if ( diff < 0 ) { sign=-1; }
- //minimal turn angle:
- //if the absolute difference is above 180°, calculate the difference in other direction
- //if ( fabs(diff) > M_PI ) {
- // diff = 2*M_PI - fabs(diff);
- // diff *= sign;
- //}
-
- float ret = static_cast<double>(diff) * M_PI/180.0;
- return ret;
-}
-
-Point2D THIS::center( std::vector<Point2D>& points )
-{
- double numPoints = double( points.size() );
- double sumX=0, sumY=0;
- for( unsigned i=0; i < points.size(); i++ )
- {
- sumX += points[i].x();
- sumY += points[i].y();
- }
- return Point2D( sumX / numPoints, sumY / numPoints );
-}
-
-
-double THIS::randomGauss(float variance)
-{
- if (variance < 0) {
- variance = -variance;
- }
- double x1, x2, w, y1;
- do {
- x1 = 2.0 * random01() - 1.0;
- x2 = 2.0 * random01() - 1.0;
- w = x1 * x1 + x2 * x2;
- } while ( w >= 1.0 );
-
- w = sqrt((-2.0 * log(w)) / w);
- y1 = x1 * w;
- // now y1 is uniformly distributed
- return sqrt(variance) * y1;
-}
-
-double THIS::random01(unsigned long init)
-{
- static unsigned long n;
- if (init > 0) {
- n = init;
- }
- n = 1664525 * n + 1013904223;
- // create double from unsigned long
- return (double)(n/2) / (double)LONG_MAX;
-}
-
-#undef THIS
diff --git a/homer_nav_libs/src/Math/Point2D.cpp b/homer_nav_libs/src/Math/Point2D.cpp
deleted file mode 100644
index 13c321d79216a3b67091cc0c154d7da8bdb01e25..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Math/Point2D.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*******************************************************************************
- * Point2D.cpp
- *
- * (C) 2008 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Additional information:
- * $Id: $
- *******************************************************************************/
-
-#include <homer_nav_libs/Math/Point2D.h>
-
-#define THIS Point2D
-
-float THIS::getPolarAngle () const
-{
- float angle = atan ( m_Y /m_X );
- if ( m_X < 0 )
- {
- angle = - ( M_PI - angle );
- }
- while ( angle >= M_PI )
- {
- angle -= 2*M_PI;
- }
- while ( angle < -M_PI )
- {
- angle += 2*M_PI;
- }
- return angle;
-}
-
-#undef THIS
diff --git a/homer_nav_libs/src/Math/Pose.cpp b/homer_nav_libs/src/Math/Pose.cpp
deleted file mode 100644
index 129d123a62e47b56c8c1044e5faf8e06a9fdc63e..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Math/Pose.cpp
+++ /dev/null
@@ -1,117 +0,0 @@
-/*******************************************************************************
- * Pose.cpp
- *
- * (C) 2006 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * $Id: Pose.cpp 44313 2011-04-06 22:46:28Z agas $
- *******************************************************************************/
-
-#include <cmath>
-
-#include <homer_nav_libs/Math/Pose.h>
-#include <homer_nav_libs/Math/Transformation2D.h>
-
-using namespace std;
-
-
-#define THIS Pose
-
-THIS::THIS(float x, float y, float theta) : Point2D(x, y) {
- m_Theta = theta;
-}
-
-THIS::THIS() {
- m_Theta = 0.0;
-}
-
-THIS::~THIS() {
-}
-
-float THIS::theta() const {
- return m_Theta;
-}
-
-void THIS::setTheta(float theta) {
- m_Theta = theta;
-}
-
-Pose THIS::operator+ ( const Transformation2D& transformation ) const {
- float x, y, theta;
- x = m_X + transformation.x();
- y = m_Y + transformation.y();
- theta = m_Theta + transformation.theta();
- while (theta >= M_PI) theta -= 2*M_PI;
- while (theta < -M_PI) theta += 2*M_PI;
-
- return Pose(x, y, theta);
-}
-
-Pose THIS::operator- ( const Transformation2D& transformation ) const {
- float x, y, theta;
- x = m_X - transformation.x();
- y = m_Y - transformation.y();
- theta = m_Theta - transformation.theta();
- while (theta >= M_PI) theta -= 2*M_PI;
- while (theta < -M_PI) theta += 2*M_PI;
-
- return Pose(x, y, theta);
-}
-
-Transformation2D THIS::operator- ( const Pose& pose ) const {
- float x, y, theta;
- x = m_X - pose.x();
- y = m_Y - pose.y();
-
- float s1, s2;
- if (m_Theta > pose.theta()) {
- s1 = -( 2 * M_PI - m_Theta + pose.theta());
- s2 = m_Theta - pose.theta();
- } else {
- s1 = 2 * M_PI - pose.theta() + m_Theta;
- s2 = -(pose.theta() - m_Theta);
- }
- if (fabs(s1) > fabs(s2)) {
- theta = s2;
- } else {
- theta = s1;
- }
- while (theta >= M_PI) theta -= 2*M_PI;
- while (theta < -M_PI) theta += 2*M_PI;
-
- return Transformation2D(x, y, theta);
-}
-
-Pose THIS::interpolate(const Pose& referencePose, float t) const {
-
- float newX = m_X + t * (referencePose.x() - m_X);
- float newY = m_Y + t * (referencePose.y() - m_Y);
-
- // Calculate mean angle by adding the vem_Thetaors in unit circle
- float x1 = cosf(m_Theta);
- float y1 = sinf(m_Theta);
- float x2 = cosf(referencePose.theta());
- float y2 = sinf(referencePose.theta());
- float newTheta = atan2 (y1*(1-t)+y2*t, x1*(1-t)+x2*t);
-
- return Pose(newX, newY, newTheta);
-}
-
-//THIS::THIS( ExtendedInStream& extStrm )
-//{
-// char version;
-// extStrm >> version;
-// extStrm >> m_X;
-// extStrm >> m_Y;
-// extStrm >> m_Theta;
-//}
-
-//void THIS::storer( ExtendedOutStream& extStrm ) const
-//{
-// char version=10;
-// extStrm << version;
-// extStrm << m_X;
-// extStrm << m_Y;
-// extStrm << m_Theta;
-//}
-
diff --git a/homer_nav_libs/src/Math/Transformation2D.cpp b/homer_nav_libs/src/Math/Transformation2D.cpp
deleted file mode 100644
index f60629cdf404785af35c273d4fadd55294696d11..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Math/Transformation2D.cpp
+++ /dev/null
@@ -1,308 +0,0 @@
-/*******************************************************************************
- * Transformation2D.cpp
- *
- * (C) 2008 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * $Id: Transformation2D.cpp 44313 2011-04-06 22:46:28Z agas $
- *******************************************************************************/
-
-#include <homer_nav_libs/Math/Transformation2D.h>
-
-#include <cmath>
-#include <vector>
-#include <iostream>
-#include <sstream>
-#include <homer_nav_libs/Math/vec2.h> // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
-#include <homer_nav_libs/Math/mat2.h> // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
-#include <homer_nav_libs/Math/Point2D.h>
-#include <homer_nav_libs/Math/Line2D.h>
-
-#define THIS Transformation2D
-#define BASE CVec2
-
-THIS::Transformation2D() : BASE()
-{
- m_Theta = 0.0;
-}
-
-THIS::Transformation2D ( double x, double y, double theta ) : BASE ( x,y )
-{
- m_Theta = theta;
-}
-
-THIS::Transformation2D ( const CVec2& vec, double theta ) : BASE ( vec )
-{
- m_Theta = theta;
-}
-
-THIS::~Transformation2D()
-{
-}
-
-void THIS::set ( double x, double y, double theta )
-{
- m_X = x;
- m_Y = y;
- m_Theta = theta;
-}
-
-double THIS::theta() const
-{
- return m_Theta;
-}
-
-Transformation2D THIS::operator+ ( Transformation2D t ) const
-{
- double theta = m_Theta + t.theta();
- // TODO comment only for scan matching test
-// while ( theta >= M_PI ) theta -= 2*M_PI;
-// while ( theta < -M_PI ) theta += 2*M_PI;
- return Transformation2D ( m_X + t.x(), m_Y + t.y(), theta );
-}
-
-Transformation2D& THIS::operator+= ( Transformation2D t )
-{
- m_X += t.x();
- m_Y += t.y();
- m_Theta += t.theta();
- // TODO comment only for scan matching test
-// while ( m_Theta >= M_PI ) m_Theta -= 2*M_PI;
-// while ( m_Theta < -M_PI ) m_Theta += 2*M_PI;
- return ( *this );
-}
-
-Transformation2D THIS::operator- ( Transformation2D t ) const
-{
- float s1, s2, theta;
- if ( m_Theta > t.theta() )
- {
- s1 = - ( 2 * M_PI - m_Theta + t.theta() );
- s2 = m_Theta - t.theta();
- }
- else
- {
- s1 = 2 * M_PI - t.theta() + m_Theta;
- s2 = - ( t.theta() - m_Theta );
- }
- if ( fabs ( s1 ) > fabs ( s2 ) )
- {
- theta = s2;
- }
- else
- {
- theta = s1;
- }
- while ( theta >= M_PI ) theta -= 2*M_PI;
- while ( theta < -M_PI ) theta += 2*M_PI;
-// double theta = m_Theta - t.theta();
-// while ( theta >= M_PI ) theta -= 2*M_PI;
-// while ( theta < -M_PI ) theta += 2*M_PI;
- return Transformation2D ( m_X - t.x(), m_Y - t.y(), theta );
-}
-
-Transformation2D& THIS::operator-= ( Transformation2D t )
-{
- m_X -= t.x();
- m_Y -= t.y();
-
- float s1, s2, theta;
- if ( m_Theta > t.theta() )
- {
- s1 = - ( 2 * M_PI - m_Theta + t.theta() );
- s2 = m_Theta - t.theta();
- }
- else
- {
- s1 = 2 * M_PI - t.theta() + m_Theta;
- s2 = - ( t.theta() - m_Theta );
- }
- if ( fabs ( s1 ) > fabs ( s2 ) )
- {
- theta = s2;
- }
- else
- {
- theta = s1;
- }
- while ( theta >= M_PI ) theta -= 2*M_PI;
- while ( theta < -M_PI ) theta += 2*M_PI;
- m_Theta = theta;
-
- return ( *this );
-
-// m_X -= t.x();
-// m_Y -= t.y();
-// m_Theta -= t.theta();
-// while ( m_Theta >= M_PI ) m_Theta -= 2*M_PI;
-// while ( m_Theta < -M_PI ) m_Theta += 2*M_PI;
-// return ( *this );
-}
-
-Transformation2D THIS::operator* ( float factor ) const
-{
-
- double theta = m_Theta * factor;
- while ( theta >= M_PI ) theta -= 2*M_PI;
- while ( theta < -M_PI ) theta += 2*M_PI;
- return Transformation2D ( m_X * factor, m_Y * factor, theta );
-}
-
-Transformation2D& THIS::operator*= ( float factor )
-{
- m_X *= factor;
- m_Y *= factor;
- m_Theta *= factor;
- while ( m_Theta >= M_PI ) m_Theta -= 2*M_PI;
- while ( m_Theta < -M_PI ) m_Theta += 2*M_PI;
- return ( *this );
-}
-
-
-Transformation2D THIS::operator/ ( float factor ) const
-{
- double theta = m_Theta / factor;
- return Transformation2D ( m_X / factor, m_Y / factor, theta );
-}
-
-Transformation2D& THIS::operator/= ( float factor )
-{
- m_X /= factor;
- m_Y /= factor;
- m_Theta /= factor;
- return ( *this );
-}
-
-bool THIS::operator== ( Transformation2D t ) const
-{
- if ( t.x() == m_X && t.y() == m_Y && t.theta() == m_Theta )
- {
- return true;
- }
- else
- {
- return false;
- }
-}
-
-bool THIS::operator!= ( Transformation2D t ) const
-{
- return ! ( ( *this ) ==t );
-}
-
-bool THIS::operator<= ( Transformation2D t ) const
-{
- return ( this->magnitude() <= t.magnitude() ) && ( m_Theta <= t.theta() );
-}
-
-bool THIS::operator>= ( Transformation2D t ) const
-{
- return ( this->magnitude() >= t.magnitude() ) && ( m_Theta >= t.theta() );
-}
-
-bool THIS::operator< ( Transformation2D t ) const
-{
- return ( m_X < t.x() ) || ( m_Y < t.y() ) || ( ( m_Theta < t.theta() ) && ( *this <= t ) );
-}
-
-bool THIS::operator> ( Transformation2D t ) const
-{
- return ( m_X > t.x() ) || ( m_Y > t.y() ) || ( ( m_Theta > t.theta() ) && ( *this >= t ) );
-}
-
-Transformation2D THIS::abs() const
-{
- return Transformation2D ( std::abs ( m_X ), std::abs ( m_Y ), std::abs ( m_Theta ) );
-}
-
-Transformation2D THIS::inverse() const
-{
- return ( *this ) * ( -1.0 );
-}
-
-Point2D THIS::transform ( const Point2D& point ) const
-{
- CMat2 rotMat = CMat2 ( m_Theta );
- CVec2 transVec = CVec2 ( m_X, m_Y );
- Point2D transformedPoint = rotMat * ( point );
- transformedPoint += transVec;
- return transformedPoint;
-}
-
-std::vector<Point2D> THIS::transform ( const std::vector<Point2D>& points ) const
-{
- CMat2 rotMat = CMat2 ( m_Theta );
- CVec2 transVec = CVec2 ( m_X, m_Y );
- std::vector<Point2D> transformedPoints;
- std::vector<Point2D>::const_iterator iter = points.begin();
- while ( iter != points.end() )
- {
- Point2D currPoint = rotMat * ( *iter );
- currPoint += transVec;
- transformedPoints.push_back ( currPoint );
- iter++;
- }
- return transformedPoints;
-}
-
-// // Reihenfolge rotation/translation vertauscht !!!
-// Point2D THIS::transform ( Point2D point ) const
-// {
-// CMat2 rotMat = CMat2 ( m_Theta );
-// CVec2 transVec = CVec2 ( m_X, m_Y );
-// Point2D transformedPoint = point+transVec;
-// transformedPoint = rotMat * point;
-// return transformedPoint;
-// }
-//
-// // Reihenfolge rotation/translation vertauscht !!!
-// std::vector<Point2D> THIS::transform ( std::vector<Point2D> points ) const
-// {
-// CMat2 rotMat = CMat2 ( m_Theta );
-// CVec2 transVec = CVec2 ( m_X, m_Y );
-// std::vector<Point2D> transformedPoints;
-// std::vector<Point2D>::const_iterator iter = points.begin();
-// while ( iter != points.end() )
-// {
-// Point2D currPoint = ( *iter )+ transVec;
-// currPoint = rotMat * currPoint;
-// transformedPoints.push_back ( currPoint );
-// iter++;
-// }
-// return transformedPoints;
-// }
-
-Line2D THIS::transform ( const Line2D& line ) const
-{
- CMat2 rotMat = CMat2 ( m_Theta );
- CVec2 transVec = CVec2 ( m_X, m_Y );
- Line2D transformedLine = Line2D ( rotMat * line.start() + transVec, rotMat * line.end() + transVec );
- return transformedLine;
-}
-
-std::vector<Line2D> THIS::transform ( const std::vector<Line2D>& lines ) const
-{
- //CMat2 rotMat = CMat2 ( m_Theta );
- //CVec2 transVec = CVec2 ( m_X, m_Y );
- std::vector<Line2D> transformedLines;
- std::vector<Line2D>::const_iterator iter = lines.begin();
- while ( iter != lines.end() )
- {
- transformedLines.push_back ( transform(*iter) );
- iter++;
- }
- return transformedLines;
-}
-
-std::string THIS::toString() const
-{
- std::ostringstream str;
- str << "deltaX: " << m_X << ", deltaY: " << m_Y << ", deltaTheta: " << m_Theta;
- return str.str();
-}
-
-
-
-#undef THIS
-#undef BASE
-
diff --git a/homer_nav_libs/src/SpeedControl/CMakeLists.txt b/homer_nav_libs/src/SpeedControl/CMakeLists.txt
deleted file mode 100644
index 154b60ebbe800c67526d593377613aca5832eee6..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/SpeedControl/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-set(SpeedControl_SRC
- SpeedControl.cpp
-)
-
-add_library(SpeedControl ${SpeedControl_SRC})
diff --git a/homer_nav_libs/src/SpeedControl/SpeedControl.cpp b/homer_nav_libs/src/SpeedControl/SpeedControl.cpp
deleted file mode 100644
index c939197e0c3cf63ad6142b350b711ee7210e8e68..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/SpeedControl/SpeedControl.cpp
+++ /dev/null
@@ -1,177 +0,0 @@
-#include <cmath>
-#include <iostream>
-
-#include <ros/ros.h>
-#include "SpeedControl.h"
-#include "tools/loadRosConfig.h"
-
-using namespace std;
-
-// Robot dimensions in m
-//
-//
-// /-------------\ <-- MAX_X
-// | x |
-// | | |
-// | | |
-// | y----- |
-// | |
-// | ROBOT |
-// | |
-// \-------------/ <-- MIN_X
-// ^ ^
-// | |
-// MAX_Y MIN_Y
-//
-float ROBOT_MIN_X = -0.30;
-float ROBOT_MAX_X = 0.30;
-float ROBOT_MIN_Y = -0.27;
-float ROBOT_MAX_Y = 0.27;
-
-
-namespace {
- Eigen::AlignedBox2f InnerDangerZone,
- OuterDangerZone;
- float InnerDangerZoneFactor,
- OuterDangerZoneFactor;
-
- inline Eigen::AlignedBox2f loadRect(const string& path)
- {
- pair<float, float> pX, pY;
- ros::param::get(path + "/x_min", pX.first);
- ros::param::get(path + "/x_max", pX.second);
- ros::param::get(path + "/y_min", pY.first);
- ros::param::get(path + "/y_max", pY.second);
-
- Eigen::Vector2f first(pX.first, pY.first), second(pX.second, pY.second);
- return Eigen::AlignedBox2f(first, second);
- }
-}
-
-void SpeedControl::loadDimensions()
-{
- InnerDangerZone = loadRect("/homer_navigation/speed_control/inner_danger_zone");
- InnerDangerZoneFactor;
- ros::param::get("/homer_navigation/speed_control/inner_danger_zone/speed_factor", InnerDangerZoneFactor);
- OuterDangerZone = loadRect("/homer_navigation/speed_control/inner_danger_zone");
- OuterDangerZoneFactor;
- ros::param::get("/homer_navigation/speed_control/outer_danger_zone/speed_factor", OuterDangerZoneFactor);
- if(!OuterDangerZone.contains(InnerDangerZone))
- ROS_WARN_STREAM("InnerDangerZone is not contained in OuterDangerZone");
-}
-
-float SpeedControl::getSpeedFactor(const vector<geometry_msgs::Point>& points, float minVal, float maxVal )
-{
- float minFactor = 1.0;
- for (unsigned i = 0; i < points.size(); i++)
- {
- Eigen::Vector2f point(points[i].x, points[i].y);
- if(InnerDangerZone.contains(point))
- {
- minFactor = InnerDangerZoneFactor;
- break;
- }
- if(OuterDangerZone.contains(point))
- minFactor = OuterDangerZoneFactor;
- }
- minFactor = sqrt(minFactor);
- float range = maxVal - minVal;
- minFactor = minVal + range*minFactor;
- return minFactor;
-}
-
-float SpeedControl::getMaxMoveDistance(vector<geometry_msgs::Point> points)
-{
- float minDistance = 4; // distance in m to nearest obstacle in front
- for (unsigned int i = 0; i < points.size(); i++)
- {
- if(points[i].y > ROBOT_MIN_Y && points[i].y < ROBOT_MAX_Y && points[i].x > ROBOT_MAX_X)
- {
- float distance = sqrt((points[i].x * points[i].x) + (points[i].y * points[i].y));
- if (distance < minDistance)
- {
- minDistance = distance;
- }
- }
- }
- float maxMoveDist = minDistance - ROBOT_MAX_X;
- if (maxMoveDist < 0) {
- maxMoveDist = 0.0;
- }
- return maxMoveDist;
-}
-
-float SpeedControl::getMaxMoveDistance(std::vector< Eigen::Vector3d >* kinectData, float minObstacleHeight, float minObstacleFromRobotDistance, float maxObstacleFromRobotDistance)
-{
- // Check for obstacles in Kinect image: Look for closest point
-
- float minDistance = 4; // distance to nearest obstacle in front
-
- for(int i=0;i<kinectData->size();++i)
- {
- Eigen::Vector2d p = Eigen::Vector2d(kinectData->at(i).x(), kinectData->at(i).y());
- if(!std::isnan(p.x()))
- {
- // Filter point cloud
- if(p.x() > minObstacleFromRobotDistance && p.x() < maxObstacleFromRobotDistance && kinectData->at(i).z() > minObstacleHeight)
- {
- // Check for collisions outside of robot
- if(p.y() > ROBOT_MIN_Y && p.y() < ROBOT_MAX_Y && p.x() > ROBOT_MAX_X)
- {
- float distance = sqrt((p.x() * p.x()) + (p.y() * p.y()));
- if (distance < minDistance)
- {
- minDistance = distance;
- }
- }
- }
- }
- }
-
- float maxMoveDist = minDistance - ROBOT_MAX_X;
- if (maxMoveDist < 0) {
- maxMoveDist = 0.0;
- }
- return maxMoveDist;
-}
-
-float SpeedControl::getTurnSpeedFactor( float speedFactor, float turnAngle, float minVal, float maxVal )
-{
- //turn faster for larger angles
- float angleDependentFactor = sqrt( fabs(turnAngle) / M_PI );
- angleDependentFactor = minVal + angleDependentFactor*(maxVal-minVal);
- return sqrt( speedFactor * angleDependentFactor );
-}
-
-float SpeedControl::getMinTurnAngle(std::vector<geometry_msgs::Point> laserData, float minAngle, float maxAngle, float minDistance, float maxDistance)
-{
- float turn_factor = 1.0;
- for (unsigned int i = 0; i < laserData.size(); i++)
- {
- if(laserData[i].y > ROBOT_MIN_Y && laserData[i].y < ROBOT_MAX_Y && laserData[i].x > ROBOT_MAX_X)
- {
- float distance = sqrt((laserData[i].x * laserData[i].x) + (laserData[i].y * laserData[i].y));
- if (distance < minDistance + ROBOT_MAX_X)
- {
- turn_factor = 0.0;
- }
- else if(distance > maxDistance + ROBOT_MAX_X)
- {
- turn_factor = 1.0;
- }
- else
- {
- turn_factor = (distance - minDistance)/maxDistance;
- }
- }
- }
- float range = maxAngle - minAngle;
- return minAngle + turn_factor * range;
-}
-
-SpeedControl::SpeedControl() {
-}
-
-SpeedControl::~SpeedControl() {
-}
-
diff --git a/homer_nav_libs/src/SpeedControl/SpeedControl.h b/homer_nav_libs/src/SpeedControl/SpeedControl.h
deleted file mode 100644
index 7bef0e9a16eb1f433c7ba23e7bd1c1b671253d7b..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/SpeedControl/SpeedControl.h
+++ /dev/null
@@ -1,71 +0,0 @@
-#ifndef SPEEDCONTROL_H
-#define SPEEDCONTROL_H
-
-#include <vector>
-#include <Eigen/Geometry>
-#include <geometry_msgs/Point.h>
-
-/**
- * @class SpeedControl
- * @author Malte Knauf, Stephan Wirth
- * @brief Class for computing a speed factor with respect to a given laser measurement.
- */
-class SpeedControl {
-
- public:
-
- /**
- * @brief Loads robot and safety zone dimensions config values
- */
- static void loadDimensions();
-
- /**
- * Calculates the speed factor for the robot. If a measured obstacle lies in the "danger zone"
- * that is defined in SpeedControl.cpp, the speed factor will be below maxVal. The nearer the obstacle,
- * the smaller the speed factor.
- * @param laserData Laser measurement
- * @param minVal,maxVal range of return values
- * @return Speed factor, value between minVal and maxVal. The higher the speed factor, the safer is it to drive fast.
- */
- static float getSpeedFactor(const std::vector<geometry_msgs::Point>& points, float minVal=0.2, float maxVal=1.0);
-
- /**
- * Calculates the maximum distance the robot can move without touching an obstacle.
- * @param laserPoints Current laser measurement transformed to (valid) points in map frame
- * @param laserConf The configuration of the LRF that took the measurement
- * @return maximum distance (m) the robot can move based on the given laserscan.
- */
- static float getMaxMoveDistance(std::vector<geometry_msgs::Point> laserData);
-
- static float getMaxMoveDistance(std::vector< Eigen::Vector3d >* kinectData, float minObstacleHeight, float minObstacleFromRobotDistance, float maxObstacleFromRobotDistance);
-
- /// @return if the angle is larger, the turn speed factor will be higher
- static float getTurnSpeedFactor( float speedFactor, float turnAngle, float minVal, float maxVal );
-
- /**
- * Calculates the minimum angle between the robot's orientation and the next waypoint which is necessary
- * to trigger a rotation instead of a straight line
- * @brief getMinTurnAngle
- * @param laserData
- * @param minAngle
- * @param maxAngle
- * @return
- */
- static float getMinTurnAngle(std::vector<geometry_msgs::Point> laserData, float minAngle, float maxAngle,
- float minDistance, float maxDistance);
-
- private:
-
- /**
- * Constructor is empty and private because this class will never be instanciated.
- */
- SpeedControl();
-
- /**
- * Destructor is empty.
- */
- ~SpeedControl();
-
-};
-#endif
-
diff --git a/homer_navigation/CHANGELOG.rst b/homer_navigation/CHANGELOG.rst
deleted file mode 100644
index 5c4aae423c430c10847e012663cb8c50a2b00ed3..0000000000000000000000000000000000000000
--- a/homer_navigation/CHANGELOG.rst
+++ /dev/null
@@ -1,9 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package homer_navigation
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.1.1 (2016-11-03)
-------------------
-* fixes
-* initial commit
-* Contributors: Niklas Yann Wettengel
diff --git a/homer_navigation/CMakeLists.txt b/homer_navigation/CMakeLists.txt
deleted file mode 100644
index 94d0330758a5587db154479761e9d08fe3ee6a42..0000000000000000000000000000000000000000
--- a/homer_navigation/CMakeLists.txt
+++ /dev/null
@@ -1,55 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(homer_navigation)
-
-find_package(catkin REQUIRED COMPONENTS
- roscpp
- roslib
- homer_robbie_architecture
- nav_msgs
- sensor_msgs
- homer_mapnav_msgs
- homer_nav_libs
- tf
- cmake_modules
-)
-
-find_package(Eigen3 REQUIRED)
-
-set(CMAKE_BUILD_TYPE Release)
-
-catkin_package(
- INCLUDE_DIRS include
- CATKIN_DEPENDS
- roscpp
- roslib
- homer_robbie_architecture
- nav_msgs
- sensor_msgs
- homer_mapnav_msgs
- homer_nav_libs
- tf
-)
-
-include_directories(
- include
- ${catkin_INCLUDE_DIRS}
- ${Eigen3_INCLUDE_DIRS}
-)
-
-add_executable(homer_navigation src/homer_navigation_node.cpp)
-add_dependencies(homer_navigation ${catkin_EXPORTED_TARGETS})
-
-target_link_libraries(homer_navigation
- ${catkin_LIBRARIES}
- ${EIGEN3_LIBRARIES}
- )
-
-install(DIRECTORY include/${PROJECT_NAME}/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-
-install(TARGETS homer_navigation
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
diff --git a/homer_navigation/README.md b/homer_navigation/README.md
deleted file mode 100644
index 01fa127128c487da52e1efbe0dacd709e41fc327..0000000000000000000000000000000000000000
--- a/homer_navigation/README.md
+++ /dev/null
@@ -1,76 +0,0 @@
-# homer_navigation
-
-## Introduction
-
-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.
-
-## 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
diff --git a/homer_navigation/config/homer_navigation.yaml b/homer_navigation/config/homer_navigation.yaml
deleted file mode 100644
index 24e17193ca3af68fce77a34c7eb405381f9e1a7f..0000000000000000000000000000000000000000
--- a/homer_navigation/config/homer_navigation.yaml
+++ /dev/null
@@ -1,40 +0,0 @@
-/homer_navigation/safe_path_weight: 0.1 # 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.27 # 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: 1.5 # m if possible robot should move further away than this from obstacles
-/homer_navigation/safe_obstacle_distance/max: 3.0 # 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.23 # 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 # 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.1 # 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.2 # rad/s min turn speed for Final Turn so the Robot doesn't stop turning
-/homer_navigation/max_drive_angle: 0.3 # rad threshold for driving and turning - if above that value only turn
-
-/homer_navigation/max_move_speed: 1.0 # 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: 0.7 # factor for the max speed calculation of the obstacleDistancemap
-/homer_navigation/waypoint_speed_factor: 10 # factor for the max speed calculation with the distance to the next Waypoint
-/homer_navigation/obstacle_speed_factor: 0.5 # factor for the max speed calculation with the last laser max movement distance
-/homer_navigation/target_distance_speed_factor: 0.4 # factor for the max speed calculation with the distance to target
-
-/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
-/homer_navigation/unknown_threshold: 50 # obstacle strenght under which the obstacle is ignored by navigation
-/homer_navigation/waypoint_radius_factor: 0.25 # factor for includance of obstacle map
diff --git a/homer_navigation/config/homer_navigation_pioneer.yaml b/homer_navigation/config/homer_navigation_pioneer.yaml
deleted file mode 100644
index 0a1370d1bcb3468702aa52325a45d6e87c6bad00..0000000000000000000000000000000000000000
--- a/homer_navigation/config/homer_navigation_pioneer.yaml
+++ /dev/null
@@ -1,39 +0,0 @@
-/homer_navigation/safe_path_weight: 0.1 # 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.27 # 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.5 # 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.23 # 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 # 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.1 # rad values lower than this angle will let the navigation assume reaching the designated position
-/homer_navigation/max_turn_speed: 0.5 # 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.4 # 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.3 # factor for the max speed calculation with the distance to the next Waypoint
-/homer_navigation/obstacle_speed_factor: 0.8 # factor for the max speed calculation with the last laser max 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
-/homer_navigation/unknown_threshold: 50 # obstacle strenght under which the obstacle is ignored by navigation
-/homer_navigation/waypoint_radius_factor: 0.25 # factor for includance of obstacle map
diff --git a/homer_navigation/include/homer_navigation/homer_navigation_node.h b/homer_navigation/include/homer_navigation/homer_navigation_node.h
deleted file mode 100644
index a273994b08baa58b6c89c1369f94c5754e144515..0000000000000000000000000000000000000000
--- a/homer_navigation/include/homer_navigation/homer_navigation_node.h
+++ /dev/null
@@ -1,329 +0,0 @@
-#ifndef FastNavigationModule_H
-#define FastNavigationModule_H
-
-#include <vector>
-#include <string>
-#include <sstream>
-#include <cmath>
-
-#include <ros/ros.h>
-#include <ros/package.h>
-
-#include <tf/transform_listener.h>
-
-#include <homer_robbie_architecture/Architecture/StateMachine/StateMachine.h>
-
-#include <nav_msgs/OccupancyGrid.h>
-#include <nav_msgs/Path.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Twist.h>
-#include <sensor_msgs/LaserScan.h>
-#include <homer_mapnav_msgs/StartNavigation.h>
-#include <homer_mapnav_msgs/NavigateToPOI.h>
-#include <homer_mapnav_msgs/TargetUnreachable.h>
-#include <homer_mapnav_msgs/GetPointsOfInterest.h>
-#include <std_msgs/Int8.h>
-#include <std_msgs/Float32.h>
-#include <std_msgs/Empty.h>
-#include <std_msgs/String.h>
-#include <std_msgs/Float32.h>
-#include <homer_ptu_msgs/CenterWorldPoint.h>
-#include <homer_ptu_msgs/SetPanTilt.h>
-
-#include <homer_nav_libs/tools.h>
-#include <homer_nav_libs/Explorer/Explorer.h>
-
-class Explorer;
-/**
- * @class HomerNavigationNode
- * @author Malte Knauf, Stephan Wirth, David Gossow (RX), Florian Polster
- * @brief Performs autonomous navigation
- */
-class HomerNavigationNode {
-
- public:
-
- /**
- * @brief States of the state machines
- */
- enum ProcessState
- {
- IDLE,
- AWAITING_PATHPLANNING_MAP,
- FOLLOWING_PATH,
- AVOIDING_COLLISION,
- FINAL_TURN
- };
-
- /**
- * The constructor
- */
- HomerNavigationNode();
-
- /**
- * The destructor
- */
- virtual ~HomerNavigationNode();
-
- /** @brief Is called in constant intervals. */
- void idleProcess();
-
-
- protected:
- /** @brief Handles incoming messages. */
- void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
- void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
- void laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
- void downlaserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
- void startNavigationCallback(const homer_mapnav_msgs::StartNavigation::ConstPtr& msg);
- void moveBaseSimpleGoalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
- void stopNavigationCallback(const std_msgs::Empty::ConstPtr& msg);
-
- void navigateToPOICallback(const homer_mapnav_msgs::NavigateToPOI::ConstPtr& msg);
- void unknownThresholdCallback(const std_msgs::Int8::ConstPtr& msg);
- void maxDepthMoveDistanceCallback(const std_msgs::Float32::ConstPtr& msg);
-
-
- /** @brief initializes and refreshs parameters */
- void loadParameters();
-
- /** @brief Is called when all modules are loaded and thread has started. */
- virtual void init();
-
- /** @brief Detect & handle possible collision */
- void handleCollision();
-
- private:
- /** @brief Start navigation to m_Target on last_map_data_ */
- void startNavigation();
-
- /** @brief Check if obstacles are blocking the way in last_map_data_ */
- bool checkPath();
-
- /** @brief calculate path from current robot position to target approximation */
- void calculatePath();
-
- /** @brief Send message containing current navigation path */
- void sendPathData();
-
- /** @brief Sends target reached and stops the robot. */
- void sendTargetReachedMsg();
-
- /**
- * @brief Sends a target unreachable with given reason and stops the robot.
- * @param reason reason for unreachable target (see homer_mapnav_msgs::TargetUnreachable for possible reasons)
- */
- void sendTargetUnreachableMsg( int8_t reason );
-
- /** @brief reloads all params from the parameterserver */
- void refreshParamsCallback(const std_msgs::Empty::ConstPtr& msg);
-
- /** @brief Navigate robot to next waypoint */
- void performNextMove();
-
- /** @brief Finishes navigation or starts turning to target direction if the target position has been reached */
- void targetPositionReached();
-
- /** @return Angle from robot_pose_ to point in degrees */
- int angleToPointDeg(geometry_msgs::Point point);
-
- /** @brief Calculates current maximal backwards distance on map Data */
- bool backwardObstacle();
-
- /** @brief stops the Robot */
- void stopRobot();
-
- /**
- * @brief Sets each cell of the map to -1 outside the bounding box
- * containing the robot pose and the current target
- */
- void maskMap();
-
- /**
- * @brief Current path was finished (either successful or not),
- * sets state machine to path planning to check if the robot is already at the goal
- */
- void currentPathFinished();
-
- //convenience math functions
- /**
- * Computes minimum turn angle from angle 1 to angle 2
- * @param angle1 from angle
- * @param angle2 to angle
- * @return minimal angle needed to turn from angle 1 to angle 2 [-Pi..Pi]
- */
- static float minTurnAngle ( float angle1, float angle2 );
-
- /**
- * converts value from degree to radiant
- * @param deg Value in degree
- * @return value in radiants
- */
- static float deg2Rad ( float deg ) { return deg / 180.0*M_PI; }
-
- /**
- * converts value from radiants to degrees
- * @param rad Value in radiants
- * @return value in degrees
- */
- static float rad2Deg ( float rad ) { return rad / M_PI*180.0; }
-
- bool drawPolygon ( std::vector< geometry_msgs::Point > vertices);
- void drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value );
- bool fillPolygon ( std::vector<int> &data, int x, int y, int value );
-
- /** @brief calcs the maximal move distance from Laser and DepthData */
- void calcMaxMoveDist();
-
- /// @brief Worker instances
- Explorer* m_explorer;
-
- /// @brief State machine
- StateMachine<ProcessState> m_MainMachine;
-
- /// @brief Navigation options & data
-
- /** list of waypoints subsampled from m_PixelPath */
- std::vector<geometry_msgs::PoseStamped> m_waypoints;
-
- /** Path planned by Explorer, pixel accuracy */
- std::vector<Eigen::Vector2i> m_pixel_path;
-
- /** target point */
- geometry_msgs::Point m_target_point;
-
- /** target name if called via Navigate_to_POI */
- std::string m_target_name;
-
- /** orientation the robot should have at the target point */
- double m_target_orientation;
-
- /** allowed distance to target */
- float m_desired_distance;
-
- /** check if the final turn should be skipped */
- bool m_skip_final_turn;
-
- /**
- * check if navigation should perform fast planning. In this mode a path is only planned within
- * a bounding box containing the robot pose and the target point
- */
- bool m_fast_path_planning;
-
- /** current pose of the robot */
- geometry_msgs::Pose m_robot_pose;
-
- /** last pose of the robot */
- geometry_msgs::Pose m_robot_last_pose;
-
- /** time stamp of the last incoming laser scan */
- ros::Time m_last_laser_time;
- /** time stamp of the last incoming pose */
- ros::Time m_last_pose_time;
-
- /** Distance factor of a frontier cell considered save for exploration */
- float m_FrontierSafenessFactor;
-
- double m_SafePathWeight;
-
- ///map parameters
- double m_resolution;
- double m_width;
- double m_height;
- geometry_msgs::Pose m_origin;
-
- /// @brief Configuration parameters
-
- /** Allowed distances of obstacles to robot. Robot must move within these bounds */
- std::pair<float,float> m_AllowedObstacleDistance;
-
- /** Safe distances of obstacles to robot. If possible, robot should move within these bounds */
- std::pair<float,float> m_SafeObstacleDistance;
-
- /** threshold to sample down waypoints */
- float m_waypoint_sampling_threshold;
-
- float m_max_move_distance;
- float m_max_move_sick;
- float m_max_move_down;
- float m_max_move_depth;
-
- /** if distance to nearest obstacle is below collision distance trigger collision avoidance */
- float m_collision_distance;
-
- /** if distance to nearest obstacle is below collision distance don't drive backwards */
- float m_backward_collision_distance;
- /** do not drive back in collision avoidance when this near target */
- float m_collision_distance_near_target;
-
- /** if true, obstacles in path will be detected and path will be replanned */
- bool m_check_path;
-
- /** waypoints will only be checked for obstacles if they are closer than check_path_max_distance to robot */
- float m_check_path_max_distance;
-
- bool m_avoided_collision;
-
- float m_min_turn_angle;
- float m_max_turn_speed;
- float m_min_turn_speed;
- float m_max_move_speed;
- float m_max_drive_angle;
- float m_waypoint_radius_factor;
-
- float m_distance_to_target;
- float m_act_speed;
- float m_angular_avoidance;
-
- float m_map_speed_factor;
- float m_waypoint_speed_factor;
- float m_obstacle_speed_factor;
- float m_target_distance_speed_factor;
-
- float m_min_y;
- float m_min_x;
-
- float m_callback_error_duration;
-
- bool m_last_check_path_res;
- bool m_use_ptu;
- bool m_new_target;
-
- bool m_path_reaches_target;
- int m_last_calculations_failed;
- int m_unknown_threshold;
-
- /** last map data */
- std::vector<int8_t> * m_last_map_data;
-
- //ros specific members
- tf::TransformListener m_transform_listener;
-
- //subscribers
- ros::Subscriber m_map_sub;
- ros::Subscriber m_pose_sub;
- ros::Subscriber m_laser_data_sub;
- ros::Subscriber m_down_laser_data_sub;
- ros::Subscriber m_laser_back_data_sub;
- ros::Subscriber m_start_navigation_sub;
- ros::Subscriber m_stop_navigation_sub;
- ros::Subscriber m_navigate_to_poi_sub;
- ros::Subscriber m_unknown_threshold_sub;
- ros::Subscriber m_refresh_param_sub;
- ros::Subscriber m_max_move_depth_sub;
- ros::Subscriber m_move_base_simple_goal_sub;
-
- //publishers
- ros::Publisher m_cmd_vel_pub;
- ros::Publisher m_target_reached_string_pub;
- //ros::Publisher m_target_reached_empty_pub;
- ros::Publisher m_target_unreachable_pub;
- ros::Publisher m_path_pub;
- ros::Publisher m_ptu_center_world_point_pub;
- ros::Publisher m_set_pan_tilt_pub;
- ros::Publisher m_debug_pub;
-
- //service clients
- ros::ServiceClient m_get_POIs_client;
-};
-#endif
diff --git a/homer_navigation/launch/homer_navigation.launch b/homer_navigation/launch/homer_navigation.launch
deleted file mode 100644
index db217818f217c26ead905d64d0bb2e7523d1a821..0000000000000000000000000000000000000000
--- a/homer_navigation/launch/homer_navigation.launch
+++ /dev/null
@@ -1,4 +0,0 @@
-<launch>
- <rosparam command="load" file="$(find homer_navigation)/config/homer_navigation.yaml"/>
- <node ns="/homer_navigation" name="homer_navigation" pkg="homer_navigation" type="homer_navigation" output="screen"/>
-</launch>
diff --git a/homer_navigation/launch/homer_navigation_pioneer.launch b/homer_navigation/launch/homer_navigation_pioneer.launch
deleted file mode 100644
index df3c3ac558e5200edf361ccbc38f712c92fda251..0000000000000000000000000000000000000000
--- a/homer_navigation/launch/homer_navigation_pioneer.launch
+++ /dev/null
@@ -1,4 +0,0 @@
-<launch>
- <rosparam command="load" file="$(find homer_navigation)/config/homer_navigation_pioneer.yaml"/>
- <node ns="/homer_navigation" name="homer_navigation" pkg="homer_navigation" type="homer_navigation" output="screen"/>
-</launch>
diff --git a/homer_navigation/package.xml b/homer_navigation/package.xml
deleted file mode 100644
index 5752245cbc96c187ec0bc79a3b87078aef206aee..0000000000000000000000000000000000000000
--- a/homer_navigation/package.xml
+++ /dev/null
@@ -1,36 +0,0 @@
-<?xml version="1.0"?>
-<package>
- <name>homer_navigation</name>
- <version>0.1.1</version>
- <description>The homer_navigation package</description>
-
- <maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
- <author email="mknauf@uni-koblenz.de">Malte Knauf</author>
- <author email="fpolster@uni-koblenz.de">Florian Polster</author>
- <license>GPLv3</license>
-
- <buildtool_depend>catkin</buildtool_depend>
-
- <build_depend>roscpp</build_depend>
- <build_depend>roslib</build_depend>
- <build_depend>homer_robbie_architecture</build_depend>
- <build_depend>homer_nav_libs</build_depend>
- <build_depend>nav_msgs</build_depend>
- <build_depend>sensor_msgs</build_depend>
- <build_depend>homer_mapnav_msgs</build_depend>
- <build_depend>std_msgs</build_depend>
- <build_depend>tf</build_depend>
- <build_depend>cmake_modules</build_depend>
- <build_depend>eigen</build_depend>
-
- <run_depend>roscpp</run_depend>
- <run_depend>roslib</run_depend>
- <run_depend>homer_robbie_architecture</run_depend>
- <run_depend>homer_nav_libs</run_depend>
- <run_depend>nav_msgs</run_depend>
- <run_depend>sensor_msgs</run_depend>
- <run_depend>homer_mapnav_msgs</run_depend>
- <run_depend>std_msgs</run_depend>
- <run_depend>tf</run_depend>
-
-</package>
diff --git a/homer_navigation/readme.pdf b/homer_navigation/readme.pdf
deleted file mode 100644
index 133628daa3fde419c7bc018c0ea9183625cce4c6..0000000000000000000000000000000000000000
Binary files a/homer_navigation/readme.pdf and /dev/null differ
diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
deleted file mode 100644
index 9bad6a762fae86b98ca6c09892e27c66758838fd..0000000000000000000000000000000000000000
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ /dev/null
@@ -1,1084 +0,0 @@
-#include "homer_navigation/homer_navigation_node.h"
-
-HomerNavigationNode::HomerNavigationNode()
-{
- ros::NodeHandle nh;
-
- //subscribers
- m_map_sub = nh.subscribe<nav_msgs::OccupancyGrid>("/map", 1, &HomerNavigationNode::mapCallback, this);
- m_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/pose", 1, &HomerNavigationNode::poseCallback, this);
- m_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1, &HomerNavigationNode::laserDataCallback, this);
- m_down_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>("/front_scan", 1, &HomerNavigationNode::downlaserDataCallback, this);
- m_start_navigation_sub = nh.subscribe<homer_mapnav_msgs::StartNavigation>("/homer_navigation/start_navigation", 1, &HomerNavigationNode::startNavigationCallback, this);
- m_move_base_simple_goal_sub = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &HomerNavigationNode::moveBaseSimpleGoalCallback, this); // for RVIZ usage
- m_stop_navigation_sub = nh.subscribe<std_msgs::Empty>("/homer_navigation/stop_navigation", 1, &HomerNavigationNode::stopNavigationCallback, this);
- m_navigate_to_poi_sub = nh.subscribe<homer_mapnav_msgs::NavigateToPOI>("/homer_navigation/navigate_to_POI", 1, &HomerNavigationNode::navigateToPOICallback, this);
- m_unknown_threshold_sub = nh.subscribe<std_msgs::Int8>("/homer_navigation/unknown_threshold", 1, &HomerNavigationNode::unknownThresholdCallback, this);
- m_refresh_param_sub = nh.subscribe<std_msgs::Empty>("/homer_navigation/refresh_params", 1, &HomerNavigationNode::refreshParamsCallback, this);
- m_max_move_depth_sub = nh.subscribe<std_msgs::Float32>("/homer_navigation/max_depth_move_distance", 1, &HomerNavigationNode::maxDepthMoveDistanceCallback, this);
-
- m_cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 1);
- m_target_reached_string_pub = nh.advertise<std_msgs::String>("/homer_navigation/target_reached", 1);
- //m_target_reached_empty_pub = nh.advertise<std_msgs::Empty>("/homer_navigation/target_reached", 1);
- m_target_unreachable_pub = nh.advertise<homer_mapnav_msgs::TargetUnreachable>("/homer_navigation/target_unreachable", 1);
- m_path_pub = nh.advertise<nav_msgs::Path>("/homer_navigation/path", 1);
- m_ptu_center_world_point_pub= nh.advertise<homer_ptu_msgs::CenterWorldPoint>( "/ptu/center_world_point", 1);
- m_set_pan_tilt_pub = nh.advertise<homer_ptu_msgs::SetPanTilt>("/ptu/set_pan_tilt", 1);
- m_debug_pub = nh.advertise<std_msgs::String>("/homer_navigation/debug",1);
-
- m_get_POIs_client = nh.serviceClient<homer_mapnav_msgs::GetPointsOfInterest>("/map_manager/get_pois");
-
- m_MainMachine.setName( "HomerNavigation Main" );
- ADD_MACHINE_STATE( m_MainMachine, IDLE );
- ADD_MACHINE_STATE( m_MainMachine, AWAITING_PATHPLANNING_MAP );
- ADD_MACHINE_STATE( m_MainMachine, FOLLOWING_PATH );
- ADD_MACHINE_STATE( m_MainMachine, AVOIDING_COLLISION );
- ADD_MACHINE_STATE( m_MainMachine, FINAL_TURN );
-
- init();
-}
-
-void HomerNavigationNode::loadParameters()
-{
- //Explorer constructor
- ros::param::param("/homer_mapping/resolution", m_resolution, (double) 0.05);
- ros::param::param("/homer_navigation/allowed_obstacle_distance/min", m_AllowedObstacleDistance.first, (float) 0.3);
- ros::param::param("/homer_navigation/allowed_obstacle_distance/max", m_AllowedObstacleDistance.second, (float) 5.0);
- ros::param::param("/homer_navigation/safe_obstacle_distance/min", m_SafeObstacleDistance.first, (float) 0.7);
- ros::param::param("/homer_navigation/safe_obstacle_distance/max", m_SafeObstacleDistance.second, (float) 1.5);
- ros::param::param("/homer_navigation/frontier_safeness_factor", m_FrontierSafenessFactor, (float) 1.4);
- ros::param::param("/homer_navigation/safe_path_weight", m_SafePathWeight, (double) 1.2);
- ros::param::param("/homer_navigation/waypoint_sampling_threshold", m_waypoint_sampling_threshold, (float) 1.5);
- m_AllowedObstacleDistance.first /= m_resolution;
- m_AllowedObstacleDistance.second /= m_resolution;
- m_SafeObstacleDistance.first /= m_resolution;
- m_SafeObstacleDistance.second /= m_resolution;
-
- //check path
- ros::param::param("/homer_navigation/check_path", m_check_path, (bool) true);
- ros::param::param("/homer_navigation/check_path_max_distance", m_check_path_max_distance, (float) 2.0);
-
- //collision
- ros::param::param("/homer_navigation/collision_distance", m_collision_distance, (float) 0.3);
- ros::param::param("/homer_navigation/collision_distance_near_target", m_collision_distance_near_target, (float) 0.2);
- ros::param::param("/homer_navigation/backward_collision_distance", m_backward_collision_distance, (float) 0.5);
-
- //cmd_vel config values
- ros::param::param("/homer_navigation/min_turn_angle", m_min_turn_angle, (float) 0.15);
- ros::param::param("/homer_navigation/max_turn_speed", m_max_turn_speed, (float) 0.6);
- ros::param::param("/homer_navigation/min_turn_speed", m_min_turn_speed, (float) 0.3);
- ros::param::param("/homer_navigation/max_move_speed", m_max_move_speed, (float) 0.4);
- ros::param::param("/homer_navigation/max_drive_angle", m_max_drive_angle, (float) 0.6);
-
- //caution factors
- ros::param::param("/homer_navigation/map_speed_factor", m_map_speed_factor, (float) 1.0);
- ros::param::param("/homer_navigation/waypoint_speed_factor", m_waypoint_speed_factor, (float) 1.0);
- ros::param::param("/homer_navigation/obstacle_speed_factor", m_obstacle_speed_factor, (float) 1.0);
- ros::param::param("/homer_navigation/target_distance_speed_factor", m_target_distance_speed_factor, (float) 0.4);
-
- //robot dimensions
- ros::param::param("/homer_navigation/min_x", m_min_x, (float) 0.3);
- ros::param::param("/homer_navigation/min_y", m_min_y, (float) 0.27);
-
- //error durations
- ros::param::param("/homer_navigation/callback_error_duration", m_callback_error_duration, (float) 0.3);
-
- ros::param::param("/homer_navigation/use_ptu", m_use_ptu, (bool) false);
-
- ros::param::param("/homer_navigation/unknown_threshold", m_unknown_threshold, (int) 50);
- ros::param::param("/homer_navigation/waypoint_radius_factor", m_waypoint_radius_factor, (float) 0.25);
-}
-
-void HomerNavigationNode::init()
-{
- m_max_move_sick = 40.0;
- m_max_move_down = 40.0;
- m_max_move_depth = 40.0;
- m_robot_pose.position.x = 0.0;
- m_robot_pose.position.y = 0.0;
- m_robot_pose.position.z = 0.0;
- m_robot_pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
- m_robot_last_pose = m_robot_pose;
- m_avoided_collision = false;
- m_act_speed = 0;
- m_angular_avoidance = 0;
- m_last_calculations_failed = 0;
- m_last_check_path_res = false;
- m_new_target = true;
-
- loadParameters();
-
- m_explorer = new Explorer ( m_AllowedObstacleDistance.first, m_AllowedObstacleDistance.second,
- m_SafeObstacleDistance.first, m_SafeObstacleDistance.second,
- m_SafePathWeight, m_FrontierSafenessFactor, m_unknown_threshold );
- m_last_map_data = new std::vector<int8_t>(0);
-
- m_MainMachine.setState ( IDLE );
-}
-
-HomerNavigationNode::~HomerNavigationNode()
-{
- if(m_explorer)
- {
- delete m_explorer;
- }
- if(m_last_map_data)
- {
- delete m_last_map_data;
- }
-}
-
-void HomerNavigationNode::stopRobot()
-{
- m_act_speed = 0;
- geometry_msgs::Twist cmd_vel_msg;
- cmd_vel_msg.linear.x = 0;
- cmd_vel_msg.linear.y = 0;
- cmd_vel_msg.linear.z = 0;
- cmd_vel_msg.angular.x = 0;
- cmd_vel_msg.angular.y = 0;
- cmd_vel_msg.angular.z = 0;
- m_cmd_vel_pub.publish(cmd_vel_msg);
- ros::Duration(0.1).sleep();
- m_cmd_vel_pub.publish(cmd_vel_msg);
- ros::Duration(0.1).sleep();
- m_cmd_vel_pub.publish(cmd_vel_msg);
-}
-
-void HomerNavigationNode::idleProcess()
-{
- if ( m_MainMachine.state() == FOLLOWING_PATH )
- {
- if ( (ros::Time::now() - m_last_laser_time) > ros::Duration( m_callback_error_duration ) )
- {
- ROS_ERROR_STREAM("Laser data timeout!\n");
- stopRobot();
- }
- if ( (ros::Time::now() - m_last_pose_time) > ros::Duration( m_callback_error_duration ) )
- {
- ROS_ERROR_STREAM("Pose timeout!\n");
- stopRobot();
- }
- }
-}
-
-void HomerNavigationNode::calculatePath()
-{
- if ( m_distance_to_target < m_desired_distance && !m_new_target)
- {
- m_path_reaches_target = true;
- return;
- }
- m_explorer->setOccupancyMap(m_width, m_width, m_origin, &(*m_last_map_data)[0]);
- m_explorer->setStart( map_tools::toMapCoords( m_robot_pose.position, m_origin, m_resolution) );
-
- bool success;
- m_pixel_path = m_explorer->getPath( success );
- if ( !success)
- {
- ROS_WARN_STREAM("No path found for navigation" );
- m_last_calculations_failed++;
- ROS_INFO_STREAM("last_calculation_failed: "<<m_last_calculations_failed);
- if(m_last_calculations_failed > 8)
- {
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
- }
- else
- {
- m_last_calculations_failed = 0;
- std::vector<Eigen::Vector2i> waypoint_pixels = m_explorer->sampleWaypointsFromPath ( m_pixel_path, m_waypoint_sampling_threshold );
- m_waypoints.clear();
- ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << waypoint_pixels.size());
- if(waypoint_pixels.size() > 0)
- {
- for(std::vector<Eigen::Vector2i>::iterator it = waypoint_pixels.begin(); it != waypoint_pixels.end(); ++it)
- {
- geometry_msgs::PoseStamped poseStamped;
- poseStamped.header.frame_id = "/map";
- poseStamped.pose.position = map_tools::fromMapCoords(*it, m_origin, m_resolution);
- poseStamped.pose.orientation.x = 0.0;
- poseStamped.pose.orientation.y = 0.0;
- poseStamped.pose.orientation.z = 0.0;
- poseStamped.pose.orientation.w = 1.0;
- m_waypoints.push_back(poseStamped);
- }
- sendPathData();
- }
- else
- {
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
-
- m_last_laser_time = ros::Time::now();
- m_last_pose_time = ros::Time::now();
- }
-}
-
-void HomerNavigationNode::startNavigation()
-{
- if ( m_distance_to_target < m_desired_distance && !m_new_target)
- {
- ROS_INFO_STREAM( "Will not (re-)plan path: Target position already reached." );
- m_path_reaches_target = true;
- targetPositionReached();
- return;
- }
- ROS_INFO_STREAM( "Distance to target still too large (" << m_distance_to_target << "m; requested: " << m_desired_distance << "m)" );
-
- if(m_fast_path_planning)
- {
- maskMap();
- }
-
- m_explorer->setOccupancyMap(m_width, m_width, m_origin, &(*m_last_map_data)[0]);
- m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution));
- Eigen::Vector2i new_target_approx = m_explorer->getNearestAccessibleTarget(map_tools::toMapCoords(m_target_point, m_origin, m_resolution));
-
- geometry_msgs::Point new_target_approx_world = map_tools::fromMapCoords(new_target_approx, m_origin, m_resolution);
- ROS_INFO_STREAM("start Navigation: Approx target: " << new_target_approx_world);
-
- m_path_reaches_target = ( map_tools::distance(new_target_approx_world, m_target_point ) < m_desired_distance );
-
- bool new_approx_is_better = ( map_tools::distance( m_robot_pose.position, m_target_point ) - map_tools::distance( new_target_approx_world, m_target_point ) ) > 2 * m_desired_distance;
- if ( !new_approx_is_better && !m_path_reaches_target )
- {
- ROS_WARN_STREAM( "No better way to target found, turning and then reporting as unreachable."
- << std::endl << "Distance to target: " << m_distance_to_target << "m; requested: " << m_desired_distance << "m" );
- m_MainMachine.setState( FINAL_TURN );
- }
- else
- {
- m_explorer->setTarget ( new_target_approx);
- m_MainMachine.setState ( FOLLOWING_PATH );
- calculatePath();
- }
-}
-
-void HomerNavigationNode::sendPathData()
-{
- nav_msgs::Path msg;
- msg.header.frame_id = "/map";
- msg.header.stamp = ros::Time::now();
- if(m_waypoints.size() > 0)
- {
- msg.poses = m_waypoints;
- geometry_msgs::PoseStamped pose_stamped;
- pose_stamped.pose = m_robot_pose;
- pose_stamped.header.frame_id = "/map";
- pose_stamped.header.stamp = ros::Time::now();
- msg.poses.insert(msg.poses.begin(), pose_stamped);
- }
- m_path_pub.publish(msg);
-}
-
-void HomerNavigationNode::sendTargetReachedMsg()
-{
- stopRobot();
- m_MainMachine.setState( IDLE );
- std_msgs::String reached_string_msg;
- reached_string_msg.data = m_target_name;
- m_target_reached_string_pub.publish(reached_string_msg);
- m_waypoints.clear();
- nav_msgs::Path empty_path_msg;
- empty_path_msg.poses = m_waypoints;
- m_path_pub.publish(empty_path_msg);
- ROS_INFO_STREAM("=== Reached Target " << m_target_name << " ===");
-}
-
-void HomerNavigationNode::sendTargetUnreachableMsg( int8_t reason )
-{
- stopRobot();
- m_MainMachine.setState( IDLE );
- homer_mapnav_msgs::TargetUnreachable unreachable_msg;
- unreachable_msg.reason = reason;
- m_target_unreachable_pub.publish(unreachable_msg);
- m_waypoints.clear();
- nav_msgs::Path empty_path_msg;
- empty_path_msg.poses = m_waypoints;
- m_path_pub.publish(empty_path_msg);
- ROS_INFO_STREAM("=== TargetUnreachableMsg ===");
-}
-
-void HomerNavigationNode::targetPositionReached()
-{
- ROS_INFO_STREAM( "Target position reached. Distance to target: " << m_distance_to_target << "m. Desired distance:" << m_desired_distance << "m" );
- stopRobot();
- m_waypoints.clear();
- sendPathData();
- m_MainMachine.setState( FINAL_TURN );
- ROS_INFO_STREAM("Turning to look-at point");
-}
-
-bool HomerNavigationNode::checkPath()
-{
- if(m_pixel_path.size() != 0)
- {
- for ( unsigned i = 0; i < m_pixel_path.size()-1; i++ )
- {
- geometry_msgs::Point p = map_tools::fromMapCoords(m_pixel_path.at(i), m_origin, m_resolution);
- if ( map_tools::distance( m_robot_pose.position, p) > m_check_path_max_distance )
- {
- return true;
- }
- for(int a = 0; a < 5; a++)
- {
- if (map_tools::findValue( m_last_map_data, m_width, m_height,
- m_pixel_path[i].x() + (m_pixel_path[i+1].x() - m_pixel_path[i].x()) * a/4,
- m_pixel_path[i].y() + (m_pixel_path[i+1].y() - m_pixel_path[i].y()) * a/4,
- 90, 2 ))
- {
- ROS_WARN_STREAM("Obstacle detected in current path recalculating" );
- return false;
- }
- }
- }
- }
-}
-
-void HomerNavigationNode::handleCollision ()
-{
- if ( m_MainMachine.state() == FOLLOWING_PATH )
- {
- stopRobot();
- m_MainMachine.setState( AVOIDING_COLLISION );
- ROS_WARN_STREAM( "Collision detected while following path!" );
- }
-}
-
-void HomerNavigationNode::performNextMove()
-{
- if(m_MainMachine.state() == FOLLOWING_PATH )
- {
- if ( m_distance_to_target < m_desired_distance && !m_new_target)
- {
- ROS_INFO_STREAM( "Desired distance to target was reached." );
- targetPositionReached();
- return;
- }
- //if we have accidentaly skipped waypoints, recalculate path
- float minDistance = FLT_MAX;
- float distance;
- unsigned nearestWaypoint=0;
- for ( unsigned i=0; i<m_waypoints.size(); i++ )
- {
- distance = map_tools::distance( m_robot_pose.position, m_waypoints[i].pose.position );
- if ( distance < minDistance ) {
- nearestWaypoint = i;
- minDistance = distance;
- }
- }
- if ( nearestWaypoint != 0 )
- {
- //if the target is nearer than the waypoint don't recalculate
- if(m_waypoints.size() != 2)
- {
- ROS_WARN_STREAM("Waypoints skipped. Recalculating path!");
- calculatePath();
- if ( m_MainMachine.state() != FOLLOWING_PATH )
- {
- return;
- }
- }
- else
- {
- m_waypoints.erase(m_waypoints.begin());
- }
- }
-
- Eigen::Vector2i waypointPixel = map_tools::toMapCoords(m_waypoints[0].pose.position, m_origin, m_resolution);
- float obstacleDistanceMap = m_explorer->getObstacleTransform()->getValue( waypointPixel.x(), waypointPixel.y() ) * m_resolution;
- float waypointRadius = m_waypoint_radius_factor * obstacleDistanceMap;
-
- if ( ( waypointRadius < m_resolution ) || ( m_waypoints.size() == 1 ) )
- {
- waypointRadius = m_resolution;
- }
-
- if(m_waypoints.size() != 0)
- {
- //calculate distance between last_pose->current_pose and waypoint
- Eigen::Vector2f line; //direction of the line
- line[0] = m_robot_pose.position.x - m_robot_last_pose.position.x;
- line[1] = m_robot_pose.position.y - m_robot_last_pose.position.y;
- Eigen::Vector2f point_to_start; //vector from the point to the start of the line
- point_to_start[0] = m_robot_last_pose.position.x - m_waypoints[0].pose.position.x;
- point_to_start[1] = m_robot_last_pose.position.y - m_waypoints[0].pose.position.y;
- float dot = point_to_start[0] * line[0] + point_to_start[1] * line[1]; //dot product of point_to_start * line
- Eigen::Vector2f distance; //shortest distance vector from point to line
- distance[0] = point_to_start[0] - dot * line[0];
- distance[1] = point_to_start[1] - dot * line[1];
- float distance_to_waypoint = sqrt((double)(distance[0] * distance[0] + distance[1] * distance[1]));
-
- //check if current waypoint has been reached
- if((distance_to_waypoint < waypointRadius && m_waypoints.size() > 1) || (m_distance_to_target < waypointRadius))
- {
- m_waypoints.erase ( m_waypoints.begin() );
- }
-
- }
-
- sendPathData();
- //last wayoint reached
- if ( m_waypoints.size() == 0 )
- {
- ROS_INFO_STREAM("Last waypoint reached");
- currentPathFinished();
- return;
- }
-
- if(m_use_ptu)
- {
- ROS_INFO_STREAM("Moving PTU to center next Waypoint");
- homer_ptu_msgs::CenterWorldPoint ptu_msg;
- ptu_msg.point.x = m_waypoints[0].pose.position.x;
- ptu_msg.point.y = m_waypoints[0].pose.position.y;
- ptu_msg.point.z = 0;
- ptu_msg.permanent = true;
- m_ptu_center_world_point_pub.publish(ptu_msg);
- }
-
- float distanceToWaypoint = map_tools::distance( m_robot_pose.position, m_waypoints[0].pose.position );
- float angleToWaypoint = angleToPointDeg ( m_waypoints[0].pose.position );
- if (angleToWaypoint < -180)
- {
- angleToWaypoint += 360;
- }
- float angle = deg2Rad( angleToWaypoint );
-
- //linear speed calculation
- if(m_avoided_collision)
- {
- if( std::abs(angleToWaypoint) < 10)
- {
- m_avoided_collision = false;
- }
- }
- else if (abs(angle) > m_max_drive_angle)
- {
- m_act_speed = 0.0;
- }
- else
- {
- float obstacleMapDistance = 1;
- for(int wpi = -1; wpi < std::min((int)m_waypoints.size(),(int) 2); wpi++)
- {
- Eigen::Vector2i robotPixel;
- if(wpi == -1)
- {
- robotPixel = map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution);
- }
- else
- {
- robotPixel = map_tools::toMapCoords(m_waypoints[wpi].pose.position, m_origin, m_resolution);
- }
- obstacleMapDistance = std::min((float)obstacleMapDistance,(float)( m_explorer->getObstacleTransform()->getValue( robotPixel.x(), robotPixel.y() ) * m_resolution));
- }
-
- float max_move_distance_speed = m_max_move_speed * m_max_move_distance * m_obstacle_speed_factor;
- float max_map_distance_speed = m_max_move_speed * obstacleMapDistance * m_map_speed_factor;
- m_act_speed = std::min(std::max((float)0.1,m_distance_to_target * m_target_distance_speed_factor) ,std::min(std::min(m_max_move_speed, max_move_distance_speed), std::min(max_map_distance_speed, distanceToWaypoint * m_waypoint_speed_factor)));
- std_msgs::String tmp;
- std::stringstream str;
- str << "m_obstacle_speed " << max_move_distance_speed <<" max_map_distance_speed "<<max_map_distance_speed;
- tmp.data = str.str();
- m_debug_pub.publish(tmp);
- }
-
- //angular speed calculation
- if (angle < 0 )
- {
- angle = std::max(angle * (float) 0.8, -m_max_turn_speed);
- m_act_speed = m_act_speed + angle / 2.0;
- if(m_act_speed < 0 )
- {
- m_act_speed = 0;
- }
- }
- else
- {
- angle = std::min(angle * (float) 0.8, m_max_turn_speed);
- m_act_speed = m_act_speed - angle / 2.0;
- if(m_act_speed < 0 )
- {
- m_act_speed = 0;
- }
- }
- geometry_msgs::Twist cmd_vel_msg;
- cmd_vel_msg.linear.x = m_act_speed;
- cmd_vel_msg.angular.z = angle;
- m_cmd_vel_pub.publish(cmd_vel_msg);
-
- ROS_DEBUG_STREAM("Driving & turning" << std::endl << "linear: " << m_act_speed << " angular: " << angle << std::endl
- << "distanceToWaypoint:" << distanceToWaypoint << "angleToWaypoint: " << angleToWaypoint << std::endl );
- }
- else if(m_MainMachine.state()== AVOIDING_COLLISION)
- {
- if( m_distance_to_target < m_desired_distance && !m_new_target)
- {
- ROS_INFO_STREAM("Collision detected near target. Switch to final turn.");
- targetPositionReached();
- }
- else if ( m_max_move_distance <= m_collision_distance && m_waypoints.size() > 1 || m_max_move_distance <= m_collision_distance_near_target )
- {
- ROS_WARN_STREAM( "Maximum driving distance too short (" << m_max_move_distance << "m)! Moving back." );
- geometry_msgs::Twist cmd_vel_msg;
- if(!HomerNavigationNode::backwardObstacle())
- {
- cmd_vel_msg.linear.x = -0.2;
- }
- else
- {
- if(m_angular_avoidance == 0)
- {
- float angleToWaypoint = angleToPointDeg ( m_waypoints[0].pose.position );
- if(angleToWaypoint < -180)
- {
- angleToWaypoint += 360;
- }
- if (angleToWaypoint < 0 )
- {
- m_angular_avoidance = -0.4;
- }
- else
- {
- m_angular_avoidance = 0.4;
- }
- }
- cmd_vel_msg.angular.z = m_angular_avoidance;
- }
- m_cmd_vel_pub.publish(cmd_vel_msg);
- }
- else
- {
- m_angular_avoidance = 0;
- m_avoided_collision = true;
- ROS_WARN_STREAM( "Collision avoided. Updating path." );
- currentPathFinished();
- }
- }
- else if(m_MainMachine.state()== FINAL_TURN)
- {
- if(m_use_ptu)
- {
- //reset PTU
- homer_ptu_msgs::SetPanTilt msg;
- msg.absolute = true;
- msg.panAngle = 0;
- msg.tiltAngle = 0;
- m_set_pan_tilt_pub.publish(msg);
- }
-
- if ( m_skip_final_turn )
- {
- ROS_INFO_STREAM("Final turn skipped. Target reached.");
- if(m_path_reaches_target)
- {
- sendTargetReachedMsg();
- }
- else
- {
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
- return;
- }
-
- float turnAngle = minTurnAngle( tf::getYaw(m_robot_pose.orientation), m_target_orientation );
- ROS_DEBUG_STREAM("homer_navigation::PerformNextMove:: Final Turn. Robot orientation: " << rad2Deg(tf::getYaw(m_robot_pose.orientation)) << ". Target orientation: " << rad2Deg(m_target_orientation) << "homer_navigation::PerformNextMove:: turnAngle: " << rad2Deg(turnAngle));
-
- if (std::fabs(turnAngle) < m_min_turn_angle)
- {
- ROS_INFO_STREAM(":::::::NEAREST WALKABLE TARGET REACHED BECAUSE lower " << m_min_turn_angle);
- ROS_INFO_STREAM("target angle = "<< m_target_orientation );
- ROS_INFO_STREAM("is angle = "<< tf::getYaw(m_robot_pose.orientation));
- ROS_INFO_STREAM("m_distance_to_target = " << m_distance_to_target);
- ROS_INFO_STREAM("m_desired_distance = " << m_desired_distance);
- if(m_path_reaches_target)
- {
- sendTargetReachedMsg();
- }
- else
- {
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
- return;
- }
- else
- {
- if (turnAngle< 0 )
- {
- turnAngle= std::max(std::min(turnAngle, -m_min_turn_speed ),-m_max_turn_speed);
- }
- else
- {
- turnAngle = std::min(std::max(turnAngle, m_min_turn_speed ),m_max_turn_speed);
- }
- geometry_msgs::Twist cmd_vel_msg;
- cmd_vel_msg.angular.z = turnAngle;
- m_cmd_vel_pub.publish(cmd_vel_msg);
- }
- }
-}
-
-void HomerNavigationNode::currentPathFinished()
-{
- ROS_INFO_STREAM( "Current path was finished, initiating recalculation.");
- m_waypoints.clear();
- stopRobot();
- m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
-}
-
-int HomerNavigationNode::angleToPointDeg ( geometry_msgs::Point target )
-{
- double cx = m_robot_pose.position.x;
- double cy = m_robot_pose.position.y;
- int targetAngle = rad2Deg( atan2 ( target.y - cy, target.x - cx ) );
- int currentAngle = rad2Deg( tf::getYaw(m_robot_pose.orientation) );
-
- int angleDiff = targetAngle - currentAngle;
- angleDiff = (angleDiff + 180) % 360 - 180;
- return angleDiff;
-}
-
-bool HomerNavigationNode::drawPolygon ( std::vector< geometry_msgs::Point > vertices)
-{
- if ( vertices.size() == 0 )
- {
- ROS_INFO_STREAM( "No vertices given!" );
- return false;
- }
- //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, 30);
- }
- //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
- return fillPolygon ( data, (int)midX, (int)midY, 30 );
-}
-
-void HomerNavigationNode::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++ )
- {
- int index = x + m_width* y;
- if(index < 0 || index > data.size())
- {
- continue;
- }
- data[index] = value;
-
- xerr += dx;
- yerr += dy;
- if ( xerr > dist )
- {
- xerr -= dist;
- x += incx;
- }
- if ( yerr > dist )
- {
- yerr -= dist;
- y += incy;
- }
- }
-}
-
-bool HomerNavigationNode::fillPolygon ( std::vector<int> &data, int x, int y, int value )
-{
- int index = x + m_width * y;
- bool tmp = false;
-
- if ((int)m_last_map_data->at(index)> 90)
- {
- tmp = true;
- }
- if ( data[index] != value )
- {
- data[index] = value;
- if (fillPolygon ( data, x + 1, y, value ))
- {
- tmp = true;
- }
- if(fillPolygon ( data, x - 1, y, value ))
- {
- tmp = true;
- }
- if(fillPolygon ( data, x, y + 1, value ))
- {
- tmp = true;
- }
- if(fillPolygon ( data, x, y - 1, value ))
- {
- tmp = true;
- }
- }
- return tmp;
-}
-
-bool HomerNavigationNode::backwardObstacle()
-{
- std::vector<geometry_msgs::Point> vertices;
- geometry_msgs::Point base_link_point;
- geometry_msgs::Point map_point;
- Eigen::Vector2i map_coord;
-
- std::vector<float> x;
- std::vector<float> y;
-
- x.push_back(- m_min_x - m_backward_collision_distance);
- y.push_back(m_min_y);
-
- x.push_back(- m_min_x - m_backward_collision_distance);
- y.push_back(-m_min_y);
-
- x.push_back(-0.1);
- y.push_back(-m_min_y);
-
- x.push_back(-0.1);
- y.push_back(m_min_y);
-
- for(int i = 0; i<x.size(); i++)
- {
- base_link_point.x = x[i];
- base_link_point.y = y[i];
- map_coord = map_tools::toMapCoords(map_tools::transformPoint(base_link_point, m_transform_listener ,"/base_link", "/map"), m_origin, m_resolution);
- map_point.x = map_coord.x();
- map_point.y = map_coord.y();
- vertices.push_back(map_point);
- }
-
- return drawPolygon(vertices);
-}
-
-void HomerNavigationNode::maskMap()
-{
- //generate bounding box
- ROS_INFO_STREAM("Calculating Bounding box for fast planning");
- Eigen::Vector2i pose_pixel = map_tools::toMapCoords( m_robot_pose.position, m_origin, m_resolution);
- Eigen::Vector2i target_pixel = map_tools::toMapCoords( m_target_point, m_origin, m_resolution);
- Eigen::Vector2i safe_pixel_distance(m_AllowedObstacleDistance.first * 4,
- m_AllowedObstacleDistance.first * 4);
- Eigen::AlignedBox2i planning_box;
- planning_box.extend(pose_pixel);
- planning_box.extend(target_pixel);
- ROS_INFO_STREAM("Bounding Box: (" << planning_box.min() << " " << planning_box.max());
- Eigen::AlignedBox2i safe_planning_box(planning_box.min() - safe_pixel_distance, planning_box.max() + safe_pixel_distance);
- ROS_INFO_STREAM("safe Bounding Box: (" << safe_planning_box.min() << " " << safe_planning_box.max());
- ROS_INFO_STREAM("min in m: " <<map_tools::fromMapCoords(safe_planning_box.min(), m_origin, m_resolution));
- ROS_INFO_STREAM("max in m: "<<map_tools::fromMapCoords(safe_planning_box.max(), m_origin, m_resolution));
- for(size_t x = 0; x < m_width; x++)
- {
- for(size_t y = 0; y < m_width; y++)
- {
- if(!safe_planning_box.contains(Eigen::Vector2i(x, y)))
- {
- m_last_map_data->at(y * m_width + x) = -1;
- }
- }
- }
-}
-
-//convenience math functions
-float HomerNavigationNode::minTurnAngle( float angle1, float angle2 )
-{
- angle1 *= 180.0/M_PI;
- angle2 *= 180.0/M_PI;
-
- int diff= angle2 - angle1;
- diff = (diff + 180) % 360 - 180;
- if (diff < -180)
- {
- diff += 360;
- }
-
- float ret = static_cast<double>(diff) * M_PI/180.0;
- return ret;
-}
-
-void HomerNavigationNode::refreshParamsCallback(const std_msgs::Empty::ConstPtr& msg)
-{
- ROS_INFO_STREAM("Refreshing Parameters");
- loadParameters();
-}
-
-void HomerNavigationNode::mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
-{
- if(msg->info.height != msg->info.width)
- {
- ROS_ERROR_STREAM("Incoming Map not quadratic. No map update!");
- return;
- }
- if(m_last_map_data)
- {
- delete m_last_map_data;
- }
-
- m_last_map_data = new std::vector<int8_t>(msg->data);
- m_origin = msg->info.origin;
- m_width = msg->info.width;
- m_height = msg->info.height;
- m_resolution = msg->info.resolution;
-
- switch ( m_MainMachine.state() )
- {
- case AWAITING_PATHPLANNING_MAP:
- startNavigation();
- break;
- case FOLLOWING_PATH:
- {
- if ( m_check_path)
- {
- if( !checkPath())
- {
- if(!m_last_check_path_res)
- {
- calculatePath();
- }
- m_last_check_path_res = false;
- }
- else
- {
- m_last_check_path_res = true;
- }
- }
- break;
- }
- }
-}
-
-void HomerNavigationNode::poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
-{
- m_robot_last_pose = m_robot_pose;
- m_robot_pose = msg->pose;
- m_last_pose_time = ros::Time::now();
- m_distance_to_target = map_tools::distance( m_robot_pose.position, m_target_point );
- m_new_target = false;
- performNextMove();
-}
-
-void HomerNavigationNode::calcMaxMoveDist()
-{
- m_max_move_distance = std::min (m_max_move_sick, std::min(m_max_move_down, m_max_move_depth));
- if(m_max_move_distance <= m_collision_distance && std::fabs(m_act_speed) > 0.1 && m_waypoints.size() > 1 ||
- m_max_move_distance <= m_collision_distance_near_target && std::fabs(m_act_speed) > 0.1 && m_waypoints.size() == 1 ||
- m_max_move_distance <= 0.1 )
- {
- handleCollision();
- }
-}
-void HomerNavigationNode::maxDepthMoveDistanceCallback(const std_msgs::Float32::ConstPtr& msg)
-{
- m_max_move_depth = msg->data;
- calcMaxMoveDist();
-}
-
-void HomerNavigationNode::laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
-{
- m_last_laser_time = ros::Time::now();
- m_max_move_sick = map_tools::get_max_move_distance ( map_tools::laser_ranges_to_points(msg->ranges, msg->angle_min, msg->angle_increment, msg->range_min, msg->range_max, m_transform_listener, msg->header.frame_id, "/base_link"), m_min_x, m_min_y);
- calcMaxMoveDist();
-}
-
-
-void HomerNavigationNode::downlaserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
-{
- m_max_move_down = map_tools::get_max_move_distance ( map_tools::laser_ranges_to_points(msg->ranges, msg->angle_min, msg->angle_increment, msg->range_min, msg->range_max, m_transform_listener, msg->header.frame_id, "/base_link"), m_min_x, m_min_y);
- calcMaxMoveDist();
-}
-
-void HomerNavigationNode::startNavigationCallback(const homer_mapnav_msgs::StartNavigation::ConstPtr& msg)
-{
- m_avoided_collision = false;
- m_target_point = msg->goal.position;
- m_target_orientation = tf::getYaw(msg->goal.orientation);
- m_desired_distance = msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
- m_skip_final_turn = msg->skip_final_turn;
- m_fast_path_planning = msg->fast_planning;
- m_new_target = true;
- m_target_name = "";
-
- ROS_INFO_STREAM("Navigating to target " << m_target_point.x << ", " << m_target_point.y
- << "\nTarget orientation: " << m_target_orientation
- << "Desired distance to target: " << m_desired_distance);
-
- m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
-}
-
-void HomerNavigationNode::moveBaseSimpleGoalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
-{
- if(msg->header.frame_id != "map")
- {
- tf::StampedTransform transform;
- if(m_transform_listener.waitForTransform("map", msg->header.frame_id, msg->header.stamp, ros::Duration(1.0)))
- {
- try
- {
- m_transform_listener.lookupTransform("map", msg->header.frame_id, msg->header.stamp, transform);
- tf::Vector3 targetPos(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
- targetPos = transform * targetPos;
- m_target_point.x = targetPos.getX();
- m_target_point.y = targetPos.getY();
- m_target_orientation = tf::getYaw(transform*tf::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w));
- }
- catch(tf::TransformException ex)
- {
- ROS_ERROR_STREAM(ex.what());
- return;
- }
- }
- else
- {
- try
- {
- m_transform_listener.lookupTransform("map", msg->header.frame_id, ros::Time(0), transform);
- tf::Vector3 targetPos(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
- targetPos = transform * targetPos;
- m_target_point.x = targetPos.getX();
- m_target_point.y = targetPos.getY();
- m_target_orientation = tf::getYaw(transform*tf::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w));
- }
- catch(tf::TransformException ex)
- {
- ROS_ERROR_STREAM(ex.what());
- return;
- }
- }
- }
- else
- {
- m_target_point = msg->pose.position;
- m_target_orientation = tf::getYaw(msg->pose.orientation);
- }
- m_avoided_collision = false;
- m_desired_distance = 0.1;
- m_skip_final_turn = false;
- m_fast_path_planning = false;
- m_new_target = true;
-
- ROS_INFO_STREAM("Navigating to target via Move Base Simple x: " << m_target_point.x << ", y: " << m_target_point.y
- << "\nTarget orientation: " << m_target_orientation
- << " Desired distance to target: " << m_desired_distance
- << "\nframe_id: " << msg->header.frame_id);
-
- m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
-}
-
-void HomerNavigationNode::navigateToPOICallback(const homer_mapnav_msgs::NavigateToPOI::ConstPtr &msg)
-{
- homer_mapnav_msgs::GetPointsOfInterest srv;
- m_get_POIs_client.call(srv);
- std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
- for(it = srv.response.poi_list.pois.begin(); it != srv.response.poi_list.pois.end(); ++it)
- {
- if(it->name == msg->poi_name)
- {
- m_avoided_collision = false;
- m_target_point = it->pose.position;
- m_target_orientation = tf::getYaw(it->pose.orientation);
- m_desired_distance = msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
- m_skip_final_turn = msg->skip_final_turn;
- m_fast_path_planning = false;
- m_new_target = true;
- m_target_name = msg->poi_name;
-
- ROS_INFO_STREAM("Navigating to target " << m_target_point.x << ", " << m_target_point.y
- << "\nTarget orientation: " << m_target_orientation
- << "Desired distance to target: " << m_desired_distance);
-
- m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
- return;
- }
- }
-
- ROS_ERROR_STREAM("No point of interest with name '" << msg->poi_name << "' found in current poi list");
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::UNKNOWN);
-}
-
-void HomerNavigationNode::stopNavigationCallback(const std_msgs::Empty::ConstPtr& msg)
-{
- ROS_INFO_STREAM("Stopping navigation." );
- m_MainMachine.setState( IDLE );
- m_avoided_collision = false;
- stopRobot();
-
- m_waypoints.clear();
- nav_msgs::Path empty_path_msg;
- empty_path_msg.poses = m_waypoints;
- m_path_pub.publish(empty_path_msg);
-}
-
-void HomerNavigationNode::unknownThresholdCallback(const std_msgs::Int8::ConstPtr &msg)
-{
- m_explorer->setUnknownThreshold(static_cast<int>(msg->data));
-}
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv, "homer_navigation");
-
- HomerNavigationNode node;
-
- ros::Rate rate(50);
-
- while(ros::ok())
- {
- ros::spinOnce();
- node.idleProcess();
- rate.sleep();
- }
-
- return 0;
-}
diff --git a/homer_map_manager/images/rosgraph.png b/images/rosgraph.png
similarity index 100%
rename from homer_map_manager/images/rosgraph.png
rename to images/rosgraph.png
diff --git a/homer_map_manager/images/rosgraph.svg b/images/rosgraph.svg
similarity index 100%
rename from homer_map_manager/images/rosgraph.svg
rename to images/rosgraph.svg
diff --git a/homer_map_manager/include/homer_map_manager/Managers/MapManager.h b/include/homer_map_manager/Managers/MapManager.h
similarity index 100%
rename from homer_map_manager/include/homer_map_manager/Managers/MapManager.h
rename to include/homer_map_manager/Managers/MapManager.h
diff --git a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h b/include/homer_map_manager/Managers/MaskingManager.h
similarity index 100%
rename from homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
rename to include/homer_map_manager/Managers/MaskingManager.h
diff --git a/homer_map_manager/include/homer_map_manager/Managers/PoiManager.h b/include/homer_map_manager/Managers/PoiManager.h
similarity index 100%
rename from homer_map_manager/include/homer_map_manager/Managers/PoiManager.h
rename to include/homer_map_manager/Managers/PoiManager.h
diff --git a/homer_map_manager/include/homer_map_manager/Managers/RoiManager.h b/include/homer_map_manager/Managers/RoiManager.h
similarity index 100%
rename from homer_map_manager/include/homer_map_manager/Managers/RoiManager.h
rename to include/homer_map_manager/Managers/RoiManager.h
diff --git a/homer_map_manager/include/homer_map_manager/MapIO/image_loader.h b/include/homer_map_manager/MapIO/image_loader.h
similarity index 100%
rename from homer_map_manager/include/homer_map_manager/MapIO/image_loader.h
rename to include/homer_map_manager/MapIO/image_loader.h
diff --git a/homer_map_manager/include/homer_map_manager/MapIO/map_loader.h b/include/homer_map_manager/MapIO/map_loader.h
similarity index 100%
rename from homer_map_manager/include/homer_map_manager/MapIO/map_loader.h
rename to include/homer_map_manager/MapIO/map_loader.h
diff --git a/homer_map_manager/include/homer_map_manager/MapIO/map_saver.h b/include/homer_map_manager/MapIO/map_saver.h
similarity index 100%
rename from homer_map_manager/include/homer_map_manager/MapIO/map_saver.h
rename to include/homer_map_manager/MapIO/map_saver.h
diff --git a/homer_map_manager/include/homer_map_manager/Workers/PointOfInterest/PointOfInterest.h b/include/homer_map_manager/Workers/PointOfInterest/PointOfInterest.h
similarity index 100%
rename from homer_map_manager/include/homer_map_manager/Workers/PointOfInterest/PointOfInterest.h
rename to include/homer_map_manager/Workers/PointOfInterest/PointOfInterest.h
diff --git a/homer_map_manager/include/homer_map_manager/map_manager_node.h b/include/homer_map_manager/map_manager_node.h
similarity index 100%
rename from homer_map_manager/include/homer_map_manager/map_manager_node.h
rename to include/homer_map_manager/map_manager_node.h
diff --git a/homer_map_manager/mainpage.dox b/mainpage.dox
similarity index 100%
rename from homer_map_manager/mainpage.dox
rename to mainpage.dox
diff --git a/homer_map_manager/manifest.xml b/manifest.xml
similarity index 100%
rename from homer_map_manager/manifest.xml
rename to manifest.xml
diff --git a/homer_map_manager/package.xml b/package.xml
similarity index 100%
rename from homer_map_manager/package.xml
rename to package.xml
diff --git a/homer_map_manager/readme.pdf b/readme.pdf
similarity index 100%
rename from homer_map_manager/readme.pdf
rename to readme.pdf
diff --git a/homer_map_manager/src/Managers/MapManager.cpp b/src/Managers/MapManager.cpp
similarity index 100%
rename from homer_map_manager/src/Managers/MapManager.cpp
rename to src/Managers/MapManager.cpp
diff --git a/homer_map_manager/src/Managers/MaskingManager.cpp b/src/Managers/MaskingManager.cpp
similarity index 100%
rename from homer_map_manager/src/Managers/MaskingManager.cpp
rename to src/Managers/MaskingManager.cpp
diff --git a/homer_map_manager/src/Managers/PoiManager.cpp b/src/Managers/PoiManager.cpp
similarity index 100%
rename from homer_map_manager/src/Managers/PoiManager.cpp
rename to src/Managers/PoiManager.cpp
diff --git a/homer_map_manager/src/Managers/RoiManager.cpp b/src/Managers/RoiManager.cpp
similarity index 100%
rename from homer_map_manager/src/Managers/RoiManager.cpp
rename to src/Managers/RoiManager.cpp
diff --git a/homer_map_manager/src/MapIO/image_loader.cpp b/src/MapIO/image_loader.cpp
similarity index 100%
rename from homer_map_manager/src/MapIO/image_loader.cpp
rename to src/MapIO/image_loader.cpp
diff --git a/homer_map_manager/src/MapIO/map_loader.cpp b/src/MapIO/map_loader.cpp
similarity index 100%
rename from homer_map_manager/src/MapIO/map_loader.cpp
rename to src/MapIO/map_loader.cpp
diff --git a/homer_map_manager/src/MapIO/map_saver.cpp b/src/MapIO/map_saver.cpp
similarity index 100%
rename from homer_map_manager/src/MapIO/map_saver.cpp
rename to src/MapIO/map_saver.cpp
diff --git a/homer_map_manager/src/Workers/PointOfInterest/CMakeLists.txt b/src/Workers/PointOfInterest/CMakeLists.txt
similarity index 100%
rename from homer_map_manager/src/Workers/PointOfInterest/CMakeLists.txt
rename to src/Workers/PointOfInterest/CMakeLists.txt
diff --git a/homer_map_manager/src/map_manager_node.cpp b/src/map_manager_node.cpp
similarity index 100%
rename from homer_map_manager/src/map_manager_node.cpp
rename to src/map_manager_node.cpp