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

This reverts commit d5e27568.

homer_mapping/README.md

+# 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
+
+

homer_mapping/config/homer_mapping.yaml

+/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

homer_mapping/config/homer_mapping_followme.yaml

+/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

homer_mapping/launch/homer_mapping.launch

+<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>

homer_mapping/launch/homer_mapping_followme.launch

+<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>

homer_mapping/launch/homer_mapping_ros.launch

+<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>

homer_mapping/launch/homer_mapping_rviz.launch

+<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>

homer_mapping/mainpage.dox

+/**
+\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.
+-->
+
+
+*/

homer_mapping/manifest.xml

+<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>
+
+

homer_mapping/package.xml

+<package>
+  <name>homer_mapping</name>
+  <version>1.0.1</version>
+  <description>
+
+     homer_mapping
+
+  </description>
+  <maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
+  <author email="mknauf@uni-koblenz.de">Malte Knauf</author>
+
+  <license>GPLv3</license>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>roslib</build_depend>
+  <build_depend>homer_nav_libs</build_depend>
+
+  <build_depend>homer_mapnav_msgs</build_depend>
+ 
+
+  <run_depend>std_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>roslib</run_depend>  
+  <run_depend>homer_nav_libs</run_depend>
+
+  <run_depend>homer_mapnav_msgs</run_depend>
+</package>

homer_mapping/src/CMakeLists.txt

+add_subdirectory(Workers)
+add_subdirectory(Modules)

homer_mapping/src/OccupancyMap/CMakeLists.txt

+# 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
+)

homer_mapping/src/OccupancyMap/Math/CMakeLists.txt

+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})

homer_mapping/src/OccupancyMap/Math/Circle2D.cpp

+/*******************************************************************************
+ *  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

homer_mapping/src/OccupancyMap/Math/Circle2D.h

+/*******************************************************************************
+ *  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

homer_mapping/src/OccupancyMap/Math/Homography.cpp

+/*******************************************************************************
+ *  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

homer_mapping/src/OccupancyMap/Math/Homography.h

+/*******************************************************************************
+ *  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