From 53b48d21aff1500001dda32b462b6c8a51cfce1a Mon Sep 17 00:00:00 2001
From: Florian Polster <fpolster@uni-koblenz.de>
Date: Wed, 8 Mar 2017 19:34:17 +0100
Subject: [PATCH] holonmic drive particles
---
homer_mapping/config/homer_mapping.yaml | 6 +-
.../homer_mapping/OccupancyMap/OccupancyMap.h | 252 +++++++++------
.../ParticleFilter/ParticleFilter.h | 306 +++++++++---------
.../homer_mapping/ParticleFilter/SlamFilter.h | 181 +++++++----
.../include/homer_mapping/slam_node.h | 87 ++---
.../src/OccupancyMap/OccupancyMap.cpp | 50 +--
.../src/ParticleFilter/SlamFilter.cpp | 172 ++++++----
homer_mapping/src/slam_node.cpp | 30 ++
8 files changed, 619 insertions(+), 465 deletions(-)
diff --git a/homer_mapping/config/homer_mapping.yaml b/homer_mapping/config/homer_mapping.yaml
index cf75bf30..17ca7bef 100644
--- a/homer_mapping/config/homer_mapping.yaml
+++ b/homer_mapping/config/homer_mapping.yaml
@@ -8,9 +8,9 @@
/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: 1.0 # percent
+/particlefilter/error_values/rotation_error_rotating: 20.0 # percent
+/particlefilter/error_values/rotation_error_translating: 0.50 # degrees per meter
+/particlefilter/error_values/translation_error_translating: 8.0 # percent
/particlefilter/error_values/translation_error_rotating: 0.09 # m per degree
/particlefilter/error_values/move_jitter_while_turning: 0.05 # m per degree
diff --git a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
index 63f51b97..51cc9e9f 100644
--- a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
+++ b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
@@ -1,19 +1,19 @@
#ifndef OCCUPANCYMAP_H
#define OCCUPANCYMAP_H
-#include <vector>
+#include <iostream>
#include <list>
#include <string>
-#include <iostream>
+#include <vector>
#include <Eigen/Geometry>
-#include <homer_nav_libs/Math/Pose.h>
-#include <homer_nav_libs/Math/Point2D.h>
#include <homer_nav_libs/Math/Box2D.h>
+#include <homer_nav_libs/Math/Point2D.h>
+#include <homer_nav_libs/Math/Pose.h>
-#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
+#include <nav_msgs/OccupancyGrid.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/LaserScan.h>
@@ -23,60 +23,67 @@ 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)
+ * 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;
+struct RangeMeasurement {
+ geometry_msgs::Point sensorPos;
+ geometry_msgs::Point endPos;
+ float range;
+ bool free;
};
/**
- * Used in struct MeasurePoint to specify if a measurement point is at the border of a scan segment
+ * Used in struct MeasurePoint to specify if a measurement point is at the
+ * border of a scan segment
*/
-enum BorderType
-{
- NoBorder,
- LeftBorder,
- RightBorder
-};
+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
+ * 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;
+struct MeasurePoint {
+ Point2D hitPos;
+ Point2D frontPos;
+ BorderType borderType;
};
/**
* @class OccupancyMap
*
- * @author Malte Knauf, Stephan Wirth, Susanne Maur (RX), David Gossow (RX), Susanne Thierfelder (R16)
+ * @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).
+ * 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:
+ public:
static const int8_t INACCESSIBLE = 100;
static const int8_t OBSTACLE = 99;
static const int8_t OCCUPIED = 98;
@@ -92,16 +99,20 @@ class OccupancyMap {
/**
* Constructor for a loaded map.
*/
- OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin, float resolution, int width, int height, Box2D<int> exploredRegion);
+ OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin,
+ float resolution, int width, int height,
+ Box2D<int> exploredRegion);
/**
- * Copy constructor, copies all members inclusive the arrays that lie behind the pointers.
+ * 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.
+ * Method to init all members with default values from the configuration
+ * file. All arrays are initialized.
*/
void initMembers();
@@ -121,7 +132,7 @@ class OccupancyMap {
/**
* @return The resolution of the map in m.
*/
-// int resolution() const;
+ // int resolution() const;
geometry_msgs::Pose origin() const;
@@ -134,7 +145,7 @@ class OccupancyMap {
* @return Height of the map.
*/
int height() const;
-
+
/**
* This method is used to reset all HighSensitive areas
*/
@@ -148,65 +159,82 @@ class OccupancyMap {
/**
* @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
+ * 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.
+ * If the measurement is smaller than VALID_MAX_RANGE, markOccupied() is
+ * called for the endpoint.
* @param laserData The laser data msg.
*/
- void insertLaserData( sensor_msgs::LaserScan::ConstPtr laserData, tf::Transform transform);
+ void insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
+ tf::Transform transform);
- void insertRanges( vector<RangeMeasurement> ranges , ros::Time stamp = ros::Time::now() );
+ void insertRanges(vector<RangeMeasurement> ranges,
+ ros::Time stamp = ros::Time::now());
/**
- * @brief gives a list specially processed coordinates to be used for computeLaserScanProbability
+ * @brief gives a list specially processed coordinates to be used for
+ * computeLaserScanProbability
*/
- std::vector<MeasurePoint> getMeasurePoints( sensor_msgs::LaserScanConstPtr laserData );
+ std::vector<MeasurePoint> getMeasurePoints(
+ sensor_msgs::LaserScanConstPtr laserData);
/**
- * This method computes a score that describes how good the given hypothesis matches with the map
+ * 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
+ * @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 );
+ float computeScore(Pose robotPose, std::vector<MeasurePoint> measurePoints);
/**
- * @return QImage of size m_metaData.width, m_metaData.height with values of m_OccupancyProbability scaled to 0-254
+ * @return QImage of size m_metaData.width, m_metaData.height with values of
+ * m_OccupancyProbability scaled to 0-254
*/
- QImage getProbabilityQImage(int trancparencyThreshold, bool showInaccessible) const;
+ QImage getProbabilityQImage(int trancparencyThreshold,
+ bool showInaccessible) const;
- //puma2::ColorImageRGB8* getUpdateImage( bool withMap=true ); TODO
+ // 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
+ // 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] 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_metaData.resolution)
*/
- void getOccupancyProbabilityImage(vector<int8_t> &data, nav_msgs::MapMetaData& metaData);
+ void getOccupancyProbabilityImage(vector<int8_t>& data,
+ nav_msgs::MapMetaData& metaData);
/**
- * This method marks free the position of the robot (according to its dimensions).
+ * 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
+ * @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);
+ 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
+ * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the
+ * map
*/
double evaluateByContrast();
@@ -218,13 +246,12 @@ class OccupancyMap {
/**
* Sets cells of this map to free or occupied according to maskMap
*/
- void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg);
+ void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
- void changeMapSize(int x_add_left, int y_add_up, int x_add_right, int y_add_down );
-
-
- protected:
+ void changeMapSize(int x_add_left, int y_add_up, int x_add_right,
+ int y_add_down);
+ protected:
/**
* This method increments m_MeasurementCount for pixel p.
* @param p Pixel that has been measured.
@@ -232,13 +259,15 @@ class OccupancyMap {
void incrementMeasurementCount(Eigen::Vector2i p);
/**
- * This method increments the occupancy count int m_OccupancyCount for pixel 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.
+ * This method increments m_MeasurementCount and if neccessary
+ * m_OccupancyCount for all pixels.
*/
void applyChanges();
@@ -250,19 +279,21 @@ class OccupancyMap {
*/
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 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.
+ * This method computes the values for m_OccupancyProbabilities from
+ * m_MeasurementCount and m_OccupancyCount.
*/
void computeOccupancyProbabilities();
@@ -272,42 +303,47 @@ class OccupancyMap {
void clearCurrentChanges();
/**
- * This method resets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY and m_MaxChangeY.
+ * 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.
+ * 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
+ * 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.
+ * 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 metadata of the map
- */
- nav_msgs::MapMetaData m_metaData;
+ void cleanUp();
+ /**
+ * Stores the metadata of the map
+ */
+ nav_msgs::MapMetaData m_metaData;
/**
- * Stores the size of the map arrays, i.e. m_metaData.width * m_metaData.height
+ * Stores the size of the map arrays, i.e. m_metaData.width *
+ * m_metaData.height
* for faster computation.
*/
unsigned m_ByteSize;
@@ -321,28 +357,32 @@ class OccupancyMap {
// 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.
+ // Counts how often a pixel is hit by a measurement that says the pixel is
+ // occupied.
unsigned short* m_OccupancyCount;
- // Used for setting flags for cells, that have been modified during the current update.
+ // 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.
*/
- //minimum range classified as free in case of errorneous laser measurement
+ // 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
+ // 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
+ // 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
+ // minimum distance in m between two samples for probability calculation
float m_MeasureSamplingStep;
-
- //bool to reset high_sensitive Areas on the next iteration
+
+ // bool to reset high_sensitive Areas on the next iteration
bool m_reset_high;
/**
@@ -351,7 +391,8 @@ class OccupancyMap {
Box2D<int> m_ChangedRegion;
/**
- * Defines a bounding box around the area in the map, which is already explored.
+ * Defines a bounding box around the area in the map, which is already
+ * explored.
*/
Box2D<int> m_ExploredRegion;
@@ -359,12 +400,11 @@ class OccupancyMap {
* ros transform listener
*/
tf::TransformListener m_tfListener;
-
+
/**
* ros transformation laser to base_link
- */
+ */
tf::StampedTransform m_laserTransform;
tf::Transform m_latestMapTransform;
-
};
#endif
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
index 651f7587..0d605af1 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
@@ -1,10 +1,10 @@
#ifndef PARTICLEFILTER_H
#define PARTICLEFILTER_H
-#include <iostream>
-#include <cmath>
#include <limits.h>
#include <omp.h>
+#include <cmath>
+#include <iostream>
#include <ros/ros.h>
@@ -19,19 +19,22 @@ const float MIN_EFFECTIVE_PARTICLE_WEIGHT = 0.2;
*
* @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
+ * 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:
+ public:
/**
- * The constructor initializes the random number generator and allocates the memory for the particle lists.
+ * 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.
*/
@@ -48,8 +51,10 @@ class ParticleFilter {
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
+ * @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;
@@ -59,21 +64,23 @@ class ParticleFilter {
*/
ParticleType* getBestParticle() const;
-
- protected:
-
+ 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.
+ * @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
+ * 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
@@ -81,20 +88,28 @@ class ParticleFilter {
void sort(int leftIndex, int rightIndex);
/**
- * This method normalizes the weights of the particles. After calling this function, the sum of the weights of
+ * 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
+ * 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.
+ * 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();
@@ -105,13 +120,15 @@ class ParticleFilter {
virtual void drift() = 0;
/**
- * This method has to be implemented in sub-classes. It is used to determine the weight of each particle.
+ * 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.
+ * 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;
@@ -130,123 +147,116 @@ class ParticleFilter {
template <class ParticleType>
ParticleFilter<ParticleType>::ParticleFilter(int particleNum) {
- // initialize particle lists
- m_CurrentList = new ParticleType*[particleNum];
- m_LastList = new ParticleType*[particleNum];
+ // initialize particle lists
+ m_CurrentList = new ParticleType*[particleNum];
+ m_LastList = new ParticleType*[particleNum];
- // initialize random number generator
- random01(time(0));
+ // initialize random number generator
+ random01(time(0));
- m_ParticleNum = particleNum;
+ 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;
- }
+ 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;
+ 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;
+ 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--;
+ // SOMETHING LEFT TO SORT?
+ if (indexLeft >= indexRight) {
+ // ready!
+ return;
}
- // 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;
- }
+ // 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++;
+ }
+ }
- // sort left side
- sort(indexLeft, le - 1);
- // sort right side
- sort(le + 1, indexRight);
+ 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);
+ 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.");
}
- }
- else
- {
- ROS_WARN_STREAM( "Particle weights VERY small: " << weightSum << ". Got "<< m_ParticleNum << " particles.");
- }
}
template <class ParticleType>
-void ParticleFilter<ParticleType>::resample()
-{
+void ParticleFilter<ParticleType>::resample() {
// swap pointers
ParticleType** help = m_LastList;
m_LastList = m_CurrentList;
@@ -259,62 +269,58 @@ void ParticleFilter<ParticleType>::resample()
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;
+ 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++;
+ 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];
+ 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);
+ // 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 ++;
+ int effectiveParticleNum = 0;
+ for (int i = 0; i < m_ParticleNum; i++) {
+ if (m_CurrentList[i]->getWeight() > MIN_EFFECTIVE_PARTICLE_WEIGHT) {
+ effectiveParticleNum++;
+ }
}
- }
- return effectiveParticleNum;
+ return effectiveParticleNum;
}
-
template <class ParticleType>
ParticleType* ParticleFilter<ParticleType>::getBestParticle() const {
- return m_CurrentList[0];
+ return m_CurrentList[0];
}
-
#endif
-
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
index 89026615..68be9519 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
@@ -1,15 +1,15 @@
#ifndef SLAMFILTER_H
#define SLAMFILTER_H
-#include <vector>
+#include <homer_mapping/OccupancyMap/OccupancyMap.h>
#include <homer_mapping/ParticleFilter/ParticleFilter.h>
#include <homer_mapping/ParticleFilter/SlamParticle.h>
#include <homer_nav_libs/Math/Pose.h>
-#include <homer_mapping/OccupancyMap/OccupancyMap.h>
+#include <vector>
-#include <sensor_msgs/LaserScan.h>
-#include <homer_nav_libs/Math/Transformation2D.h>
#include <homer_nav_libs/Math/Math.h>
+#include <homer_nav_libs/Math/Transformation2D.h>
+#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
@@ -19,7 +19,6 @@
#include "ros/ros.h"
-
class OccupancyMap;
/**
@@ -27,28 +26,31 @@ class OccupancyMap;
*
* @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.
+ * @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.
+ * 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.
- */
+ 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 );
+ SlamFilter(SlamFilter& slamFilter);
/**
* The destructor releases the OccupancyMap and the particles.
@@ -57,23 +59,27 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
/**
* This method runs the filter routine.
- * The given odometry measurement is used as movement hypothesis, the laserData-vector is used
+ * 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);
+ 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).
+ * @return The Pose of the most important particle (particle with highest
+ * weight).
*/
Pose getLikeliestPose(ros::Time poseTime = ros::Time::now());
/**
- * This method can be used to retrieve the most likely map that is stored by the particle filter.
+ * 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;
@@ -82,42 +88,49 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
* 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)
+ * @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)
+ * @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)
+ * @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)
+ * @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)
+ * @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.
+ * 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);
@@ -129,20 +142,25 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
/**
* 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)
+ * @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
+ * @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);
+ 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.
+ * @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;
@@ -152,67 +170,78 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
std::vector<SlamParticle*>* getParticles() const;
/**
- * @return Vector of current particle weights. The vector is sorted by weight, highest weight first.
+ * @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.
+ * 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.
+ * @param[out] poseVarianceT The variance of particle poses in T rotation.
*/
- void getPoseVariances(int particleNum, float& poseVarianceX, float& poseVarianceY);
+ void getPoseVariances(int particleNum, float& poseVarianceX,
+ float& poseVarianceY, float& poseVarianceT);
/**
- * This method reduces the number of particles used in this SlamFilter to the given value.
+ * 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
+ * @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:
+ 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 );
+ 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.
+ * 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).
+ * 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.
+ * 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.
+ * This method updates the map by inserting the current laser measurement at
+ * the pose of the likeliest particle.
*/
void updateMap();
@@ -225,50 +254,63 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
/**
* 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)
+ * 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
+ * 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
+ * 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.
+ * 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.
+ * 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
+ * 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.
+ * 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.
+ * 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;
@@ -294,7 +336,8 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
bool m_FirstRun;
/**
- * This variabe is true, if the SlamFilter is used for mapping and updates the map,
+ * 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;
@@ -304,7 +347,7 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
/// Pose of robot during last map update
Pose m_LastUpdatePose;
-
+
tf::Transform m_latestTransform;
/**
@@ -319,11 +362,9 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
* @param f input
* @return square of input
*/
- template<class T>
- T sqr(T f)
- {
+ template <class T>
+ T sqr(T f) {
return f * f;
}
};
#endif
-
diff --git a/homer_mapping/include/homer_mapping/slam_node.h b/homer_mapping/include/homer_mapping/slam_node.h
index 5e4081a4..3ee8814b 100644
--- a/homer_mapping/include/homer_mapping/slam_node.h
+++ b/homer_mapping/include/homer_mapping/slam_node.h
@@ -1,36 +1,37 @@
#ifndef SLAM_NODE_H
#define SLAM_NODE_H
-#include <vector>
+#include <stdlib.h>
+#include <cmath>
+#include <fstream>
+#include <iostream>
#include <map>
#include <sstream>
-#include <vector>
-#include <iostream>
-#include <fstream>
#include <sstream>
-#include <cmath>
-#include <stdlib.h>
+#include <vector>
+#include <vector>
#include <homer_nav_libs/Math/Pose.h>
#include <tf/transform_broadcaster.h>
-#include <sensor_msgs/LaserScan.h>
-#include <nav_msgs/Odometry.h>
-#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <std_msgs/Empty.h>
-#include <std_msgs/Bool.h>
-#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/Odometry.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/LaserScan.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Empty.h>
#include <tf/tf.h>
-#include <homer_mapping/ParticleFilter/SlamFilter.h>
+#include <homer_mapping/OccupancyMap/OccupancyMap.h>
#include <homer_mapping/ParticleFilter/HyperSlamFilter.h>
+#include <homer_mapping/ParticleFilter/SlamFilter.h>
#include <homer_nav_libs/Math/Box2D.h>
-#include <homer_mapping/OccupancyMap/OccupancyMap.h>
class OccupancyMap;
class SlamFilter;
@@ -49,15 +50,13 @@ class HyperSlamFilter;
* 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:
-
+class SlamNode {
+ public:
/**
- * The constructor adds the message types and prepares the module for receiving them.
+ * The constructor adds the message types and prepares the module for
+ * receiving them.
*/
- SlamNode(ros::NodeHandle *nh);
+ SlamNode(ros::NodeHandle* nh);
/**
* This method initializes the member variables.
@@ -69,20 +68,20 @@ public:
*/
virtual ~SlamNode();
-private:
-
+ 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 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);
+ void callbackInitialPose(
+ const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
/**
* This function resets the current maps to the initial state.
@@ -101,16 +100,19 @@ private:
void sendMapDataMessage(ros::Time mapTime = ros::Time::now());
/**
- * This variables stores the last odometry measurement as reference that is used
+ * This variables stores the last odometry measurement as reference that is
+ * used
* to compute the pose of the robot during a specific laserscan.
*/
Pose m_ReferenceOdometryPose;
- Pose m_LastLikeliestPose;
+ Pose m_LastLikeliestPose;
/**
- * This variable stores the time of the last odometry measurement as reference
- * which is used to compute the pose of the robot during a specific laserscan.
+ * 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;
@@ -131,7 +133,6 @@ private:
*/
ros::Time m_LastMappingTime;
-
/**
* This variable stores a pointer to the hyper slam filter
*/
@@ -149,11 +150,11 @@ private:
*/
bool m_DoMapping;
- /**
- * Vectors used to queue laser and odom messages to find a fit
- */
- std::vector<sensor_msgs::LaserScan::ConstPtr> m_laser_queue;
- std::vector<nav_msgs::Odometry::ConstPtr> m_odom_queue;
+ /**
+ * Vectors used to queue laser and odom messages to find a fit
+ */
+ std::vector<sensor_msgs::LaserScan::ConstPtr> m_laser_queue;
+ std::vector<nav_msgs::Odometry::ConstPtr> m_odom_queue;
/**
* duration to wait between two particle filter steps
@@ -176,11 +177,11 @@ private:
ros::Subscriber m_LoadMapSubscriber;
ros::Subscriber m_MaskingSubscriber;
ros::Subscriber m_ResetHighSubscriber;
- ros::Subscriber m_InitialPoseSubscriber;
+ ros::Subscriber m_InitialPoseSubscriber;
ros::Publisher m_PoseStampedPublisher;
+ ros::Publisher m_PoseArrayPublisher;
ros::Publisher m_SLAMMapPublisher;
-
};
#endif
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index e8cf10cc..9587eda9 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -323,6 +323,7 @@ void OccupancyMap::insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
pout = m_laserTransform * pin;
rangeMeasurement.endPos.x = pout.x();
rangeMeasurement.endPos.y = pout.y();
+ rangeMeasurement.range = range;
rangeMeasurement.free = true;
ranges.push_back(rangeMeasurement);
}
@@ -336,6 +337,7 @@ void OccupancyMap::insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
pout = m_laserTransform * pin;
rangeMeasurement.endPos.x = pout.x();
rangeMeasurement.endPos.y = pout.y();
+ rangeMeasurement.range = laserData->ranges[i];
rangeMeasurement.free = false;
ranges.push_back(rangeMeasurement);
errorFound = false;
@@ -448,39 +450,44 @@ void OccupancyMap::insertRanges(vector<RangeMeasurement> ranges,
continue;
}
m_ChangedRegion.enclose(endPixel.x(), endPixel.y());
- // paint free ranges
- drawLine(m_CurrentChanges, sensorPixel, endPixel, ::FREE);
if (!ranges[i].free) {
unsigned offset =
endPixel.x() + m_metaData.width * endPixel.y();
- m_CurrentChanges[offset] = ::OCCUPIED;
+ if (ranges[i].range < 10) {
+ m_CurrentChanges[offset] = ::OCCUPIED;
+ } else {
+ m_CurrentChanges[offset] = ::SAFETY_BORDER;
+ }
}
}
lastEndPixel = endPixel;
}
+ // paint free pixels
+ for (unsigned i = 0; i < ranges.size(); i++) {
+ Eigen::Vector2i endPixel = map_pixel[i];
+
+ if (endPixel != lastEndPixel) {
+ if (endPixel.x() >= m_metaData.width || endPixel.x() < 0 ||
+ endPixel.y() >= m_metaData.height || endPixel.y() < 0) {
+ continue;
+ }
+ // paint free ranges
+ drawLine(m_CurrentChanges, sensorPixel, endPixel, ::FREE);
+ }
+ lastEndPixel = endPixel;
+ }
+
m_ChangedRegion.clip(
Box2D<int>(0, 0, m_metaData.width - 1, m_metaData.height - 1));
m_ExploredRegion.enclose(m_ChangedRegion);
applyChanges();
computeOccupancyProbabilities();
if (need_x_left + need_x_right + need_y_down + need_y_up > 0) {
- // keep square aspect ration till homer_gui can handle other maps
- // int need_x = need_x_left + need_x_right;
- // int need_y = need_y_up + need_y_down;
- // if(need_x > need_y)
- //{
- // need_y_down += need_x - need_y;
- //}
- // else if (need_y > need_x)
- //{
- // need_x_right += need_y - need_x;
- //}
-
- ROS_INFO_STREAM("new map size!");
- ROS_INFO_STREAM(" " << need_x_left << " " << need_y_up << " "
- << need_x_right << " " << need_y_down);
+ // ROS_INFO_STREAM("new map size!");
+ // ROS_INFO_STREAM(" " << need_x_left << " " << need_y_up << " "
+ //<< need_x_right << " " << need_y_down);
changeMapSize(need_x_left, need_y_up, need_x_right, need_y_down);
}
}
@@ -712,10 +719,9 @@ void OccupancyMap::drawLine(DataT* data, Eigen::Vector2i& startPixel,
if (data[index] == NO_CHANGE) {
data[index] = value;
}
- /* if ( data[index] == OCCUPIED || data[index] == SAFETY_BORDER )
- {
- return;
- }*/
+ if (data[index] == OCCUPIED || data[index] == SAFETY_BORDER) {
+ return;
+ }
xerr += dx;
yerr += dy;
if (xerr > dist) {
diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
index 3cb7f8d9..72f741e4 100644
--- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
@@ -2,9 +2,9 @@
#include <omp.h>
// minimum move for translation in m
-const float MIN_MOVE_DISTANCE2 = 0.001 * 0.001;
+const float MIN_MOVE_DISTANCE2 = 0.05 * 0.05;
// minimum turn in radiants
-const float MIN_TURN_DISTANCE2 = 0.01 * 0.01;
+const float MIN_TURN_DISTANCE2 = 0.1 * 0.01;
const float M_2PI = 2 * M_PI;
@@ -282,6 +282,7 @@ void SlamFilter::filter(Pose currentPose,
m_ReferencePoseOdometry = currentPose;
m_ReferenceMeasurementTime = measurementTime;
+ resample();
measure();
ROS_INFO_STREAM("first run!");
normalize();
@@ -296,13 +297,14 @@ void SlamFilter::filter(Pose currentPose,
Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
- bool moving = sqr(trans.x()) + sqr(trans.y()) > 0 || sqr(trans.theta()) > 0;
+ bool moving = sqr(trans.x()) + sqr(trans.y()) > 0.0000001 ||
+ sqr(trans.theta()) > 0.000001;
// do not resample if move to small and last move is min 0.5 sec away
// if (sqr(trans.x()) + sqr(trans.y()) < MIN_MOVE_DISTANCE2 &&
// sqr(trans.theta()) < MIN_TURN_DISTANCE2 &&
//(ros::Time::now() - m_LastMoveTime).toSec() > 1.0)
- if (!moving && (ros::Time::now() - m_LastMoveTime).toSec() > 1.0)
+ if (!moving && (ros::Time::now() - m_LastMoveTime).toSec() > 0.5)
// if(false)
{
ROS_DEBUG_STREAM("Move too small, will not resample.");
@@ -340,7 +342,7 @@ void SlamFilter::filter(Pose currentPose,
bool update =
((std::fabs(transSinceLastUpdate.theta()) > m_UpdateMinMoveAngle) ||
- (transSinceLastUpdate.magnitude() > m_UpdateMinMoveDistance)) &&
+ (transSinceLastUpdate.magnitude() > m_UpdateMinMoveDistance)) ||
((measurementTime - m_LastUpdateTime) > m_MaxUpdateInterval);
if (m_DoMapping && update) {
@@ -353,16 +355,19 @@ void SlamFilter::filter(Pose currentPose,
} else {
thetaPerSecond = trans.theta() / elapsedSeconds;
}
- float poseVarianceX, poseVarianceY;
- getPoseVariances(50, poseVarianceX, poseVarianceY);
-
- if (std::fabs(thetaPerSecond) < m_MaxRotationPerSecond &&
- poseVarianceX < 0.05 && poseVarianceY < 0.05) {
+ float poseVarianceX, poseVarianceY, poseVarianceT;
+ getPoseVariances(500, poseVarianceX, poseVarianceY, poseVarianceT);
+ ROS_DEBUG_STREAM("variances: " << poseVarianceX << " " << poseVarianceY
+ << " " << poseVarianceT);
+
+ if (poseVarianceT < 0.001 && poseVarianceX < 0.01 &&
+ poseVarianceY < 0.01 &&
+ m_EffectiveParticleNum > m_ParticleNum / 7) {
updateMap();
m_LastUpdatePose = likeliestPose;
m_LastUpdateTime = measurementTime;
} else {
- ROS_WARN_STREAM("No mapping performed - variance to high");
+ ROS_DEBUG_STREAM("No mapping performed - variance to high");
}
} else {
stream << "No map update performed.";
@@ -399,33 +404,39 @@ void SlamFilter::drift() {
float ct = m_CurrentPoseOdometry.theta();
Transformation2D odoTrans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
+ float rotToPose = atan2(odoTrans.y(), odoTrans.x()) - rt;
+
+ while (rotToPose >= M_PI) rotToPose -= M_2PI;
+ while (rotToPose < -M_PI) rotToPose += M_2PI;
+
+ float poseRotation = odoTrans.theta();
// 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;
- }
+ // 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;
+ // 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];
@@ -434,44 +445,57 @@ void SlamFilter::drift() {
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;
+ // float posX = pose.x() + deltaTrans * cos(pose.theta() + deltaRot1);
+ // float posY = pose.y() + deltaTrans * sin(pose.theta() + deltaRot1);
+ // float theta = pose.theta() + deltaRot1 + deltaRot2;
+ float posX = pose.x() + distance * cos(pose.theta() + rotToPose);
+ float posY = pose.y() + distance * sin(pose.theta() + rotToPose);
+ float theta = pose.theta() + poseRotation;
while (theta > M_PI) theta -= M_2PI;
while (theta <= -M_PI) theta += M_2PI;
// save new pose
particle->setRobotPose(posX, posY, theta);
- int i = 1;
- // calculating parallel on 8 threats
- // TODO: ERROR ON RESET MAPS
- // omp_set_num_threads(4);
- // #pragma omp parallel for
- for (i = 1; i < m_ParticleNum; i++) {
+ 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;
+ //
+ //
+ // float estDeltaRot1 =
+ // deltaRot1 + randomGauss(m_Alpha1 * fabs(poseRotation) +
+ // m_Alpha2 * fabs(distance));
+ // float estDeltaTrans =
+ // deltaTrans -
+ // randomGauss(m_Alpha3 * deltaTrans +
+ // m_Alpha4 o (fabs(deltaRot1) + fabs(deltaRot2)));
+ float estDist = distance + randomGauss(m_Alpha3 * fabs(distance) +
+ m_Alpha4 * (fabs(poseRotation)));
+ // float estDeltaRot2 =
+ // deltaRot2 -
+ // randomGauss(m_Alpha1 * fabs(deltaRot2) + m_Alpha2 * deltaTrans);
+ // float estDeltaRot2 = poseRotation +
+ // randomGauss(m_Alpha1 * 2 * fabs(poseRotation) +
+ // m_Alpha2 * fabs(distance));
+
+ float estRotToPose =
+ rotToPose + randomGauss(m_Alpha1 * fabs(poseRotation) +
+ m_Alpha2 * fabs(distance));
+
+ float estDeltaRot =
+ poseRotation + randomGauss(m_Alpha1 * fabs(poseRotation) +
+ m_Alpha2 * fabs(distance));
+
+ float posX = pose.x() + estDist * cos(pose.theta() + estRotToPose) +
+ randomGauss(m_Alpha5 * fabs(estDeltaRot)) +
+ randomGauss(0.0004);
+ float posY = pose.y() + estDist * sin(pose.theta() + estRotToPose) +
+ randomGauss(m_Alpha5 * fabs(estDeltaRot)) +
+ randomGauss(0.0004);
+ float theta = pose.theta() + estDeltaRot + randomGauss(0.00005);
// save new pose
while (theta > M_PI) theta -= M_2PI;
@@ -554,7 +578,7 @@ void SlamFilter::reduceParticleNumber(int newParticleNum) {
}
Pose SlamFilter::getLikeliestPose(ros::Time poseTime) {
- float percentage = 0.4; // TODO param? //test
+ float percentage = 1; // TODO param? //test
float sumX = 0, sumY = 0, sumDirX = 0, sumDirY = 0;
int numParticles = static_cast<int>(percentage / 100 * m_ParticleNum);
if (0 == numParticles) {
@@ -587,7 +611,7 @@ Pose SlamFilter::getLikeliestPose(ros::Time poseTime) {
OccupancyMap* SlamFilter::getLikeliestMap() const { return m_OccupancyMap; }
void SlamFilter::getPoseVariances(int particleNum, float& poseVarianceX,
- float& poseVarianceY) {
+ float& poseVarianceY, float& poseVarianceT) {
// the particles of m_CurrentList are sorted by their weights
if (particleNum > m_ParticleNum || particleNum <= 0) {
particleNum = m_ParticleNum;
@@ -595,27 +619,33 @@ void SlamFilter::getPoseVariances(int particleNum, float& poseVarianceX,
// calculate average pose
float averagePoseX = 0;
float averagePoseY = 0;
+ float averagePoseT = 0;
float robotX = 0.0;
float robotY = 0.0;
- float robotTheta = 0.0;
+ float robotT = 0.0;
for (int i = 0; i < particleNum; i++) {
- m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
+ m_CurrentList[i]->getRobotPose(robotX, robotY, robotT);
averagePoseX += robotX;
averagePoseY += robotY;
+ averagePoseT += robotT;
}
- averagePoseX /= particleNum;
- averagePoseY /= particleNum;
+ averagePoseX /= (float)particleNum;
+ averagePoseY /= (float)particleNum;
+ averagePoseT /= (float)particleNum;
// calculate standard deviation of pose
poseVarianceX = 0.0;
poseVarianceY = 0.0;
+ poseVarianceT = 0.0;
for (int i = 0; i < particleNum; i++) {
- m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
+ m_CurrentList[i]->getRobotPose(robotX, robotY, robotT);
poseVarianceX += (averagePoseX - robotX) * (averagePoseX - robotX);
poseVarianceY += (averagePoseY - robotY) * (averagePoseY - robotY);
+ poseVarianceT += (averagePoseT - robotT) * (averagePoseT - robotT);
}
- poseVarianceX /= particleNum;
- poseVarianceY /= particleNum;
+ poseVarianceX /= (float)particleNum;
+ poseVarianceY /= (float)particleNum;
+ poseVarianceT /= (float)particleNum;
}
double SlamFilter::evaluateByContrast() {
diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp
index 86014344..704b5498 100644
--- a/homer_mapping/src/slam_node.cpp
+++ b/homer_mapping/src/slam_node.cpp
@@ -27,6 +27,8 @@ SlamNode::SlamNode(ros::NodeHandle* nh) : m_HyperSlamFilter(0) {
// advertise topics
m_PoseStampedPublisher =
nh->advertise<geometry_msgs::PoseStamped>("/pose", 2);
+ m_PoseArrayPublisher =
+ nh->advertise<geometry_msgs::PoseArray>("/pose_array", 2);
m_SLAMMapPublisher =
nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
@@ -299,6 +301,34 @@ void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg) {
// to geometry_msgs::Quaternion
poseMsg.pose.orientation = quatMsg;
m_PoseStampedPublisher.publish(poseMsg);
+
+ geometry_msgs::PoseArray poseArray = geometry_msgs::PoseArray();
+
+ poseArray.header = poseMsg.header;
+
+ std::vector<Pose>* poses =
+ m_HyperSlamFilter->getBestSlamFilter()->getParticlePoses();
+
+ for (int i = 0; i < poses->size(); i++) {
+ geometry_msgs::Pose tmpPose = geometry_msgs::Pose();
+
+ tmpPose.position.x = poses->at(i).x();
+ tmpPose.position.y = poses->at(i).y();
+
+ tf::Quaternion quatTF =
+ tf::createQuaternionFromYaw(poses->at(i).theta());
+ geometry_msgs::Quaternion quatMsg;
+ tf::quaternionTFToMsg(quatTF,
+ quatMsg); // conversion from tf::Quaternion
+ // to geometry_msgs::Quaternion
+ tmpPose.orientation = quatMsg;
+
+ poseArray.poses.push_back(tmpPose);
+ }
+ delete poses;
+
+ m_PoseArrayPublisher.publish(poseArray);
+
// broadcast transform map -> base_link
// tf::Transform transform(quatTF,tf::Vector3(currentPose.x(),
// currentPose.y(), 0.0));
--
GitLab