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 84fbe5a4f43acdc5d8f0963c20d7c3739c182d14..39a46914d0f3b5b945ca0675ead28a7947da2370 100644
--- a/README.md
+++ b/README.md
@@ -1,367 +1,42 @@
-## homer_mapnav (indigo) - 1.0.10-0
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav` on `Tue, 01 Dec 2015 23:59:02 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.9-0`
-- old version: `1.0.9-0`
-- new version: `1.0.10-0`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.10`
-- rosdep version: `0.11.4`
-- rosdistro version: `0.4.2`
-- vcstools version: `0.1.38`
-
-
-## homer_mapnav (indigo) - 1.0.9-0
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav` on `Tue, 01 Dec 2015 17:52:28 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.8-0`
-- old version: `1.0.8-0`
-- new version: `1.0.9-0`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.10`
-- rosdep version: `0.11.4`
-- rosdistro version: `0.4.2`
-- vcstools version: `0.1.38`
-
-
-## homer_mapnav (indigo) - 1.0.8-0
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav` on `Sun, 29 Nov 2015 23:46:05 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.7-0`
-- old version: `1.0.7-0`
-- new version: `1.0.8-0`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.10`
-- rosdep version: `0.11.4`
-- rosdistro version: `0.4.2`
-- vcstools version: `0.1.38`
-
-
-## homer_mapnav (indigo) - 1.0.7-0
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav` on `Sat, 28 Nov 2015 22:58:14 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.6-0`
-- old version: `1.0.6-0`
-- new version: `1.0.7-0`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.10`
-- rosdep version: `0.11.4`
-- rosdistro version: `0.4.2`
-- vcstools version: `0.1.38`
-
-
-## homer_mapnav (indigo) - 1.0.6-0
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav` on `Fri, 27 Nov 2015 14:33:00 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.5-0`
-- old version: `1.0.5-0`
-- new version: `1.0.6-0`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.10`
-- rosdep version: `0.11.4`
-- rosdistro version: `0.4.2`
-- vcstools version: `0.1.38`
-
-
-## homer_mapnav (indigo) - 1.0.5-0
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav` on `Tue, 24 Nov 2015 13:28:24 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.4-0`
-- old version: `1.0.4-0`
-- new version: `1.0.5-0`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.10`
-- rosdep version: `0.11.4`
-- rosdistro version: `0.4.2`
-- vcstools version: `0.1.38`
-
-
-## homer_mapnav (indigo) - 1.0.4-0
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav` on `Fri, 20 Nov 2015 02:07:32 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.2-0`
-- old version: `1.0.2-0`
-- new version: `1.0.4-0`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.10`
-- rosdep version: `0.11.4`
-- rosdistro version: `0.4.2`
-- vcstools version: `0.1.38`
-
-
-## homer_mapnav (indigo) - 1.0.2-0
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav` on `Fri, 20 Nov 2015 00:10:16 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.1-5`
-- old version: `1.0.1-5`
-- new version: `1.0.2-0`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.10`
-- rosdep version: `0.11.4`
-- rosdistro version: `0.4.2`
-- vcstools version: `0.1.38`
-
-
-## homer_mapnav (indigo) - 1.0.1-5
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav --edit` on `Tue, 17 Nov 2015 15:51:44 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.1-2`
-- old version: `1.0.1-4`
-- new version: `1.0.1-5`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.10`
-- rosdep version: `0.11.4`
-- rosdistro version: `0.4.2`
-- vcstools version: `0.1.38`
-
-
-## homer_mapnav (indigo) - 1.0.1-4
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav --edit` on `Tue, 17 Nov 2015 15:42:59 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.1-2`
-- old version: `1.0.1-3`
-- new version: `1.0.1-4`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.10`
-- rosdep version: `0.11.4`
-- rosdistro version: `0.4.2`
-- vcstools version: `0.1.38`
-
-
-## homer_mapnav (indigo) - 1.0.1-3
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav --edit` on `Tue, 17 Nov 2015 15:29:03 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.1-2`
-- old version: `1.0.1-2`
-- new version: `1.0.1-3`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.10`
-- rosdep version: `0.11.4`
-- rosdistro version: `0.4.2`
-- vcstools version: `0.1.38`
-
-
-## homer_mapnav (indigo) - 1.0.1-2
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav --edit` on `Fri, 09 Oct 2015 09:48:20 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.1-1`
-- old version: `1.0.1-1`
-- new version: `1.0.1-2`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.7`
-- rosdep version: `0.11.1`
-- rosdistro version: `0.4.1`
-- vcstools version: `0.1.36`
-
-
-## homer_mapnav (indigo) - 1.0.1-1
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav --edit` on `Fri, 25 Sep 2015 06:07:06 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- rosdistro version: `1.0.1-0`
-- old version: `1.0.1-0`
-- new version: `1.0.1-1`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.7`
-- rosdep version: `0.11.1`
-- rosdistro version: `0.4.1`
-- vcstools version: `0.1.36`
-
-
-## homer_mapnav (indigo) - 1.0.1-0
-
-The packages in the `homer_mapnav` repository were released into the `indigo` distro by running `/usr/bin/bloom-release --rosdistro indigo --track indigo homer_mapnav --edit` on `Tue, 15 Sep 2015 09:11:11 -0000`
-
-These packages were released:
-- `homer_map_manager`
-- `homer_mapnav_msgs`
-- `homer_mapping`
-- `homer_nav_libs`
-- `homer_navigation`
-
-Version of package(s) in repository `homer_mapnav`:
-- upstream repository: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
-- release repository: unknown
-- rosdistro version: `null`
-- old version: `null`
-- new version: `1.0.1-0`
-
-Versions of tools used:
-- bloom version: `0.5.20`
-- catkin_pkg version: `0.2.7`
-- rosdep version: `0.11.1`
-- rosdistro version: `0.4.1`
-- vcstools version: `0.1.36`
-
-
-# mapping and navigation
-
-Here you can find the homer mapping and navigation packages
+# 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/config/custom.xml b/config/custom.xml
similarity index 100%
rename from homer_map_manager/config/custom.xml
rename to config/custom.xml
diff --git a/homer_map_manager/config/default.xml b/config/default.xml
similarity index 100%
rename from homer_map_manager/config/default.xml
rename to config/default.xml
diff --git a/homer_map_manager/config/merged.xml b/config/merged.xml
similarity index 100%
rename from homer_map_manager/config/merged.xml
rename to config/merged.xml
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 0701f2d2368a2eb695a3bbbd3253d6a1d3c0bcaf..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/CHANGELOG.rst
+++ /dev/null
@@ -1,44 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package mapnav_msgs
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.0.11 (2015-12-02)
--------------------
-
-1.0.10 (2015-12-02)
--------------------
-
-1.0.9 (2015-12-01)
-------------------
-
-1.0.8 (2015-11-30)
-------------------
-
-1.0.7 (2015-11-28)
-------------------
-* updated changelog
-* Contributors: Niklas Yann Wettengel
-
-1.0.6 (2015-11-27)
-------------------
-
-1.0.5 (2015-11-24)
-------------------
-
-1.0.4 (2015-11-20)
-------------------
-
-1.0.3 (2015-11-20)
-------------------
-
-1.0.2 (2015-11-20)
-------------------
-* added cmake_modules as build dependency in package.xml
-* added Maintainers
-* added raphael as maintainer
-* Contributors: Niklas Yann Wettengel, Raphael Memmesheimer
-
-1.0.1 (2015-09-08)
-------------------
-* init
-* Contributors: Raphael Memmesheimer
diff --git a/homer_mapnav_msgs/CMakeLists.txt b/homer_mapnav_msgs/CMakeLists.txt
deleted file mode 100644
index c5edc11e334e92dc5d95adbf5abdc1355be0822a..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
- DeletePointOfInterest.msg
- DoMapping.msg
- MapLayers.msg
- ModifyMap.msg
- PointOfInterest.msg
- PointsOfInterest.msg
- RegionOfInterest.msg
- RegionsOfInterest.msg
- ModifyPOI.msg
- TargetUnreachable.msg
- StartNavigation.msg
- StopNavigation.msg
- NavigateToPOI.msg
-)
-
-add_service_files(FILES
- GetPointsOfInterest.srv
- GetRegionsOfInterest.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/mainpage.dox b/homer_mapnav_msgs/mainpage.dox
deleted file mode 100644
index 4ef2ddd8001bb5b374f696fbfb3acf5fa679e353..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/mainpage.dox
+++ /dev/null
@@ -1,14 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b map_messages
-
-<!--
-Provide an overview of your package.
--->
-
--->
-
-
-*/
diff --git a/homer_mapnav_msgs/manifest.xml b/homer_mapnav_msgs/manifest.xml
deleted file mode 100644
index 77dfdadc4acb35ad6f67b30823f35e3dee2256e9..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/manifest.xml
+++ /dev/null
@@ -1,16 +0,0 @@
-<package>
- <description brief="map_messages">
-
- map_messages
-
- </description>
- <author>Malte Knauf</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://ros.org/wiki/map_messages</url>
-
- <depend package="geometry_msgs"/>
- <depend package="nav_msgs"/>
-</package>
-
-
diff --git a/homer_mapnav_msgs/msg/DeletePointOfInterest.msg b/homer_mapnav_msgs/msg/DeletePointOfInterest.msg
deleted file mode 100644
index 42f2bc1cc3dc0b574632e549a1d344b9b30d8c44..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/DeletePointOfInterest.msg
+++ /dev/null
@@ -1 +0,0 @@
-string name
diff --git a/homer_mapnav_msgs/msg/DoMapping.msg b/homer_mapnav_msgs/msg/DoMapping.msg
deleted file mode 100644
index 119c83954d4fd5292506ec0c0b5317a1b1a1b886..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/DoMapping.msg
+++ /dev/null
@@ -1 +0,0 @@
-bool state
diff --git a/homer_mapnav_msgs/msg/MapLayers.msg b/homer_mapnav_msgs/msg/MapLayers.msg
deleted file mode 100644
index 78ccd311dfb5a3a156992a443037b426ddd1e7f3..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/MapLayers.msg
+++ /dev/null
@@ -1,7 +0,0 @@
-int32 SLAM_LAYER=0
-int32 MASKING_LAYER=1
-int32 KINECT_LAYER=2
-int32 SICK_LAYER=3
-
-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 b9a709d3407d623f4bd9800cb338b1edc514646f..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/msg/ModifyMap.msg
+++ /dev/null
@@ -1,17 +0,0 @@
-int32 FREE = 0 # Area is free
-int32 BLOCKED = 100 # Area blocked by user (use UNBLOCK to .. )
-int32 OBSTACLE = 99 # Area is occupied by an obstacle (e.g. 3d scan)
-int32 NOT_MASKED = -1 # Area is not masked: use slam map
-int32 HIGH_SENSITIV = -2 # Area is highly sensitiv for Obstacles
-int32 KINECT = 102 # Area is blocked by Kinect Obstacle
-
-
-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/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/StopNavigation.msg b/homer_mapnav_msgs/msg/StopNavigation.msg
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
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 e669921f5162a4249b95264a1009ac3481c61dad..0000000000000000000000000000000000000000
--- a/homer_mapnav_msgs/package.xml
+++ /dev/null
@@ -1,27 +0,0 @@
-<package>
- <name>homer_mapnav_msgs</name>
- <version>1.0.11</version>
- <description>
-
- mapnav_msgs
-
- </description>
- <maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
- <maintainer email="niyawe@uni-koblenz.de">Niklas Yann Wettengel</maintainer>
- <maintainer email="heuer@uni-koblenz.de">Gregor Heuer</maintainer>
- <author email="mknauf@uni-koblenz.de">Malte Knauf</author>
- <author email="raphael@uni-koblenz.de">Raphael Memmesheimer</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>
- <build_depend>cmake_modules</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/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/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 59caf0fd42ce1b060cd3c677ab1acf5781c22a33..0000000000000000000000000000000000000000
--- a/homer_mapping/CHANGELOG.rst
+++ /dev/null
@@ -1,54 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package homer_mapping
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.0.11 (2015-12-02)
--------------------
-* added std_srvs dependency
-* fixed libraries
-* Contributors: Niklas Yann Wettengel
-
-1.0.10 (2015-12-02)
--------------------
-
-1.0.9 (2015-12-01)
-------------------
-
-1.0.8 (2015-11-30)
-------------------
-* added qt4 build dependency
-* Contributors: Niklas Yann Wettengel
-
-1.0.7 (2015-11-28)
-------------------
-* updated changelog
-* updated catkin_depends
-* Contributors: Niklas Yann Wettengel
-
-* updated catkin_depends
-* Contributors: Niklas Yann Wettengel
-
-1.0.6 (2015-11-27)
-------------------
-* removed env HOMER_DIR from CMakeLists.txt
-* Contributors: Niklas Yann Wettengel
-
-1.0.5 (2015-11-24)
-------------------
-
-1.0.4 (2015-11-20)
-------------------
-
-1.0.3 (2015-11-20)
-------------------
-
-1.0.2 (2015-11-20)
-------------------
-* added cmake_modules as build dependency in package.xml
-* added Maintainers
-* Contributors: Niklas Yann Wettengel
-
-1.0.1 (2015-09-08)
-------------------
-* init
-* Contributors: Raphael Memmesheimer
diff --git a/homer_mapping/CMakeLists.txt b/homer_mapping/CMakeLists.txt
deleted file mode 100644
index b0b71008b5ff459f6c1738ac7f61cd67acf7eb5f..0000000000000000000000000000000000000000
--- a/homer_mapping/CMakeLists.txt
+++ /dev/null
@@ -1,48 +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
-)
-
-catkin_package(
- CATKIN_DEPENDS
- roscpp
- homer_mapnav_msgs
- sensor_msgs
- nav_msgs
- homer_nav_libs
- tf
- roslib
-)
-
-set(CMAKE_BUILD_TYPE Release)
-
-
-include_directories(
- ${catkin_INCLUDE_DIRS}
-)
-
-add_subdirectory(${PROJECT_SOURCE_DIR}/src/OccupancyMap)
-add_subdirectory(${PROJECT_SOURCE_DIR}/src/ParticleFilter)
-
-add_executable(homer_mapping src/slam_node.cpp)
-
-target_link_libraries(
- homer_mapping
- ${catkin_LIBRARIES}
- ParticleFilter
- OccupancyMap
-)
-
-add_dependencies(
- homer_mapping
- ${catkin_EXPORTED_TARGETS}
-)
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 1f4abdc68782d54fe156cb060198f59bb0047137..0000000000000000000000000000000000000000
--- a/homer_mapping/config/homer_mapping.yaml
+++ /dev/null
@@ -1,30 +0,0 @@
-/homer_mapping/size: 35 #size of one edge of the map in m. map is quadratic
-/homer_mapping/resolution: 0.05 #meter per cell
-
-#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.15 #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: 5.0 #percent
-/particlefilter/error_values/rotation_error_translating: 0.0 #degrees per meter
-/particlefilter/error_values/translation_error_translating: 5.0 #percent
-/particlefilter/error_values/translation_error_rotating: 0.0 #m per degree
-/particlefilter/error_values/move_jitter_while_turning: 0.05 #30.0 #m per degree
-
-
-/particlefilter/hyper_slamfilter/particlefilter_num: 1
-
-/particlefilter/particle_num: 1000
-/particlefilter/max_rotation_per_second: 1.0 #maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted
-/particlefilter/wait_time: 0.08 #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: 5 # degrees
-/particlefilter/update_min_move_dist: 0.1 #m
-/particlefilter/max_update_interval: 2 #seconds
-
-/selflocalization/scatter_var_xy: 0.05 #m
-/selflocalization/scatter_var_theta: 0.2 #radiants
diff --git a/homer_mapping/config/homer_mapping_followme.yaml b/homer_mapping/config/homer_mapping_followme.yaml
deleted file mode 100644
index 13792fb81a934e38ca4c48b2bb47120477bd4fe3..0000000000000000000000000000000000000000
--- a/homer_mapping/config/homer_mapping_followme.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-/homer_mapping/size: 35 #size of one edge of the map in m. map is quadratic
-/homer_mapping/resolution: 0.1 #meter per cell
-
-/particlefilter/error_values/rotation_error_rotating: 0.05 #percent
-/particlefilter/error_values/rotation_error_translating: 5.0 #degrees per meter
-/particlefilter/error_values/translation_error_translating: 8.0 #percent
-/particlefilter/error_values/translation_error_rotating: 0.005 #m per degree
-/particlefilter/error_values/move_jitter_while_turning: 0.05 #30.0 #m per degree
diff --git a/homer_mapping/launch/homer_mapping.launch b/homer_mapping/launch/homer_mapping.launch
deleted file mode 100644
index e9e5c5d81dc2f029448bf5413a0e63a78d197d17..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="map_manager" type="map_manager" output="screen"/>
-</launch>
diff --git a/homer_mapping/launch/homer_mapping_followme.launch b/homer_mapping/launch/homer_mapping_followme.launch
deleted file mode 100644
index bdc7208d75daa504e552e8fbc7ef7e14621d998a..0000000000000000000000000000000000000000
--- a/homer_mapping/launch/homer_mapping_followme.launch
+++ /dev/null
@@ -1,5 +0,0 @@
-<launch>
- <rosparam command="load" file="$(find homer_mapping)/config/homer_mapping_followme.yaml"/>
- <node name="homer_mapping" pkg="homer_mapping" type="homer_mapping" output="screen"/>
- <node name="map_manager" pkg="map_manager" type="map_manager" output="screen"/>
-</launch>
diff --git a/homer_mapping/launch/homer_mapping_ros.launch b/homer_mapping/launch/homer_mapping_ros.launch
deleted file mode 100644
index e9e5c5d81dc2f029448bf5413a0e63a78d197d17..0000000000000000000000000000000000000000
--- a/homer_mapping/launch/homer_mapping_ros.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="map_manager" type="map_manager" output="screen"/>
-</launch>
diff --git a/homer_mapping/launch/homer_mapping_rviz.launch b/homer_mapping/launch/homer_mapping_rviz.launch
deleted file mode 100644
index 8ac72e946980fa7a835942c4a6b6c9367893f2b3..0000000000000000000000000000000000000000
--- a/homer_mapping/launch/homer_mapping_rviz.launch
+++ /dev/null
@@ -1,6 +0,0 @@
-<launch>
- <param name="use_sim_time" type="bool" value="true"/>
- <node name="tf_static_publisher" pkg="homer_mapping" type="static_publisher"/>
- <node name="homer_mapping" pkg="homer_mapping" type="homer_mapping" output="screen"/>
- <node name="rviz" pkg="rviz" type="rviz"/>
-</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 8b06810c975bf14c9ec4ab3360146d8aecab9950..0000000000000000000000000000000000000000
--- a/homer_mapping/package.xml
+++ /dev/null
@@ -1,43 +0,0 @@
-<package>
- <name>homer_mapping</name>
- <version>1.0.11</version>
- <description>
-
- homer_mapping
-
- </description>
- <maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
- <maintainer email="raphael@uni-koblenz.de">Raphael Memmesheimer</maintainer>
- <maintainer email="niyawe@uni-koblenz.de">Niklas Yann Wettengel</maintainer>
- <maintainer email="heuer@uni-koblenz.de">Gregor Heuer</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>cmake_modules</build_depend>
- <build_depend>homer_mapnav_msgs</build_depend>
- <build_depend>libqt4</build_depend>
- <build_depend>libqt4-dev</build_depend>
- <build_depend>libqtgui4</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>libqt4</run_depend>
- <run_depend>libqt4-dev</run_depend>
- <run_depend>libqtgui4</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/CMakeLists.txt b/homer_mapping/src/OccupancyMap/CMakeLists.txt
deleted file mode 100644
index 9aaff58227bf4b4280d53d45fcc8631a5ce435d8..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/CMakeLists.txt
+++ /dev/null
@@ -1,31 +0,0 @@
-# QT related
-find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED)
-find_package(tf REQUIRED)
-include(${QT_USE_FILE})
-add_definitions(${QT_DEFINITIONS})
-
-set(
- OccupancyMap_SRC
- OccupancyMap.cpp
-)
-
-include_directories(
- ${QT_INCLUDE_DIR}
-)
-
-add_library(
- OccupancyMap
- ${OccupancyMap_SRC}
-)
-
-target_link_libraries(
- OccupancyMap
- ${QT_LIBRARIES}
- ${tf_LIBRARIES}
-)
-
-add_dependencies(
- OccupancyMap
- or_msgs_gencpp
- homer_mapnav_msgs_gencpp
-)
diff --git a/homer_mapping/src/OccupancyMap/Math/Box2D.h b/homer_mapping/src/OccupancyMap/Math/Box2D.h
deleted file mode 100644
index 5979b68a773e1ed642cdee35247aadb5e712661a..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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 "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_mapping/src/OccupancyMap/Math/CMakeLists.txt b/homer_mapping/src/OccupancyMap/Math/CMakeLists.txt
deleted file mode 100644
index ff84de22b45fbd64c1e12672a645772a91defb15..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/CMakeLists.txt
+++ /dev/null
@@ -1,17 +0,0 @@
-set(Math_SRC
- Line2D.cpp
- Polygon2D.cpp
- misc.cpp
- Transformation2D.cpp
- mat3.cpp
- Circle2D.cpp
- Pose.cpp
- Obb2D.cpp
- Matrix.cpp
- Math.cpp
- Point2D.cpp
- Vector3D.cpp
- Homography.cpp
-)
-
-add_library(Math ${Math_SRC})
diff --git a/homer_mapping/src/OccupancyMap/Math/Circle2D.cpp b/homer_mapping/src/OccupancyMap/Math/Circle2D.cpp
deleted file mode 100644
index edaadf42edfd6a6a32f8be40268616427cfb1779..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Circle2D.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-/*******************************************************************************
- * Circle2D.cpp
- *
- * (C) 2006 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- *******************************************************************************/
-
-#include "Circle2D.h"
-#include "vec2.h"
-
-#include <math.h>
-
-#define THIS Circle2D
-
-using namespace std;
-
-THIS::THIS() {
- m_Radius=0.0;
-}
-
-THIS::THIS(double x, double y, double radius) {
- m_Center=Point2D(x,y);
- m_Radius=radius;
-}
-
-THIS::THIS( Point2D center, double radius) {
- m_Center=center;
- m_Radius=radius;
-}
-
-THIS::~THIS() {
-}
-
-double THIS::x() const{
- return m_Center.x();
-}
-
-double THIS::y() const{
- return m_Center.y();
-}
-
-double THIS::radius() const
-{
- return m_Radius;
-}
-
-Point2D THIS::center() const
-{
- return m_Center;
-}
-
-void THIS::setX(double x) {
- m_Center.setX(x);
-}
-
-void THIS::setY(double y) {
- m_Center.setY(y);
-}
-
-void THIS::setCenter( Point2D center )
-{
- m_Center=center;
-}
-
-void THIS::setRadius( double radius )
-{
- m_Radius=radius;
-}
-
-
-vector<Point2D> THIS::vertices( int steps )
-{
- vector<Point2D> myVertices;
- myVertices.reserve( steps+1 );
- for( float alpha=0.0; alpha<M_PI*2; alpha+=M_PI*2/float(steps) ) {
- myVertices.push_back( m_Center + CVec2( sin(alpha)*m_Radius, cos(alpha)*m_Radius ) );
- }
- myVertices.push_back( m_Center + CVec2( sin(M_PI*2)*m_Radius, cos(M_PI*2)*m_Radius ) );
- return myVertices;
-}
-
-
-#undef THIS
diff --git a/homer_mapping/src/OccupancyMap/Math/Circle2D.h b/homer_mapping/src/OccupancyMap/Math/Circle2D.h
deleted file mode 100644
index 68f280e5c064a671995d1c4b058978c7985ae720..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Circle2D.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*******************************************************************************
- * Circle2D.h
- *
- * (C) 2008 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * $Id :$
- *******************************************************************************/
-
-#ifndef Circle2D_H
-#define Circle2D_H
-
-#include "Point2D.h"
-#include <vector>
-
-/**
- * @class Circle2D
- * @author David Gossow
- */
-class Circle2D {
-
- public:
-
- Circle2D();
-
- /** Creates a new 2D Circle given by center and radius */
- Circle2D( double x, double y, double radius );
- Circle2D( Point2D center, double radius );
-
- /**
- * Destructor, does nothing.
- */
- ~Circle2D();
-
- double x() const;
- double y() const;
- double radius() const;
- Point2D center() const;
-
- void setX(double x);
- void setY(double y);
- void setCenter( Point2D center );
- void setRadius( double radius );
-
- std::vector<Point2D> vertices( int steps=40 );
-
- protected:
-
- Point2D m_Center;
- double m_Radius;
-};
-
-#endif
diff --git a/homer_mapping/src/OccupancyMap/Math/Homography.cpp b/homer_mapping/src/OccupancyMap/Math/Homography.cpp
deleted file mode 100644
index 8a2a719e140205326ccae658d52b5ac11a22caf6..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Homography.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-/*******************************************************************************
- * Homography.cpp
- *
- * (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *******************************************************************************/
-
-#include "Homography.h"
-
-#include <math.h>
-#include <string.h>
-#include <sstream>
-
-#define THIS Homography
-
-THIS::THIS ( double homMat[9] )
-{
- memcpy( m_HomMat, homMat, 9*sizeof(double) );
-}
-
-THIS::THIS ( const THIS& other )
-{
- memcpy( m_HomMat, other.m_HomMat, 9*sizeof(double) );
-}
-
-THIS& THIS::operator=( const Homography& other )
-{
- memcpy( m_HomMat, other.m_HomMat, 9*sizeof(double) );
- return *this;
-}
-
-Point2D THIS::transform ( Point2D point2 )
-{
- if ( !point2.isValid() )
- {
- return point2;
- }
- else
- {
- double x = point2.x();
- double y = point2.y();
- double Z = 1. / ( m_HomMat[6] * x + m_HomMat[7] * y + m_HomMat[8] );
- double X = ( m_HomMat[0] * x + m_HomMat[1] * y + m_HomMat[2] ) * Z;
- double Y = ( m_HomMat[3] * x + m_HomMat[4] * y + m_HomMat[5] ) * Z;
- return Point2D( X, Y );
- }
-}
-
-void THIS::transform ( std::vector<Point2D>& points2, std::vector<Point2D> &projPoints )
-{
- projPoints.reserve( points2.size() );
-
- // Translate src_corners to dst_corners using homography
- for ( unsigned i = 0; i < points2.size(); i++ )
- {
- if ( !points2[i].isValid() )
- {
- projPoints.push_back( points2[i] );
- }
- else
- {
- double x = points2[i].x();
- double y = points2[i].y();
- double Z = 1. / ( m_HomMat[6] * x + m_HomMat[7] * y + m_HomMat[8] );
- double X = ( m_HomMat[0] * x + m_HomMat[1] * y + m_HomMat[2] ) * Z;
- double Y = ( m_HomMat[3] * x + m_HomMat[4] * y + m_HomMat[5] ) * Z;
- projPoints.push_back( Point2D( X, Y ) );
- }
- }
-}
-
-bool THIS::checkValidity ( std::vector<Point2D>& points2 )
-{
- // Translate src_corners to dst_corners using homography
- for ( unsigned i = 0; i < points2.size(); i++ )
- {
- if ( !points2[i].isValid() )
- {
- continue;
- }
- else
- {
- double x = points2[i].x();
- double y = points2[i].y();
- double Z = 1. / ( m_HomMat[6] * x + m_HomMat[7] * y + m_HomMat[8] );
- if ( Z < 0 )
- {
- return false;
- }
- }
- }
- return true;
-}
-
-std::string THIS::toString()
-{
- std::ostringstream s;
- for ( int j=0; j< 3; j++ )
- {
- for ( int i=0; i< 3; i++ )
- {
- s << m_HomMat[i+3*j] << " ";
- }
- s << std::endl;
- }
- return s.str();
-}
-
-#undef THIS
diff --git a/homer_mapping/src/OccupancyMap/Math/Homography.h b/homer_mapping/src/OccupancyMap/Math/Homography.h
deleted file mode 100644
index c8829e92d13ff50cc6cebe2871797ab0ea7bc428..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Homography.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*******************************************************************************
- * CvHomography.h
- *
- * (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *******************************************************************************/
-
-#ifndef Homography_H
-#define Homography_H
-
-#include "Point2D.h"
-#include <vector>
-
-/**
- * @class Homography
- * @brief Represents a homography
- * @author David Gossow
- */
-class Homography
-{
- public:
-
- Homography( ) {}
-
- Homography( double homMat[9] );
-
- Homography( const Homography& other );
-
- Homography& operator=( const Homography& other );
-
- /** Transform point2 using the homography */
- Point2D transform ( Point2D point2 );
-
- /** Transform keyPoints2 using the homography and store them in projPoints
- * @return if one of the resulting points has z<0, that means it would lie behind the camera, return false
- */
- void transform ( std::vector<Point2D>& points2, std::vector<Point2D> &projPoints );
-
- /// @return true if all the given points lie in front of the camera (z>0)
- bool checkValidity ( std::vector<Point2D>& points2 );
-
- std::string toString();
-
- // private: // FIXME made public to create ROS message
-
- double m_HomMat[9];
-};
-
-#endif
diff --git a/homer_mapping/src/OccupancyMap/Math/Line2D.cpp b/homer_mapping/src/OccupancyMap/Math/Line2D.cpp
deleted file mode 100644
index f5807c313dab246c435fcdba12af6eab300c550f..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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 "Line2D.h"
-#include "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_mapping/src/OccupancyMap/Math/Line2D.h b/homer_mapping/src/OccupancyMap/Math/Line2D.h
deleted file mode 100644
index e8c210bb6433e979708ae73cab36e218a53550e1..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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 "vec2.h"
-#include "mat2.h"
-#include "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_mapping/src/OccupancyMap/Math/Math.cpp b/homer_mapping/src/OccupancyMap/Math/Math.cpp
deleted file mode 100644
index 3f31c5bfe5df6679d34372082b3a8b8ed7e60696..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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 "Math.h"
-#include <math.h>
-
-#include "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_mapping/src/OccupancyMap/Math/Math.h b/homer_mapping/src/OccupancyMap/Math/Math.h
deleted file mode 100644
index bf00135a1a53a24343e7ec20e5f0bc7e13599b4d..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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 "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_mapping/src/OccupancyMap/Math/Matrix.cpp b/homer_mapping/src/OccupancyMap/Math/Matrix.cpp
deleted file mode 100644
index abe383651eac95095d9fad2804407cb190c4d829..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Matrix.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-/*******************************************************************************
- * Matrix.cpp
- *
- * (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Additional information:
- * $Id: $
- *******************************************************************************/
-
-#include "Matrix.h"
-
-#define THIS Matrix
-
-THIS THIS::transpose() {
- Matrix newMatrix(m_Columns, m_Lines);
- for (unsigned i = 0; i < m_Lines*m_Columns; i++) {
- newMatrix.setValue(i/m_Columns, i%m_Lines, m_Values[i/m_Lines * i%m_Columns]);
- }
- return newMatrix;
-}
-
-#undef THIS
diff --git a/homer_mapping/src/OccupancyMap/Math/Matrix.h b/homer_mapping/src/OccupancyMap/Math/Matrix.h
deleted file mode 100644
index 89f5f39a51528a72b1db798895048b0d60789729..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Matrix.h
+++ /dev/null
@@ -1,116 +0,0 @@
-/*******************************************************************************
- * Matrix.h
- *
- * (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Additional information:
- * $Id: $
- *******************************************************************************/
-
-#ifndef MATRIX_H
-#define MATRIX_H
-
-#include "mat2.h"
-#include "mat3.h"
-
-/**
- * @class Matrix
- * @brief This class describes a Matrix
- * @author Michael Dahl
- */
-class Matrix
-{
- public:
-
- /** @brief The constructor */
- Matrix(unsigned lines, unsigned columns){
- m_Lines = lines;
- m_Columns = columns;
- m_Values = new double[lines*columns];
- for (unsigned i = 0; i< lines*columns; i++) {
- m_Values[i] = 0;
- }
- }
-
- Matrix(const CMat2 matrix2) {
- m_Lines = 2;
- m_Columns = 2;
- m_Values = new double[4];
- for(unsigned i = 0; i < m_Lines * m_Columns; i++) {
- m_Values[i] = matrix2.valueAt(i);
- }
- }
-
- Matrix(const CMat3 matrix3) {
- m_Lines = 3;
- m_Columns = 3;
- m_Values = new double[9];
- for(unsigned i = 0; i < m_Lines * m_Columns; i++) {
- m_Values[i] = matrix3.valueAt(i);
- }
- }
-
- /** @brief The destructor */
- ~Matrix() {
- delete[] m_Values;
- }
-
- inline double getValue(int line, int column) const{
- return m_Values[line*m_Columns + column];
- }
-
- inline void setValue(int line, int column, double value) {
- m_Values[line*m_Columns + column] = value;
- }
-
- Matrix transpose();
-
- Matrix operator *(double scalar){
- Matrix returnMatrix(m_Lines, m_Columns);
- for (unsigned i = 0; i < m_Lines * m_Columns; i++) {
- returnMatrix.setValue(i/m_Lines, i%m_Lines, this->getValue(i/m_Lines, i%m_Lines) * scalar);
- }
- return returnMatrix;
- }
-
- Matrix operator /(double scalar){
- Matrix returnMatrix(m_Lines, m_Columns);
- for (unsigned i = 0; i < m_Lines * m_Columns; i++) {
- returnMatrix.setValue(i/m_Lines, i%m_Lines, this->getValue(i/m_Lines, i%m_Lines) / scalar);
- }
- return returnMatrix;
- }
-
- Matrix operator *(const Matrix rhs){
- Matrix returnMatrix(m_Lines, m_Columns);
- for (unsigned i = 0; i < m_Lines * m_Columns; i++) {
- returnMatrix.setValue(i/m_Lines, i%m_Lines, this->getValue(i/m_Lines, i%m_Lines) * rhs.getValue(i%m_Lines, i/m_Lines));
- }
- return returnMatrix;
- }
-
- Matrix operator +(const Matrix rhs){
- Matrix returnMatrix(m_Lines, m_Columns);
- for (unsigned i = 0; i < m_Lines * m_Columns; i++) {
- returnMatrix.setValue(i/m_Lines, i%m_Lines, this->getValue(i/m_Lines, i%m_Lines) + rhs.getValue(i/m_Lines, i%m_Lines));
- }
- return returnMatrix;
- }
-
- Matrix operator -(const Matrix rhs){
- Matrix returnMatrix(m_Lines, m_Columns);
- for (unsigned i = 0; i < m_Lines * m_Columns; i++) {
- returnMatrix.setValue(i/m_Lines, i%m_Lines, this->getValue(i/m_Lines, i%m_Lines) - rhs.getValue(i/m_Lines, i%m_Lines));
- }
- return returnMatrix;
- }
-
- private:
-
- unsigned int m_Lines;
- unsigned int m_Columns;
- double* m_Values;
-};
-
-#endif
diff --git a/homer_mapping/src/OccupancyMap/Math/Obb2D.cpp b/homer_mapping/src/OccupancyMap/Math/Obb2D.cpp
deleted file mode 100755
index 9d4a2ae82f83c1b7934698ca6c2f4a3c910b27e0..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Obb2D.cpp
+++ /dev/null
@@ -1,149 +0,0 @@
-#include <stdlib.h>
-
-
-#include "Obb2D.h"
-#include <iostream>
-
-//----------------------------------------------------------------------------------------------------------
-OBB2D::OBB2D()
-{
-}
-//----------------------------------------------------------------------------------------------------------
-OBB2D::~OBB2D()
-{
-}
-//----------------------------------------------------------------------------------------------------------
-std::pair<CVec2,CVec2> OBB2D::computeAABB() const
-{
- CVec2 mins(999999,999999);
- CVec2 maxs(-999999,-999999);
- for (int i=0;i<4;i++)
- {
- if (mPoints[i][0]<mins[0]) mins[0]=mPoints[i][0];
- if (mPoints[i][1]<mins[1]) mins[1]=mPoints[i][1];
- if (mPoints[i][0]>maxs[0]) maxs[0]=mPoints[i][0];
- if (mPoints[i][1]>maxs[1]) maxs[1]=mPoints[i][1];
- }
- return std::make_pair(mins,maxs);
-}
-//----------------------------------------------------------------------------------------------------------
-// Polygon an Kante (clipStart, clipEnd) clippen, Originalpunkte werden ueberschrieben
-int clipEdge(const CVec2* points, int numPoints, const CVec2& clipStart, const CVec2& clipEnd, CVec2* dest)
-{
- int result=0;
-
- // Normale zeigt nach aussen
- CVec2 n(clipEnd[1] - clipStart[1],
- clipStart[0] - clipEnd[0]);
-
- //n=n*(-1);
-
- float d0=clipStart*n;
-
- int i=numPoints-1;
- for (int iNext=0;iNext<numPoints;iNext++)
- {
- const CVec2& a=points[i];
- const CVec2& b=points[iNext];
-
- float adotn=a*n;
- float bdotn=b*n;
- float da=adotn-d0;
- float db=bdotn-d0;
-
- if (da<=0)
- {
- if (db<=0)
- {
- // beide innerhalb
- //outputPoints.push_back(b);
- dest[result++]=b;
- }
- else
- {
- // a drinnen, b draussen => schnittpunkt
- float t=-da/(bdotn-adotn);
- //outputPoints.push_back(a+t*(b-a));
- dest[result++]=a+t*(b-a);
- }
- }
- else
- {
- if (db<=0)
- {
- // a draussen, b drinnen => schnittpunkt
- float t=-da/(bdotn-adotn);
- //outputPoints.push_back(a+t*(b-a));
- //outputPoints.push_back(b);
- dest[result++]=a+t*(b-a);
- dest[result++]=b;
- }
- else
- {
- // beide ausserhalb
- }
- }
-
- i=iNext;
- }
-
- return result;
-}
-//----------------------------------------------------------------------------------------------------------
-float OBB2D::computeClippedArea(const OBB2D& clipPoly)
-{
- CVec2* tmp1=(CVec2*)alloca(20*sizeof(CVec2));
- CVec2* tmp2=(CVec2*)alloca(20*sizeof(CVec2));
-
- int res=0;
-
- res=clipEdge(mPoints,4,clipPoly[3],clipPoly[0],tmp1);
- res=clipEdge(tmp1,res,clipPoly[0],clipPoly[1],tmp2);
- res=clipEdge(tmp2,res,clipPoly[1],clipPoly[2],tmp1);
- res=clipEdge(tmp1,res,clipPoly[2],clipPoly[3],tmp2);
-
- /*
- static int maxres=0;
- if (res>=maxres)
- {
- std::cout << "res: " << res << std::endl;
- maxres=res;
- }*/
-
-
- float area=0;
- for (int i=0;i<res;i++)
- {
- area+=(tmp2[i][1]+tmp2[(i+1)%res][1])*(tmp2[i][0]-tmp2[(i+1)%res][0]);
- }
- area=0.5f*fabs(area);
- return area;
-}
-//----------------------------------------------------------------------------------------------------------
-/*float Polygon2D::computeArea() const
-{
-#if 0
- float area=0;
- CVec2 last=mPoints[1];
- for (int i=2;i<mPoints.size();i++)
- {
- CVec2 a=last-mPoints[0];
- CVec2 b=mPoints[i]-mPoints[0];
- area+=0.5f*fabs(a.x*b.y-a.y*b[0]);
-
- last=mPoints[i];
- }
- return area;
-#else
- float area2=0;
- for (int i=0;i<mPoints.size();i++)
- {
- area2+=(mPoints[i][1]+mPoints[(i+1)%mPoints.size()][1])*(mPoints[i][0]-mPoints[(i+1)%mPoints.size()][0]);//mPoints[i].x*mPoints[i+1].y-mPoints[i+1].x*mPoints[i][1];
- }
- area2=0.5f*fabs(area2);
- //std::cout << "area: " << area << " " << area2 << std::endl;
- return area2;
-#endif
-}*/
-//----------------------------------------------------------------------------------------------------------
-
diff --git a/homer_mapping/src/OccupancyMap/Math/Obb2D.h b/homer_mapping/src/OccupancyMap/Math/Obb2D.h
deleted file mode 100755
index ec7313abfd2193f3aedf751dcca668f8626519cd..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Obb2D.h
+++ /dev/null
@@ -1,33 +0,0 @@
-#ifndef _OBB2D_H
-#define _OBB_H
-
-#include "vec2.h"
-#include <vector>
-
-/**
- * Obolete. Used by Robbie 9 for line clipping.
- * Bad documentation. For questions ask F. Neuhaus.
- */
-
-class OBB2D
-{
- public:
-
- OBB2D();
- ~OBB2D();
-
- CVec2& operator[](int i) { return mPoints[i]; };
- const CVec2& operator[](int i) const { return mPoints[i]; };
-
- float computeClippedArea(const OBB2D& clipPoly);
-
- std::pair<CVec2,CVec2> computeAABB() const;
-
-
- private:
-
- CVec2 mPoints[4];
-};
-
-#endif
-
diff --git a/homer_mapping/src/OccupancyMap/Math/Pixel.h b/homer_mapping/src/OccupancyMap/Math/Pixel.h
deleted file mode 100644
index 026fd3a6909cadc954b3148f15b6a73d4c632c18..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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 "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_mapping/src/OccupancyMap/Math/Point2D.cpp b/homer_mapping/src/OccupancyMap/Math/Point2D.cpp
deleted file mode 100644
index c8ae09b7ab40761665dc5694db808f370b1768c3..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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 "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_mapping/src/OccupancyMap/Math/Point2D.h b/homer_mapping/src/OccupancyMap/Math/Point2D.h
deleted file mode 100644
index 94810ef901253ad677b4a59a823232f14589da1c..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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 "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_mapping/src/OccupancyMap/Math/Polygon2D.cpp b/homer_mapping/src/OccupancyMap/Math/Polygon2D.cpp
deleted file mode 100644
index 0c5d5e52fa65722335789d736bc3f163dcdff6ce..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Polygon2D.cpp
+++ /dev/null
@@ -1,131 +0,0 @@
-/*******************************************************************************
- * Polygon2D.cpp
- *
- * (C) 2008 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Additional information:
- * $Id: Polygon2D.cpp 44313 2011-04-06 22:46:28Z agas $
- *******************************************************************************/
-
-#include <stdlib.h>
-#include "Polygon2D.h"
-#include "Point2D.h"
-#include "Line2D.h"
-
-
-#define THIS Polygon2D
-
-using namespace std;
-
-vector<Line2D> THIS::getLines() const
-{
- vector<Line2D> lines;
- vector<Point2D>::const_iterator pointIt = m_Points.begin() +1;
- while ( pointIt != m_Points.end() )
- {
- lines.push_back ( Line2D ( * ( pointIt-1 ), *pointIt ) );
- pointIt++;
- }
- lines.push_back ( Line2D ( * ( m_Points.end()-1 ), *m_Points.begin() ) );
-
- return lines;
-}
-
-void THIS::clipLines ( std::vector<Line2D>& linesToClip ) const
-{
- // for each line to clip
- vector<Line2D>::iterator linesIt = linesToClip.begin();
- while ( linesIt != linesToClip.end() )
- {
- if ( !clipLine ( *linesIt ) )
- {
- // line lies completely outside of the polygon
- linesToClip.erase ( linesIt );
- continue;
- }
- linesIt++;
- }
-}
-
-bool THIS::clipLine ( Line2D& lineToClip ) const
-{
- vector<Line2D> polygonLines = getLines();
- CVec2 v = lineToClip.vec();
-
- float tIn = 0.0;
- float tOut = 1.0;
-
- // for each line of the polygon
- vector<Line2D>::const_iterator polygonIt = polygonLines.begin();
- while ( polygonIt != polygonLines.end() )
- {
- CVec2 n = ( *polygonIt ).getNormal();
-// cout << "polLine "<< ( *polygonIt ).toString() << endl;
-// cout << "n "<< n.toString() << endl;
-
- CVec2 w0 = lineToClip.start()- ( *polygonIt ).start();
- CVec2 w1 = lineToClip.end()- ( *polygonIt ).start();
-
-// cout << "w0 " <<w0.toString() << endl;
-// cout << "w1 " <<w1.toString() << endl;
-
- float c0 = w0.dot ( n );
- float c1 = w1.dot ( n );
-
- // test if line lies completely inside
- if ( c0 < 0 && c1 < 0 )
- {
- // do nothing;
- polygonIt++;
- continue;
- }
- // test if line lies completely outside
- else if ( c0 >= 0 && c1 >= 0 )
- {
-/* cout << "return 1" << endl;*/
- return false;
- }
- // clip lines
- else
- {
- float denominator = v.dot ( n );
-
- if ( denominator == 0 )
- {
- // impossible to happen
- polygonIt++;
- continue;
- }
-
- float t = -c0/denominator;
-
- // test for "in" or "out" point
- if ( denominator < 0 && t > tIn )
- {
- // intersection point is an "in point"
- tIn = t;
- }
- else if ( denominator > 0 && t < tOut )
- {
- // intersection point is an "out point"
- tOut = t;
- }
- }
- polygonIt++;
- }
-
- if ( tIn > tOut ) {
-/* cout << "return 2" << endl;*/
- return false;
- }
-
- lineToClip.setEnd ( lineToClip.start() + tOut * v );
- lineToClip.setStart ( lineToClip.start() + tIn * v );
-
-// cout << "return true" << endl;
-
- return true;
-}
-
-#undef THIS
diff --git a/homer_mapping/src/OccupancyMap/Math/Polygon2D.h b/homer_mapping/src/OccupancyMap/Math/Polygon2D.h
deleted file mode 100644
index 16f5ca6e8b3686ae40f293740fc92d2f74641e06..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Polygon2D.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/*******************************************************************************
- * Polygon2D.h
- *
- * (C) 2008 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * $Id: Polygon2D.h 44313 2011-04-06 22:46:28Z agas $
- *******************************************************************************/
-
-#ifndef POLYGON2D_H
-#define POLYGON2D_H
-
-#include <vector>
-
-class Polygon2D;
-class Point2D;
-class Line2D;
-
-/**
- * @class Polygon2D
- *
- * @author Susanne Maur
- *
- */
-class Polygon2D
-{
- public:
-
- inline Polygon2D() {}
-
- /**
- * Creates a new polygon
- * The points must be given counterclockwise.
- */
- inline Polygon2D ( std::vector<Point2D>& points )
- {
- m_Points = points;
- }
-
- inline ~Polygon2D() {}
-
- inline std::vector<Point2D> getPoints() const{
- return m_Points;
- }
-
- /**
- * Returns the line representation of the polygon.
- * @return line representation of the polygon.
- */
- std::vector<Line2D> getLines() const;
-
- /**
- * Clips lines to this polygon. Not tested!!!
- * Implementation of Cyrus-Beck.
- * The polygon must be difined counterclockwise.
- * @param linesToClip The lines to clip.
- */
- void clipLines ( std::vector<Line2D>& linesToClip ) const;
-
- /**
- * Clips a line to this polygon.
- * Implementation of Cyrus-Beck.
- * The polygon must be difined counterclockwise.
- * @param lineToClip The lines to clip.
- * @return True if it was possible to clip the line, false if the line lies completely outside.
- */
- bool clipLine ( Line2D& lineToClip ) const;
-
- private:
-
- std::vector<Point2D> m_Points;
-
-};
-
-#endif
-
diff --git a/homer_mapping/src/OccupancyMap/Math/Pose.cpp b/homer_mapping/src/OccupancyMap/Math/Pose.cpp
deleted file mode 100644
index 6011f280d10b9061a2aa90c0df3f1cc54d9f98b2..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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 "Pose.h"
-#include "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_mapping/src/OccupancyMap/Math/Pose.h b/homer_mapping/src/OccupancyMap/Math/Pose.h
deleted file mode 100644
index d0f6574c5856b9de9413be634dd7b251365242e7..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Pose.h
+++ /dev/null
@@ -1,74 +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 "Point2D.h"
-//#include "Architecture/Serializer/ExtendedOutStream.h" // TODO kann wahrscheinlich weg
-//#include "Architecture/Serializer/ExtendedInStream.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_mapping/src/OccupancyMap/Math/Transformation2D.cpp b/homer_mapping/src/OccupancyMap/Math/Transformation2D.cpp
deleted file mode 100644
index 693c7a396f40a7e5c16be4d35d952c4afdfac4ed..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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 "Transformation2D.h"
-
-#include <cmath>
-#include <vector>
-#include <iostream>
-#include <sstream>
-#include "vec2.h" // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
-#include "mat2.h" // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
-#include "Point2D.h"
-#include "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_mapping/src/OccupancyMap/Math/Transformation2D.h b/homer_mapping/src/OccupancyMap/Math/Transformation2D.h
deleted file mode 100644
index 59a54d827c71bb08384861b0e78cf1d20cf60c98..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Transformation2D.h
+++ /dev/null
@@ -1,144 +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 "Workers/Math/Vec.h"
-#include "Point2D.h"
-#include "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_mapping/src/OccupancyMap/Math/Vector3D.cpp b/homer_mapping/src/OccupancyMap/Math/Vector3D.cpp
deleted file mode 100644
index 495846a703c6bf8fc640e62f5bd2d0ad0d247772..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Vector3D.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-/*******************************************************************************
- * Vector3D.cpp
- *
- * (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Additional information:
- * $Id: $
- *******************************************************************************/
-
-#include "Vector3D.h"
-
-#define THIS Vector3D
-
-using namespace std;
-
-std::string THIS::toString( int precision, std::string name )
-{
- std::ostringstream s;
-
- s.precision( precision );
- s.setf(ios::fixed,ios::floatfield);
-
- for ( unsigned row=0; row<3; row++ )
- {
- if ( name != "" )
- {
- if ( row==1 )
- {
- s << name << " = ";
- }
- else
- {
- s.width( name.length()+3 );
- s << "";
- }
- }
-
- s << "( ";
- s.width( precision+4 );
- switch ( row )
- {
- case 0:
- s << m_X << " ";
- break;
- case 1:
- s << m_Y << " ";
- break;
- case 2:
- s << m_Z << " ";
- break;
- }
- s << ")" << endl;
- }
- return s.str();
-}
-
-#undef THIS
diff --git a/homer_mapping/src/OccupancyMap/Math/Vector3D.h b/homer_mapping/src/OccupancyMap/Math/Vector3D.h
deleted file mode 100644
index ae656232cce5ebfb8dfd160cf94e7b492651d500..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/Vector3D.h
+++ /dev/null
@@ -1,82 +0,0 @@
-/*******************************************************************************
- * Vector3D.h
- *
- * (C) 2006 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Author: Frank Neuhaus
- *******************************************************************************/
-
-#ifndef VEC3_H
-#define VEC3_H
-
-#include <math.h>
-#include <assert.h>
-#include <string>
-#include "vec2.h"
-
-class Vector3D
-{
- public:
-
- Vector3D();
- Vector3D ( float x, float y, float z );
- Vector3D ( const CVec2& v, float z);
-
- Vector3D ( const Vector3D& v2 );
-
- Vector3D operator+ ( const Vector3D& vVector ) const;
- Vector3D& operator+= ( const Vector3D& vVector );
- Vector3D operator+() const;
-
- Vector3D operator- ( const Vector3D& vVector ) const;
- Vector3D& operator-= ( const Vector3D& vVector );
- Vector3D operator-() const;
-
- // Dot
- float operator* ( const Vector3D& vVector ) const;
-
- // Cross
- Vector3D operator^ ( const Vector3D& vVector2 ) const;
-
- // Scalar Mult
- Vector3D operator* ( const float num ) const;
- Vector3D& operator*= ( const float num );
- Vector3D operator/ ( float num ) const;
- Vector3D& operator/= ( const float num );
-
- bool operator < ( const Vector3D& vVec ) const;
- bool operator > ( const Vector3D& vVec ) const;
- bool operator== ( const Vector3D& v1 ) const;
-
- float operator [] ( const unsigned int i ) const;
- float& operator [] ( const unsigned int i );
-
- void set ( float fx, float fy, float fz );
-
- float x() const {
- return m_X;
- }
-
- float y() const {
- return m_Y;
- }
-
- float z() const {
- return m_Z;
- }
-
- void lerp ( const Vector3D& v1, const Vector3D& v2, float f );
-
- float magnitude() const;
-
- std::string toString( int precision=3, std::string name="" );
-
- private:
- float m_X, m_Y, m_Z;
-};
-
-#include "vec3_inl.h"
-
-
-#endif
diff --git a/homer_mapping/src/OccupancyMap/Math/mat2.h b/homer_mapping/src/OccupancyMap/Math/mat2.h
deleted file mode 100644
index 254e8dc68fd581091914fd501fe18892cac7bc2f..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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 "Point2D.h"
-#include "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_mapping/src/OccupancyMap/Math/mat2_inl.h b/homer_mapping/src/OccupancyMap/Math/mat2_inl.h
deleted file mode 100644
index e3f77e161151e98c7df7f06b43db1d40fd9c4753..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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_mapping/src/OccupancyMap/Math/mat3.cpp b/homer_mapping/src/OccupancyMap/Math/mat3.cpp
deleted file mode 100755
index 306877e83b14cbce9a21908a9f812c9b1bb14175..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/mat3.cpp
+++ /dev/null
@@ -1,189 +0,0 @@
-/*******************************************************************************
- * mat3.h
- *
- * (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Author: Frank Neuhaus, Susanne Maur
- *******************************************************************************/
-
-#include "mat3.h"
-#include "math.h"
-
-#define THIS CMat3
-
-//----------------------------------------------------------------------------------------------------------
-THIS::~CMat3()
-{
-}
-//----------------------------------------------------------------------------------------------------------
-void THIS::transpose()
-{
- float temp;
-
- temp=fMatrix[1];
- fMatrix[1]=fMatrix[3];
- fMatrix[3]=temp;
-
- temp=fMatrix[2];
- fMatrix[2]=fMatrix[6];
- fMatrix[6]=temp;
-
- temp=fMatrix[5];
- fMatrix[5]=fMatrix[7];
-
- fMatrix[7]=temp;
-}
-
-//----------------------------------------------------------------------------------------------------------
-
-void THIS::reverse()
-{
- CMat3 temp;
-
- temp[0]=fMatrix[4]*fMatrix[8] - fMatrix[5]*fMatrix[7];
- temp[1]=fMatrix[2]*fMatrix[7] - fMatrix[1]*fMatrix[8];
- temp[2]=fMatrix[1]*fMatrix[5] - fMatrix[2]*fMatrix[4];
- temp[3]=fMatrix[5]*fMatrix[6] - fMatrix[3]*fMatrix[8];
- temp[4]=fMatrix[0]*fMatrix[8] - fMatrix[2]*fMatrix[6];
- temp[5]=fMatrix[2]*fMatrix[3] - fMatrix[0]*fMatrix[5];
- temp[6]=fMatrix[3]*fMatrix[7] - fMatrix[4]*fMatrix[6];
- temp[7]=fMatrix[1]*fMatrix[6] - fMatrix[0]*fMatrix[7];
- temp[8]=fMatrix[0]*fMatrix[4] - fMatrix[1]*fMatrix[3];
-
- *this = (temp) * (1.0/determinant());
-}
-
-//----------------------------------------------------------------------------------------------------------
-void THIS::loadIdentity()
-{
- fMatrix[0]=1.0f; fMatrix[1]=0; fMatrix[2]=0;
- fMatrix[3]=0; fMatrix[4]=1.0f; fMatrix[5]=0;
- fMatrix[6]=0; fMatrix[7]=0; fMatrix[8]=1.0f;
-}
-//----------------------------------------------------------------------------------------------------------
-void THIS::makeRotationX(float fA)
-{
- // 1 0 0 0
- // 0 c -s 0
- // 0 s c 0
- // 0 0 0 1
- float c=cosf(fA);
- float s=sinf(fA);
- fMatrix[0]=1; fMatrix[1]=0; fMatrix[2]=0;
- fMatrix[3]=0; fMatrix[4]=c; fMatrix[5]=s;
- fMatrix[6]=0; fMatrix[7]=-s; fMatrix[8]=c;
-
-}
-//----------------------------------------------------------------------------------------------------------
-void THIS::makeRotationY(float fA)
-{
- // c 0 s 0
- // 0 1 0 0
- //-s 0 c 0
- // 0 0 0 1
- float c=cosf(fA);
- float s=sinf(fA);
- fMatrix[0]=c; fMatrix[1]=0; fMatrix[2]=-s;
- fMatrix[3]=0; fMatrix[4]=1; fMatrix[5]=0;
- fMatrix[6]=s; fMatrix[7]=0; fMatrix[8]=c;
-}
-//----------------------------------------------------------------------------------------------------------
-void THIS::makeRotationZ(float fA)
-{
- // c -s 0 0
- // s c 0 0
- // 0 0 1 0
- // 0 0 0 1
-
- float c=cosf(fA);
- float s=sinf(fA);
- fMatrix[0]=c; fMatrix[1]=s; fMatrix[2]=0;
- fMatrix[3]=-s; fMatrix[4]=c; fMatrix[5]=0;
- fMatrix[6]=0; fMatrix[7]=0; fMatrix[8]=1;
-}
-//----------------------------------------------------------------------------------------------------------
-void THIS::makeScale(const Vector3D& vScale)
-{
- fMatrix[0]=vScale[0]; fMatrix[1]=0; fMatrix[2]=0;
- fMatrix[3]=0; fMatrix[4]=vScale[1]; fMatrix[5]=0;
- fMatrix[6]=0; fMatrix[7]=0; fMatrix[8]=vScale[2];
-}
-//----------------------------------------------------------------------------------------------------------
-CMat3 THIS::operator*(const CMat3& mat) const
-{
- CMat3 r;
- float* r0=(float*)&r.fMatrix;
- float* m2=(float*)&fMatrix;
- float* m1=(float*)&mat.fMatrix;
-
- r0[0]=m1[0]*m2[0]+ m1[1]*m2[3]+ m1[2]*m2[6];
- r0[1]=m1[0]*m2[1]+ m1[1]*m2[4]+ m1[2]*m2[7];
- r0[2]=m1[0]*m2[2]+ m1[1]*m2[5]+ m1[2]*m2[8];
-
- r0[3]=m1[3]*m2[0]+ m1[4]*m2[3]+ m1[5]*m2[6];
- r0[4]=m1[3]*m2[1]+ m1[4]*m2[4]+ m1[5]*m2[7];
- r0[5]=m1[3]*m2[2]+ m1[4]*m2[5]+ m1[5]*m2[8];
-
- r0[6]=m1[6]*m2[0]+ m1[7]*m2[3]+ m1[8]*m2[6];
- r0[7]=m1[6]*m2[1]+ m1[7]*m2[4]+ m1[8]*m2[7];
- r0[8]=m1[6]*m2[2]+ m1[7]*m2[5]+ m1[8]*m2[8];
-
-
- return r;
-}
-//----------------------------------------------------------------------------------------------------------
-CMat3 THIS::operator *(float f) const {
- CMat3 newMatrix;
- for (unsigned i = 0; i < 9; i++) {
- newMatrix[i] = fMatrix[i] * f;
- }
- return newMatrix;
-}
-//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-
-CMat3& THIS::operator *=(float f) {
- for (unsigned i = 0; i < 9; i++) {
- fMatrix[i] *= f;
- }
- return *this;
-}
-//----------------------------------------------------------------------------------------------------------
-Vector3D THIS::operator *(const Vector3D& v) const {
- Vector3D temp;
- temp[0]=fMatrix[0]*v[0]+fMatrix[1]*v[1]+fMatrix[2]*v[2];
- temp[1]=fMatrix[3]*v[0]+fMatrix[4]*v[1]+fMatrix[5]*v[2];
- temp[2]=fMatrix[6]*v[0]+fMatrix[7]*v[1]+fMatrix[8]*v[2];
- return temp;
-}
-//----------------------------------------------------------------------------------------------------------
-CMat3 THIS::operator +(const CMat3& mat) {
- CMat3 newMatrix;
- for (unsigned i = 0; i < 9; i++) {
- newMatrix[i] = mat.valueAt(i) + valueAt(i);
- }
- return newMatrix;
-}
-
-//----------------------------------------------------------------------------------------------------------
-std::string THIS::toString() const
-{
- std::ostringstream st;
- for (int i=0;i<3;i++)
- {
- for (int j=0;j<3;j++)
- {
- st<<m[j][i]<<" ";
- }
- st<<"\n";
- }
- return st.str();
-}
-//----------------------------------------------------------------------------------------------------------
-float THIS::determinant() const {
- return fMatrix[0]*fMatrix[4]*fMatrix[8] + fMatrix[1]*fMatrix[5]*fMatrix[6 ] + fMatrix[2]*fMatrix[3]*fMatrix[7]
- - fMatrix[2]*fMatrix[4]*fMatrix[6] - fMatrix[1]*fMatrix[3]*fMatrix[8] - fMatrix[0]*fMatrix[5]*fMatrix[7];
-}
-//----------------------------------------------------------------------------------------------------------
-
-#undef THIS
diff --git a/homer_mapping/src/OccupancyMap/Math/mat3.h b/homer_mapping/src/OccupancyMap/Math/mat3.h
deleted file mode 100755
index 629b9e7d6f46dd8f8d7c1794846f7062c99c678e..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/mat3.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/*******************************************************************************
- * mat3.h
- *
- * (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Author: Frank Neuhaus, Susanne Maur
- *******************************************************************************/
-#ifndef MAT3_H
-#define MAT3_H
-
-#include <iostream>
-#include <sstream>
-#include <assert.h>
-#include "Vector3D.h"
-
-class CMat3 {
- public:
- CMat3();
- CMat3( float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz );
- ~CMat3();
-
- /** overwritten operator**/
- CMat3 operator *(const CMat3 &mat) const;
- CMat3 operator *(float f) const;
- CMat3& operator *=(float f);
- Vector3D operator *(const Vector3D& v) const;
- float& operator [](const unsigned value);
- CMat3 operator +(const CMat3& mat1);
-
- /** @return value at position **/
- float valueAt(unsigned i) const;
-
- /** set value at position **/
- void setValue(unsigned line, unsigned column, float value);
-
- /** @return determinant of matrix **/
- float determinant() const;
-
- /** transpose matrix **/
- void transpose();
- /** reverse matrix **/
- void reverse();
-
- /** create identity matrix **/
- void loadIdentity();
-
- /** create rotation matrix **/
- void makeRotationX(float fA);
- void makeRotationY(float fA);
- void makeRotationZ(float fA);
-
- /** create scale matrix **/
- void makeScale(const Vector3D& vScale);
- //void BuildRPY(float fRoll, float fPitch, float fYaw);
-
- std::string toString() const;
-
- private:
- union
- {
- float fMatrix[9];
- float m[3][3];
- struct
- {
- float xx, xy, xz;
- float yx, yy, yz;
- float zx, zy, zz;
- };
- };
-};
-
-#include "mat3.inl"
-
-#endif
-
diff --git a/homer_mapping/src/OccupancyMap/Math/mat3.inl b/homer_mapping/src/OccupancyMap/Math/mat3.inl
deleted file mode 100755
index ae2e6c5fd6adf6de4941053fc6cd9f5457b08092..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/mat3.inl
+++ /dev/null
@@ -1,44 +0,0 @@
-/*******************************************************************************
- * mat3.h
- *
- * (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Author: Frank Neuhaus, Susanne Maur
- *******************************************************************************/
-
-#define THIS CMat3
-
-//----------------------------------------------------------------------------------------------------------
-inline THIS::CMat3()
-{
- for(int i=0;i<9;i++) fMatrix[i]=0.0f;
-}
-//----------------------------------------------------------------------------------------------------------
-inline THIS::CMat3( float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz )
-{
- fMatrix[0] = xx;
- fMatrix[1] = xy;
- fMatrix[2] = xz;
- fMatrix[3] = yx;
- fMatrix[4] = yy;
- fMatrix[5] = yz;
- fMatrix[6] = zx;
- fMatrix[7] = zy;
- fMatrix[8] = zz;
-}
-//----------------------------------------------------------------------------------------------------------
-inline float& THIS::operator [](const unsigned value) {
- return fMatrix[value];
-}
-//----------------------------------------------------------------------------------------------------------
-inline float THIS::valueAt(unsigned i) const {
- return fMatrix[i];
-}
-//----------------------------------------------------------------------------------------------------------
-inline void THIS::setValue(unsigned line, unsigned column, float value) {
- m[line][column] = value;
-}
-//----------------------------------------------------------------------------------------------------------
-
-#undef THIS
diff --git a/homer_mapping/src/OccupancyMap/Math/misc.cpp b/homer_mapping/src/OccupancyMap/Math/misc.cpp
deleted file mode 100644
index 1f4facd6e65d02c5fceb39d8f0abfd60bfca18bf..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/misc.cpp
+++ /dev/null
@@ -1,390 +0,0 @@
-/*******************************************************************************
- * misc.cpp
- *
- * (C) 2006 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Author: Frank Neuhaus
- *******************************************************************************/
-
-#include "vec2.h"
-#include "misc.h"
-#include <iostream>
-#include "math.h"
-#include <assert.h>
-#include "Obb2D.h"
-
-/**
-* m - center of the circle
-* r - radius of the circle
-* x - starting point of the ray
-* t - ray
-* f - param - return value
-*/
-bool intersectRayCircle(const CVec2& m, float r, const CVec2& x, const CVec2& t, float& f)
-{
- //|x-m|=r
- //|x+r*t-m|=r
- //(x+r*t-m)^2=r^2
- //((x-m)+r*t)^2=r^2
- //(x-m)^2+2*(x-m)*r*t+r^2*t^2=r^2
-
- //r^2*t^2+r*2*(x-m)*t+(x-m)^2-r^2
-
-
- float invtSqr=1.0f/t.sqr();
- float p = 2.0f*((x-m)*t)*invtSqr;
- float q = ((m-x).sqr() - r*r)*invtSqr;
- float diskr = p*p*0.25f - q;
-
- if (diskr < 0)
- {
- return false;
- }
-
- diskr=sqrtf(diskr);
- f = -0.5f*p - diskr;
-
- if (f<0)
- {
- f = -0.5f*p + diskr;
- if (f>0)
- {
- return true;
- }
- else
- {
- return false;
- }
- }
-/*
- if (f<0)
- {
- std::cout << "smaller" << f << std::endl;
- if (t*(m-x)>0)
- {
- f=0.001;
- return true;
- }
- else
- return false;
- }
- */
- return true;
-
-}
-
-
-bool intersectRayLineSegment(const CVec2& a, const CVec2& b, const CVec2& x, const CVec2& t, float&f)
-{
- CVec2 n=(b-a).getNormal();
-
- float denom=t*n;
-
- if (fabs(denom)<0.000001f)
- return false;
-
- f=(n*a-n*x)/denom;
-
- if (f<0)
- {
- return false;
- }
-
- CVec2 pt=x+t*f;
- if ((a-pt)*(b-pt)<0)
- return true;
- return false;
-}
-
-float shortestDistanceToLineSegment(const CVec2& a, const CVec2& b, const CVec2& x)
-{
- CVec2 dir=b-a;
-
- if ( (dir*dir) < 0.00001 )
- {
- float m1=(x-a).magnitude();
- float m2=(x-b).magnitude();
- if (m1<m2) return m1;
- else return m2;
- }
-
- float r=(dir*x-dir*a)/(dir*dir);
- if (r<=0.0f)
- return (x-a).magnitude();
- if (r>=1.0f)
- return (x-b).magnitude();
-
- return (a+r*dir-x).magnitude();
-
-}
-
-bool intersectRayCapsule(const CVec2& x, const CVec2& t, const CVec2& a, const CVec2& b, float radius, float& f)
-{
- f=99999999;
- float r;
- bool hadInt=false;
- if (intersectRayCircle(a,radius,x,t,r))
- {
- if (r<f)
- f=r;
- hadInt=true;
- }
- if (intersectRayCircle(b,radius,x,t,r))
- {
- if (r<f)
- f=r;
- hadInt=true;
- }
- CVec2 n=normalize(b-a).getNormal();
- if (intersectRayLineSegment(a+radius*n,b+radius*n,x,t,r))
- {
- if (r<f)
- f=r;
- hadInt=true;
- }
- if (intersectRayLineSegment(a-radius*n,b-radius*n,x,t,r))
- {
- if (r<f)
- f=r;
- hadInt=true;
- }
- return hadInt;
-}
-
-// c - punkt wo arc startet
-// p - zentrum des arcs
-// r0 - arc radius
-// hd - heading
-// m1 - kreiszentrum
-// r1 - kreisradius
-// angle - output
-bool intersectPathCircle(const CVec2& c, const CVec2& p, float r0, const CVec2& hd, const CVec2& m1, float r1, float& angle)
-{
- CVec2 vec=p-m1;
- float s=vec.sqr();
- if (s>sqr(fabs(r0)+r1))
- return false;
-
- if (s<sqr(fabs(r0)-r1))
- return false;
-
- float d=sqrtf(s);
-
- float b=(r0*r0-r1*r1+s)/(2*d);
-
- CVec2 mid=p-vec*(b/d);
-
- float h=sqrtf(r0*r0-b*b);
- //std::cout << " h : " << h << std::endl;
-
- CVec2 n=(vec*(h/d)).getNormal();
-
-
- //CVec2 pm=p-m1;
-
- CVec2 p1=(mid+n-p);
- CVec2 p2=(mid-n-p);
-
- CVec2 pc=normalize(c-p);
-
- float angle0=acosf(normalize(p1)*pc);
- float angle1=acosf(normalize(p2)*pc);
-
- //std::cout << "angle0: " << angle0 << std::endl;
-
- if ((p1*hd)<0) angle0=2*M_PI-angle0;
- if ((p2*hd)<0) angle1=2*M_PI-angle1;
-
- if (angle0<angle1) angle=angle0;
- else angle=angle1;
-
- //angle=angle0;
-
- return true;
-
-
-}
-
-bool intersectPathLine(const CVec2& c, const CVec2& p, float r0, const CVec2& hd, const CVec2& p1, const CVec2& p2, float& angle)
-{
- CVec2 vec=p2-p1;
-
-
- /*
- float invtSqr=1.0f/t.sqr();
- float p = 2.0f*((x-m)*t)*invtSqr;
- float q = ((m-x).sqr() - r*r)*invtSqr;
- float diskr = p*p*0.25f - q;
- */
-
- if (((p1-p).sqr()<r0*r0)&&((p2-p).sqr()<r0*r0))
- return false;
-
-
- float invtSqr=1.0f/vec.sqr();
- float pa = 2.0f*((p1-p)*vec)*invtSqr;
- float qi = ((p-p1).sqr() - r0*r0)*invtSqr;
- float diskr = pa*pa*0.25f - qi;
-
- if (diskr < 0)
- {
- return false;
- }
-
- float sqrtfdiskr=sqrtf(diskr);
-
- float f1 = -0.5f*pa - sqrtfdiskr;
- float f2 = -0.5f*pa + sqrtfdiskr;
-
- if (f1>1) return false;
- if (f2<0) return false;
-
- //std::cout << "f1: " << f1 << " f2: " << f2 << std::endl;
-
- CVec2 int1=p1+f1*vec;
- CVec2 int2=p1+f2*vec;
-
- //assert((int2-int1).magnitude()<(p1-p2).magnitude());
-
- CVec2 pc=normalize(c-p);
-
- float angle0=acosf(normalize(int1-p)*pc);
- float angle1=acosf(normalize(int2-p)*pc);
-
-
- if ((int1-p)*hd<0)
- {
- angle0=2*M_PI-angle0;
- }
- if ((int2-p)*hd<0)
- {
- angle1=2*M_PI-angle1;
- }
-
-
- if (angle0<angle1) angle=angle0;
- else angle=angle1;
-
- if (f1<0) angle=angle1;
- if (f2>1) angle=angle0;
-
-
-
- return true;
-
-}
-
-// c - punkt wo arc startet
-// p - arc zentrum
-// r0 - radius des arcs
-// hd - heading
-// a - linienanfang
-// b - linienende
-// radius - linienradius
-// r [out] - wie weit ging der arc?
-
-bool intersectPathCapsule(const CVec2& c, const CVec2& p, float r0, const CVec2& hd, const CVec2& a, const CVec2& b, float radius, float& f)
-{
- f=99999999;
- float r;
- bool hadInt=false;
- if (intersectPathCircle(c,p,r0,hd,a,radius,r))
- {
- if (r<f)
- f=r;
- hadInt=true;
- }
- if (intersectPathCircle(c,p,r0,hd,b,radius,r))
- {
- if (r<f)
- f=r;
- hadInt=true;
- }
- CVec2 n=normalize(b-a).getNormal();
- if (intersectPathLine(c,p,r0,hd,a+radius*n,b+radius*n,r))
- {
- if (r<f)
- f=r;
- hadInt=true;
- }
- if (intersectPathLine(c,p,r0,hd,a-radius*n,b-radius*n,r))
- {
- if (r<f)
- f=r;
- hadInt=true;
- }
- return hadInt;
-}
-
-
-bool isInAABB(const std::pair<CVec2,CVec2>& aabb, const CVec2& p)
-{
- const CVec2& mins=aabb.first;
- const CVec2& maxs=aabb.second;
- if ((p[0]>mins[0]) &&(p[1]>mins[1])&&(p[0]<maxs[0]) &&(p[1]<maxs[1]))
- return true;
- return false;
-}
-
-bool testAABBOverlap(const std::pair<CVec2,CVec2>& a, const std::pair<CVec2,CVec2>& b)
-{
- const CVec2& vMins=b.first;
- const CVec2& vMaxs=b.second;
- CVec2 B=(vMins+vMaxs)*0.5f;
- CVec2 A=(a.first+a.second)*0.5f;
- CVec2 E=a.second-A;
- CVec2 bE=vMaxs-B;
-
- const CVec2 T = B - A;//vector from A to B
- return fabs(T[0]) <= (E[0] + bE[0])
- &&
- fabs(T[1]) <= (E[1] + bE[1]);
-
-}
-
-float computeOBBIntersection(const CVec2& a, const CVec2& b, const CVec2& c, const CVec2& d, float size)
-{
- OBB2D g;
- CVec2 dab=normalize(b-a)*size;
- CVec2 nab=dab.getNormal();
- g[0]=a+nab-dab;
- g[1]=a-nab-dab;
- g[2]=b-nab+dab;
- g[3]=b+nab+dab;
-
-
- OBB2D h;
- CVec2 dcd=normalize(d-c)*size;
- CVec2 ncd=dcd.getNormal();
- h[0]=c+ncd-dcd;
- h[1]=c-ncd-dcd;
- h[2]=d-ncd+dcd;
- h[3]=d+ncd+dcd;
-
- std::pair<CVec2,CVec2> aabb=g.computeAABB();
- std::pair<CVec2,CVec2> aabb2=h.computeAABB();
-
- //static int all=0;
- //all++;
-
-
- if (!testAABBOverlap(aabb,aabb2))
- {
- return 0;
- }
-
- //h.clip(g);
- /*
- static int clips=0;
- clips++;
- if (clips%10000==0)
- std::cout<<"clips: " << clips << " rate: " << float(clips)/float(all) << std::endl;
- */
- return h.computeClippedArea(g)/(2*size*((a-b).magnitude()+2*size));
-
-}
-
-
-
-
diff --git a/homer_mapping/src/OccupancyMap/Math/misc.h b/homer_mapping/src/OccupancyMap/Math/misc.h
deleted file mode 100644
index 5175e3cd490488a5562afa42cba815244474fd57..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/misc.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*******************************************************************************
- * misc.h
- *
- * (C) 2006 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Author: Frank Neuhaus
- *******************************************************************************/
-
-#ifndef MISC_H
-#define MISC_H
-
-template<class T>
-T sqr(T f)
-{
- return f*f;
-}
-
-bool intersectRayCircle(const CVec2& m, float r, const CVec2& x, const CVec2& t, float& f);
-bool intersectRayLineSegment(const CVec2& a, const CVec2& b, const CVec2& x, const CVec2& t, float&f);
-float shortestDistanceToLineSegment(const CVec2& a, const CVec2& b, const CVec2& x);
-bool intersectRayCapsule(const CVec2& x, const CVec2& t, const CVec2& a, const CVec2& b, float radius, float& f);
-
-bool intersectPathCircle(const CVec2& c, const CVec2& p, float r0, const CVec2& hd, const CVec2& m1, float r1, float& angle);
-bool intersectPathLine(const CVec2& c, const CVec2& p, float r0, const CVec2& hd, const CVec2& p1, const CVec2& p2, float& angle);
-
-bool intersectPathCapsule(const CVec2& c, const CVec2& p, float r0, const CVec2& hd, const CVec2& a, const CVec2& b, float radius, float& f);
-
-float computeOBBIntersection(const CVec2& a, const CVec2& b, const CVec2& c, const CVec2& d, float size);
-
-#endif
-
diff --git a/homer_mapping/src/OccupancyMap/Math/vec2.h b/homer_mapping/src/OccupancyMap/Math/vec2.h
deleted file mode 100644
index 39a72c8eb60c6b6bf836dc7d7be932c376e25fd0..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/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_mapping/src/OccupancyMap/Math/vec3_inl.h b/homer_mapping/src/OccupancyMap/Math/vec3_inl.h
deleted file mode 100644
index 533eabbb26e04cf4efa6ea60a890c529bf309476..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/Math/vec3_inl.h
+++ /dev/null
@@ -1,179 +0,0 @@
-/*******************************************************************************
- * vec3_inl.h
- *
- * (C) 2006 AG Aktives Sehen <agas@uni-koblenz.de>
- * Universitaet Koblenz-Landau
- *
- * Author: Frank Neuhaus
- *******************************************************************************/
-
-#define THIS Vector3D
-
-#include <sstream>
-
-//----------------------------------------------------------------------------------------------------------
-inline THIS::THIS()
-{}
-//----------------------------------------------------------------------------------------------------------
-inline THIS::THIS ( float x, float y, float z )
-{
- m_X = x; m_Y = y; m_Z = z;
-};
-//----------------------------------------------------------------------------------------------------------
-inline THIS::THIS ( const Vector3D& v2 )
-{
- m_X=v2[0];
- m_Y=v2[1];
- m_Z=v2[2];
-};
-//----------------------------------------------------------------------------------------------------------
-inline THIS::THIS ( const CVec2& v, float z)
-{
- m_X=v[0];
- m_Y=v[1];
- m_Z=z;
-}
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D THIS::operator+ ( const Vector3D& vVector ) const
-{
- return Vector3D ( vVector[0] + m_X, vVector[1] + m_Y, vVector[2] + m_Z );
-};
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D THIS::operator-() const
-{
- return Vector3D ( -m_X,-m_Y,-m_Z );
-};
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D THIS::operator+() const
-{
- return Vector3D ( m_X,m_Y,m_Z );
-};
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D THIS::operator- ( const Vector3D& vVector ) const
-{
- return Vector3D ( m_X-vVector[0],m_Y-vVector[1],m_Z-vVector[2] );
-};
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D THIS::operator* ( const float num ) const
-{
- return Vector3D ( m_X * num, m_Y * num, m_Z * num );
-};
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D& THIS::operator*= ( const float num )
-{
- m_X*=num; m_Y*=num; m_Z*=num;
- return *this;
-}
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D& THIS::operator/= ( const float num )
-{
- m_X/=num; m_Y/=num; m_Z/=num;
- return *this;
-}
-//----------------------------------------------------------------------------------------------------------
-inline float THIS::operator* ( const Vector3D& vVector ) const
-{
- return vVector[0]*m_X+vVector[1]*m_Y+vVector[2]*m_Z;
-};
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D& THIS::operator+= ( const Vector3D& vVector )
-{
- m_X+=vVector[0];
- m_Y+=vVector[1];
- m_Z+=vVector[2];
- return *this;
-}
-//----------------------------------------------------------------------------------------------------------
-inline bool THIS::operator < ( const Vector3D& vVec ) const
-{
- if ( m_X<vVec[0] ) return true;
- if ( m_X>vVec[0] ) return false;
- if ( m_Y<vVec[1] ) return true;
- if ( m_Y>vVec[1] ) return false;
- return ( m_Z<vVec[2] );
-}
-//----------------------------------------------------------------------------------------------------------
-inline bool THIS::operator > ( const Vector3D& vVec ) const
-{
- if ( m_X<vVec[0] ) return false;
- if ( m_X>vVec[0] ) return true;
- if ( m_Y<vVec[1] ) return false;
- if ( m_Y>vVec[1] ) return true;
- return ( m_Z>vVec[2] );
-}
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D& THIS::operator-= ( const Vector3D& vVector )
-{
- m_X-=vVector[0];
- m_Y-=vVector[1];
- m_Z-=vVector[2];
- return *this;
-}
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D THIS::operator^ ( const Vector3D& vVector2 ) const
-{
- Vector3D vNormal;
-
- // Calculate the cross product with the non communitive equation
- vNormal[0] = ( ( m_Y * vVector2[2] ) - ( m_Z * vVector2[1] ) );
- vNormal[1] = ( ( m_Z * vVector2[0] ) - ( m_X * vVector2[2] ) );
- vNormal[2] = ( ( m_X * vVector2[1] ) - ( m_Y * vVector2[0] ) );
-
- // Return the cross product
- return vNormal;
-};
-//----------------------------------------------------------------------------------------------------------
-inline bool THIS::operator== ( const Vector3D& v1 ) const
-{
- // this is evil!
- assert ( 0 );
-
- static const float EPS=1.0f/100.0f;
- if ( fabs ( v1[0]-m_X ) >EPS ) return false;
- if ( fabs ( v1[1]-m_Y ) >EPS ) return false;
- if ( fabs ( v1[2]-m_Z ) >EPS ) return false;
-
- return true;
-};
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D THIS::operator/ ( float num ) const
-{
- return Vector3D ( m_X / num, m_Y / num, m_Z / num );
-};
-//----------------------------------------------------------------------------------------------------------
-inline float THIS::operator [] ( const unsigned int i ) const
-{
- return ( ( float* ) this ) [i];
-}
-//----------------------------------------------------------------------------------------------------------
-inline float& THIS::operator [] ( const unsigned int i )
-{
- return ( ( float* ) this ) [i];
-}
-//----------------------------------------------------------------------------------------------------------
-inline void THIS::set ( float fx, float fy, float fz )
-{
- m_X=fx; m_Y=fy; m_Z=fz;
-}
-//----------------------------------------------------------------------------------------------------------
-inline void THIS::lerp ( const Vector3D& v1, const Vector3D& v2,float f )
-{
- ( *this ) =v1* ( 1-f ) +v2*f;
-}
-//----------------------------------------------------------------------------------------------------------
-inline float THIS::magnitude() const
-{
- return sqrtf ( m_X*m_X+m_Y*m_Y+m_Z*m_Z );
-}
-//----------------------------------------------------------------------------------------------------------
-//----------------------------------------------------------------------------------------------------------
-//----------------------------------------------------------------------------------------------------------
-inline Vector3D operator* ( const float n,const Vector3D& v )
-{
- return Vector3D ( v[0]*n,v[1]*n,v[2]*n );
-};
-//----------------------------------------------------------------------------------------------------------
-
-
-#undef THIS
-
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
deleted file mode 100644
index 811d4f31bc18c44d1eb5a3a21754424b23ed8db8..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ /dev/null
@@ -1,931 +0,0 @@
-#include "OccupancyMap.h"
-
-#include "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 "tools/loadRosConfig.h"
-#include "tools/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;
- loadConfigValue("/homer_mapping/size", mapSize);
- loadConfigValue("/homer_mapping/resolution", m_Resolution);
-
- //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;
-
- loadConfigValue("/homer_mapping/backside_checking", m_BacksideChecking);
- loadConfigValue("/homer_mapping/obstacle_borders", m_ObstacleBorders);
- loadConfigValue("/homer_mapping/measure_sampling_step", m_MeasureSamplingStep);
- loadConfigValue("/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();
-}
-
-
-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;
-
- loadConfigValue("/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::LaserScanConstPtr laserData )
-{
- markRobotPositionFree();
-
- m_LaserMaxRange = laserData->range_max;
- m_LaserMinRange = laserData->range_min;
- tf::StampedTransform laserTransform;
- try
- {
- m_tfListener.lookupTransform("/base_link", laserData->header.frame_id, ros::Time(0), laserTransform);
- }
- catch (tf::TransformException ex) {
- ROS_ERROR_STREAM(ex.what());
- }
-
- m_LaserPos.x = laserTransform.getOrigin().getX();
- m_LaserPos.y = 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*0.8 )
- {
- range = m_LaserMaxRange*0.8;
- }
-
- //choose smaller range
- for ( unsigned j=lastValidIndex+1; j<i; j++ )
- {
- rangeMeasurement.endPos = map_tools::laser_range_to_point(range, j, laserData->angle_min, laserData->angle_increment, m_tfListener, laserData->header.frame_id, "/base_link");// laserConf->nativeLaserToRobot ( j, range ); //TODO use tf
- rangeMeasurement.free = true;
- ranges.push_back ( rangeMeasurement );
- }
- }
- rangeMeasurement.endPos = map_tools::laser_range_to_point(laserData->ranges[i], i, laserData->angle_min, laserData->angle_increment, m_tfListener, laserData->header.frame_id, "/base_link");
- 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 );
-}
-
-
-void OccupancyMap::insertRanges ( vector<RangeMeasurement> ranges )
-{
- 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_tfListener, "/base_link", "/map");
- 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_tfListener, "/base_link", "/map");
- 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_tfListener, "/base_link", "/map");
- 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_tfListener, "/base_link", "/map");
- 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
- for ( unsigned i=0; i<ranges.size(); i++ )
- {
- geometry_msgs::Point sensorPosWorld = map_tools::transformPoint(ranges[i].sensorPos, m_tfListener, "/base_link", "/map");
- geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_tfListener, "/base_link", "/map");
- Eigen::Vector2i sensorPixel = map_tools::toMapCoords(sensorPosWorld, m_Origin, m_Resolution);
- 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 )
- {
- geometry_msgs::Point hitPosMsg = map_tools::laser_range_to_point(laserData->ranges[i], i, laserData->angle_min, laserData->angle_increment, m_tfListener, laserData->header.frame_id, "/base_link"); //laserConf->nativeLaserToRobot ( i, laserData[i] ); //tf
- Point2D hitPos(hitPosMsg.x, hitPosMsg.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_tfListener, "/base_link", "/map");
- Eigen::Vector2i robotPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution);
-
- int width = 0.3 / 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/OccupancyMap/OccupancyMap.h b/homer_mapping/src/OccupancyMap/OccupancyMap.h
deleted file mode 100644
index 718b2dab1ebb42ea22a949aef0aa4f3e2c4e3002..0000000000000000000000000000000000000000
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.h
+++ /dev/null
@@ -1,379 +0,0 @@
-#ifndef OCCUPANCYMAP_H
-#define OCCUPANCYMAP_H
-
-#include <vector>
-#include <list>
-#include <string>
-#include <iostream>
-
-#include <Eigen/Geometry>
-
-#include "Math/Pose.h"
-#include "Math/Point2D.h"
-#include "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::LaserScanConstPtr laserData );
-
- void insertRanges( vector<RangeMeasurement> ranges );
-
- /**
- * @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;
-
-};
-#endif
diff --git a/homer_mapping/src/ParticleFilter/CMakeLists.txt b/homer_mapping/src/ParticleFilter/CMakeLists.txt
deleted file mode 100644
index b144c3d030dbf1e8061528266dbfb4a1aa582fc0..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/CMakeLists.txt
+++ /dev/null
@@ -1,18 +0,0 @@
-set(
- ParticleFilter_SRC
- HyperSlamFilter.cpp
- SlamParticle.cpp
- SlamFilter.cpp
- Particle.cpp
-)
-
-add_library(
- ParticleFilter
- ${ParticleFilter_SRC}
-)
-
-target_link_libraries(
- ParticleFilter
- OccupancyMap
- ${homer_nav_libs_LIBRARIES}
-)
diff --git a/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp b/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
deleted file mode 100755
index cd005dbd3b597b41e5b8594d7b938f5abe1219e8..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
+++ /dev/null
@@ -1,191 +0,0 @@
-#include "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 = (rand() % 100) < 80;
- //if mapping is on, switch on with 80% probability to introduce some randomness in different particle filters
- m_SlamFilters[i]->setMapping( m_DoMapping && randOnOff );
- m_SlamFilters[i]->filter(currentPose, laserData, measurementTime, filterDuration);
- }
-
- //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/HyperSlamFilter.h b/homer_mapping/src/ParticleFilter/HyperSlamFilter.h
deleted file mode 100755
index d02c2b6d23274eb95e2b21f0818585cd650227ca..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/HyperSlamFilter.h
+++ /dev/null
@@ -1,163 +0,0 @@
-#ifndef HYPERSLAMFILTER_H
-#define HYPERSLAMFILTER_H
-
-#include <vector>
-#include "ParticleFilter.h"
-#include "SlamParticle.h"
-#include "SlamFilter.h"
-#include "Math/Pose.h"
-#include "../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/src/ParticleFilter/Particle.cpp b/homer_mapping/src/ParticleFilter/Particle.cpp
deleted file mode 100644
index bce3b5b1a07ed91f6003bf463bf0d2915aef85f9..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/Particle.cpp
+++ /dev/null
@@ -1,10 +0,0 @@
-#include "Particle.h"
-
-Particle::Particle(float weight, int id) {
- m_Weight = weight;
- m_Id = id;
-}
-
-Particle::~Particle() {
-}
-
diff --git a/homer_mapping/src/ParticleFilter/Particle.h b/homer_mapping/src/ParticleFilter/Particle.h
deleted file mode 100644
index bd96aa857da906c16c47914e38c7bbe000178e2b..0000000000000000000000000000000000000000
--- a/homer_mapping/src/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/src/ParticleFilter/ParticleFilter.h b/homer_mapping/src/ParticleFilter/ParticleFilter.h
deleted file mode 100644
index fa6a7f996c7afa158b0d45be40d01f679aed3f11..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/ParticleFilter.h
+++ /dev/null
@@ -1,310 +0,0 @@
-#ifndef PARTICLEFILTER_H
-#define PARTICLEFILTER_H
-
-#include <iostream>
-#include <cmath>
-#include <limits.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) {
- for (int 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/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
deleted file mode 100644
index 2ebdbd16beb072f34101f9d82564c96c875e9667..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp
+++ /dev/null
@@ -1,653 +0,0 @@
-#include "SlamFilter.h"
-
-#include "Math/Transformation2D.h"
-#include "Math/Math.h"
-
-#include "tools/loadRosConfig.h"
-
-#include "tf/transform_broadcaster.h"
-
-#include <vector>
-#include <cmath>
-#include <fstream>
-#include <sstream>
-
-#include "ros/ros.h"
-
-// minimum move for translation in m
-const float MIN_MOVE_DISTANCE2 = 0.02 * 0.02;
-// minimum turn in radiants
-const float MIN_TURN_DISTANCE2 = 0.1 * 0.1;
-
-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;
- loadConfigValue("/particlefilter/error_values/rotation_error_rotating", rotationErrorRotating);
- float rotationErrorTranslating = 0.0;
- loadConfigValue("/particlefilter/error_values/rotation_error_translating", rotationErrorTranslating);
- float translationErrorTranslating = 0.0;
- loadConfigValue("/particlefilter/error_values/translation_error_translating", translationErrorTranslating);
- float translationErrorRotating = 0.0;
- loadConfigValue("/particlefilter/error_values/translation_error_translating", translationErrorRotating);
- float moveJitterWhileTurning = 0.0;
- loadConfigValue("/particlefilter/error_values/move_jitter_while_turning", moveJitterWhileTurning);
- loadConfigValue("/particlefilter/max_rotation_per_second", m_MaxRotationPerSecond);
-
- int updateMinMoveAngleDegrees;
- loadConfigValue("/particlefilter/update_min_move_angle", updateMinMoveAngleDegrees);
- m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
- loadConfigValue("/particlefilter/update_min_move_dist", m_UpdateMinMoveDistance);
- double maxUpdateInterval;
- loadConfigValue("/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;
- loadConfigValue("/particlefilter/error_values/rotation_error_rotating", rotationErrorRotating);
- float rotationErrorTranslating = 0.0;
- loadConfigValue("/particlefilter/error_values/rotation_error_translating", rotationErrorTranslating);
- float translationErrorTranslating = 0.0;
- loadConfigValue("/particlefilter/error_values/translation_error_translating", translationErrorTranslating);
- float translationErrorRotating = 0.0;
- loadConfigValue("/particlefilter/error_values/translation_error_translating", translationErrorRotating);
- float moveJitterWhileTurning = 0.0;
- loadConfigValue("/particlefilter/error_values/move_jitter_while_turning", moveJitterWhileTurning);
- loadConfigValue("/particlefilter/max_rotation_per_second", m_MaxRotationPerSecond);
-
- int updateMinMoveAngleDegrees;
- loadConfigValue("/particlefilter/update_min_move_angle", updateMinMoveAngleDegrees);
- m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
- loadConfigValue("/particlefilter/update_min_move_dist", m_UpdateMinMoveDistance);
- double maxUpdateInterval;
- loadConfigValue("/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 )
- {
- m_OccupancyMap->insertLaserData ( laserData );
- }
- m_CurrentLaserData = 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 / 5 )
- {
- resample();
- ROS_INFO_STREAM( "Particles too scattered, resampling." );
- }
- }
- else
- {
- resample();
- }
-
- // filter steps
- drift();
- measure();
- normalize();
-
- sort ( 0, m_ParticleNum - 1 );
-
- // mapping step
- trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
- double elapsedSeconds = ( measurementTime - m_ReferenceMeasurementTime ).toSec();
- double thetaPerSecond;
- if(elapsedSeconds == 0.0)
- thetaPerSecond = trans.theta();
- else
- thetaPerSecond = trans.theta() / elapsedSeconds;
-
- Pose likeliestPose = getLikeliestPose();
- 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.";
- m_LastUpdatePose = likeliestPose;
- m_LastUpdateTime = measurementTime;
- if ( thetaPerSecond < m_MaxRotationPerSecond )
- {
- updateMap();
- }
- else
- {
- ROS_INFO_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 );
- for ( int 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 );
-
- for ( int 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()
-{
- getLikeliestPose(); //call to trigger tf broadcast
- m_OccupancyMap->insertLaserData ( m_CurrentLaserData );
-}
-
-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() const
-{
- float percentage = 3;
- int numParticles = static_cast<int> ( percentage / 100 * m_ParticleNum );
- if ( 0 == numParticles ) numParticles = 1;
- float sumX = 0, sumY = 0, sumDirX = 0, sumDirY = 0;
- for ( int i = 0; i < numParticles; i++ )
- {
- float robotX, robotY, robotTheta;
- SlamParticle* particle = m_CurrentList[i];
- particle->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 );
- //broadcast transform map -> base_link
- tf::Transform transform(tf::createQuaternionFromYaw(meanTheta),
- tf::Vector3(sumX / numParticles, sumY / numParticles, 0.0));
- tf::TransformBroadcaster tfBroadcaster;
- tfBroadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "base_link"));
- return Pose ( sumX / numParticles, sumY / numParticles, 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/SlamFilter.h b/homer_mapping/src/ParticleFilter/SlamFilter.h
deleted file mode 100644
index f8d969016b630fdb59a4eeaaa4f5c42861e92e27..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/SlamFilter.h
+++ /dev/null
@@ -1,314 +0,0 @@
-#ifndef SLAMFILTER_H
-#define SLAMFILTER_H
-
-#include <vector>
-#include "ParticleFilter.h"
-#include "SlamParticle.h"
-#include "Math/Pose.h"
-#include "../OccupancyMap/OccupancyMap.h"
-
-#include "sensor_msgs/LaserScan.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() const;
-
- /**
- * 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;
-
- /**
- * 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/src/ParticleFilter/SlamParticle.cpp b/homer_mapping/src/ParticleFilter/SlamParticle.cpp
deleted file mode 100644
index 7b501f0ca5b60cf12875a2e045e93d0a93d98ad2..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/SlamParticle.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-#include "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/ParticleFilter/SlamParticle.h b/homer_mapping/src/ParticleFilter/SlamParticle.h
deleted file mode 100644
index 71da4a81ec7ff45f77d88b8b9518481b5f51163c..0000000000000000000000000000000000000000
--- a/homer_mapping/src/ParticleFilter/SlamParticle.h
+++ /dev/null
@@ -1,72 +0,0 @@
-#ifndef SLAMPARTICLE_H
-#define SLAMPARTICLE_H
-
-#include <iostream>
-#include <fstream>
-
-#include "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/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp
deleted file mode 100644
index 6d196437e43972df84d799c1474ad54f1190c974..0000000000000000000000000000000000000000
--- a/homer_mapping/src/slam_node.cpp
+++ /dev/null
@@ -1,318 +0,0 @@
-#include <sstream>
-#include <vector>
-#include <iostream>
-#include <fstream>
-#include <sstream>
-#include <cmath>
-#include <stdlib.h>
-
-#include "slam_node.h"
-
-//receive:
-#include "sensor_msgs/LaserScan.h"
-#include "nav_msgs/Odometry.h"
-#include "nav_msgs/OccupancyGrid.h"
-#include "tf/tf.h"
-
-#include "tools/loadRosConfig.h"
-
-#include "ParticleFilter/SlamFilter.h"
-#include "ParticleFilter/HyperSlamFilter.h"
-#include "Math/Box2D.h"
-#include "OccupancyMap/OccupancyMap.h"
-
-SlamNode::SlamNode(ros::NodeHandle* nh)
- : m_HyperSlamFilter( 0 )
-{
- init();
-
- // subscribe to topics
- m_LaserScanSubscriber = nh->subscribe("/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_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);
-
- m_InitialPoseSubscriber = nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, &SlamNode::callbackInitialPose, this);
-
- // advertise topics
- m_PoseStampedPublisher = nh->advertise<geometry_msgs::PoseStamped>("/pose", 1);
- m_SLAMMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
-}
-
-void SlamNode::init()
-{
- double waitTime;
- loadConfigValue("/particlefilter/wait_time", waitTime);
- m_WaitDuration = ros::Duration(waitTime);
- loadConfigValue("/selflocalization/scatter_var_xy", m_ScatterVarXY);
- loadConfigValue("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
-
- m_DoMapping = true;
-
- int particleNum;
- loadConfigValue("/particlefilter/particle_num", particleNum);
- int particleFilterNum;
- loadConfigValue("/particlefilter/hyper_slamfilter/particlefilter_num", particleFilterNum);
- m_HyperSlamFilter = new HyperSlamFilter ( particleFilterNum, particleNum );
-
- m_ReferenceOdometryTime = ros::Time(0);
- m_LaserDataTime = ros::Time(0);;
-
- m_LastLaserMessageId = 0;
- m_LastMapSendTime = ros::Time(0);
- m_LastPositionSendTime = ros::Time(0);
- m_LastMappingTime = ros::Time(0);
-}
-
-SlamNode::~SlamNode()
-{
- delete m_HyperSlamFilter;
-}
-
-void SlamNode::processMeasurements ( ros::Time odoTime, Pose currentOdometryPose )
-{
- // laserscan in between current odometry reading and m_ReferenceOdometry
- // -> calculate pose of robot during laser scan
- ros::Duration d1 = m_LaserDataTime - m_ReferenceOdometryTime;
- ros::Duration d2 = odoTime - 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);
-
- Pose interpolatedPose = m_ReferenceOdometryPose.interpolate ( currentOdometryPose, timeFactor );
- m_HyperSlamFilter->filter( interpolatedPose, m_LastLaserData, m_LaserDataTime, duration);
-}
-
-void SlamNode::resetMaps()
-{
- ROS_INFO( "Resetting maps.." );
-
- delete m_HyperSlamFilter;
- m_HyperSlamFilter = 0;
- init();
-
- sendMapDataMessage();
- sendPositionDataMessage();
-}
-
-void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg)
-{
- m_HyperSlamFilter->resetHigh();
-
-}
-
-
-void SlamNode::sendPositionDataMessage()
-{
- Pose pose = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose();
-
- geometry_msgs::PoseStamped poseMsg;
- //header
- poseMsg.header.stamp = ros::Time::now();
- poseMsg.header.frame_id = "map";
-
- //position and orientation
- poseMsg.pose.position.x = pose.x();
- poseMsg.pose.position.y = pose.y();
- poseMsg.pose.position.z = 0.0;
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(pose.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(pose.x(), pose.y(), 0.0));
- m_tfBroadcaster.sendTransform(tf::StampedTransform(transform, poseMsg.header.stamp, "map", "base_link"));
- m_LastLaserMessageId = 0;
-}
-
-void SlamNode::sendMapDataMessage()
-{
- 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 = ros::Time::now();
- 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::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg)
-{
- m_LaserDataTime = ros::Time::now();//msg->header.stamp; TODO use msg stamp
- m_LastLaserData = msg;
-}
-
-void SlamNode::callbackOdometry( const nav_msgs::Odometry::ConstPtr& msg) {
- ros::Time currentOdometryTime = ros::Time::now();//msg->header.stamp; TODO use msg stamp once cu2wd node publishes time in odometry msg
-
- float odoX = msg->pose.pose.position.x;
- float odoY = msg->pose.pose.position.y;
- geometry_msgs::Quaternion quat = msg->pose.pose.orientation;
- float odoTheta = tf::getYaw(quat);
-
- Pose currentOdometryPose ( odoX, odoY, odoTheta );
-
- // check if we have a laserscan in between two odometry readings (or at the same time)
- bool mappingPossible = ( currentOdometryTime - m_LastMappingTime > m_WaitDuration ) &&
- ( !m_ReferenceOdometryTime.isZero()) &&
- ( m_LaserDataTime >= m_ReferenceOdometryTime ) &&
- ( currentOdometryTime >= m_LaserDataTime );
-
- if ( mappingPossible )
- {
- ros::Time startTime = ros::Time::now();
- processMeasurements ( currentOdometryTime, currentOdometryPose );
- ros::Time finishTime = ros::Time::now();
-
- // send map max. every 500 ms
- if ( (finishTime - m_LastMapSendTime).toSec() > 0.5 )
- {
- sendMapDataMessage();
- m_LastMapSendTime = finishTime;
- }
- sendPositionDataMessage();
- m_LastPositionSendTime=finishTime;
- m_LastMappingTime=currentOdometryTime;
-
- ROS_DEBUG_STREAM( "Pos. data delay: " << (finishTime - startTime).toSec() << "s" );
- ROS_DEBUG_STREAM("Map send Interval: " << ( finishTime - m_LastPositionSendTime ).toSec() << "s" );
- }
- m_ReferenceOdometryPose = currentOdometryPose;
- m_ReferenceOdometryTime = currentOdometryTime;
-}
-
-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::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::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(12);
- while (ros::ok())
- {
- ros::spinOnce();
- loop_rate.sleep();
- }
- return 0;
-}
-
diff --git a/homer_mapping/src/slam_node.h b/homer_mapping/src/slam_node.h
deleted file mode 100644
index 803c5c4ba77f4765eb3bd4b9d163d77310572ce1..0000000000000000000000000000000000000000
--- a/homer_mapping/src/slam_node.h
+++ /dev/null
@@ -1,186 +0,0 @@
-#ifndef SLAM_NODE_H
-#define SLAM_NODE_H
-
-#include <vector>
-#include <map>
-
-#include "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 "std_msgs/Empty.h"
-#include "std_msgs/Bool.h"
-#include "geometry_msgs/PoseWithCovarianceStamped.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.
- *
- * @param odoTime timestamp of this odometry data
- * @param currentOdometryPose the current odometry measurements of the robot
- */
- void processMeasurements(ros::Time odoTime, Pose currentOdometryPose);
-
- /**
- * This method retrieves the current map of the slam filter and sends a map
- * data message containing the map.
- */
- void sendMapDataMessage();
-
- /**
- * This method gets the current position from the filter thread and sends it
- * in a position data message.
- */
- void sendPositionDataMessage();
-
- /**
- * This variable stores the identification number of a GetPositionDataM.
- * It is needed for a correct response message.
- */
- unsigned int m_LastLaserMessageId;
-
- /**
- * 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;
-
- /**
- * 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::LaserScanConstPtr m_LastLaserData;
-
- /**
- * This variable stores the time of last laser measurement.
- */
- ros::Time m_LaserDataTime;
-
- /**
- * 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;
-
- /**
- * 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_nav_libs/CHANGELOG.rst b/homer_nav_libs/CHANGELOG.rst
deleted file mode 100644
index d1e43a0c4dd98f715d4b3bbb84211819315890d4..0000000000000000000000000000000000000000
--- a/homer_nav_libs/CHANGELOG.rst
+++ /dev/null
@@ -1,60 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package homer_nav_libs
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.0.11 (2015-12-02)
--------------------
-
-1.0.10 (2015-12-02)
--------------------
-* fixed install()
-* Contributors: Niklas Yann Wettengel
-
-1.0.9 (2015-12-01)
-------------------
-* install header files
-* Contributors: Niklas Yann Wettengel
-
-1.0.8 (2015-11-30)
-------------------
-
-1.0.7 (2015-11-28)
-------------------
-* added install()
-* updated changelog
-* Contributors: Niklas Yann Wettengel
-
-1.0.6 (2015-11-27)
-------------------
-* added export tags
-* added eigen as run_depend
-* removed env HOMER_DIR from CMakeLists.txt
-* Contributors: Niklas Yann Wettengel
-
-1.0.5 (2015-11-24)
-------------------
-* added missing files
-* Contributors: Niklas Yann Wettengel
-
-1.0.4 (2015-11-20)
-------------------
-* changed build dependency from libeigen3-dev to eigen
-* Contributors: Niklas Yann Wettengel
-
-1.0.3 (2015-11-20)
-------------------
-* added libeigen3-dev build dependency
-* Contributors: Niklas Yann Wettengel
-
-1.0.2 (2015-11-20)
-------------------
-* added cmake_modules as build dependency in package.xml
-* added Maintainers
-* removed components
-* Raphael as maintainer added
-* Contributors: Niklas Yann Wettengel, Raphael Memmesheimer
-
-1.0.1 (2015-09-08)
-------------------
-* init
-* Contributors: Raphael Memmesheimer
diff --git a/homer_nav_libs/CMakeLists.txt b/homer_nav_libs/CMakeLists.txt
deleted file mode 100644
index 6f3968cd6ec28f1de5378df829445d24f027e218..0000000000000000000000000000000000000000
--- a/homer_nav_libs/CMakeLists.txt
+++ /dev/null
@@ -1,30 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(homer_nav_libs)
-
-find_package(catkin REQUIRED
- roscpp
- geometry_msgs
- tf
- cmake_modules
-)
-
-find_package(Eigen REQUIRED)
-
-catkin_package(
- INCLUDE_DIRS src
- LIBRARIES Explorer SpeedControl MappingMath
- CATKIN_DEPENDS roscpp geometry_msgs tf
- DEPENDS Eigen)
-
-
-include_directories(
- src
- ${catkin_INCLUDE_DIRS}
- ${Eigen_INCLUDE_DIRS}
-)
-
-add_subdirectory(src/Explorer)
-add_subdirectory(src/SpeedControl)
-add_subdirectory(src/Math)
-
-install(DIRECTORY src/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_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/package.xml b/homer_nav_libs/package.xml
deleted file mode 100644
index 401147727e23cd8974b31992936a60c678d37e69..0000000000000000000000000000000000000000
--- a/homer_nav_libs/package.xml
+++ /dev/null
@@ -1,32 +0,0 @@
-<?xml version="1.0"?>
-<package>
- <name>homer_nav_libs</name>
- <version>1.0.11</version>
- <description>The nav_libs package</description>
-
- <maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
- <maintainer email="niyawe@uni-koblenz.de">Niklas Yann Wettengel</maintainer>
- <maintainer email="heuer@uni-koblenz.de">Gregor Heuer</maintainer>
- <maintainer email="raphael@uni-koblenz.de">Raphael Memmesheimer</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>
-
- <export>
- <cpp cflags="-I${prefix}/src" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lExplorer"/>
- <cpp cflags="-I${prefix}/src" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lMappingMath"/>
- <cpp cflags="-I${prefix}/src" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lSpeedControl"/>
- </export>
-
-</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/CMakeLists.txt b/homer_nav_libs/src/Explorer/CMakeLists.txt
deleted file mode 100644
index 0a223eb657f1c15884b9c8afc22a9b7adddf230a..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Explorer/CMakeLists.txt
+++ /dev/null
@@ -1,7 +0,0 @@
-set(Explorer_SRC
- Explorer.cpp
-)
-
-add_library(Explorer ${Explorer_SRC})
-
-install(TARGETS Explorer DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
diff --git a/homer_nav_libs/src/Explorer/Explorer.cpp b/homer_nav_libs/src/Explorer/Explorer.cpp
deleted file mode 100644
index 65a447f22fb939e3c50e64d47488905374e52ad9..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Explorer/Explorer.cpp
+++ /dev/null
@@ -1,1426 +0,0 @@
-#include <cmath>
-#include <iostream>
-#include <queue>
-#include <sstream>
-
-#include "tools/tools.h"
-
-#include "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 )
-{
- // TODO VS
- ros::Time start = ros::Time::now();
-
- 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;
- }
-
- ROS_ERROR_STREAM("starting: computeApproachableMaps at " << (ros::Time::now() - start));// TODO VS
- computeApproachableMaps();
- ROS_ERROR_STREAM("finished: computeApproachableMaps at " << (ros::Time::now() - start));// TODO VS
- 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...");
- ROS_ERROR_STREAM("starting iteration over obstacle transform at " << (ros::Time::now() - start));// TODO VS
- int minSqrDist=INT_MAX;
- computeWalkableMaps();
- for ( int x = 0; x < m_ObstacleTransform->height(); x++ )
- {
- for ( int y = 0; y < m_ObstacleTransform->width(); y++ )
- {
- if ( isApproachable ( x,y ) && 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_ERROR_STREAM("finished iteration over obstacle transform at " << (ros::Time::now() - start));// TODO VS
- }
- 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 )
-{
- 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()
-{
- 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 );
- }
- else
- {
- m_ObstacleTransform->setValue ( x, y, OBSTACLE );
- }
- }
- }
-
- 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()
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return;
- }
-
- if ( m_ExplorationTransform ) { return; }
-
- ROS_INFO_STREAM("computeExplorationTransform: before obstacle transform");
- computeObstacleTransform();
- ROS_INFO_STREAM("computeExplorationTransform: before cost transform");
- computeCostTransform();
- ROS_INFO_STREAM("computeExplorationTransform: before target map");
- computeTargetMap();
- ROS_INFO_STREAM("computeExplorationTransform: before walkable maps");
- computeWalkableMaps();
- ROS_INFO_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_INFO_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_INFO_STREAM("computeExplorationTransform: After first looop");
- // Now go through the coordinates in the queue
- int xVal, yVal;
- ROS_INFO_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_INFO_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_INFO_STREAM("Desired Distance > 0: Executing getExplorationTransformPath");
- return getExplorationTransformPath( success );
- }
- ROS_INFO_STREAM("Computing Path Transform");
- computePathTransform();
- ROS_INFO_STREAM("Finished Path Transform");
- /*
- ROS_INFO_STREAM("Explorer: Path Transform: " << m_TargetDistanceTransform->width() << " " << m_TargetDistanceTransform->height());
- ROS_INFO_STREAM("---------------------");
- for(int x = 0; x < 10; x++) {
- std::stringstream str;
- str << "|";
- for(int y = 0; y < 10; y++) {
- str << (double)(m_TargetDistanceTransform->getValue(x, y)) << "|";
- }
- ROS_INFO(str.str().c_str());
- }
- ROS_INFO_STREAM("---------------------");
- */
- 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_INFO_STREAM("Exploration Transform: Before obstacle transform");
- computeObstacleTransform();
- ROS_INFO_STREAM("Exploration Transform: Before exploration transform");
- computeExplorationTransform();
- ROS_INFO_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/Explorer/Explorer.h b/homer_nav_libs/src/Explorer/Explorer.h
deleted file mode 100644
index 29a51ccf7cda5ae44e03abbe4d9fb33a1dd093f4..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Explorer/Explorer.h
+++ /dev/null
@@ -1,355 +0,0 @@
-#ifndef EXPLORER_H
-#define EXPLORER_H
-
-#include <vector>
-#include <geometry_msgs/Pose.h>
-
-#include "GridMap.h"
-#include "tools/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/src/Explorer/GridMap.h b/homer_nav_libs/src/Explorer/GridMap.h
deleted file mode 100644
index 64b3469eb331afcb9ff24067564fa1ff479ce234..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/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/src/Math/Box2D.h b/homer_nav_libs/src/Math/Box2D.h
deleted file mode 100644
index 5979b68a773e1ed642cdee35247aadb5e712661a..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/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 "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/src/Math/CMakeLists.txt b/homer_nav_libs/src/Math/CMakeLists.txt
deleted file mode 100644
index 050f702ad89046027dedbd08c010a9188a7e77f5..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Math/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-set(Math_SRC
- Line2D.cpp
- Transformation2D.cpp
- Pose.cpp
- Math.cpp
- Point2D.cpp
-)
-
-add_library(MappingMath ${Math_SRC})
-
-install(TARGETS MappingMath DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
diff --git a/homer_nav_libs/src/Math/Line2D.cpp b/homer_nav_libs/src/Math/Line2D.cpp
deleted file mode 100644
index f5807c313dab246c435fcdba12af6eab300c550f..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 "Line2D.h"
-#include "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/Line2D.h b/homer_nav_libs/src/Math/Line2D.h
deleted file mode 100644
index e8c210bb6433e979708ae73cab36e218a53550e1..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/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 "vec2.h"
-#include "mat2.h"
-#include "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/src/Math/Math.cpp b/homer_nav_libs/src/Math/Math.cpp
deleted file mode 100644
index 3f31c5bfe5df6679d34372082b3a8b8ed7e60696..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 "Math.h"
-#include <math.h>
-
-#include "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/Math.h b/homer_nav_libs/src/Math/Math.h
deleted file mode 100644
index bf00135a1a53a24343e7ec20e5f0bc7e13599b4d..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/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 "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/src/Math/Pixel.h b/homer_nav_libs/src/Math/Pixel.h
deleted file mode 100644
index 026fd3a6909cadc954b3148f15b6a73d4c632c18..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/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 "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/src/Math/Point2D.cpp b/homer_nav_libs/src/Math/Point2D.cpp
deleted file mode 100644
index c8ae09b7ab40761665dc5694db808f370b1768c3..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 "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/Point2D.h b/homer_nav_libs/src/Math/Point2D.h
deleted file mode 100644
index 94810ef901253ad677b4a59a823232f14589da1c..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/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 "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/src/Math/Pose.cpp b/homer_nav_libs/src/Math/Pose.cpp
deleted file mode 100644
index 6011f280d10b9061a2aa90c0df3f1cc54d9f98b2..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 "Pose.h"
-#include "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/Pose.h b/homer_nav_libs/src/Math/Pose.h
deleted file mode 100644
index d0f6574c5856b9de9413be634dd7b251365242e7..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Math/Pose.h
+++ /dev/null
@@ -1,74 +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 "Point2D.h"
-//#include "Architecture/Serializer/ExtendedOutStream.h" // TODO kann wahrscheinlich weg
-//#include "Architecture/Serializer/ExtendedInStream.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/src/Math/Transformation2D.cpp b/homer_nav_libs/src/Math/Transformation2D.cpp
deleted file mode 100644
index 693c7a396f40a7e5c16be4d35d952c4afdfac4ed..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 "Transformation2D.h"
-
-#include <cmath>
-#include <vector>
-#include <iostream>
-#include <sstream>
-#include "vec2.h" // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
-#include "mat2.h" // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
-#include "Point2D.h"
-#include "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/Math/Transformation2D.h b/homer_nav_libs/src/Math/Transformation2D.h
deleted file mode 100644
index 59a54d827c71bb08384861b0e78cf1d20cf60c98..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/Math/Transformation2D.h
+++ /dev/null
@@ -1,144 +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 "Workers/Math/Vec.h"
-#include "Point2D.h"
-#include "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/src/Math/mat2.h b/homer_nav_libs/src/Math/mat2.h
deleted file mode 100644
index 254e8dc68fd581091914fd501fe18892cac7bc2f..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/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 "Point2D.h"
-#include "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/src/Math/mat2_inl.h b/homer_nav_libs/src/Math/mat2_inl.h
deleted file mode 100644
index e3f77e161151e98c7df7f06b43db1d40fd9c4753..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/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/src/Math/vec2.h b/homer_nav_libs/src/Math/vec2.h
deleted file mode 100644
index 39a72c8eb60c6b6bf836dc7d7be932c376e25fd0..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/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/src/SpeedControl/CMakeLists.txt b/homer_nav_libs/src/SpeedControl/CMakeLists.txt
deleted file mode 100644
index d0cbcb8f31160f7872f4e6dd0c6c0f1500ae0373..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/SpeedControl/CMakeLists.txt
+++ /dev/null
@@ -1,7 +0,0 @@
-set(SpeedControl_SRC
- SpeedControl.cpp
-)
-
-add_library(SpeedControl ${SpeedControl_SRC})
-
-install(TARGETS SpeedControl DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
diff --git a/homer_nav_libs/src/SpeedControl/SpeedControl.cpp b/homer_nav_libs/src/SpeedControl/SpeedControl.cpp
deleted file mode 100644
index c33eff33aa4863f6dbdd75ae55ffad525eea0d90..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.45;
-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;
- loadConfigValue(path + "/x_min", pX.first);
- loadConfigValue(path + "/x_max", pX.second);
- loadConfigValue(path + "/y_min", pY.first);
- loadConfigValue(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;
- loadConfigValue("/homer_navigation/speed_control/inner_danger_zone/speed_factor", InnerDangerZoneFactor);
- OuterDangerZone = loadRect("/homer_navigation/speed_control/inner_danger_zone");
- OuterDangerZoneFactor;
- loadConfigValue("/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_nav_libs/src/tools/loadRosConfig.h b/homer_nav_libs/src/tools/loadRosConfig.h
deleted file mode 100644
index a51a99ed1ddd8c40f5df5a40bee7e23bdff0b88d..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/tools/loadRosConfig.h
+++ /dev/null
@@ -1,46 +0,0 @@
-
-#ifndef LOAD_ROS_CONFIG_H
-#define LOAD_ROS_CONFIG_H
-
-#include <ros/ros.h>
-#include <string>
-
-template <typename T>
-inline bool loadConfigValue(std::string valueName, T &variable, T default_variable = T() )
-{
- if(ros::param::has(valueName))
- {
- ros::param::get(valueName, variable);
- ROS_INFO_STREAM(valueName << ": " << variable);
- return true;
- }
- else
- {
- ROS_WARN_STREAM("No Parameter: " << valueName << ". Defaulting to "<<default_variable<<".");
- variable = default_variable;
- return false;
- }
-}
-
-template <typename T>
-inline bool loadConfigValue(std::string valueName, std::vector<T> &variable)
-{
- if(ros::param::has(valueName))
- {
- ros::param::get(valueName, variable);
- ROS_INFO_STREAM(valueName << ":");
-
- for (int i = 0; i < variable.size(); ++i)
- ROS_INFO_STREAM("Value "<< i << " :" << variable[i]);
-
- return true;
- }
- else
- {
- ROS_WARN_STREAM("No Parameter: " << valueName << ". Defaulting to empty vector.");
- variable.clear();
- return false;
- }
-}
-
-#endif
diff --git a/homer_nav_libs/src/tools/tools.h b/homer_nav_libs/src/tools/tools.h
deleted file mode 100644
index e106745044b3c4d36f586c23e3e26ea233331634..0000000000000000000000000000000000000000
--- a/homer_nav_libs/src/tools/tools.h
+++ /dev/null
@@ -1,258 +0,0 @@
-#ifndef TOOLS_H
-#define TOOLS_H
-
-#include <Eigen/Geometry>
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/Pose.h>
-#include <tf/transform_listener.h>
-#include <ros/ros.h>
-
-/**
- * @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)
- {
- geometry_msgs::PointStamped pin;
- geometry_msgs::PointStamped pout;
- pin.header.frame_id = from_frame;
- pin.point = point;
- try
- {
- listener.transformPoint(to_frame, pin, 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)
- {
- 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, pin, 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)
- {
- 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, pin, pout);
- ret.push_back(pout.point);
- }
- catch (tf::TransformException ex){
- //ROS_ERROR("%s",ex.what());
- }
-
- alpha += angle_step;
- }
- return ret;
- }
-
- /**
- * @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_navigation/CHANGELOG.rst b/homer_navigation/CHANGELOG.rst
deleted file mode 100644
index 37ba899bc0adeb6c31640e5b3bdfc4ef92e3aec0..0000000000000000000000000000000000000000
--- a/homer_navigation/CHANGELOG.rst
+++ /dev/null
@@ -1,56 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package homer_navigation
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.0.11 (2015-12-02)
--------------------
-
-1.0.10 (2015-12-02)
--------------------
-
-1.0.9 (2015-12-01)
-------------------
-
-1.0.8 (2015-11-30)
-------------------
-
-1.0.7 (2015-11-28)
-------------------
-* added install()
-* updated changelog
-* updated catkin_depends
-* Contributors: Niklas Yann Wettengel
-
-* updated catkin_depends
-* Contributors: Niklas Yann Wettengel
-
-1.0.6 (2015-11-27)
-------------------
-* removed env HOMER_DIR from CMakeLists.txt
-* Contributors: Niklas Yann Wettengel
-
-1.0.5 (2015-11-24)
-------------------
-* added missing files
-* Contributors: Niklas Yann Wettengel
-
-1.0.4 (2015-11-20)
-------------------
-* changed build dependency from libeigen3-dev to eigen
-* Contributors: Niklas Yann Wettengel
-
-1.0.3 (2015-11-20)
-------------------
-* added libeigen3-dev build dependency
-* Contributors: Niklas Yann Wettengel
-
-1.0.2 (2015-11-20)
-------------------
-* added cmake_modules as build dependency in package.xml
-* added Maintainers
-* Contributors: Niklas Yann Wettengel
-
-1.0.1 (2015-09-08)
-------------------
-* init
-* Contributors: Raphael Memmesheimer
diff --git a/homer_navigation/CMakeLists.txt b/homer_navigation/CMakeLists.txt
deleted file mode 100644
index 240de6260f5aead23f276f7a63b562b739cb9a62..0000000000000000000000000000000000000000
--- a/homer_navigation/CMakeLists.txt
+++ /dev/null
@@ -1,29 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(homer_navigation)
-
-find_package(catkin REQUIRED COMPONENTS
- roscpp roslib nav_msgs sensor_msgs homer_mapnav_msgs homer_nav_libs tf cmake_modules robbie_architecture
-)
-
-find_package(Eigen REQUIRED)
-
-catkin_package(
- INCLUDE_DIRS include
- CATKIN_DEPENDS roscpp roslib nav_msgs sensor_msgs homer_mapnav_msgs homer_nav_libs tf robbie_architecture
-)
-
-set(CMAKE_BUILD_TYPE Release)
-
-include_directories(
- include
- ${catkin_INCLUDE_DIRS}
- ${Eigen_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}
- ${Eigen_LIBRARIES}
- )
diff --git a/homer_navigation/README.md b/homer_navigation/README.md
deleted file mode 100644
index ae2cb85d54e9ab04344156eec1de8f71d5684be2..0000000000000000000000000000000000000000
--- a/homer_navigation/README.md
+++ /dev/null
@@ -1,79 +0,0 @@
-# homer_navigation
-
-
-## Known Issues / Todo's
-
-Die Pfadplanung dauert im Gegensatz zur alten Robbie-Implementierung zu lange. Je mehr freie Fläche vorhanden ist, desto länger dauert die Berechnung eines Pfades.
-
-## 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 z.B. vom map_manager veschickt 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
-* `/robot_platform/MoveRobot (robot_platform/MoveRobot)`: Ãœber dieses Topic wird der aktuelle Fahrbefehl an die Roboterplattform geschickt.
-* `/robot_platform/TurnRobot (robot_platform/TurnRobot)`: Ãœber dieses Topic wird der aktuelle Drehbefehl an die Roboterplattform geschickt.
-* `/robot_platform/StopRobot (robot_platform/StopRobot)`: Ãœber dieses Topic wird der Roboter am Ziel beziehungsweise vor einem Hindernis gestoppt.
-* `/homer_navigation/target_reached (std_msgs/Empty)`: Wenn der Roboter sein Ziel erreicht hat, wird eine Message über dieses Topic veschickt.
-* `/homer_navigation/target_unreachable (map_messages/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.
-
-#### 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 (nav_msgs/LaserScan)`: Der aktuelle LaserScan wird zur Hindernisvermeidung verwendet.
-* `/robot_platform/task_finished (std_msgs/Empty)`: Wird vom Roboter nach erfolgter AUsführung einer Bewegung verschickt. Die Navigation verwendet dies am Ende der Navigation, um nach der letzten Drehung auf dem Zielpunkt die TargetReached-Nachricht zu versenden.
-* `/homer_navigation/start_navigation (map_messages/StartNavigation)`: Startet die Pfadplanung und anschließend die Navigation zur mitgelieferten Zielpose
-* `/homer_navigation/stop_navigation (map_messages/StopNavigation)`: Stoppt die aktuelle Navigation.
-* `/homer_navigation/navigate_to_POI (map_messages/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 (%).
-
-## 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:` Gewicht, um Bevorzugung sicherer (d.h. größerer Abstand zu Hindernissen) Pfade im Gegensatz zu kürzeren Pfaden einzustellen. 0 bedeutet kürzester Pfad, aber keine Sicherheit. Je höher, desto sicherer.
-* `/homer_navigation/allowed_obstacle_distance/min:` Mindestdistanz zum nächsten Hindernis, die der Roboter bei der Navigation besitzen muss.
-* `/homer_navigation/allowed_obstacle_distance/max:` Maximaldistanz zum nächsten Hindernis, die der Roboter bei der Navigation besitzen muss (wichtig für die Lokalisierung, abhängig von der Reichweite der Abstandssensoren).
-* `/homer_navigation/safe_obstacle_distance/min:` Mindestdistanz des Roboters zum nächsten Hindernis für eine sichere Navigation. Gewichtung wird mit safe_path_weight eingestellt.
-* `/homer_navigation/safe_obstacle_distance/max:` Maximaldistanz des Roboters zum nächsten Hindernis.
-* `/homer_navigation/frontier_safeness_factor:` Faktor der minimal erlaubten Distanz zum nächsten Hindernis, den eine Grenzzelle zu unbekanntem Gebiet mindestens haben muss, um als sicher und damit befahrbar zu gelten.
-* `/homer_navigation/collision_distance:` Wenn die Distanz zum nächsten Hindernis in Metern (gemessen ab der Front des Roboters) kleiner wird als der angegebene Parameter, wird der aktuelle Fahrbefehl gestoppt und die Pfadplanung neugestartet.
-* `/homer_navigation/collision_distance_near_target:` Nähe in Metern zum Ziel, ab der der Roboter bei einer Kollision nicht mehr zurückfährt und neuplant, sondern stoppt und eine TargetReached-Nachricht versendet.
-* `/homer_navigation/backward_distance:` Strecke in Metern, die der Roboter bei einer bevorstehenden Kollision zurückfährt, bevor er einen neuen Pfad plant.
-* `/homer_navigation/check_path:` Wenn auf "true" gesetzt, wird der aktuell geplante Pfad auf neu hinzukommende Hindernisse geprüft und gegebenenfalls neugeplant.
-* `/homer_navigation/check_path_max_errors:` Wenn Pfade gecheckt werden, wird umgeplant, sobald sich "check_path_max_errors"-mal hintereinander ein Hindernis in der ankommenden Karte im Pfad befindet.
-* `/homer_navigation/check_path_max_distance:` Der Pfad wird nur in der Nähe des Roboters bis zur im Parameter angegebenen Distanz auf neu hinzukommende Hindernisse überprüft
-* `/homer_navigation/turn_threshold_angle:` Wenn der Winkel zwischen der Orientierung des Roboters und der Strecke zum nächsten Wegpunkt unter dem angegebenen Wert liegt, soll geradeaus gefahren werden, ansonsten soll sich der Roboter um diesen Winkel in Richtung des Wegpunkts drehen.
-* `/homer_navigation/waypoint_sampling_threshold:` Parameter, um Anzahl der Wegpunkte im Pfad zu minimieren. 0 bedeutet keine Reduzierung. Je höher der Wert, desto mehr Wegpunkte werden gelöscht.
-* `/homer_navigation/speed_control/last_speedfactor_count:` Größe des Vektors, der die zuletzt berechneten Geschwindigkeitsfaktoren berechnet. Wird benutzt, um aus dem Mittelwert zusammen mit der aktuellen Distanz zum nächsten Hindernis die aktuelle Geschwindigkeit zu berechnen.
-* `/homer_navigation/speed_control/min_move_speedfactor:` Minimaler Faktor der maximal möglichen Geschwindigkeit, mit der der Roboter jemals fahren darf.
-* `/homer_navigation/speed_control/min_turn_speedfactor_moving:` Minimaler Faktor der maximal möglichen Drehgeschwindigkeit, mit der der Roboter sich während einer Geradeausfahrt drehen darf (aktuell mit dem cu2wd nicht möglich).
-* `/homer_navigation/speed_control/min_turn_speedfactor_standing:` Minimaler Faktor der maximal möglichen Drehgeschwindigkeit, mit der sich der Roboter im Stand drehen darf.
-* `/homer_navigation/speed_control/inner_danger_zone/x_min:` Wenn sich in der mit den nächsten vier Parametern beschriebenen Boundingbox um den Roboter ein Hindernis befindet, darf sich der Roboter nur mit der vorher eingestellten nachfolgend eingestellten Geschwindigkeit bewegen.
-* `/homer_navigation/speed_control/inner_danger_zone/x_max:`
-* `/homer_navigation/speed_control/inner_danger_zone/y_min:`
-* `/homer_navigation/speed_control/inner_danger_zone/y_max:`
-* `/homer_navigation/speed_control/inner_danger_zone/speed_factor:` Faktor der maximalen Geschwindigkeit, mit der sich der Roboter in seiner inneren Gefahrenzone bewegen darf.
-* `/homer_navigation/speed_control/outer_danger_zone/x_min:` Eine zweite Boundingbox, die die innere umschließen muss.
-* `/homer_navigation/speed_control/outer_danger_zone/x_max:`
-* `/homer_navigation/speed_control/outer_danger_zone/y_min:`
-* `/homer_navigation/speed_control/outer_danger_zone/y_max:`
-* `/homer_navigation/speed_control/outer_danger_zone/speed_factor:` Faktor der maximalen Geschwindigkeit, mit der sich der Roboter in seiner äußeren Gefahrenzone bewegen darf.
-* `/homer_navigation/max_rot_vel:` Maximal mögliche Rotationsgeschwindigkeit des Roboters in Radiants/Sekunde.
-* `/homer_navigation/max_trans_vel:` Maximal mögliche Geschwindigkeit des Roboters in Metern/Sekunde.
-
-
diff --git a/homer_navigation/config/homer_navigation.yaml b/homer_navigation/config/homer_navigation.yaml
deleted file mode 100644
index a9e7aff8d43121fb9779d66ecaee6fe25bc08565..0000000000000000000000000000000000000000
--- a/homer_navigation/config/homer_navigation.yaml
+++ /dev/null
@@ -1,48 +0,0 @@
-/homer_navigation/safe_path_weight: 1.5 #weight for safer path in relation to shortest path
-/homer_navigation/allowed_obstacle_distance/min: 0.32 #m robot must move within these bounds
-/homer_navigation/allowed_obstacle_distance/max: 5.0 #m
-/homer_navigation/safe_obstacle_distance/min: 0.7 #m if possible robot should move within these bounds
-/homer_navigation/safe_obstacle_distance/max: 5.0 #m
-
-/homer_navigation/frontier_safeness_factor: 1.5 #factor of distance to an obstacle of a frontier cell which is considered safe
-
-/homer_navigation/collision_distance: 0.20 #m
-
-/homer_navigation/collision_distance_near_target: 0.4 #m distance to target where obstacle avoidance won't be executed, to avoid strange for- and backward moving activities near a target
-/homer_navigation/backward_distance: 0.07 #m
-
-/homer_navigation/check_path: true
-/homer_navigation/check_path_max_errors: 2
-/homer_navigation/check_path_max_distance: 2.0 #m
-
-/homer_navigation/turn_threshold_angle: 15 #°
-
-/homer_navigation/waypoint_sampling_threshold: 2
-
-/homer_navigation/speed_control/last_speedfactor_count: 10
-/homer_navigation/speed_control/min_move_speedfactor: 1.0
-/homer_navigation/speed_control/min_turn_speedfactor_moving: 1.0
-/homer_navigation/speed_control/min_turn_speedfactor_standing: 0.5
-
-/homer_navigation/speed_control/inner_danger_zone/x_min: -0.35
-/homer_navigation/speed_control/inner_danger_zone/x_max: 1.1
-/homer_navigation/speed_control/inner_danger_zone/y_min: -0.35
-/homer_navigation/speed_control/inner_danger_zone/y_max: 0.35
-
-/homer_navigation/speed_control/inner_danger_zone/speed_factor: 0.3
-
-/homer_navigation/speed_control/outer_danger_zone/x_min: -0.45
-/homer_navigation/speed_control/outer_danger_zone/x_max: 1.6
-/homer_navigation/speed_control/outer_danger_zone/y_min: -0.45
-/homer_navigation/speed_control/outer_danger_zone/y_max: 0.45
-
-/homer_navigation/speed_control/outer_danger_zone/speed_factor: 0.6
-
-/homer_navigation/max_rot_vel: 0.4 # rad/s
-/homer_navigation/max_trans_vel: 0.3 # m/s
-
-/homer_navigation/use_cmd_vel: true # test
-/homer_navigation/min_turn_angle: 0.15 # 0.17
-/homer_navigation/max_turn_speed: 0.6 # rad/s 0.6
-/homer_navigation/max_move_speed: 0.4 # m/s 0.4
-/homer_navigation/max_drive_angle: 0.6 # if above that value only turn
diff --git a/homer_navigation/config/homer_navigation_followme.yaml b/homer_navigation/config/homer_navigation_followme.yaml
deleted file mode 100644
index 7b13dfc3d9b00556b7294f8019bcf4697effcbc8..0000000000000000000000000000000000000000
--- a/homer_navigation/config/homer_navigation_followme.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-#/homer_navigation/collision_distance: 0.23 #m
-/homer_navigation/collision_distance: 0.47 #m
-
-/homer_navigation/collision_distance_near_target: 0.2 #m distance to target where obstacle avoidance won't be executed, to avoid strange for- and backward moving activities near a target
-/homer_navigation/backward_distance: 0.07 #m
-/homer_navigation/check_path_max_errors: 5
-
-/homer_navigation/min_turn_angle/min: 20 #°
-/homer_navigation/min_turn_angle/max: 20
-/homer_navigation/min_turn_angle/min_distance: 0.5 #m
-/homer_navigation/min_turn_angle/max_distance: 2.0 #m
-
-/homer_navigation/waypoint_sampling_threshold: 4
-
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 d0d95b1bbbd280237c17dd9817a6a041814c9a93..0000000000000000000000000000000000000000
--- a/homer_navigation/include/homer_navigation/homer_navigation_node.h
+++ /dev/null
@@ -1,372 +0,0 @@
-#ifndef FastNavigationModule_H
-#define FastNavigationModule_H
-
-#include <vector>
-#include <string>
-#include <map>
-#include <deque>
-
-#include <Eigen/Geometry>
-
-#include <ros/ros.h>
-#include <tf/transform_listener.h>
-
-#include "Architecture/StateMachine/StateMachine.h"
-
-#include <nav_msgs/OccupancyGrid.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <sensor_msgs/LaserScan.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <homer_mapnav_msgs/StartNavigation.h>
-#include <homer_mapnav_msgs/StopNavigation.h>
-#include <homer_mapnav_msgs/NavigateToPOI.h>
-#include <std_msgs/Int8.h>
-
-class Explorer;
-/**
- * @class HomerNavigationNode
- * @author Malte Knauf, Stephan Wirth, David Gossow (RX)
- * @brief Performs autonomous exploration and navigation
- */
-class HomerNavigationNode {
-
- public:
-
- /**
- * @brief States of the state machines
- */
- enum MapType
- {
- SLAM_MAP,
- NAVIGATION_MAP
- };
-
- enum ProcessState
- {
- IDLE,
- AWAITING_EXPLORATION_MAP,
- AWAITING_PATHPLANNING_MAP,
- FOLLOWING_PATH,
- AVOIDING_COLLISION,
- FINAL_TURN,
- TARGET_REACHED,
- TILTED,
- STALLED
- };
-
- /**
- * The constructor
- */
- HomerNavigationNode();
-
- /**
- * The destructor
- */
- virtual ~HomerNavigationNode();
-
- /** @brief Is called in constant intervals. */
- void idleProcess();
-
-
- protected:
-
- /** @brief Handles incoming messages. */
- //virtual std::set<Message*> processMessages();
- 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 backLaserCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
- void startNavigationCallback(const homer_mapnav_msgs::StartNavigation::ConstPtr& msg);
- void stopNavigationCallback(const homer_mapnav_msgs::StopNavigation::ConstPtr& msg);
- void navigateToPOICallback(const homer_mapnav_msgs::NavigateToPOI::ConstPtr& msg);
- void unknownThresholdCallback(const std_msgs::Int8::ConstPtr& msg);
- void moveBaseSimpleGoalCallback(const geometry_msgs::PoseStamped::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 Start exploration on last_map_data_ */
- void startExploration();
-
- /** @brief Check if obstacles are blocking the way in last_map_data_ */
- void 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 );
-
- 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 Distance from robot_pose_ to point */
- double distanceTo(geometry_msgs::Point point);
-
- /** @return Angle from robot_pose_ to point in degrees */
- int angleToPointDeg(geometry_msgs::Point point);
-
- /** @brief Set status info */
- void actualizeStatusInfo();
-
- /** @brief Calculates current maximal speed based on laser data */
- float calcSpeedFactor();
-
- /** @brief Calculates current maximal backwards distance on map Data */
- float obstacleBackwardDistance();
-
-
- /**
- * @brief Send move message
- * @param distance_m distance to drive in m
- * @param speed_mPerSec driving speed in m/s
- * @param drive permanently until stopped
- */
-
- void sendMoveMessage(double distance_m , double speed_mPerSec, bool permanent = false);
-
- /**
- * @brief Send TurnMessage and set current turn action
- * @param theta angle to turn (in degrees)
- * @param speed turning speed
- * @param turn permanently until stopped
- */
- void sendTurnMessage(double theta, double speed, bool permanent = false);
-
- void sendStopRobot();
-
- /**
- * @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 mean of given input values
- * @param values Container with values from which to compute mean
- * @return mean value
- */
- template<class ContainerT>
- static double mean ( const ContainerT& values );
-
- /**
- * 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; }
-
- /// @brief Worker instances
-
- Explorer* explorer_;
-
- /// @brief State machines
-
- StateMachine<MapType> m_MapTypeMachine;
- StateMachine<ProcessState> m_MainMachine;
-
- /// @brief Navigation options & data
-
- /** list of waypoints subsampled from m_PixelPath */
- std::vector<geometry_msgs::PoseStamped> waypoints_;
-
- /** Path planned by Explorer, pixel accuracy */
- std::vector<Eigen::Vector2i> pixel_path_;
-
- /** target point */
- geometry_msgs::Point target_point_;
-
- /** approximation of the target point is target is in an occupied cell */
- Eigen::Vector2i target_approx_;
-
- /** orientation the robot should have at the target point */
- double target_orientation_;
-
- /** allowed distance to target */
- float desired_distance_;
-
- /** check if the final turn should be skipped */
- bool 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 fast_path_planning_;
-
- /** current pose of the robot */
- geometry_msgs::Pose robot_pose_;
-
- /** current laser scan */
- std::vector<geometry_msgs::Point> laser_points_;
- std::vector<geometry_msgs::Point> m_laser_points_map;
- std::vector<geometry_msgs::Point> m_back_laser;
-
- /** time stamp of the last incoming laser scan */
- ros::Time last_laser_time_;
-
- /** Distance factor of a frontier cell considered save for exploration */
- float m_FrontierSafenessFactor;
-
- /** stores the last m_SpeedFactorMeanFilterSize speed factors */
- std::deque<float> m_LastSpeedFactors;
- /** stores the mean of the last speed factors */
- std::deque<float> m_LastMeanSpeedFactors;
- /** maximal count of stored last speed factors bevore oldest one will be overwritten */
- int m_SpeedFactorMeanFilterSize;
-
- /** minimal speed factor while moving */
- float m_MinMoveSpeedFactor;
-
- /** minimal turn speed factor while moving */
- float m_MinTurnSpeedFactorMoving;
-
- /** minimal turn speed factor while standing */
- float m_MinTurnSpeedFactorStanding;
-
- double m_SafePathWeight;
-
- /** Number of subsequent times that an obstacle was detected in the planned path */
- int invalid_path_count_;
-
- ///map parameters
- double resolution_;
- double width_;
- double height_;
- geometry_msgs::Pose origin_;
-
- /// @brief Configuration parameters
-
- /** maximum move speed of the robot */
- float m_MaxTransVel;
- /** maximum turn speed of the robot */
- int m_MaxRotVel;
-
- bool m_use_cmd_vel_;
-
- /** 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 waypoint_sampling_threshold_;
-
- /** if distance to nearest obstacle is below collision distance trigger collision avoidance */
- float collision_distance_;
- /** do not drive back in collision avoidance when this near target */
- float collision_distance_near_target_;
- /** drive this distance backwards when avoiding collision */
- float backward_distance_;
-
- /** threshold angle between robot and next waypoint when to turn instead of driving forward */
- double turn_threshold_angle_;
-
- /** if true, obstacles in path will be detected and path will be replanned */
- bool check_path_;
- /** path will be replanned, if obstacle is present in at least check_path_max_errors consecutive incoming maps */
- int check_path_max_errors_;
-
- /** waypoints will only be checked for obstacles if they are closer than check_path_max_distance to robot */
- float check_path_max_distance_;
-
- bool m_avoided_collision;
-
- double m_min_turn_angle;
- double m_max_turn_speed;
- double m_max_move_speed;
- double m_max_drive_angle;
-
- float m_act_speed;
- float m_act_angle;
-
- bool m_path_reaches_target;
-
- /** timestamp of last incoming map */
- ros::Time last_map_timestamp_;
-
- /** last map data */
- std::vector<int8_t> * last_map_data_;
-
- //ros specific members
- tf::TransformListener transform_listener_;
-
- //subscribers
- ros::Subscriber map_sub_;
- ros::Subscriber pose_sub_;
- ros::Subscriber laser_data_sub_;
- ros::Subscriber laser_back_data_sub_;
- ros::Subscriber robot_data_sub_;
- ros::Subscriber task_finished_sub_;
- ros::Subscriber start_navigation_sub_;
- ros::Subscriber stop_navigation_sub_;
- ros::Subscriber navigate_to_poi_sub_;
- ros::Subscriber unknown_threshold_sub_;
- ros::Subscriber refresh_param_sub_;
- ros::Subscriber m_move_base_simple_goal_sub_;
-
- //publishers
- ros::Publisher move_robot_pub_;
- ros::Publisher turn_robot_pub_;
- ros::Publisher stop_robot_pub_;
- ros::Publisher cmd_vel_pub_;
- ros::Publisher target_reached_pub_;
- ros::Publisher target_unreachable_pub_;
- ros::Publisher path_pub_;
-
-
- //service clients
- ros::ServiceClient 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_followme.launch b/homer_navigation/launch/homer_navigation_followme.launch
deleted file mode 100644
index 419db5e814b5c2c120b60e549283e73751339e0a..0000000000000000000000000000000000000000
--- a/homer_navigation/launch/homer_navigation_followme.launch
+++ /dev/null
@@ -1,4 +0,0 @@
-<launch>
- <rosparam command="load" file="$(find homer_navigation)/config/homer_navigation_followme.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_ros.launch b/homer_navigation/launch/homer_navigation_ros.launch
deleted file mode 100644
index 04a131d5b115687e8d631c02a603d97cd0afe736..0000000000000000000000000000000000000000
--- a/homer_navigation/launch/homer_navigation_ros.launch
+++ /dev/null
@@ -1,6 +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">
- <remap from="/robot_platform/cmd_vel" to="/cmd_vel"/>
- </node>
-</launch>
diff --git a/homer_navigation/package.xml b/homer_navigation/package.xml
deleted file mode 100644
index 3526b91403ca0e8eb26d9898e36372f004ffc144..0000000000000000000000000000000000000000
--- a/homer_navigation/package.xml
+++ /dev/null
@@ -1,43 +0,0 @@
-<?xml version="1.0"?>
-<package>
- <name>homer_navigation</name>
- <version>1.0.11</version>
- <description>The homer_navigation package</description>
-
- <maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
- <maintainer email="niyawe@uni-koblenz.de">Niklas Yann Wettengel</maintainer>
- <maintainer email="heuer@uni-koblenz.de">Gregor Heuer</maintainer>
- <maintainer email="raphael@uni-koblenz.de">Raphael Memmesheimer</maintainer>
-
- <author email="mknauf@uni-koblenz.de">Malte Knauf</author>
- <license>GPLv3</license>
-
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>roslib</build_depend>
- <build_depend>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>tf</build_depend>
- <build_depend>cmake_modules</build_depend>
- <build_depend>eigen</build_depend>
- <build_depend>robbie_architecture</build_depend>
-
- <run_depend>roscpp</run_depend>
- <run_depend>roslib</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>tf</run_depend>
- <run_depend>robbie_architecture</run_depend>
- <!-- The export tag contains other, unspecified, tags -->
- <export>
- <!-- You can specify that this package is a metapackage here: -->
- <!-- <metapackage/> -->
-
- <!-- Other tools can request additional information be placed here -->
-
- </export>
-</package>
diff --git a/homer_navigation/readme.pdf b/homer_navigation/readme.pdf
deleted file mode 100644
index 808199ca0b0e7635cc36e1000a53c87196827351..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 5897946e6fe76ac3cbe0b658f0660b7c2398bdf4..0000000000000000000000000000000000000000
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ /dev/null
@@ -1,1069 +0,0 @@
-
-#include <vector>
-#include <iostream>
-#include <sstream>
-#include <cmath>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <time.h>
-
-#include <ros/package.h>
-
-#include "homer_navigation/homer_navigation_node.h"
-
-//messages
-#include <std_msgs/Empty.h>
-#include <homer_mapnav_msgs/TargetUnreachable.h>
-#include <homer_mapnav_msgs/GetPointsOfInterest.h>
-#include <homer_mapnav_msgs/ModifyMap.h>
-#include <nav_msgs/Path.h>
-#include <std_msgs/Int8.h>
-#include <tools/tools.h>
-#include <tools/loadRosConfig.h>
-
-//nav_libs
-#include "Explorer/Explorer.h"
-#include "SpeedControl/SpeedControl.h"
-
-using namespace std;
-
-
-HomerNavigationNode::HomerNavigationNode()
-{
- ros::NodeHandle nh;
-
- srand(time(NULL));// Initialize random numbers
-
- //subscribers
- map_sub_ = nh.subscribe<nav_msgs::OccupancyGrid>("/map", 1, &HomerNavigationNode::mapCallback, this);
- pose_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("/pose", 1, &HomerNavigationNode::poseCallback, this);
- laser_data_sub_ = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1, &HomerNavigationNode::laserDataCallback, this);
- laser_back_data_sub_ = nh.subscribe<sensor_msgs::LaserScan>("/back_scan", 1, &HomerNavigationNode::backLaserCallback, this);
- start_navigation_sub_ = nh.subscribe<homer_mapnav_msgs::StartNavigation>("/homer_navigation/start_navigation", 1, &HomerNavigationNode::startNavigationCallback, this);
- stop_navigation_sub_ = nh.subscribe<homer_mapnav_msgs::StopNavigation>("/homer_navigation/stop_navigation", 1, &HomerNavigationNode::stopNavigationCallback, this);
- navigate_to_poi_sub_ = nh.subscribe<homer_mapnav_msgs::NavigateToPOI>("/homer_navigation/navigate_to_POI", 1, &HomerNavigationNode::navigateToPOICallback, this);
- unknown_threshold_sub_ = nh.subscribe<std_msgs::Int8>("/homer_navigation/unknown_threshold", 1, &HomerNavigationNode::unknownThresholdCallback, this);
- refresh_param_sub_ = nh.subscribe<std_msgs::Empty>("/homer_navigation/refresh_params", 1, &HomerNavigationNode::refreshParamsCallback, this);
-
- cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 1);
- target_reached_pub_ = nh.advertise<std_msgs::Empty>("/homer_navigation/target_reached", 1);
- target_unreachable_pub_ = nh.advertise<homer_mapnav_msgs::TargetUnreachable>("/homer_navigation/target_unreachable", 1);
- path_pub_ = nh.advertise<nav_msgs::Path>("/homer_navigation/path", 1);
- get_POIs_client_ = nh.serviceClient<homer_mapnav_msgs::GetPointsOfInterest>("/map_manager/get_pois");
-
-
- m_move_base_simple_goal_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &HomerNavigationNode::moveBaseSimpleGoalCallback, this); // for RVIZ usage
-
-
-
- m_MapTypeMachine.setName( "HomerNavigation Node" );
- ADD_MACHINE_STATE( m_MapTypeMachine, SLAM_MAP );
- ADD_MACHINE_STATE( m_MapTypeMachine, NAVIGATION_MAP );
- m_MapTypeMachine.setState( SLAM_MAP );
-
- m_MainMachine.setName( "HomerNavigation Main" );
- ADD_MACHINE_STATE( m_MainMachine, IDLE );
- ADD_MACHINE_STATE( m_MainMachine, AWAITING_EXPLORATION_MAP );
- 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 );
- ADD_MACHINE_STATE( m_MainMachine, TARGET_REACHED );
-
- m_avoided_collision = false;
-
- init();
- m_act_speed = 0;
- m_act_angle = 0;
-}
-
-void HomerNavigationNode::loadParameters()
-{
- SpeedControl::loadDimensions();
-
- //Explorer constructor
- loadConfigValue("/homer_navigation/safe_path_weight", m_SafePathWeight);
- loadConfigValue("/homer_mapping/resolution", resolution_);
- loadConfigValue("/homer_navigation/allowed_obstacle_distance/min", m_AllowedObstacleDistance.first);
- loadConfigValue("/homer_navigation/allowed_obstacle_distance/max", m_AllowedObstacleDistance.second);
- m_AllowedObstacleDistance.first /= resolution_;
- m_AllowedObstacleDistance.second/= resolution_;
- loadConfigValue("/homer_navigation/safe_obstacle_distance/min", m_SafeObstacleDistance.first);
- loadConfigValue("/homer_navigation/safe_obstacle_distance/max", m_SafeObstacleDistance.second);
- m_SafeObstacleDistance.first /= resolution_;
- m_SafeObstacleDistance.second /= resolution_;
- loadConfigValue("/homer_navigation/frontier_safeness_factor", m_FrontierSafenessFactor);
-
- //Explorer
- loadConfigValue("/homer_navigation/waypoint_sampling_threshold", waypoint_sampling_threshold_);
-
- //check path
- loadConfigValue("/homer_navigation/check_path", check_path_);
- loadConfigValue("/homer_navigation/check_path_max_errors", check_path_max_errors_);
- loadConfigValue("/homer_navigation/check_path_max_distance", check_path_max_distance_);
-
- //collision
- loadConfigValue("/homer_navigation/collision_distance", collision_distance_);
- loadConfigValue("/homer_navigation/collision_distance_near_target", collision_distance_near_target_);
-
- //moveRobot config values
- loadConfigValue("/homer_navigation/backward_distance", backward_distance_);
- loadConfigValue("/homer_navigation/max_trans_vel", m_MaxTransVel);
- loadConfigValue("/homer_navigation/max_rot_vel", m_MaxRotVel);
- loadConfigValue("/homer_navigation/speed_control/last_speedfactor_count", m_SpeedFactorMeanFilterSize);
- loadConfigValue("/homer_navigation/speed_control/min_move_speedfactor", m_MinMoveSpeedFactor);
- loadConfigValue("/homer_navigation/speed_control/min_turn_speedfactor_moving", m_MinTurnSpeedFactorMoving);
- loadConfigValue("/homer_navigation/speed_control/min_turn_speedfactor_standing", m_MinTurnSpeedFactorStanding);
- loadConfigValue("/homer_navigation/turn_threshold_angle", turn_threshold_angle_);
-
- //cmd_vel config values
- loadConfigValue("/homer_navigation/use_cmd_vel", m_use_cmd_vel_);
- loadConfigValue("/homer_navigation/min_turn_angle", m_min_turn_angle);
- loadConfigValue("/homer_navigation/max_turn_speed", m_max_turn_speed);
- loadConfigValue("/homer_navigation/max_move_speed", m_max_move_speed);
- loadConfigValue("/homer_navigation/max_drive_angle", m_max_drive_angle);
-
-}
-
-
-void HomerNavigationNode::init()
-{
- last_map_timestamp_ = ros::Time(0);
-
- loadParameters();
-
- robot_pose_.position.x = 0.0;
- robot_pose_.position.y = 0.0;
- robot_pose_.position.z = 0.0;
- robot_pose_.orientation = tf::createQuaternionMsgFromYaw(0.0);
-
- last_laser_time_ = ros::Time::now();
-
- explorer_ = new Explorer ( m_AllowedObstacleDistance.first, m_AllowedObstacleDistance.second,
- m_SafeObstacleDistance.first, m_SafeObstacleDistance.second,
- m_SafePathWeight, m_FrontierSafenessFactor );
-
- m_MainMachine.setState ( IDLE );
-}
-
-HomerNavigationNode::~HomerNavigationNode()
-{
- if ( explorer_ )
- {
- delete explorer_;
- }
- if(last_map_data_)
- {
- delete last_map_data_;
- }
-}
-
-void HomerNavigationNode::sendStopRobot()
-{
- m_act_speed = 0;
- m_act_angle = 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;
- cmd_vel_pub_.publish(cmd_vel_msg);
-}
-
-void HomerNavigationNode::idleProcess()
-{
- if ( m_MainMachine.state() == FOLLOWING_PATH )
- {
- if ( (ros::Time::now() - last_laser_time_) > ros::Duration(2.0) )
- {
- ROS_ERROR_STREAM( "Laser data timeout!\n");
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::LASER_TIMEOUT);
- }
- }
-}
-
-
-void HomerNavigationNode::calculatePath()
-{
- float desired_distance_2 = desired_distance_ - 0.05;
- if ( desired_distance_2 < resolution_ )
- {
- desired_distance_2 = resolution_;
- }
- explorer_->setStart ( map_tools::toMapCoords( robot_pose_.position, origin_, resolution_) );
-
- if ( map_tools::distance( map_tools::fromMapCoords( target_approx_, origin_, resolution_ ), target_point_ ) <= desired_distance_ )
- {
- ROS_INFO_STREAM( "There is a way to the target circle. Planning direct path." );
- explorer_->setTarget ( map_tools::toMapCoords( target_point_, origin_, resolution_ ), desired_distance_2 / resolution_ );
- }
- else
- {
- ROS_INFO_STREAM( "The target area is not reachable. Trying to go as close as possible." );
- explorer_->setTarget ( target_approx_, 0 );
- }
-
- bool success;
- pixel_path_ = explorer_->getPath( success );
- if ( !success )
- {
- ROS_WARN_STREAM("No path found for navigation, reporting as unreachable." );
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
- else
- {
- ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << pixel_path_.size());
- std::vector<Eigen::Vector2i> waypoint_pixels = explorer_->sampleWaypointsFromPath ( pixel_path_, waypoint_sampling_threshold_ );
- waypoints_.clear();
- for(std::vector<Eigen::Vector2i>::iterator it = waypoint_pixels.begin(); it != waypoint_pixels.end(); ++it)
- {
- geometry_msgs::PoseStamped poseStamped;
- poseStamped.pose.position = map_tools::fromMapCoords(*it, origin_, 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;
- waypoints_.push_back(poseStamped);
- }
-
- ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << waypoints_.size());
-
- sendPathData();
- //make sure no timeout occurs after long calculation time
- last_laser_time_ = ros::Time::now();
- }
-}
-
-
-void HomerNavigationNode::startExploration()
-{
- ROS_INFO_STREAM( "Starting exploration." );
-/*
- m_Explorer->resetExploration();
- m_Explorer->setOccupancyMap ( gridSize, gridSize, mapData, m_ExploredRegion );
- m_Explorer->setFrontierSafenessFactor( m_FrontierSafenessFactor );
- m_Explorer->setStart ( m_CoordinateConverter->worldToMap ( m_RobotPose ) );
-
- bool success;
- m_PixelPath= m_Explorer->getExplorationTransformPath( success );
-
- if ( success ) {
- m_Target = m_CoordinateConverter->mapToWorld( m_PixelPath.back() );
- m_TargetApprox = m_PixelPath.back();
- }
-
- vector<Pixel> waypointPixels = m_Explorer->sampleWaypointsFromPath ( m_PixelPath, m_WaypointSamplingThreshold );
- m_Waypoints = m_CoordinateConverter->mapToWorld ( waypointPixels );
-*/
-
- if ( pixel_path_.size() == 0 )
- {
- ROS_WARN_STREAM("No path found for exploration, sending NoTargetM." );
- //sendMessage ( new NoTargetM() );
- m_MainMachine.setState( IDLE );
- }
- else
- {
- m_MainMachine.setState ( FOLLOWING_PATH );
- sendPathData();
- }
- //make sure no timeout occurs after long calculation time
- last_laser_time_ = ros::Time::now();
-}
-
-
-void HomerNavigationNode::startNavigation()
-{
- ROS_INFO_STREAM("Starting navigation to " << target_point_.x << "," << target_point_.y);
-
- if ( distanceTo(target_point_) < desired_distance_ )
- {
- ROS_INFO_STREAM( "Will not (re-)plan path: Target position already reached." );
- targetPositionReached();
- return;
- }
- ROS_INFO_STREAM( "Distance to target still too large (" << distanceTo( target_point_ ) << "m; requested: " << desired_distance_ << "m)" );
-
- ROS_DEBUG_STREAM("homer_navigation_node::startNavigation: target point " << target_point_.x << " " << target_point_.y);
- Eigen::Vector2i new_target = map_tools::toMapCoords(target_point_, origin_, resolution_);
- ROS_DEBUG_STREAM("homer_navigation_node::startNavigation: target cell " << new_target);
- Eigen::Vector2i new_target_approx;
-
- switch ( m_MapTypeMachine.state() )
- {
- case SLAM_MAP:
- {
- ROS_INFO_STREAM( "Resetting occupancy map." );
-
- if(fast_path_planning_)
- {
- maskMap();
- }
- explorer_->setOccupancyMap(width_, width_, origin_, &(*last_map_data_)[0]);
- explorer_->setStart(map_tools::toMapCoords(robot_pose_.position, origin_, resolution_));
- new_target_approx = explorer_->getNearestAccessibleTarget(new_target);
- break;
- }
- case NAVIGATION_MAP:
- ROS_INFO_STREAM( "Updating obstacles in externally assigned navigation map." );
- explorer_->updateObstacles( width_, width_, origin_, &(*last_map_data_)[0] );
- explorer_->setStart(map_tools::toMapCoords(robot_pose_.position, origin_, resolution_));
- new_target_approx = new_target;
- break;
- }
-
- geometry_msgs::Point new_target_approx_world = map_tools::fromMapCoords(new_target_approx, origin_, resolution_);
- ROS_INFO_STREAM("start Navigation: Approx target: " << new_target_approx_world);
-
- bool new_approx_is_better = ( map_tools::distance( robot_pose_.position, target_point_ ) - map_tools::distance( new_target_approx_world, target_point_ ) ) > 0.2;
- bool new_approx_reaches_target = ( map_tools::distance(new_target_approx_world, target_point_ ) < desired_distance_ );
- m_path_reaches_target = new_approx_reaches_target;
- if ( !new_approx_is_better && !new_approx_reaches_target )
- {
- ROS_WARN_STREAM( "No better way to target found, turning and then reporting as unreachable."
- << endl << "Distance to target: " << distanceTo( target_point_ ) << "m; requested: " << desired_distance_ << "m" );
- m_MainMachine.setState( FINAL_TURN );
- }
- else
- {
- target_approx_ = new_target_approx;
- m_MainMachine.setState ( FOLLOWING_PATH );
- calculatePath();
- }
- }
-
-void HomerNavigationNode::sendPathData()
-{
- geometry_msgs::PoseStamped pose_stamped;
- pose_stamped.pose = robot_pose_;
- pose_stamped.header.frame_id = "/map";
- pose_stamped.header.stamp = ros::Time::now();
-
- nav_msgs::Path msg;
- msg.poses = waypoints_;
- if(waypoints_.size() > 0)
- msg.poses.insert(msg.poses.begin(), pose_stamped);
- path_pub_.publish(msg);
- }
-
-void HomerNavigationNode::sendTargetReachedMsg() {
- sendStopRobot();
- m_MainMachine.setState( IDLE );
- std_msgs::Empty reached_msg;
- target_reached_pub_.publish(reached_msg);
- waypoints_.clear();
- nav_msgs::Path empty_path_msg;
- empty_path_msg.poses = waypoints_;
- path_pub_.publish(empty_path_msg);
- ROS_INFO_STREAM("TargetReachedMsg");
-}
-
-void HomerNavigationNode::sendTargetUnreachableMsg( int8_t reason ) {
- sendStopRobot();
- m_MainMachine.setState( IDLE );
- homer_mapnav_msgs::TargetUnreachable unreachable_msg;
- unreachable_msg.reason = reason;
- target_unreachable_pub_.publish(unreachable_msg);
- waypoints_.clear();
- nav_msgs::Path empty_path_msg;
- empty_path_msg.poses = waypoints_;
- path_pub_.publish(empty_path_msg);
- ROS_INFO_STREAM("TargetUnreachableMsg");
-}
-
-void HomerNavigationNode::targetPositionReached()
-{
- //we're as close s we can get, target reached
- ROS_INFO_STREAM( "Target position reached. Distance to target: " << distanceTo( target_point_ ) << "m. Desired distance:" << desired_distance_ << "m" );
- sendStopRobot();
- // usleep( 30000 );
- waypoints_.clear();
- sendPathData();
- m_MainMachine.setState( FINAL_TURN );
- ROS_INFO_STREAM("Turning to look-at point");
-}
-
-
-void HomerNavigationNode::checkPath()
-{
- invalid_path_count_=0;
- for ( unsigned i=0; i<pixel_path_.size(); i++ )
- {
- geometry_msgs::Point p = map_tools::fromMapCoords(pixel_path_.at(i), origin_, resolution_);
- if ( distanceTo(p) > check_path_max_distance_ )
- {
- continue;
- }
- if (map_tools::findValue( last_map_data_, width_, height_, pixel_path_[i].x(), pixel_path_[i].y(), 90, m_AllowedObstacleDistance.first ) )
- {
- invalid_path_count_++;
- ROS_WARN_STREAM("Obstacle detected in current path " << invalid_path_count_ << " of " << check_path_max_errors_ << " times.");
-
- if ( invalid_path_count_ >= check_path_max_errors_ )
- {
- ROS_WARN_STREAM( "Replanning path." );
- currentPathFinished();
- }
- return;
- }
- }
-}
-
-void HomerNavigationNode::handleCollision ()
-{
- if ( m_MainMachine.state()== FOLLOWING_PATH )
- {
- sendStopRobot();
- if( distanceTo( target_point_) < collision_distance_near_target_ )
- {
- ROS_INFO_STREAM("Collision detected near target. Switch to final turn.");
- targetPositionReached();
- }
- else
- {
- m_MainMachine.setState( AVOIDING_COLLISION );
- ROS_WARN_STREAM( "Collision detected while following path!" );
- }
- }
-}
-
-float HomerNavigationNode::calcSpeedFactor()
-{
- float speedFactor = SpeedControl::getSpeedFactor(laser_points_, m_MinMoveSpeedFactor, 1.0 );
-
- m_LastSpeedFactors.push_back( speedFactor );
- if ( m_LastSpeedFactors.size() > m_SpeedFactorMeanFilterSize ) { m_LastSpeedFactors.pop_front(); }
-
- float speedFactorMean = mean( m_LastSpeedFactors );
-
- return speedFactorMean;
-}
-
-
-void HomerNavigationNode::performNextMove()
-{
- float maxMoveDistance = SpeedControl::getMaxMoveDistance ( laser_points_ );
- float speedFactor = calcSpeedFactor();
-
- switch ( m_MainMachine.state() )
- {
- case FOLLOWING_PATH:
- {
- if ( distanceTo( target_point_ ) < desired_distance_ )
- {
- ROS_INFO_STREAM( "Desired distance to target was reached." );
- targetPositionReached();
- return;
- }
-
- float waypointRadiusLaser = 0.125 * maxMoveDistance;
-
- Eigen::Vector2i waypointPixel = map_tools::toMapCoords(waypoints_[0].pose.position, origin_, resolution_);
- float obstacleDistanceMap = explorer_->getObstacleTransform()->getValue( waypointPixel.x(), waypointPixel.y() );
- float waypointRadiusMap = 0.125 * obstacleDistanceMap * resolution_;
-
- float waypointRadius = waypointRadiusLaser + waypointRadiusMap;
- if ( ( waypointRadius < resolution_ ) || ( waypoints_.size() == 1 ) )
- {
- waypointRadius = resolution_;
- }
-
- //if we have accidentaly skipped waypoints, recalculate path
- float minDistance=FLT_MAX;
- unsigned nearestWaypoint=0;
- for ( unsigned i=0; i<waypoints_.size(); i++ )
- {
- if ( distanceTo( waypoints_[i].pose.position ) < minDistance ) {
- nearestWaypoint = i;
- minDistance = distanceTo( waypoints_[i].pose.position );
- }
- }
- if ( nearestWaypoint != 0 )
- {
- ROS_WARN_STREAM("Waypoints skipped. Recalculating path!");
- calculatePath();
- if ( m_MainMachine.state() != FOLLOWING_PATH ) { break; }
- }
-
- ROS_DEBUG_STREAM("NextMove DEBUG VALUES: maxMoveDistance="<< maxMoveDistance <<"\n"
- << "\tWaypointRadiusLaser=" << waypointRadiusLaser << "\n\t"
- << "obstacleDistanceMap=" << obstacleDistanceMap << "\n\t"
- << "");
-
- //check if current waypoint has been reached
- if ( (waypoints_.size() != 0) && ( distanceTo( waypoints_[0].pose.position ) < waypointRadius ) )
- {
- waypoints_.erase ( waypoints_.begin() );
- ROS_DEBUG_STREAM("homer_navigation::performNextMove(): Current waypoint has been reached! Still " << waypoints_.size() << " to reach!");
- }
-
- sendPathData();
-
- ROS_ERROR_STREAM("waypointRadius: " << waypointRadius);
- //last wayoint reached
- if ( waypoints_.size() == 0 )
- {
- ROS_INFO_STREAM("Last waypoint reached");
- currentPathFinished();
- return;
- }
-
- geometry_msgs::Point currentWaypoint = waypoints_[0].pose.position;
-
- double distanceToWaypoint = distanceTo ( currentWaypoint );
- double angleToWaypoint = angleToPointDeg ( currentWaypoint );
-
- ROS_DEBUG_STREAM("homer_navigation::performNextMove(): Distance to waypoint: "<<distanceToWaypoint<<" Angle to waypoint: "<< (angleToWaypoint) << " Waypoint: " << currentWaypoint << "Robot Pose: " << robot_pose_.position << " Robot orientation " << tf::getYaw(robot_pose_.orientation)) ;
-
- ostringstream stream;
- stream.precision(2);
-
- if (!m_use_cmd_vel_)
- {
- ROS_ERROR_STREAM("Not supported");
- }
- //move to next waypoint if heading in right direction
- else // else use cmd_vel
- {
- if (angleToWaypoint < -180)
- {
- angleToWaypoint += 360;
- }
-
- //linear speed calculation
- double speed = distanceToWaypoint;
- if (speed < 0 )
- {
- speed = max(speed,-m_max_move_speed);
- }
- else
- {
- speed = min(speed,m_max_move_speed);
- }
- if(m_avoided_collision)
- {
- if( std::abs(angleToWaypoint) < 10)
- {
- m_avoided_collision = false;
- }
- else
- {
- speed = 0;
- }
- }
- //linear speed calculation end
- //angular speed calculation
- double angle = angleToWaypoint*3.14/180.0;
- if (abs(angle) < m_min_turn_angle)
- {
- angle = 0.0;
- }
- else
- {
- if (abs(angle) > m_max_drive_angle)
- {
- speed = 0.0;
- }
- if (angle < 0 )
- {
- angle = max(angle,-m_max_turn_speed);
- }
- else
- {
- angle = min(angle,m_max_turn_speed);
- }
- if (distanceToWaypoint < 1.0)
- {
- angle *= distanceToWaypoint;
- }
- // min speed for angles because under 0.35 the machine is really really slow
- if (speed < 0.07)
- {
- if ( angle < 0 )
- {
- angle = min(angle,-0.45);
- }
- else
- {
- angle = max(angle,0.45);
- }
- }
- }
- //angular speed calculation end
-
- m_act_speed = speed;
- m_act_angle = angle;
- geometry_msgs::Twist cmd_vel_msg;
- cmd_vel_msg.linear.x = speed;
- cmd_vel_msg.angular.z = angle;
- cmd_vel_pub_.publish(cmd_vel_msg);
-
- stream << "Driving & turning" << endl;
- stream << "linear: " << speed << " angular: " << angle << endl;
- stream << "distanceToWaypoint:" << distanceToWaypoint << "angleToWaypoint: " << angleToWaypoint << endl;
- }
- ROS_INFO_STREAM( stream.str() );
- break;
- }
-
- case AVOIDING_COLLISION:
- {
- if( distanceTo( target_point_) < collision_distance_near_target_ )
- {
- ROS_INFO_STREAM("Collision detected near target. Switch to final turn.");
- targetPositionReached();
- }
- else if ( maxMoveDistance <= collision_distance_ )
- {
- ostringstream stream;
- stream << "Maximum driving distance too short (" << maxMoveDistance << "m)! Moving back.";
- ROS_WARN_STREAM( stream.str() );
- if (!m_use_cmd_vel_)
- {
- ROS_ERROR_STREAM("Not supported");
- }
- else // cmd_vel
- {
- geometry_msgs::Twist cmd_vel_msg;
- if(HomerNavigationNode::obstacleBackwardDistance() > 0.4)
- {
- cmd_vel_msg.linear.x = -0.3;
- }
- else
- {
- cmd_vel_msg.angular.z = -0.45;
- }
- cmd_vel_pub_.publish(cmd_vel_msg);
- }
- }
- else
- {
- m_avoided_collision = true;
- ROS_WARN_STREAM( "Collision avoided. Updating path." );
- currentPathFinished();
- }
- break;
- }
- case FINAL_TURN:
- {
- if ( 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);
- }
- break;
- }
-
- double turnAngle = minTurnAngle( tf::getYaw(robot_pose_.orientation), target_orientation_ );
- ROS_INFO_STREAM("homer_navigation::PerformNextMove:: Final Turn. Robot orientation: " << rad2Deg(tf::getYaw(robot_pose_.orientation)) << ". Target orientation: " << rad2Deg(target_orientation_) );
- ROS_DEBUG_STREAM ( "homer_navigation::PerformNextMove:: turnAngle: " << rad2Deg(turnAngle));
- if (m_use_cmd_vel_ ) // final turn with cmd_vel
- {
- if (turnAngle< 0 )
- {
- turnAngle= max(turnAngle,-m_max_turn_speed);
- }
- else
- {
- turnAngle = min(turnAngle,m_max_turn_speed);
- }
-
- if (abs(turnAngle) < m_min_turn_angle)
- {
- ROS_INFO_STREAM(":::::::TARGET REACHED BECAUSE lower "<<m_min_turn_angle);
- if( m_path_reaches_target )
- {
- sendTargetReachedMsg();
- }
- else
- {
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
- }
- else
- {
- geometry_msgs::Twist cmd_vel_msg;
- cmd_vel_msg.angular.z = turnAngle;
- cmd_vel_pub_.publish(cmd_vel_msg);
- }
- }
- break;
- }
-
- case AWAITING_EXPLORATION_MAP:
- case AWAITING_PATHPLANNING_MAP:
- {
- //make sure that the robot doesn't move
- ROS_INFO_STREAM("Awaiting pathplanning map");
- sendStopRobot();
- break;
- }
- case IDLE:
- break;
- }
-}
-
-void HomerNavigationNode::currentPathFinished() // also used for replanning
-{
- ROS_INFO_STREAM( "Current path was finished, initiating recalculation.");
- waypoints_.clear();
- sendStopRobot();
- m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
-
-}
-
-// returns angle to target point in degrees(!)
-int HomerNavigationNode::angleToPointDeg ( geometry_msgs::Point target )
-{
- double cx = robot_pose_.position.x;
- double cy = robot_pose_.position.y;
- int targetAngle = rad2Deg( atan2 ( target.y - cy, target.x - cx ) );
- int currentAngle = rad2Deg( tf::getYaw(robot_pose_.orientation) );
-
- int angleDiff = targetAngle - currentAngle;
- angleDiff = (angleDiff + 180) % 360 - 180;
- return angleDiff;
-}
-
-
-double HomerNavigationNode::distanceTo ( geometry_msgs::Point target )
-{
- double cx = robot_pose_.position.x;
- double cy = robot_pose_.position.y;
- double distance_to_target_2 = ( cx - target.x ) * ( cx - target.x ) + ( cy - target.y ) * ( cy - target.y );
- return sqrt ( distance_to_target_2 );
-}
-
-float HomerNavigationNode::obstacleBackwardDistance()
-{
- float min_y = -0.27;
- float max_y = 0.27;
-
- float back_distance = 4;
-
- for(float depth = -0.3 ; depth > -1.0; depth -= 0.01)
- {
- for(float y = min_y ; y <= max_y ; y += 0.1)
- {
- geometry_msgs::Point base_link_point;
- base_link_point.x = depth;
- base_link_point.y = y;
- geometry_msgs::Point map_point = map_tools::transformPoint(base_link_point, transform_listener_ ,"/base_link", "/map");
- int i = map_tools::map_index(map_point, origin_ ,width_,resolution_);
- if(last_map_data_->at(i) > 90)
- {
- if(back_distance > HomerNavigationNode::distanceTo(map_point))
- {
- back_distance = HomerNavigationNode::distanceTo(map_point);
- }
- }
- }
- if(back_distance != 4)
- {
- break;
- }
- }
- ROS_ERROR_STREAM("Back Distance: "<< back_distance);
- return back_distance;
-}
-
-void HomerNavigationNode::actualizeStatusInfo()
-{
- ostringstream stream;
- stream << m_MapTypeMachine.stateString() << '\n'
- << m_MainMachine.stateString();
- ROS_DEBUG_STREAM( stream.str() );
-}
-
-void HomerNavigationNode::maskMap()
-{
- //generate bounding box
- ROS_INFO_STREAM("Calculating Bounding box for fast planning");
- Eigen::Vector2i pose_pixel = map_tools::toMapCoords( robot_pose_.position, origin_, resolution_);
- Eigen::Vector2i target_pixel = map_tools::toMapCoords( target_point_, origin_, 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(), origin_, resolution_));
- ROS_INFO_STREAM("max in m: "<<map_tools::fromMapCoords(safe_planning_box.max(), origin_, resolution_));
- for(size_t x = 0; x < width_; x++)
- {
- for(size_t y = 0; y < width_; y++)
- {
- if(!safe_planning_box.contains(Eigen::Vector2i(x, y)))
- {
- last_map_data_->at(y * 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;
-
- float ret = static_cast<double>(diff) * M_PI/180.0;
- return ret;
-}
-
-template<class ContainerT>
-double HomerNavigationNode::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() );
-}
-
-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(msg->header.stamp != last_map_timestamp_)
- {
- last_map_timestamp_ = msg->header.stamp;
- last_map_data_ = new std::vector<int8_t>(msg->data);
- origin_ = msg->info.origin;
- width_ = msg->info.width;
- height_ = msg->info.height;
- resolution_ = msg->info.resolution;
-
- Eigen::Vector2i map_point;
- if(m_laser_points_map.size() > 0)
- {
- for(int i = 0; i < m_laser_points_map.size(); i++)
- {
- geometry_msgs::Point& point = m_laser_points_map[i];
- map_point = map_tools::toMapCoords(point, origin_, resolution_);
- int k = map_point.y() * width_ + map_point.x();
- if(k < 0 || k > width_*height_)
- {
- continue;
- }
- last_map_data_->at(k) = homer_mapnav_msgs::ModifyMap::BLOCKED;
- }
- }
- if(m_back_laser.size() > 0 )
- {
- for(int i = 0; i < m_back_laser.size(); i++)
- {
- geometry_msgs::Point& point = m_back_laser[i];
- map_point = map_tools::toMapCoords(point, origin_, resolution_);
- int k = map_point.y() * width_ + map_point.x();
- if(k < 0 || k > width_*height_)
- {
- continue;
- }
- last_map_data_->at(k) = homer_mapnav_msgs::ModifyMap::BLOCKED;
- }
- }
-
- switch ( m_MainMachine.state() )
- {
- case AWAITING_EXPLORATION_MAP:
- if ( m_MapTypeMachine.state() == SLAM_MAP )
- {
- startExploration();
- }
- break;
-
- case AWAITING_PATHPLANNING_MAP:
- startNavigation();
- break;
- case FOLLOWING_PATH:
- {
- if ( check_path_)
- {
- checkPath();
- }
-
- break;
- }
- default:
- break;
- }
- }
-}
-
-void HomerNavigationNode::poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
-{
- robot_pose_ = msg->pose;
- performNextMove();
-}
-
-void HomerNavigationNode::laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
-{
- float frontal_obstacle_distance;
- bool collision_detected;
-
- last_laser_time_ = ros::Time::now();
- laser_points_ = map_tools::laser_ranges_to_points(msg->ranges, msg->angle_min, msg->angle_increment, msg->range_min, msg->range_max, transform_listener_, msg->header.frame_id, "/base_link");
- m_laser_points_map = map_tools::laser_ranges_to_points( msg->ranges, msg->angle_min, msg->angle_increment, msg->range_min, msg->range_max, transform_listener_, msg->header.frame_id, "/map");
-
- if(m_act_speed == 0)
- {
- return;
- }
- frontal_obstacle_distance = SpeedControl::getMaxMoveDistance ( laser_points_ );
- collision_detected = frontal_obstacle_distance <= collision_distance_;
- if(collision_detected)
- {
- handleCollision();
- }
-}
-
-void HomerNavigationNode::backLaserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
-{
- m_back_laser = map_tools::laser_ranges_to_points( msg->ranges, msg->angle_min, msg->angle_increment, msg->range_min, msg->range_max, transform_listener_, msg->header.frame_id, "/map");
-}
-
-void HomerNavigationNode::startNavigationCallback(const homer_mapnav_msgs::StartNavigation::ConstPtr& msg)
-{
- ROS_INFO_STREAM("Start navigating to (" << msg->goal.position.x << ", " << msg->goal.position.y << ")");
- if (m_MainMachine.state() != IDLE) {
- ROS_WARN_STREAM( "Aborting current operation and starting navigation.\n");
- }
-
- m_MapTypeMachine.setState(SLAM_MAP);
-
- target_point_ = msg->goal.position;
- target_orientation_ = tf::getYaw(msg->goal.orientation);
- desired_distance_ = msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
- skip_final_turn_ = msg->skip_final_turn;
- fast_path_planning_ = msg->fast_planning;
-
- m_LastSpeedFactors.clear();
-
- ROS_INFO_STREAM("Navigating to target " << target_point_.x << ", " << target_point_.y
- << "\nTarget orientation: " << target_orientation_
- << "Desired distance to target: " << desired_distance_);
-
- m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
-}
-
-
-void HomerNavigationNode::moveBaseSimpleGoalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
-{
- m_avoided_collision = false;
- target_point_ = msg->pose.position;
- target_orientation_ = tf::getYaw(msg->pose.orientation);
- desired_distance_ = 0.1;// msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
- skip_final_turn_ = false;
- fast_path_planning_ = false;
-
- ROS_INFO_STREAM("Navigating to target via Move Base Simple x: " << target_point_.x << ", y: " << target_point_.y
- << "\nTarget orientation: " << target_orientation_
- << "Desired distance to target: " << desired_distance_ );
-
- m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
-}
-
-void HomerNavigationNode::navigateToPOICallback(const homer_mapnav_msgs::NavigateToPOI::ConstPtr &msg)
-{
- m_avoided_collision = false;
- std::string name = msg->poi_name;
- homer_mapnav_msgs::GetPointsOfInterest srv;
- 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 == name)
- {
- ROS_INFO_STREAM("Start navigating to (" << it->pose.position.x << ", " << it->pose.position.y << ")");
- if (m_MainMachine.state() != IDLE) {
- ROS_WARN_STREAM( "Aborting current operation and starting navigation.\n");
- }
-
- m_MapTypeMachine.setState(SLAM_MAP);
-
- sendStopRobot();
-
- target_point_ = it->pose.position;
- target_orientation_ = tf::getYaw(it->pose.orientation);
- desired_distance_ = msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
- skip_final_turn_ = msg->skip_final_turn;
-
-
- m_LastSpeedFactors.clear();
-
- ROS_INFO_STREAM("Navigating to target " << target_point_.x << ", " << target_point_.y
- << "\nTarget orientation: " << target_orientation_
- << "Desired distance to target: " << 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");
-}
-
-void HomerNavigationNode::stopNavigationCallback(const homer_mapnav_msgs::StopNavigation::ConstPtr& msg)
-{
- ROS_INFO_STREAM("Stopping navigation." );
- // stop exploring
- m_MainMachine.setState( IDLE );
- m_avoided_collision = false;
- sendStopRobot();
-
- waypoints_.clear();
- nav_msgs::Path empty_path_msg;
- empty_path_msg.poses = waypoints_;
- path_pub_.publish(empty_path_msg);
-}
-
-void HomerNavigationNode::unknownThresholdCallback(const std_msgs::Int8::ConstPtr &msg)
-{
- explorer_->setUnknownThreshold(static_cast<int>(msg->data));
-}
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv, "homer_navigation");
-
- HomerNavigationNode node;
-
- ros::Rate rate(12);
-
- 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/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/CMakeLists.txt b/src/Managers/CMakeLists.txt
similarity index 100%
rename from homer_map_manager/src/Managers/CMakeLists.txt
rename to src/Managers/CMakeLists.txt
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/MapManager.h b/src/Managers/MapManager.h
similarity index 100%
rename from homer_map_manager/src/Managers/MapManager.h
rename to src/Managers/MapManager.h
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/MaskingManager.h b/src/Managers/MaskingManager.h
similarity index 100%
rename from homer_map_manager/src/Managers/MaskingManager.h
rename to src/Managers/MaskingManager.h
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/PoiManager.h b/src/Managers/PoiManager.h
similarity index 100%
rename from homer_map_manager/src/Managers/PoiManager.h
rename to src/Managers/PoiManager.h
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/Managers/RoiManager.h b/src/Managers/RoiManager.h
similarity index 100%
rename from homer_map_manager/src/Managers/RoiManager.h
rename to src/Managers/RoiManager.h
diff --git a/homer_map_manager/src/MapIO/CMakeLists.txt b/src/MapIO/CMakeLists.txt
similarity index 100%
rename from homer_map_manager/src/MapIO/CMakeLists.txt
rename to src/MapIO/CMakeLists.txt
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/image_loader.h b/src/MapIO/image_loader.h
similarity index 100%
rename from homer_map_manager/src/MapIO/image_loader.h
rename to src/MapIO/image_loader.h
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_loader.h b/src/MapIO/map_loader.h
similarity index 100%
rename from homer_map_manager/src/MapIO/map_loader.h
rename to src/MapIO/map_loader.h
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/MapIO/map_saver.h b/src/MapIO/map_saver.h
similarity index 100%
rename from homer_map_manager/src/MapIO/map_saver.h
rename to src/MapIO/map_saver.h
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/Workers/PointOfInterest/PointOfInterest.h b/src/Workers/PointOfInterest/PointOfInterest.h
similarity index 100%
rename from homer_map_manager/src/Workers/PointOfInterest/PointOfInterest.h
rename to src/Workers/PointOfInterest/PointOfInterest.h
diff --git a/homer_map_manager/src/main.cpp b/src/main.cpp
similarity index 100%
rename from homer_map_manager/src/main.cpp
rename to src/main.cpp
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
diff --git a/homer_map_manager/src/map_manager_node.h b/src/map_manager_node.h
similarity index 100%
rename from homer_map_manager/src/map_manager_node.h
rename to src/map_manager_node.h
diff --git a/tracks.yaml b/tracks.yaml
deleted file mode 100644
index 3ec3f7abdcf96a57064a343d5a358beb3c73c02f..0000000000000000000000000000000000000000
--- a/tracks.yaml
+++ /dev/null
@@ -1,23 +0,0 @@
-tracks:
- indigo:
- actions:
- - bloom-export-upstream :{vcs_local_uri} :{vcs_type} --tag :{release_tag} --display-uri
- :{vcs_uri} --name :{name} --output-dir :{archive_dir_path}
- - git-bloom-import-upstream :{archive_path} :{patches} --release-version :{version}
- --replace
- - git-bloom-generate -y rosrelease :{ros_distro} --source upstream -i :{release_inc}
- - git-bloom-generate -y rosdebian --prefix release/:{ros_distro} :{ros_distro}
- -i :{release_inc}
- - git-bloom-generate -y rosrpm --prefix release/:{ros_distro} :{ros_distro} -i
- :{release_inc}
- devel_branch: null
- last_version: 1.0.10
- name: upstream
- patches: null
- release_inc: '0'
- release_repo_url: null
- release_tag: :{version}
- ros_distro: indigo
- vcs_type: git
- vcs_uri: https://gitlab.uni-koblenz.de/robbie/homer_mapnav.git
- version: :{auto}