Commit c8ff70f9 authored by Niklas Yann Wettengel's avatar Niklas Yann Wettengel

initial commit

parents
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
homer_tools
)
find_package(OpenMP)
if (OPENMP_FOUND)
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()
find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED)
find_package(tf REQUIRED)
set(CMAKE_BUILD_TYPE Release)
include_directories(
include
${catkin_INCLUDE_DIRS}
${QT_INCLUDE_DIR}
)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS
roscpp
homer_mapnav_msgs
homer_nav_libs
)
include(${QT_USE_FILE})
add_definitions(${QT_DEFINITIONS})
add_library( OccupancyMap
src/OccupancyMap/OccupancyMap.cpp
)
target_link_libraries(
OccupancyMap
${catkin_LIBRARIES}
${QT_LIBRARIES}
${tf_LIBRARIES}
)
set(
ParticleFilter_SRC
src/ParticleFilter/HyperSlamFilter.cpp
src/ParticleFilter/SlamParticle.cpp
src/ParticleFilter/SlamFilter.cpp
src/ParticleFilter/Particle.cpp
)
add_library(
ParticleFilter
${ParticleFilter_SRC}
)
target_link_libraries(
ParticleFilter
${catkin_LIBRARIES}
)
add_executable(homer_mapping src/slam_node.cpp)
target_link_libraries(
homer_mapping
${catkin_LIBRARIES}
ParticleFilter
OccupancyMap
)
add_dependencies(
homer_mapping
${catkin_EXPORTED_TARGETS}
)
# 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/size: 40 # size of one edge of the map in m. map is quadratic
/homer_mapping/resolution: 0.05 # m meter per cell
/homer_mapping/max_laser: 20.0 # m max range for including range into map
#map config values
/homer_mapping/backside_checking: false # Enable checking to avoid matching front- and backside of obstacles, e.g. walls. Useful when creating high resolution maps
/homer_mapping/obstacle_borders: true # Leaves a small border around obstacles unchanged when inserting a laser scan. Improves stability of generated map
/homer_mapping/measure_sampling_step: 0.1 # m Minimum distance in m between two samples for probability calculation
/homer_mapping/laser_scanner/free_reading_distance: 0.8 # Minimum distance in m to be classified as free in case of errorneous measurement
/particlefilter/error_values/rotation_error_rotating: 10.0 # percent
/particlefilter/error_values/rotation_error_translating: 2.0 # degrees per meter
/particlefilter/error_values/translation_error_translating: 10.0 # percent
/particlefilter/error_values/translation_error_rotating: 0.05 # m per degree
/particlefilter/error_values/move_jitter_while_turning: 0.1 # m per degree
/particlefilter/hyper_slamfilter/particlefilter_num: 1
/particlefilter/particle_num: 1000
/particlefilter/max_rotation_per_second: 0.4 # maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted
/particlefilter/wait_time: 0.05 # minimum time to wait between two slam steps in seconds
#the map is only updated when the robot has turned a minimal angle, has moved a minimal distance or a maximal time has passed
/particlefilter/update_min_move_angle: 2 # degrees
/particlefilter/update_min_move_dist: 0.05 # m
/particlefilter/max_update_interval: 0.5 # sec
/selflocalization/scatter_var_xy: 0.1 # m
/selflocalization/scatter_var_theta: 0.2 # radiants
/map_manager/roi_updates: true # toggles if a entering or exiting of a roi is beeing published
map_manager/roi_polling_time: 0.5 # sec timeinterval after which the roi status is checked
This diff is collapsed.
#ifndef HYPERSLAMFILTER_H
#define HYPERSLAMFILTER_H
#include <vector>
#include <homer_mapping/ParticleFilter/ParticleFilter.h>
#include <homer_mapping/ParticleFilter/SlamParticle.h>
#include <homer_mapping/ParticleFilter/SlamFilter.h>
#include <homer_nav_libs/Math/Pose.h>
#include <homer_mapping/OccupancyMap/OccupancyMap.h>
#include <sensor_msgs/LaserScan.h>
class OccupancyMap;
/**
* @class HyperSlamFilter
*
* @author Malte Knauf, Stephan Wirth, Susanne Maur
*
* @brief This class is used to determine the robot's most likely pose with given map and given laser data.
*
* A particle filter is a descrete method to describe and compute with a probability distribution.
* This particle filter uses an occupancy map to determine the probability of robot states.
* The robot states are stored in a particle together with their weight @see SlamParticle.
*
* @see SlamParticle
* @see ParticleFilter
* @see OccupancyMap
*/
class HyperSlamFilter {
public:
/**
* This constructor initializes the random number generator and sets the member variables to the given values.
* @param particleNum Number of particleFilters to use.
*/
HyperSlamFilter(int particleFilterNum, int particleNum);
/**
* The destructor releases the OccupancyMap and the particles.
*/
~HyperSlamFilter();
/**
* This method runs the filter routine.
* The given odometry measurement is used as movement hypothesis, the laserData-vector is used
* as measurement and is used to weight the particles.
* @param currentPoseOdometry Odometry data of time t.
* @param laserData msg containing the laser measurement.
* @param measurementTime Time stamp of the measurement.
* @param filterDurationTime Returns the time in ms that the filtering needed
*/
void filter(Pose currentPoseOdometry, sensor_msgs::LaserScanConstPtr laserData, ros::Time measurementTime,
ros::Duration &filterDuration);
/**
* Computes and sets the new value for m_Alpha1.
* @param percent Rotation error while rotating (see class constructor for details)
*/
void setRotationErrorRotating(float percent);
/**
* Computes and sets the new value for m_Alpha2.
* @param degreesPerMeter Rotation error while translating (see class constructor for details)
*/
void setRotationErrorTranslating(float degreesPerMeter);
/**
* Computes and sets the new value for m_Alpha3.
* @param percent Translation error while translating (see class constructor for details)
*/
void setTranslationErrorTranslating(float percent);
/**
* Computes and sets the new value for m_Alpha4.
* @param mPerDegree Translation error while rotating (see class constructor for details)
*/
void setTranslationErrorRotating(float mPerDegree);
/**
* Computes and sets the new value for m_Alpha5.
* @param mPerDegree Move jitter while turning (see class constructor for details)
*/
void setMoveJitterWhileTurning(float mPerDegree);
/**
* Sets a new minimal size of a cluster of scan points which is considered in scan matching.
* @param clusterSize Minimal size of a cluster in mm of scan points which is considered in scan matching.
*/
void setScanMatchingClusterSize(float clusterSize);
/**
* Sets whether the map is updated or just used for self-localization.
* @param doMapping True if robot shall do mapping, false otherwise.
*/
void setMapping(bool doMapping);
/**
* Deletes the current occupancy map and copies a new one into the system.
* @param occupancyMap The occupancy map to load into the system (copies are being made)
*/
void setOccupancyMap(OccupancyMap* occupancyMap);
/**
* Sets the robot pose in the current occupancy map.
* @param Robot pose.
* @param scatterVariance if not equal to 0 the poses are equally scattered around the pose
*/
void setRobotPose(Pose pose, double scatterVarXY=0.0, double scatterVarTheta=0.0);
/**
*Returns the best SlamFilter
*/
SlamFilter* getBestSlamFilter();
void resetHigh();
/**
* Factor (default 0.98) of the contrast of the best particle.
* If the contrast of the worst particle is below this threshold
* it will be replaced by the best particle
* @param deletionThreshold see above
*/
void setDeletionThreshold(double deletionThreshold);
/**
* applies masking to map of slam filter set in GUI
* @param msg masking message received from GUI
*/
void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
for(unsigned i = 0; i < m_ParticleFilterNum; ++i)
{
m_SlamFilters[i]->applyMasking(msg);
}
}
private:
/** Used SlamFilters */
std::vector <SlamFilter*> m_SlamFilters;
/** Number of SlamFilters */
unsigned m_ParticleFilterNum;
/** Number of Particles of SlamFilter */
unsigned m_ParticleNum;
/** */
double m_DeletionThreshold;
/** Best SLAM Filter */
SlamFilter* m_BestSlamFilter;
/** Worst SlamFilter */
SlamFilter* m_WorstSlamFilter;
bool m_DoMapping;
};
#endif
#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
#ifndef PARTICLEFILTER_H
#define PARTICLEFILTER_H
#include <iostream>
#include <cmath>
#include <limits.h>
#include <omp.h>
#include <ros/ros.h>
class Particle;
const float MIN_EFFECTIVE_PARTICLE_WEIGHT = 0.2;
/**
* @class ParticleFilter
*
* @author Malte Knauf, Stephan Wirth
*
* @brief This class is a template class for a particle filter.
*
* A particle filter is a descrete method to describe and compute with a probability distribution.
* This template class implements the basic methods for a particle filter: sort() and resample().
* Use this class do derivate your custom particle filter from it. Use a self-defined subclass of
* Particle as ParticleType.
*
* @see Particle
*/
template <class ParticleType>
class ParticleFilter {
public:
/**
* The constructor initializes the random number generator and allocates the memory for the particle lists.
* The lists will have particleNum elements.
* @param particleNum Number of particles for the filter.
*/
ParticleFilter<ParticleType>(int particleNum);
/**
* The destructor releases the particle lists.
*/
virtual ~ParticleFilter();
/**
* @return Number of particles used in this filter
*/
int getParticleNum();
/**
* @return The number of effective particles (according to "Improving Grid-based SLAM with Rao-Blackwellized Particle
* Filters by Adaptive Proposals and Selective Resampling (2005)" by Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
*/
int getEffectiveParticleNum() const;
int getEffectiveParticleNum2() const;
/**
* @return Pointer to the particle that has the highest weight.
*/
ParticleType* getBestParticle() const;
protected:
/**
* This method generates a random variable in the interval [0,1].
* @param init The initial value for the static random base number. When running the constructor of this
* class, this method is run once with the C-function time() as parameter to initialize it.
* Then you should use it without parameter.
* @return Random value between 0 and 1
*/
double random01(unsigned long init = 0) const;
/**
* This method sorts the particles in m_CurrentList from leftIndex to rightIndex according to their weight.
* The particle with the highest weight is at position 0 after calling this function. The algorithm used here is
* known as quicksort and works recursively.
* @param leftIndex Left index of area to sort
* @param rightIndex Right index of area to sort
*/
void sort(int leftIndex, int rightIndex);
/**
* This method normalizes the weights of the particles. After calling this function, the sum of the weights of
* all particles in m_CurrentList equals 1.0.
* In this function the sum of all weights of the particles of m_CurrentList is computed and each weight of each
* particle is devided through this sum.
*/
void normalize();
/**
* This method selects a new set of particles out of an old set according to their weight
* (importance resampling). The particles from the list m_CurrentList points to are used as source,
* m_LastList points to the destination list. The pointers m_CurrentList and m_LastList are switched.
* The higher the weight of a particle, the more particles are drawn (copied) from this particle.
* The weight remains untouched, because measure() will be called afterwards.
* This method only works on a sorted m_CurrentList, therefore sort() is called first.
*/
void resample();
/**
* This method drifts the particles (second step of a filter process).
* Has to be implemented in sub-classes (pure virtual function).
*/
virtual void drift() = 0;
/**
* This method has to be implemented in sub-classes. It is used to determine the weight of each particle.
*/
virtual void measure() = 0;
/**
* These two pointers point to m_ParticleListOne and to m_ParticleListTwo.
* The particles are drawn from m_LastList to m_CurrentList to avoid new and delete commands.
* In each run, the pointers are switched in resample().
*/
ParticleType** m_CurrentList;
ParticleType** m_LastList;
/**
* Stores the number of particles.
*/
int m_ParticleNum;
/**
* Stores the number of effective particles.
*/
int m_EffectiveParticleNum;
};
template <class ParticleType>
ParticleFilter<ParticleType>::ParticleFilter(int particleNum) {
// initialize particle lists
m_CurrentList = new ParticleType*[particleNum];
m_LastList = new ParticleType*[particleNum];
// initialize random number generator
random01(time(0));
m_ParticleNum = particleNum;
}
template <class ParticleType>
ParticleFilter<ParticleType>::~ParticleFilter() {
if (m_CurrentList) {
delete[] m_CurrentList;
m_CurrentList = 0;
}
if (m_LastList) {
delete[] m_LastList;
m_LastList = 0;
}
}
template <class ParticleType>
int ParticleFilter<ParticleType>::getParticleNum() {
return m_ParticleNum;
}
template <class ParticleType>
double ParticleFilter<ParticleType>::random01(unsigned long init) const {
static unsigned long n;
if (init > 0) {
n = init;
}
n = 1664525 * n + 1013904223;
// create double from unsigned long
return (double)(n/2) / (double)LONG_MAX;
}
template <class ParticleType>
void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight) {
// SOMETHING LEFT TO SORT?
if (indexLeft >= indexRight) {
// ready!
return;
}
// CREATE PARTITION
int le = indexLeft;
int ri = indexRight;
int first = le;
int pivot = ri--;
while(le <= ri) {
// skip from left
while(m_CurrentList[le]->getWeight() > m_CurrentList[pivot]->getWeight()) {
le++;
}
// skip from right
while((ri >= first) && (m_CurrentList[ri]->getWeight() <= m_CurrentList[pivot]->getWeight())) {
ri--;
}
// now we have two elements to swap
if(le < ri) {
// swap
ParticleType* temp = m_CurrentList[le];
m_CurrentList[le] = m_CurrentList[ri];
m_CurrentList[ri] = temp;
le++;
}
}
if(le != pivot) {
// swap
ParticleType* temp = m_CurrentList[le];
m_CurrentList[le] = m_CurrentList[pivot];
m_CurrentList[pivot] = temp;
}
// sort left side
sort(indexLeft, le - 1);
// sort right side
sort(le + 1, indexRight);
}
template <class ParticleType>
void ParticleFilter<ParticleType>::normalize() {
float weightSum = 0.0;
for (int i = 0; i < m_ParticleNum; i++) {
weightSum += m_CurrentList[i]->getWeight();
}
// only normalize if weightSum is big enough to divide
if (weightSum > 0.000001)
{
//calculating parallel on 8 threats
omp_set_num_threads(8);
int i = 0;
// #pragma omp parallel for
for ( i = 0; i < m_ParticleNum; i++)
{
float newWeight = m_CurrentList[i]->getWeight() / weightSum;
m_CurrentList[i]->setWeight(newWeight);
}
}
else
{
ROS_WARN_STREAM( "Particle weights VERY small: " << weightSum << ". Got "<< m_ParticleNum << " particles.");
}
}
template <class ParticleType>
void ParticleFilter<ParticleType>::resample()
{
// swap pointers
ParticleType** help = m_LastList;
m_LastList = m_CurrentList;
m_CurrentList = help;
// now we copy from m_LastList to m_CurrentList
int drawIndex = 0;
// index of the particle where we are drawing to
int targetIndex = 0;
int numToDraw = 0;
do {
numToDraw = static_cast<int>(round((m_ParticleNum * m_LastList[drawIndex]->getWeight()) + 0.5));
for (int i = 0; i < numToDraw; i++) {
*m_CurrentList[targetIndex++] = *m_LastList[drawIndex];
// don't draw too much
if (targetIndex >= m_ParticleNum) {
break;
}
}
drawIndex++;
} while (numToDraw > 0 && targetIndex < m_ParticleNum);
// fill the rest of the particle list
for (int i = targetIndex; i < m_ParticleNum; i++) {
float particlePos = random01();
float weightSum = 0.0;
drawIndex = 0;
weightSum += m_LastList[drawIndex]->getWeight();
while (weightSum < particlePos) {
weightSum += m_LastList[++drawIndex]->getWeight();
}
*m_CurrentList[i] = *m_LastList[drawIndex];
}
}
template <class ParticleType>
int ParticleFilter<ParticleType>::getEffectiveParticleNum() const {
// does not work with normalized particle weights
// does not work with our weights at all (algorithm of Grisetti)
float squareSum = 0;
for (int i = 0; i < m_ParticleNum; i++) {
float weight = m_CurrentList[i]->getWeight();
squareSum += weight * weight;
}
return static_cast<int>(1.0f / squareSum);
}
template <class ParticleType>
int ParticleFilter<ParticleType>::getEffectiveParticleNum2() const {
// does not work with normalized particle weights
int effectiveParticleNum = 0;
for (int i = 0; i < m_ParticleNum; i++) {
if (m_CurrentList[i]->getWeight() > MIN_EFFECTIVE_PARTICLE_WEIGHT) {
effectiveParticleNum ++;
}
}
return effectiveParticleNum;
}