From 9bf2c72636e2c0b05627b83c35ad106b7de202ae Mon Sep 17 00:00:00 2001
From: Florian Polster <fpolster@uni-koblenz.de>
Date: Fri, 17 Mar 2017 17:23:30 +0100
Subject: [PATCH] mapping with support for holonomic odometry readings
---
homer_mapping/config/homer_mapping.yaml | 14 +-
.../homer_mapping/OccupancyMap/OccupancyMap.h | 735 +++----
.../ParticleFilter/HyperSlamFilter.h | 11 +-
.../ParticleFilter/ParticleFilter.h | 6 +-
.../homer_mapping/ParticleFilter/SlamFilter.h | 75 +-
.../ParticleFilter/SlamParticle.h | 3 +
.../include/homer_mapping/slam_node.h | 275 ++-
.../src/OccupancyMap/OccupancyMap.cpp | 1778 ++++++++---------
.../src/ParticleFilter/HyperSlamFilter.cpp | 12 +-
.../src/ParticleFilter/SlamFilter.cpp | 986 ++++-----
.../src/ParticleFilter/SlamParticle.cpp | 11 +
homer_mapping/src/slam_node.cpp | 749 +++----
12 files changed, 2253 insertions(+), 2402 deletions(-)
diff --git a/homer_mapping/config/homer_mapping.yaml b/homer_mapping/config/homer_mapping.yaml
index 17ca7bef..4ae14b9b 100644
--- a/homer_mapping/config/homer_mapping.yaml
+++ b/homer_mapping/config/homer_mapping.yaml
@@ -1,18 +1,18 @@
-/homer_mapping/size: 4 # size of one edge of the map in m. map is quadratic
-/homer_mapping/resolution: 0.04 # m meter per cell
+/homer_mapping/size: 4 # size of one edge of the map in m
+/homer_mapping/resolution: 0.03 # m meter per cell
#map config values
/homer_mapping/backside_checking: true # 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
+/homer_mapping/laser_scanner/free_reading_distance: 0.1 # Minimum distance in m to be classified as free in case of errorneous measurement
/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
+/particlefilter/error_values/rotation_error_translating: 0.05 # degrees per meter
+/particlefilter/error_values/translation_error_translating: 6.0 # percent
+/particlefilter/error_values/translation_error_rotating: 0.02 # m per degree
+/particlefilter/error_values/move_jitter_while_turning: 0.02 # m per degree
/particlefilter/hyper_slamfilter/particlefilter_num: 1
diff --git a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
index 51cc9e9f..ecf26ef6 100644
--- a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
+++ b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
@@ -1,22 +1,25 @@
#ifndef OCCUPANCYMAP_H
#define OCCUPANCYMAP_H
-#include <iostream>
-#include <list>
-#include <string>
-#include <vector>
-
-#include <Eigen/Geometry>
-
+#include <homer_mapnav_msgs/ModifyMap.h>
#include <homer_nav_libs/Math/Box2D.h>
+#include <homer_nav_libs/Math/Math.h>
#include <homer_nav_libs/Math/Point2D.h>
#include <homer_nav_libs/Math/Pose.h>
-
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/OccupancyGrid.h>
-#include <tf/transform_listener.h>
-
+#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
+#include <tf/transform_listener.h>
+#include <Eigen/Geometry>
+#include <QtGui/QImage>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <list>
+#include <sstream>
+#include <string>
+#include <vector>
class QImage;
@@ -32,19 +35,38 @@ using namespace std;
* @param free indicates if the laser range hit an obstacle (false) or not
* (true)
*/
-struct RangeMeasurement {
- geometry_msgs::Point sensorPos;
- geometry_msgs::Point endPos;
- float range;
- 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
*/
-enum BorderType { NoBorder, LeftBorder, RightBorder };
+enum BorderType
+{
+ NoBorder,
+ LeftBorder,
+ RightBorder
+};
+
+const float UNKNOWN_LIKELIHOOD = 0.3;
+
+// Flags of current changes //
+const char NO_CHANGE = 0;
+const char OCCUPIED = 1;
+const char FREE = 2;
+// the safety border around occupied pixels which is left unchanged
+const char SAFETY_BORDER = 3;
+const char CONTRAST = 4;
+///////////////////////////////
+// assumed laser measure count for loaded maps
+const int LOADED_MEASURECOUNT = 10;
/**
* Structure to store a measurement point for computeLaserScanProbability()
* @param hitPos Position of measured obstacle (robot coordinates)
@@ -54,10 +76,12 @@ enum BorderType { NoBorder, LeftBorder, RightBorder };
* @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;
+ CVec2 normal;
};
/**
@@ -82,329 +106,352 @@ struct MeasurePoint {
* coordinate system).
* The mapping data has to be inserted via the method insertLaserData().
*/
-class OccupancyMap {
- public:
- static const int8_t INACCESSIBLE = 100;
- static const int8_t OBSTACLE = 99;
- static const int8_t OCCUPIED = 98;
- static const int8_t UNKNOWN = 50;
- static const int8_t NOT_SEEN_YET = -1;
- static const int8_t FREE = 0;
-
- /**
- * The default constructor calls initMembers().
- */
- OccupancyMap();
-
- /**
- * Constructor for a loaded map.
- */
- OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin,
- float resolution, int width, int height,
- Box2D<int> exploredRegion);
-
- /**
- * Copy constructor, copies all members inclusive the arrays that lie behind
- * the pointers.
- * @param occupancyMap Source for copying
- */
- OccupancyMap(const OccupancyMap& occupancyMap);
-
- /**
- * Method to init all members with default values from the configuration
- * file. All arrays are initialized.
- */
- void initMembers();
-
- /**
- * Assignment operator, copies all members (deep copy!)
- * @param source Source to copy from
- * @return Reference to copied OccupancyMap
- */
- OccupancyMap& operator=(const OccupancyMap& source);
-
- /**
- * Deletes all dynamically allocated memory.
- */
- ~OccupancyMap();
-
- /*
- /**
- * @return The resolution of the map in m.
- */
- // int resolution() const;
-
- geometry_msgs::Pose origin() const;
-
- /**
- * @return Width of the map.
- */
- int width() const;
-
- /**
- * @return Height of the map.
- */
- int height() const;
-
- /**
- * This method is used to reset all HighSensitive areas
- */
- void resetHighSensitive();
-
- /**
- * @return Probability of pixel p being occupied.
- */
- float getOccupancyProbability(Eigen::Vector2i p);
-
- /**
- * @brief This function inserts the data of a laserscan into the map.
- *
- * With the given data, start and end cells of a laser beam are computed and
- * given to the
- * method markLineFree().
- * If the measurement is smaller than VALID_MAX_RANGE, markOccupied() is
- * called for the endpoint.
- * @param laserData The laser data msg.
- */
- void insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
- tf::Transform transform);
-
- void insertRanges(vector<RangeMeasurement> ranges,
- ros::Time stamp = ros::Time::now());
-
- /**
- * @brief gives a list specially processed coordinates to be used for
- * computeLaserScanProbability
- */
- std::vector<MeasurePoint> getMeasurePoints(
- sensor_msgs::LaserScanConstPtr laserData);
-
- /**
- * This method computes a score that describes how good the given hypothesis
- * matches with the map
- * @param robotPose The pose of the robot
- * @return The "fitting factor". The higher the factor, the better the
- * fitting.
- * This factor is NOT normalized, it is a positive float between 0
- * and FLOAT_MAX
- */
- float computeScore(Pose robotPose, std::vector<MeasurePoint> measurePoints);
-
- /**
- * @return QImage of size m_metaData.width, m_metaData.height with values of
- * m_OccupancyProbability scaled to 0-254
- */
- QImage getProbabilityQImage(int trancparencyThreshold,
- bool showInaccessible) const;
-
- // puma2::ColorImageRGB8* getUpdateImage( bool withMap=true ); TODO
-
- /**
- * Returns an "image" of the obstacles e.g. seen in the 3D scans
- * @returns image with dark red dots in areas where the obstacles were seen
- */
- // puma2::ColorImageRGB8* getObstacleImage ( ); TODO
-
- /**
- * Returns an "image" of occupancy probability image.
- * @param[out] data vector containing occupancy probabilities. 0 = free, 100
- * = occupied, -1 = NOT_KNOWN
- * @param[out] width Width of data array
- * @param[out] height Height of data array
- * @param[out] resolution Resolution of the map (m_metaData.resolution)
- */
- void getOccupancyProbabilityImage(vector<int8_t>& data,
- nav_msgs::MapMetaData& metaData);
-
- /**
- * This method marks free the position of the robot (according to its
- * dimensions).
- */
- void markRobotPositionFree();
-
- /**
- * @brief Computes the contrast of a single pixel.
- * @param prob probability value (100=occupied, 50=NOT_KNOWN, 0=free) of a
- * pixel.
- * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of this
- * pixel
- */
- double contrastFromProbability(int8_t prob);
-
- /**
- * @brief This method computes the sharpness of the occupancy grid
- * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the
- * map
- */
- double evaluateByContrast();
-
- /// GETTERS
-
- Box2D<int> getExploredRegion() { return m_ExploredRegion; }
- Box2D<int> getChangedRegion() { return m_ChangedRegion; }
-
- /**
- * Sets cells of this map to free or occupied according to maskMap
- */
- void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
-
- 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.
- */
- void incrementMeasurementCount(Eigen::Vector2i p);
-
- /**
- * This method increments the occupancy count int m_OccupancyCount for pixel
- * p.
- * @param p Occupied pixel.
- */
- void incrementOccupancyCount(Eigen::Vector2i p);
-
- /**
- * This method increments m_MeasurementCount and if neccessary
- * m_OccupancyCount for all pixels.
- */
- void applyChanges();
-
- void clearChanges();
-
- /**
- * This method scales the counts of all pixels down to the given value.
- * @param maxCount Maximum value to which all counts are set.
- */
- void scaleDownCounts(int maxCount);
-
- /**
- * This function paints a line from a start pixel to an end pixel.
- * The computation is made with the Bresenham algorithm.
- * @param data array on which the line shall be painted
- * @param startPixel starting coordinates of the beam
- * @param endPixel ending coordinates of the beam
- * @param value The value with which the lines are marked.
- */
- template <class DataT>
- void drawLine(DataT* data, Eigen::Vector2i& startPixel,
- Eigen::Vector2i& endPixel, char value);
-
- /**
- * This method computes the values for m_OccupancyProbabilities from
- * m_MeasurementCount and m_OccupancyCount.
- */
- void computeOccupancyProbabilities();
-
- /**
- * This method sets all values of m_CurrentChanges to NO_CHANGE.
- */
- void clearCurrentChanges();
-
- /**
- * This method resets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY
- * and m_MaxChangeY.
- * This means that no current changes are assumed.
- */
- void resetChangedRegion();
-
- /**
- * This method updates the values of m_MinChangeX, m_MaxChangeX,
- * m_MinChangeY and m_MaxChangeY to current changes.
- * The area around the current robot pose will be included to the changed
- * region.
- * @param robotPose The current pose of the robot.
- */
- void updateChangedRegion(Pose robotPose);
-
- /**
- * This method sets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY
- * and m_MaxChangeY
- * to initial values so that the complete map will be processed.
- */
- void maximizeChangedRegion();
-
- /**
- * This method resets all values of m_ExploredX, m_MaxExploredX,
- * m_MinExploredY and m_MaxExploredY.
- */
- void resetExploredRegion();
-
- /**
- * Deletes all allocated members.
- */
- void cleanUp();
-
- /**
- * Stores the metadata of the map
- */
- nav_msgs::MapMetaData m_metaData;
-
- /**
- * Stores the size of the map arrays, i.e. m_metaData.width *
- * m_metaData.height
- * for faster computation.
- */
- unsigned m_ByteSize;
-
- /**
- * Array to store occupancy probability values.
- * Values between 0 and 1.
- */
- float* m_OccupancyProbability;
-
- // Counts how often a pixel is hit by a measurement.
- unsigned short* m_MeasurementCount;
-
- // Counts how often a pixel is hit by a measurement that says the pixel is
- // occupied.
- unsigned short* m_OccupancyCount;
-
- // 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
- float m_FreeReadingDistance;
- // enables checking to avoid matching front- and backside of an obstacle,
- // e.g. wall
- bool m_BacksideChecking;
- // leaves a small border around obstacles unchanged when inserting a laser
- // scan
- bool m_ObstacleBorders;
- // minimum distance in m between two samples for probability calculation
- float m_MeasureSamplingStep;
-
- // bool to reset high_sensitive Areas on the next iteration
- bool m_reset_high;
-
- /**
- * Defines a bounding box around the changes in the current map.
- */
- Box2D<int> m_ChangedRegion;
-
- /**
- * Defines a bounding box around the area in the map, which is already
- * explored.
- */
- Box2D<int> m_ExploredRegion;
-
- /**
- * ros transform listener
- */
- tf::TransformListener m_tfListener;
-
- /**
- * ros transformation laser to base_link
- */
- tf::StampedTransform m_laserTransform;
- tf::Transform m_latestMapTransform;
+class OccupancyMap
+{
+public:
+ static const int8_t INACCESSIBLE = 100;
+ static const int8_t OBSTACLE = 99;
+ static const int8_t OCCUPIED = 98;
+ static const int8_t UNKNOWN = 50;
+ static const int8_t NOT_SEEN_YET = -1;
+ static const int8_t FREE = 0;
+
+ /**
+ * The default constructor calls initMembers().
+ */
+ OccupancyMap();
+
+ /**
+ * Constructor for a loaded map.
+ */
+ OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin,
+ float resolution, int width, int height,
+ Box2D<int> exploredRegion);
+
+ /**
+ * Copy constructor, copies all members inclusive the arrays that lie behind
+ * the pointers.
+ * @param occupancyMap Source for copying
+ */
+ OccupancyMap(const OccupancyMap& occupancyMap);
+
+ /**
+ * Method to init all members with default values from the configuration
+ * file. All arrays are initialized.
+ */
+ void initMembers();
+
+ /**
+ * Assignment operator, copies all members (deep copy!)
+ * @param source Source to copy from
+ * @return Reference to copied OccupancyMap
+ */
+ OccupancyMap& operator=(const OccupancyMap& source);
+
+ /**
+ * Deletes all dynamically allocated memory.
+ */
+ ~OccupancyMap();
+
+ /*
+ /**
+ * @return The resolution of the map in m.
+ */
+ // int resolution() const;
+
+ geometry_msgs::Pose origin() const;
+
+ /**
+ * @return Width of the map.
+ */
+ int width() const;
+
+ /**
+ * @return Height of the map.
+ */
+ int height() const;
+
+ /**
+ * This method is used to reset all HighSensitive areas
+ */
+ void resetHighSensitive();
+
+ /**
+ * @return Probability of pixel p being occupied.
+ */
+ float getOccupancyProbability(Eigen::Vector2i p);
+
+ /**
+ * @brief This function inserts the data of a laserscan into the map.
+ *
+ * With the given data, start and end cells of a laser beam are computed and
+ * given to the
+ * method markLineFree().
+ * If the measurement is smaller than VALID_MAX_RANGE, markOccupied() is
+ * called for the endpoint.
+ * @param laserData The laser data msg.
+ */
+ void insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
+ tf::Transform transform);
+
+ void insertRanges(vector<RangeMeasurement> ranges,
+ ros::Time stamp = ros::Time::now());
+
+ /**
+ * @brief gives a list specially processed coordinates to be used for
+ * computeLaserScanProbability
+ */
+ std::vector<MeasurePoint>
+ getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData);
+
+ /**
+ * This method computes a score that describes how good the given hypothesis
+ * matches with the map
+ * @param robotPose The pose of the robot
+ * @return The "fitting factor". The higher the factor, the better the
+ * fitting.
+ * This factor is NOT normalized, it is a positive float between 0
+ * and FLOAT_MAX
+ */
+ float computeScore(Pose robotPose, std::vector<MeasurePoint> measurePoints);
+
+ /**
+ * @return QImage of size m_metaData.width, m_metaData.height with values of
+ * m_OccupancyProbability scaled to 0-254
+ */
+ QImage getProbabilityQImage(int trancparencyThreshold,
+ bool showInaccessible) const;
+
+ // puma2::ColorImageRGB8* getUpdateImage( bool withMap=true ); TODO
+
+ /**
+ * Returns an "image" of the obstacles e.g. seen in the 3D scans
+ * @returns image with dark red dots in areas where the obstacles were seen
+ */
+ // puma2::ColorImageRGB8* getObstacleImage ( ); TODO
+
+ /**
+ * Returns an "image" of occupancy probability image.
+ * @param[out] data vector containing occupancy probabilities. 0 = free, 100
+ * = occupied, -1 = NOT_KNOWN
+ * @param[out] width Width of data array
+ * @param[out] height Height of data array
+ * @param[out] resolution Resolution of the map (m_metaData.resolution)
+ */
+ void getOccupancyProbabilityImage(vector<int8_t>& data,
+ nav_msgs::MapMetaData& metaData);
+
+ /**
+ * This method marks free the position of the robot (according to its
+ * dimensions).
+ */
+ void markRobotPositionFree();
+
+ /**
+ * @brief Computes the contrast of a single pixel.
+ * @param prob probability value (100=occupied, 50=NOT_KNOWN, 0=free) of a
+ * pixel.
+ * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of this
+ * pixel
+ */
+ double contrastFromProbability(int8_t prob);
+
+ /**
+ * @brief This method computes the sharpness of the occupancy grid
+ * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the
+ * map
+ */
+ double evaluateByContrast();
+
+ /// GETTERS
+
+ Box2D<int> getExploredRegion()
+ {
+ return m_ExploredRegion;
+ }
+ Box2D<int> getChangedRegion()
+ {
+ return m_ChangedRegion;
+ }
+
+ /**
+ * Sets cells of this map to free or occupied according to maskMap
+ */
+ void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+
+ 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.
+ */
+ void incrementMeasurementCount(Eigen::Vector2i p);
+
+ /**
+ * This method increments the occupancy count int m_OccupancyCount for pixel
+ * p.
+ * @param p Occupied pixel.
+ */
+ void incrementOccupancyCount(Eigen::Vector2i p);
+
+ /**
+ * This method increments m_MeasurementCount and if neccessary
+ * m_OccupancyCount for all pixels.
+ */
+ void applyChanges();
+
+ void clearChanges();
+
+ /**
+ * This method scales the counts of all pixels down to the given value.
+ * @param maxCount Maximum value to which all counts are set.
+ */
+ void scaleDownCounts(int maxCount);
+
+ /**
+ * This function paints a line from a start pixel to an end pixel.
+ * The computation is made with the Bresenham algorithm.
+ * @param data array on which the line shall be painted
+ * @param startPixel starting coordinates of the beam
+ * @param endPixel ending coordinates of the beam
+ * @param value The value with which the lines are marked.
+ */
+ void drawLine(Eigen::Vector2i& startPixel,
+ Eigen::Vector2i& endPixel, char value);
+
+ /**
+ * This method computes the values for m_OccupancyProbabilities from
+ * m_MeasurementCount and m_OccupancyCount.
+ */
+ void computeOccupancyProbabilities();
+
+ /**
+ * This method sets all values of m_CurrentChanges to NO_CHANGE.
+ */
+ void clearCurrentChanges();
+
+ /**
+ * This method resets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY
+ * and m_MaxChangeY.
+ * This means that no current changes are assumed.
+ */
+ void resetChangedRegion();
+
+ /**
+ * This method updates the values of m_MinChangeX, m_MaxChangeX,
+ * m_MinChangeY and m_MaxChangeY to current changes.
+ * The area around the current robot pose will be included to the changed
+ * region.
+ * @param robotPose The current pose of the robot.
+ */
+ void updateChangedRegion(Pose robotPose);
+
+ /**
+ * This method sets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY
+ * and m_MaxChangeY
+ * to initial values so that the complete map will be processed.
+ */
+ void maximizeChangedRegion();
+
+ /**
+ * This method resets all values of m_ExploredX, m_MaxExploredX,
+ * m_MinExploredY and m_MaxExploredY.
+ */
+ void resetExploredRegion();
+
+
+ /**
+ * 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
+ * for faster computation.
+ */
+ unsigned m_ByteSize;
+
+ /**
+ * Array to store occupancy probability values.
+ * Values between 0 and 1.
+ */
+ float* m_OccupancyProbability;
+
+ // Counts how often a pixel is hit by a measurement.
+ unsigned short* m_MeasurementCount;
+
+ // Counts how often a pixel is hit by a measurement that says the pixel is
+ // occupied.
+ unsigned short* m_OccupancyCount;
+
+ // 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;
+
+
+ struct PixelValue
+ {
+ float OccupancyProbability;
+ unsigned short MeasurementCount;
+ unsigned short OccupancyCount;
+ unsigned char CurrentChange;
+ unsigned short HighSensitive;
+
+ PixelValue()
+ {
+ OccupancyProbability = UNKNOWN_LIKELIHOOD;
+ OccupancyCount = 0;
+ MeasurementCount = 0;
+ CurrentChange = NO_CHANGE;
+ HighSensitive = 0;
+ }
+ };
+
+ std::vector<PixelValue> m_MapPoints;
+
+ /**
+ * Store values from config files.
+ */
+ // minimum range classified as free in case of errorneous laser measurement
+ float m_FreeReadingDistance;
+ // enables checking to avoid matching front- and backside of an obstacle,
+ // e.g. wall
+ bool m_BacksideChecking;
+ // leaves a small border around obstacles unchanged when inserting a laser
+ // scan
+ bool m_ObstacleBorders;
+ // minimum distance in m between two samples for probability calculation
+ float m_MeasureSamplingStep;
+
+ // bool to reset high_sensitive Areas on the next iteration
+ bool m_reset_high;
+
+ /**
+ * Defines a bounding box around the changes in the current map.
+ */
+ Box2D<int> m_ChangedRegion;
+
+ /**
+ * Defines a bounding box around the area in the map, which is already
+ * explored.
+ */
+ Box2D<int> m_ExploredRegion;
+
+ /**
+ * ros transform listener
+ */
+ tf::TransformListener m_tfListener;
+
+ /**
+ * ros transformation laser to base_link
+ */
+ tf::StampedTransform m_laserTransform;
+ tf::Transform m_latestMapTransform;
};
#endif
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
index c85da226..ac9f1553 100755
--- a/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
@@ -48,11 +48,8 @@ class HyperSlamFilter {
* 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);
+ void filter(Transformation2D trans, sensor_msgs::LaserScanConstPtr laserData);
/**
* Computes and sets the new value for m_Alpha1.
@@ -84,12 +81,6 @@ class HyperSlamFilter {
*/
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.
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
index 0d605af1..e04970da 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
@@ -4,6 +4,8 @@
#include <limits.h>
#include <omp.h>
#include <cmath>
+#include <homer_nav_libs/Math/Transformation2D.h>
+#include <sensor_msgs/LaserScan.h>
#include <iostream>
#include <ros/ros.h>
@@ -117,13 +119,13 @@ class ParticleFilter {
* 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;
+ virtual void drift(Transformation2D odoTrans) = 0;
/**
* This method has to be implemented in sub-classes. It is used to determine
* the weight of each particle.
*/
- virtual void measure() = 0;
+ virtual void measure(sensor_msgs::LaserScanPtr laserData) = 0;
/**
* These two pointers point to m_ParticleListOne and to m_ParticleListTwo.
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
index 68be9519..06be040f 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
@@ -64,18 +64,15 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
* 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(Transformation2D trans,
+ sensor_msgs::LaserScanConstPtr laserData);
/**
* @return The Pose of the most important particle (particle with highest
* weight).
*/
- Pose getLikeliestPose(ros::Time poseTime = ros::Time::now());
+ Pose getLikeliestPose();
/**
* This method can be used to retrieve the most likely map that is stored by
@@ -84,11 +81,6 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
*/
OccupancyMap* getLikeliestMap() const;
- /**
- * This function prints out the list of particles to stdout via cout.
- */
- void printParticles() const;
-
void resetHigh();
/**
@@ -126,14 +118,6 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
*/
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.
@@ -231,19 +215,13 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
* This method drifts the particles according to the last two odometry
* readings (time t-1 and time t).
*/
- void drift();
+ void drift(Transformation2D odoTrans);
/**
* This method weightens each particle according to the given laser
* measurement in m_LaserData.
*/
- void measure();
-
- /**
- * This method updates the map by inserting the current laser measurement at
- * the pose of the likeliest particle.
- */
- void updateMap();
+ void measure(sensor_msgs::LaserScanPtr laserData);
/**
* For weightening the particles, the filter needs a map.
@@ -252,16 +230,6 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
*/
OccupancyMap* m_OccupancyMap;
- /**
- * threshold values for when the map will be updated.
- * The map is only updated when the robot has turned a minimal angle
- * (m_UpdateMinMoveAngle in radiants),
- * has moved a minimal distance (m_UpdateMinMoveDistance in m) or a maximal
- * time has passed (m_MaxUpdateInterval)
- */
- float m_UpdateMinMoveAngle;
- float m_UpdateMinMoveDistance;
- ros::Duration m_MaxUpdateInterval;
/**
* This variable holds the rotation error that the robot makes while it is
@@ -306,30 +274,6 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
*/
float m_Alpha5;
- /**
- * The maximal rotation if mapping is performed. If the rotation is bigger,
- * mapping is interrupted.
- * This value may depend on the computing power, because it is influenced by
- * the size of time intervals of mapping.
- */
- float m_MaxRotationPerSecond;
-
- /**
- * Last laser data.
- */
- sensor_msgs::LaserScanPtr m_CurrentLaserData;
-
- /**
- * Last two odometry measurements.
- */
- Pose m_ReferencePoseOdometry;
- Pose m_CurrentPoseOdometry;
-
- /**
- * Time stamp of the last sensor measurement.
- */
- ros::Time m_ReferenceMeasurementTime;
-
/**
* True if it is the first run of SlamFilter, false otherwise.
*/
@@ -342,18 +286,9 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
*/
bool m_DoMapping;
- /** Points used in last measure() step */
- vector<MeasurePoint> m_MeasurePoints;
-
- /// Pose of robot during last map update
- Pose m_LastUpdatePose;
-
- tf::Transform m_latestTransform;
-
/**
* Time stamp of the last particle filter step
*/
- ros::Time m_LastUpdateTime;
ros::Time m_LastMoveTime;
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h
index dc05b056..8cc3c11c 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h
@@ -5,6 +5,7 @@
#include <fstream>
#include <homer_mapping/ParticleFilter/Particle.h>
+#include <homer_nav_libs/Math/Pose.h>
/**
* @class SlamParticle
@@ -47,6 +48,7 @@ class SlamParticle : public Particle
* @param robotTheta Orientation of the robot (radiants).
*/
void setRobotPose ( float robotX, float robotY, float robotTheta );
+ void setRobotPose (Pose pose);
/**
* Returns the content of the three members m_RobotPositionX, m_RobotPositionY, m_RobotOrientation.
@@ -55,6 +57,7 @@ class SlamParticle : public Particle
* @param[out] robotTheta Orientation of the robot (radiants).
*/
void getRobotPose ( float& robotX, float& robotY, float& robotTheta );
+ Pose getRobotPose ();
private:
diff --git a/homer_mapping/include/homer_mapping/slam_node.h b/homer_mapping/include/homer_mapping/slam_node.h
index 3ee8814b..b056faab 100644
--- a/homer_mapping/include/homer_mapping/slam_node.h
+++ b/homer_mapping/include/homer_mapping/slam_node.h
@@ -7,31 +7,24 @@
#include <iostream>
#include <map>
#include <sstream>
-#include <sstream>
-#include <vector>
#include <vector>
-#include <homer_nav_libs/Math/Pose.h>
-
-#include <tf/transform_broadcaster.h>
-
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/PoseWithCovarianceStamped.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_nav_libs/Math/Pose.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/OccupancyMap/OccupancyMap.h>
-#include <homer_mapping/ParticleFilter/HyperSlamFilter.h>
-#include <homer_mapping/ParticleFilter/SlamFilter.h>
-#include <homer_nav_libs/Math/Box2D.h>
+#include <tf/transform_broadcaster.h>
class OccupancyMap;
class SlamFilter;
@@ -50,138 +43,130 @@ 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:
- /**
- * The constructor adds the message types and prepares the module for
- * receiving them.
- */
- SlamNode(ros::NodeHandle* nh);
-
- /**
- * This method initializes the member variables.
- */
- virtual void init();
-
- /**
- * The destructor deletes the filter thread instance.
- */
- virtual ~SlamNode();
-
- private:
- /**
- * Callback methods for all incoming messages
- */
- void callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg);
- void callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg);
- void callbackUserDefPose(const geometry_msgs::Pose::ConstPtr& msg);
- void callbackDoMapping(const std_msgs::Bool::ConstPtr& msg);
- void callbackResetMap(const std_msgs::Empty::ConstPtr& msg);
- void callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
- void callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
- void callbackResetHigh(const std_msgs::Empty::ConstPtr& msg);
- void callbackInitialPose(
- const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
-
- /**
- * This function resets the current maps to the initial state.
- */
- void resetMaps();
-
- /**
- * This function processes the current odometry data in combination with the
- * last send odometry and laser informations to pass on corresponding data
- * to the filter threads.
-
- /**
- * This method retrieves the current map of the slam filter and sends a map
- * data message containing the map.
- */
- void sendMapDataMessage(ros::Time mapTime = ros::Time::now());
-
- /**
- * This variables stores the last odometry measurement as reference that is
- * used
- * to compute the pose of the robot during a specific laserscan.
- */
- Pose m_ReferenceOdometryPose;
-
- Pose m_LastLikeliestPose;
-
- /**
- * This variable stores the time of the last odometry measurement as
- * reference
- * which is used to compute the pose of the robot during a specific
- * laserscan.
- */
- ros::Time m_ReferenceOdometryTime;
-
- /**
- * This variable stores the time the last map message was sent to be able to
- * compute the time for the next map send.
- */
- ros::Time m_LastMapSendTime;
- ros::Time m_LastPositionSendTime;
-
- /**
- * This variable stores the last laser measurement.
- */
- sensor_msgs::LaserScan::ConstPtr m_LastLaserData;
-
- /**
- * time stamp of last particle filter step
- */
- ros::Time m_LastMappingTime;
-
- /**
- * This variable stores a pointer to the hyper slam filter
- */
- HyperSlamFilter* m_HyperSlamFilter;
-
- /**
- * Scatter variances in self localization.
- */
- double m_ScatterVarXY;
- double m_ScatterVarTheta;
-
- /**
- * This variabe is true, if the slam algorithm is used for mapping and
- * keeps updating the map, false otherwise.
- */
- bool m_DoMapping;
-
- /**
- * Vectors used to queue laser and odom messages to find a fit
- */
- std::vector<sensor_msgs::LaserScan::ConstPtr> m_laser_queue;
- std::vector<nav_msgs::Odometry::ConstPtr> m_odom_queue;
-
- /**
- * duration to wait between two particle filter steps
- */
- ros::Duration m_WaitDuration;
-
- /**
- * Broadcasts the transform map -> base_link
- */
- tf::TransformBroadcaster m_tfBroadcaster;
-
- /**
- * subscribers and publishers
- */
- ros::Subscriber m_LaserScanSubscriber;
- ros::Subscriber m_OdometrySubscriber;
- ros::Subscriber m_UserDefPoseSubscriber;
- ros::Subscriber m_DoMappingSubscriber;
- ros::Subscriber m_ResetMapSubscriber;
- ros::Subscriber m_LoadMapSubscriber;
- ros::Subscriber m_MaskingSubscriber;
- ros::Subscriber m_ResetHighSubscriber;
- ros::Subscriber m_InitialPoseSubscriber;
-
- ros::Publisher m_PoseStampedPublisher;
- ros::Publisher m_PoseArrayPublisher;
- ros::Publisher m_SLAMMapPublisher;
+class SlamNode
+{
+public:
+ /**
+ * The constructor adds the message types and prepares the module for
+ * receiving them.
+ */
+ SlamNode(ros::NodeHandle* nh);
+
+ /**
+ * This method initializes the member variables.
+ */
+ virtual void init();
+
+ /**
+ * The destructor deletes the filter thread instance.
+ */
+ virtual ~SlamNode();
+
+private:
+ /**
+ * Callback methods for all incoming messages
+ */
+ void callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg);
+ void callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg);
+ void callbackUserDefPose(const geometry_msgs::Pose::ConstPtr& msg);
+ void callbackDoMapping(const std_msgs::Bool::ConstPtr& msg);
+ void callbackResetMap(const std_msgs::Empty::ConstPtr& msg);
+ void callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+ void callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+ void callbackResetHigh(const std_msgs::Empty::ConstPtr& msg);
+ void callbackInitialPose(
+ const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
+
+ /**
+ * This function resets the current maps to the initial state.
+ */
+ void resetMaps();
+
+ /**
+ * This function processes the current odometry data in combination with the
+ * last send odometry and laser informations to pass on corresponding data
+ * to the filter threads.
+
+ /**
+ * This method retrieves the current map of the slam filter and sends a map
+ * data message containing the map.
+ */
+ void sendMapDataMessage(ros::Time mapTime = ros::Time::now());
+
+ /**
+ * This variables stores the last odometry measurement as reference that is
+ * used
+ * to compute the pose of the robot during a specific laserscan.
+ */
+ Pose m_ReferenceOdometryPose;
+ Pose m_lastUsedPose;
+
+ Pose m_LastLikeliestPose;
+
+ /**
+ * This variable stores the time of the last odometry measurement as
+ * reference
+ * which is used to compute the pose of the robot during a specific
+ * laserscan.
+ */
+ ros::Time m_ReferenceOdometryTime;
+
+ /**
+ * This variable stores the time the last map message was sent to be able to
+ * compute the time for the next map send.
+ */
+ ros::Time m_LastMapSendTime;
+
+
+ /**
+ * This variable stores a pointer to the hyper slam filter
+ */
+ HyperSlamFilter* m_HyperSlamFilter;
+
+ /**
+ * Scatter variances in self localization.
+ */
+ double m_ScatterVarXY;
+ double m_ScatterVarTheta;
+
+ /**
+ * This variabe is true, if the slam algorithm is used for mapping and
+ * keeps updating the map, false otherwise.
+ */
+ bool m_DoMapping;
+
+ /**
+ * Vectors used to queue laser and odom messages to find a fit
+ */
+ std::vector<sensor_msgs::LaserScan::ConstPtr> m_laser_queue;
+ std::vector<nav_msgs::Odometry::ConstPtr> m_odom_queue;
+
+ /**
+ * duration to wait between two particle filter steps
+ */
+ ros::Duration m_WaitDuration;
+
+ /**
+ * Broadcasts the transform map -> base_link
+ */
+ tf::TransformBroadcaster m_tfBroadcaster;
+
+ /**
+ * subscribers and publishers
+ */
+ ros::Subscriber m_LaserScanSubscriber;
+ ros::Subscriber m_OdometrySubscriber;
+ ros::Subscriber m_UserDefPoseSubscriber;
+ ros::Subscriber m_DoMappingSubscriber;
+ ros::Subscriber m_ResetMapSubscriber;
+ ros::Subscriber m_LoadMapSubscriber;
+ ros::Subscriber m_MaskingSubscriber;
+ ros::Subscriber m_ResetHighSubscriber;
+ ros::Subscriber m_InitialPoseSubscriber;
+
+ ros::Publisher m_PoseStampedPublisher;
+ ros::Publisher m_PoseArrayPublisher;
+ ros::Publisher m_SLAMMapPublisher;
};
#endif
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index 9587eda9..02979b3f 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -1,965 +1,965 @@
#include <homer_mapping/OccupancyMap/OccupancyMap.h>
-
-#include <homer_nav_libs/Math/Math.h>
-
-#include <QtGui/QImage>
-#include <cmath>
-#include <fstream>
-#include <sstream>
-#include <vector>
-
-#include <Eigen/Geometry>
-
-#include <ros/ros.h>
-#include <tf/transform_listener.h>
-
-#include <homer_mapnav_msgs/ModifyMap.h>
#include <homer_nav_libs/tools.h>
-// uncomment this to get extended information on the tracer
-//#define TRACER_OUTPUT
-
using namespace std;
-const float UNKNOWN_LIKELIHOOD = 0.3;
-
-// Flags of current changes //
-const char NO_CHANGE = 0;
-const char OCCUPIED = 1;
-const char FREE = 2;
-// the safety border around occupied pixels which is left unchanged
-const char SAFETY_BORDER = 3;
-///////////////////////////////
-
-// assumed laser measure count for loaded maps
-const int LOADED_MEASURECOUNT = 10;
-
-OccupancyMap::OccupancyMap() {
- float mapSize, resolution;
- ros::param::get("/homer_mapping/size", mapSize);
- ros::param::get("/homer_mapping/resolution", resolution);
-
- // add one safety pixel
- m_metaData.resolution = resolution;
- m_metaData.width = mapSize / m_metaData.resolution + 1;
- m_metaData.height = mapSize / m_metaData.resolution + 1;
- m_ByteSize = m_metaData.width * m_metaData.height;
-
- m_metaData.origin.position.x =
- -(m_metaData.width * m_metaData.resolution / 2.0);
- m_metaData.origin.position.y =
- -(m_metaData.height * m_metaData.resolution / 2.0);
- m_metaData.origin.orientation.w = 1.0;
- m_metaData.origin.orientation.x = 0.0;
- m_metaData.origin.orientation.y = 0.0;
- m_metaData.origin.orientation.z = 0.0;
- initMembers();
+OccupancyMap::OccupancyMap()
+{
+ float mapSize, resolution;
+ ros::param::get("/homer_mapping/size", mapSize);
+ ros::param::get("/homer_mapping/resolution", resolution);
+
+ // add one safety pixel
+ m_metaData.resolution = resolution;
+ m_metaData.width = mapSize / m_metaData.resolution + 1;
+ m_metaData.height = mapSize / m_metaData.resolution + 1;
+ m_ByteSize = m_metaData.width * m_metaData.height;
+
+ m_metaData.origin.position.x =
+ -(m_metaData.width * m_metaData.resolution / 2.0);
+ m_metaData.origin.position.y =
+ -(m_metaData.height * m_metaData.resolution / 2.0);
+ m_metaData.origin.orientation.w = 1.0;
+ m_metaData.origin.orientation.x = 0.0;
+ m_metaData.origin.orientation.y = 0.0;
+ m_metaData.origin.orientation.z = 0.0;
+ initMembers();
}
OccupancyMap::OccupancyMap(float*& occupancyProbability,
geometry_msgs::Pose origin, float resolution,
- int width, int height, Box2D<int> exploredRegion) {
- m_metaData.origin = origin;
- m_metaData.resolution = resolution;
- m_metaData.width = width;
- m_metaData.height = height;
- m_ByteSize = m_metaData.width * m_metaData.height;
- initMembers();
-
- m_ExploredRegion = exploredRegion;
- m_ChangedRegion = exploredRegion;
-
- if (m_OccupancyProbability) {
- delete[] m_OccupancyProbability;
- }
- m_OccupancyProbability = occupancyProbability;
- for (unsigned i = 0; i < m_ByteSize; i++) {
- if (m_OccupancyProbability[i] != 0.5) {
- m_MeasurementCount[i] = LOADED_MEASURECOUNT;
- m_OccupancyCount[i] =
- m_OccupancyProbability[i] * (float)LOADED_MEASURECOUNT;
- }
- }
+ int width, int height, Box2D<int> exploredRegion)
+{
+ m_metaData.origin = origin;
+ m_metaData.resolution = resolution;
+ m_metaData.width = width;
+ m_metaData.height = height;
+ m_ByteSize = m_metaData.width * m_metaData.height;
+ initMembers();
+
+ m_ExploredRegion = exploredRegion;
+ m_ChangedRegion = exploredRegion;
+
+ for (unsigned i = 0; i < m_ByteSize; i++)
+ {
+ if (occupancyProbability[i] != 0.5)
+ {
+ m_MapPoints[i].OccupancyProbability = occupancyProbability[i];
+ m_MapPoints[i].MeasurementCount = LOADED_MEASURECOUNT;
+ m_MapPoints[i].OccupancyCount =
+ m_MapPoints[i].OccupancyProbability * LOADED_MEASURECOUNT;
+ }
+ }
}
-OccupancyMap::OccupancyMap(const OccupancyMap& occupancyMap) {
- m_OccupancyProbability = 0;
- m_MeasurementCount = 0;
- m_OccupancyCount = 0;
- m_CurrentChanges = 0;
- m_HighSensitive = 0;
-
- *this = occupancyMap;
+OccupancyMap::OccupancyMap(const OccupancyMap& occupancyMap)
+{
+ *this = occupancyMap;
}
-OccupancyMap::~OccupancyMap() { cleanUp(); }
-
-void OccupancyMap::initMembers() {
- ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
- ros::param::get("/homer_mapping/obstacle_borders", m_ObstacleBorders);
- ros::param::get("/homer_mapping/measure_sampling_step",
- m_MeasureSamplingStep);
- ros::param::get("/homer_mapping/laser_scanner/free_reading_distance",
- m_FreeReadingDistance);
-
- m_OccupancyProbability = new float[m_ByteSize];
- m_MeasurementCount = new unsigned short[m_ByteSize];
- m_OccupancyCount = new unsigned short[m_ByteSize];
- m_CurrentChanges = new unsigned char[m_ByteSize];
- m_HighSensitive = new unsigned short[m_ByteSize];
- for (unsigned i = 0; i < m_ByteSize; i++) {
- m_OccupancyProbability[i] = UNKNOWN_LIKELIHOOD;
- m_OccupancyCount[i] = 0;
- m_MeasurementCount[i] = 0;
- m_CurrentChanges[i] = NO_CHANGE;
- m_HighSensitive[i] = 0;
- }
-
- m_ExploredRegion =
- Box2D<int>(m_metaData.width / 2.1, m_metaData.height / 2.1,
- m_metaData.width / 1.9, m_metaData.height / 1.9);
- maximizeChangedRegion();
+OccupancyMap::~OccupancyMap()
+{
}
-OccupancyMap& OccupancyMap::operator=(const OccupancyMap& occupancyMap) {
- // free allocated memory
- cleanUp();
-
- m_metaData = occupancyMap.m_metaData;
-
- m_ExploredRegion = occupancyMap.m_ExploredRegion;
- m_ByteSize = occupancyMap.m_ByteSize;
-
- ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
-
- // re-allocate all arrays
- m_OccupancyProbability = new float[m_ByteSize];
- m_MeasurementCount = new unsigned short[m_ByteSize];
- m_OccupancyCount = new unsigned short[m_ByteSize];
- m_CurrentChanges = new unsigned char[m_ByteSize];
- m_HighSensitive = new unsigned short[m_ByteSize];
-
- // copy array data
- memcpy(m_OccupancyProbability, occupancyMap.m_OccupancyProbability,
- m_ByteSize * sizeof(*m_OccupancyProbability));
- memcpy(m_MeasurementCount, occupancyMap.m_MeasurementCount,
- m_ByteSize * sizeof(*m_MeasurementCount));
- memcpy(m_OccupancyCount, occupancyMap.m_OccupancyCount,
- m_ByteSize * sizeof(*m_OccupancyCount));
- memcpy(m_CurrentChanges, occupancyMap.m_CurrentChanges,
- m_ByteSize * sizeof(*m_CurrentChanges));
- memcpy(m_HighSensitive, occupancyMap.m_HighSensitive,
- m_ByteSize * sizeof(*m_HighSensitive));
-
- return *this;
+void OccupancyMap::initMembers()
+{
+ ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
+ ros::param::get("/homer_mapping/obstacle_borders", m_ObstacleBorders);
+ ros::param::get("/homer_mapping/measure_sampling_step",
+ m_MeasureSamplingStep);
+ ros::param::get("/homer_mapping/laser_scanner/free_reading_distance",
+ m_FreeReadingDistance);
+
+ m_MapPoints.resize(m_ByteSize);
+
+ m_ExploredRegion =
+ Box2D<int>(m_metaData.width / 2.1, m_metaData.height / 2.1,
+ m_metaData.width / 1.9, m_metaData.height / 1.9);
+ maximizeChangedRegion();
}
-void OccupancyMap::changeMapSize(int x_add_left, int y_add_up, int x_add_right,
- int y_add_down) {
- int new_width = m_metaData.width + x_add_left + x_add_right;
- int new_height = m_metaData.height + y_add_up + y_add_down;
-
- m_ByteSize = new_width * new_height;
- // allocate all arrays
- float* OccupancyProbability = new float[m_ByteSize];
- unsigned short* MeasurementCount = new unsigned short[m_ByteSize];
- unsigned short* OccupancyCount = new unsigned short[m_ByteSize];
- unsigned char* CurrentChanges = new unsigned char[m_ByteSize];
- unsigned short* HighSensitive = new unsigned short[m_ByteSize];
-
- for (unsigned i = 0; i < m_ByteSize; i++) {
- OccupancyProbability[i] = UNKNOWN_LIKELIHOOD;
- OccupancyCount[i] = 0;
- MeasurementCount[i] = 0;
- CurrentChanges[i] = NO_CHANGE;
- HighSensitive[i] = 0;
- }
-
- for (int y = 0; y < m_metaData.height; y++) {
- for (int x = 0; x < m_metaData.width; x++) {
- int i = y * m_metaData.width + x;
- int in = (y + y_add_up) * new_width + (x + x_add_left);
- OccupancyProbability[in] = m_OccupancyProbability[i];
- MeasurementCount[in] = m_MeasurementCount[i];
- OccupancyCount[in] = m_OccupancyCount[i];
- CurrentChanges[in] = m_CurrentChanges[i];
- HighSensitive[in] = m_HighSensitive[i];
- }
- }
-
- m_ExploredRegion.setMinX(m_ExploredRegion.minX() + x_add_left);
- m_ExploredRegion.setMaxX(m_ExploredRegion.maxX() + x_add_left);
- m_ExploredRegion.setMinY(m_ExploredRegion.minY() + y_add_up);
- m_ExploredRegion.setMaxY(m_ExploredRegion.maxY() + y_add_up);
-
- m_ChangedRegion.setMinX(m_ChangedRegion.minX() + x_add_left);
- m_ChangedRegion.setMaxX(m_ChangedRegion.maxX() + x_add_left);
- m_ChangedRegion.setMinY(m_ChangedRegion.minY() + y_add_up);
- m_ChangedRegion.setMaxY(m_ChangedRegion.maxY() + y_add_up);
+OccupancyMap& OccupancyMap::operator=(const OccupancyMap& occupancyMap)
+{
+ m_metaData = occupancyMap.m_metaData;
+ m_ExploredRegion = occupancyMap.m_ExploredRegion;
+ m_ByteSize = occupancyMap.m_ByteSize;
- m_metaData.width = new_width;
- m_metaData.height = new_height;
- m_metaData.origin.position.x -= (x_add_left)*m_metaData.resolution;
- m_metaData.origin.position.y -= (y_add_up)*m_metaData.resolution;
+ m_MapPoints = occupancyMap.m_MapPoints;
- cleanUp();
+ return *this;
+}
- m_OccupancyProbability = OccupancyProbability;
- m_MeasurementCount = MeasurementCount;
- m_OccupancyCount = OccupancyCount;
- m_CurrentChanges = CurrentChanges;
- m_HighSensitive = HighSensitive;
+void OccupancyMap::changeMapSize(int x_add_left, int y_add_up, int x_add_right,
+ int y_add_down)
+{
+ int new_width = m_metaData.width + x_add_left + x_add_right;
+ int new_height = m_metaData.height + y_add_up + y_add_down;
+
+ m_ByteSize = new_width * new_height;
+
+ std::vector<PixelValue> tmpMap;
+ tmpMap.resize(m_ByteSize);
+
+ for (int y = 0; y < m_metaData.height; y++)
+ {
+ for (int x = 0; x < m_metaData.width; x++)
+ {
+ int i = y * m_metaData.width + x;
+ int in = (y + y_add_up) * new_width + (x + x_add_left);
+ tmpMap[in] = m_MapPoints[i];
+ }
+ }
+
+ m_ExploredRegion.setMinX(m_ExploredRegion.minX() + x_add_left);
+ m_ExploredRegion.setMaxX(m_ExploredRegion.maxX() + x_add_left);
+ m_ExploredRegion.setMinY(m_ExploredRegion.minY() + y_add_up);
+ m_ExploredRegion.setMaxY(m_ExploredRegion.maxY() + y_add_up);
+
+ m_ChangedRegion.setMinX(m_ChangedRegion.minX() + x_add_left);
+ m_ChangedRegion.setMaxX(m_ChangedRegion.maxX() + x_add_left);
+ m_ChangedRegion.setMinY(m_ChangedRegion.minY() + y_add_up);
+ m_ChangedRegion.setMaxY(m_ChangedRegion.maxY() + y_add_up);
+
+ m_metaData.width = new_width;
+ m_metaData.height = new_height;
+ m_metaData.origin.position.x -= (x_add_left)*m_metaData.resolution;
+ m_metaData.origin.position.y -= (y_add_up)*m_metaData.resolution;
+
+ m_MapPoints = tmpMap;
}
-int OccupancyMap::width() const { return m_metaData.width; }
+int OccupancyMap::width() const
+{
+ return m_metaData.width;
+}
-int OccupancyMap::height() const { return m_metaData.height; }
+int OccupancyMap::height() const
+{
+ return m_metaData.height;
+}
-float OccupancyMap::getOccupancyProbability(Eigen::Vector2i p) {
- if (p.y() >= m_metaData.height || p.x() >= m_metaData.width) {
- return UNKNOWN_LIKELIHOOD;
- }
- unsigned offset = m_metaData.width * p.y() + p.x();
- return m_OccupancyProbability[offset];
+float OccupancyMap::getOccupancyProbability(Eigen::Vector2i p)
+{
+ if (p.y() >= m_metaData.height || p.x() >= m_metaData.width)
+ {
+ return UNKNOWN_LIKELIHOOD;
+ }
+ unsigned offset = m_metaData.width * p.y() + p.x();
+ return m_MapPoints[offset].OccupancyProbability;
}
-void OccupancyMap::resetHighSensitive() {
- ROS_INFO_STREAM("High sensitive Areas reseted");
- m_reset_high = true;
+void OccupancyMap::resetHighSensitive()
+{
+ ROS_INFO_STREAM("High sensitive Areas reseted");
+ m_reset_high = true;
}
-void OccupancyMap::computeOccupancyProbabilities() {
- for (int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++) {
- int yOffset = m_metaData.width * y;
- for (int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++) {
- int i = x + yOffset;
- if (m_MeasurementCount[i] > 0) {
- // int maxCount = 100; //TODO param
- // if(m_MeasurementCount[i] > maxCount * 2 -1)
- //{
- // int scalingFactor = m_MeasurementCount[i] / maxCount;
- // if ( scalingFactor != 0 )
- //{
- // m_MeasurementCount[i] /= scalingFactor;
- // m_OccupancyCount[i] /= scalingFactor;
- //}
- //}
- m_OccupancyProbability[i] =
- m_OccupancyCount[i] /
- static_cast<float>(m_MeasurementCount[i]);
- if (m_HighSensitive[i] == 1) {
- if (m_reset_high == true) {
- m_OccupancyCount[i] = 0;
- m_OccupancyProbability[i] = 0;
- }
- if (m_MeasurementCount[i] > 20) {
- m_MeasurementCount[i] = 10;
- m_OccupancyCount[i] = 10 * m_OccupancyProbability[i];
- }
- if (m_OccupancyProbability[i] > 0.3) {
- m_OccupancyProbability[i] = 1;
- }
- }
- } else {
- m_OccupancyProbability[i] = UNKNOWN_LIKELIHOOD;
- }
+void OccupancyMap::computeOccupancyProbabilities()
+{
+ for (int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++)
+ {
+ int yOffset = m_metaData.width * y;
+ for (int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++)
+ {
+ int i = x + yOffset;
+ if (m_MapPoints[i].MeasurementCount > 0)
+ {
+ m_MapPoints[i].OccupancyProbability =
+ m_MapPoints[i].OccupancyCount /
+ static_cast<float>(m_MapPoints[i].MeasurementCount);
+ if (m_MapPoints[i].HighSensitive == 1)
+ {
+ if (m_reset_high == true)
+ {
+ m_MapPoints[i].OccupancyCount = 0;
+ m_MapPoints[i].OccupancyProbability = 0;
+ }
+ if (m_MapPoints[i].MeasurementCount > 20)
+ {
+ m_MapPoints[i].MeasurementCount = 10;
+ m_MapPoints[i].OccupancyCount =
+ 10 * m_MapPoints[i].OccupancyProbability;
+ }
+ if (m_MapPoints[i].OccupancyProbability > 0.3)
+ {
+ m_MapPoints[i].OccupancyProbability = 1;
+ }
}
- }
- if (m_reset_high == true) {
- m_reset_high = false;
- }
+ }
+ else
+ {
+ m_MapPoints[i].OccupancyProbability = UNKNOWN_LIKELIHOOD;
+ }
+ }
+ }
+ if (m_reset_high == true)
+ {
+ m_reset_high = false;
+ }
}
void OccupancyMap::insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
- tf::Transform transform) {
- m_latestMapTransform = transform;
- try {
- bool got_transform = m_tfListener.waitForTransform(
- "/base_link", laserData->header.frame_id, ros::Time(0),
- ros::Duration(0.2));
- m_tfListener.lookupTransform("/base_link", laserData->header.frame_id,
- ros::Time(0), m_laserTransform);
- } catch (tf::TransformException ex) {
- ROS_ERROR_STREAM(ex.what());
- ROS_ERROR_STREAM("need transformation from base_link to laser!");
- return;
- }
- markRobotPositionFree();
-
- std::vector<RangeMeasurement> ranges;
- ranges.reserve(laserData->ranges.size());
-
- bool errorFound = false;
- int lastValidIndex = -1;
- float lastValidRange = m_FreeReadingDistance;
-
- RangeMeasurement rangeMeasurement;
- rangeMeasurement.sensorPos.x = m_laserTransform.getOrigin().getX();
- rangeMeasurement.sensorPos.y = m_laserTransform.getOrigin().getY();
-
- for (unsigned int i = 0; i < laserData->ranges.size(); i++) {
- if ((laserData->ranges[i] >= laserData->range_min) &&
- (laserData->ranges[i] <= laserData->range_max)) {
- // if we're at the end of an errorneous segment, interpolate
- // between last valid point and current point
- if (errorFound) {
- // smaller of the two ranges belonging to end points
- float range = Math::min(lastValidRange, laserData->ranges[i]);
- range -= m_metaData.resolution * 2;
- if (range < m_FreeReadingDistance) {
- range = m_FreeReadingDistance;
- } else if (range > laserData->range_max) {
- range = laserData->range_max;
- }
- // choose smaller range
- for (unsigned j = lastValidIndex + 1; j < i; j++) {
- float alpha =
- laserData->angle_min + j * laserData->angle_increment;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX(cos(alpha) * range);
- pin.setY(sin(alpha) * range);
- pin.setZ(0);
- pout = m_laserTransform * pin;
- rangeMeasurement.endPos.x = pout.x();
- rangeMeasurement.endPos.y = pout.y();
- rangeMeasurement.range = range;
- rangeMeasurement.free = true;
- ranges.push_back(rangeMeasurement);
- }
- }
- float alpha = laserData->angle_min + i * laserData->angle_increment;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX(cos(alpha) * laserData->ranges[i]);
- pin.setY(sin(alpha) * laserData->ranges[i]);
- pin.setZ(0);
- pout = m_laserTransform * pin;
- rangeMeasurement.endPos.x = pout.x();
- rangeMeasurement.endPos.y = pout.y();
- rangeMeasurement.range = laserData->ranges[i];
- rangeMeasurement.free = false;
- ranges.push_back(rangeMeasurement);
- errorFound = false;
- lastValidIndex = i;
- lastValidRange = laserData->ranges[i];
- } else {
- errorFound = true;
+ tf::Transform transform)
+{
+ m_latestMapTransform = transform;
+ try
+ {
+ m_tfListener.waitForTransform("/base_link", laserData->header.frame_id,
+ ros::Time(0), ros::Duration(0.2));
+ m_tfListener.lookupTransform("/base_link", laserData->header.frame_id,
+ ros::Time(0), m_laserTransform);
+ }
+ catch (tf::TransformException ex)
+ {
+ ROS_ERROR_STREAM(ex.what());
+ ROS_ERROR_STREAM("need transformation from base_link to laser!");
+ return;
+ }
+ markRobotPositionFree();
+
+ std::vector<RangeMeasurement> ranges;
+
+ bool errorFound = false;
+ int lastValidIndex = -1;
+ float lastValidRange = m_FreeReadingDistance;
+
+ RangeMeasurement rangeMeasurement;
+ rangeMeasurement.sensorPos.x = m_laserTransform.getOrigin().getX();
+ rangeMeasurement.sensorPos.y = m_laserTransform.getOrigin().getY();
+
+ for (unsigned int i = 0; i < laserData->ranges.size(); i++)
+ {
+ if ((laserData->ranges[i] >= laserData->range_min) &&
+ (laserData->ranges[i] <= laserData->range_max))
+ {
+ // if we're at the end of an errorneous segment, interpolate
+ // between last valid point and current point
+ if (errorFound)
+ {
+ // smaller of the two ranges belonging to end points
+ float range = std::min(lastValidRange, laserData->ranges[i]);
+ range -= m_metaData.resolution * 2;
+ if (range < m_FreeReadingDistance)
+ {
+ range = m_FreeReadingDistance;
}
- }
-
- insertRanges(ranges, laserData->header.stamp);
+ else if (range > laserData->range_max)
+ {
+ range = laserData->range_max;
+ }
+ // choose smaller range
+ for (unsigned j = lastValidIndex + 1; j < i; j++)
+ {
+ float alpha = laserData->angle_min + j * laserData->angle_increment;
+ tf::Vector3 pin;
+ tf::Vector3 pout;
+ pin.setX(cos(alpha) * range);
+ pin.setY(sin(alpha) * range);
+ pin.setZ(0);
+ pout = m_laserTransform * pin;
+ rangeMeasurement.endPos.x = pout.x();
+ rangeMeasurement.endPos.y = pout.y();
+ rangeMeasurement.range = range;
+ rangeMeasurement.free = true;
+ ranges.push_back(rangeMeasurement);
+ }
+ }
+ float alpha = laserData->angle_min + i * laserData->angle_increment;
+ tf::Vector3 pin;
+ tf::Vector3 pout;
+ pin.setX(cos(alpha) * laserData->ranges[i]);
+ pin.setY(sin(alpha) * laserData->ranges[i]);
+ pin.setZ(0);
+ pout = m_laserTransform * pin;
+ rangeMeasurement.endPos.x = pout.x();
+ rangeMeasurement.endPos.y = pout.y();
+ rangeMeasurement.range = laserData->ranges[i];
+ rangeMeasurement.free = false;
+ ranges.push_back(rangeMeasurement);
+ errorFound = false;
+ lastValidIndex = i;
+ lastValidRange = laserData->ranges[i];
+ }
+ else
+ {
+ errorFound = true;
+ }
+ }
+ insertRanges(ranges, laserData->header.stamp);
}
void OccupancyMap::insertRanges(vector<RangeMeasurement> ranges,
- ros::Time stamp) {
- if (ranges.size() < 1) {
- return;
- }
- clearChanges();
-
- Eigen::Vector2i lastEndPixel;
- int need_x_left = 0;
- int need_x_right = 0;
- int need_y_up = 0;
- int need_y_down = 0;
-
- std::vector<Eigen::Vector2i> map_pixel;
-
- for (int i = 0; i < ranges.size(); i++) {
- geometry_msgs::Point endPosWorld =
- map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
- map_pixel.push_back(map_tools::toMapCoords(
- endPosWorld, m_metaData.origin, m_metaData.resolution));
- }
- geometry_msgs::Point sensorPosWorld =
- map_tools::transformPoint(ranges[0].sensorPos, m_latestMapTransform);
- Eigen::Vector2i sensorPixel = map_tools::toMapCoords(
- sensorPosWorld, m_metaData.origin, m_metaData.resolution);
- m_ChangedRegion.enclose(sensorPixel.x(), sensorPixel.y());
-
- ////paint safety borders
- // if ( m_ObstacleBorders )
- //{
- // for ( unsigned i=0; i<ranges.size(); i++ )
- //{
- // Eigen::Vector2i endPixel = map_pixel[i];
-
- // for ( int y=endPixel.y()-1; y <= endPixel.y() +1; y++ )
- //{
- // if(y > m_metaData.height) continue;
- // for ( int x=endPixel.x()-1; x <= endPixel.x() +1; x++ )
- //{
- // if(x > m_metaData.width) continue;
- // unsigned offset=x+m_metaData.width*y;
- // if ( offset < unsigned ( m_ByteSize ) )
- //{
- // m_CurrentChanges[ offset ] = SAFETY_BORDER;
- //}
- //}
- //}
- //}
- //}
- ////paint safety ranges
- // for ( unsigned i=0; i<ranges.size(); i++ )
- //{
- // Eigen::Vector2i startPixel = map_pixel[i];
- // geometry_msgs::Point endPos;
- // endPos.x = ranges[i].endPos.x * 4;
- // endPos.y = ranges[i].endPos.y * 4;
-
- // geometry_msgs::Point endPosWorld = map_tools::transformPoint(endPos,
- // m_latestMapTransform);
- // Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld,
- // m_metaData.origin, m_metaData.resolution);
-
- // if(endPixel.x() < 0) endPixel.x() = 0;
- // if(endPixel.y() < 0) endPixel.y() = 0;
- // if(endPixel.x() >= m_metaData.width) endPixel.x() = m_metaData.width - 1;
- // if(endPixel.y() >= m_metaData.height) endPixel.y() = m_metaData.height -
- // 1;
-
- // drawLine ( m_CurrentChanges, startPixel, endPixel, SAFETY_BORDER );
- //}
-
- // paint end pixels
- for (unsigned i = 0; i < ranges.size(); i++) {
- Eigen::Vector2i endPixel = map_pixel[i];
-
- if (endPixel != lastEndPixel) {
- bool outside = false;
- if (endPixel.x() >= m_metaData.width) {
- need_x_right = std::max(
- (int)(endPixel.x() - m_metaData.width + 10), need_x_right);
- outside = true;
- }
- if (endPixel.x() < 0) {
- need_x_left = std::max((int)(-endPixel.x()) + 10, need_x_left);
- outside = true;
- }
- if (endPixel.y() >= m_metaData.height) {
- need_y_down = std::max(
- (int)(endPixel.y() - m_metaData.height) + 10, need_y_down);
- outside = true;
- }
- if (endPixel.y() < 0) {
- need_y_up = std::max((int)(-endPixel.y()) + 10, need_y_up);
- outside = true;
- }
- if (outside) {
- continue;
- }
- m_ChangedRegion.enclose(endPixel.x(), endPixel.y());
-
- if (!ranges[i].free) {
- unsigned offset =
- endPixel.x() + m_metaData.width * endPixel.y();
- if (ranges[i].range < 10) {
- m_CurrentChanges[offset] = ::OCCUPIED;
- } else {
- m_CurrentChanges[offset] = ::SAFETY_BORDER;
- }
- }
+ ros::Time stamp)
+{
+ if (ranges.size() < 1)
+ {
+ return;
+ }
+ clearChanges();
+
+ Eigen::Vector2i lastEndPixel;
+ int need_x_left = 0;
+ int need_x_right = 0;
+ int need_y_up = 0;
+ int need_y_down = 0;
+
+ std::vector<Eigen::Vector2i> map_pixel;
+
+ for (int i = 0; i < ranges.size(); i++)
+ {
+ geometry_msgs::Point endPosWorld =
+ map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
+ map_pixel.push_back(map_tools::toMapCoords(endPosWorld, m_metaData.origin,
+ m_metaData.resolution));
+ }
+ geometry_msgs::Point sensorPosWorld =
+ map_tools::transformPoint(ranges[0].sensorPos, m_latestMapTransform);
+ Eigen::Vector2i sensorPixel = map_tools::toMapCoords(
+ sensorPosWorld, m_metaData.origin, m_metaData.resolution);
+ m_ChangedRegion.enclose(sensorPixel.x(), sensorPixel.y());
+
+ // paint safety borders
+ if (m_ObstacleBorders)
+ {
+ for (unsigned i = 0; i < ranges.size(); i++)
+ {
+ if (i < 2 || i > ranges.size() - 2)
+ {
+ continue;
+ }
+ Eigen::Vector2i endPixel = map_pixel[i];
+
+ for (int y = endPixel.y() - 2; y <= endPixel.y() + 2; y++)
+ {
+ if (y > m_metaData.height || y < 0)
+ continue;
+ for (int x = endPixel.x() - 2; x <= endPixel.x() + 2; x++)
+ {
+ if (x > m_metaData.width || x < 0)
+ continue;
+ unsigned offset = x + m_metaData.width * y;
+ if (offset < unsigned(m_ByteSize))
+ {
+ m_MapPoints[offset].CurrentChange = ::CONTRAST;
+ }
}
- 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);
+ }
+ }
+ }
+ ////paint safety ranges
+ // for ( unsigned i=0; i<ranges.size(); i++ )
+ //{
+ // Eigen::Vector2i startPixel = map_pixel[i];
+ // geometry_msgs::Point endPos;
+ // endPos.x = ranges[i].endPos.x * 4;
+ // endPos.y = ranges[i].endPos.y * 4;
+
+ // geometry_msgs::Point endPosWorld = map_tools::transformPoint(endPos,
+ // m_latestMapTransform);
+ // Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld,
+ // m_metaData.origin, m_metaData.resolution);
+
+ // if(endPixel.x() < 0) endPixel.x() = 0;
+ // if(endPixel.y() < 0) endPixel.y() = 0;
+ // if(endPixel.x() >= m_metaData.width) endPixel.x() = m_metaData.width - 1;
+ // if(endPixel.y() >= m_metaData.height) endPixel.y() = m_metaData.height -
+ // 1;
+
+ // drawLine ( startPixel, endPixel, SAFETY_BORDER );
+ //}
+
+ // paint end pixels
+ for (unsigned i = 0; i < ranges.size(); i++)
+ {
+ Eigen::Vector2i endPixel = map_pixel[i];
+
+ if (endPixel != lastEndPixel)
+ {
+ bool outside = false;
+ if (endPixel.x() >= m_metaData.width)
+ {
+ need_x_right =
+ std::max((int)(endPixel.x() - m_metaData.width + 10), need_x_right);
+ outside = true;
+ }
+ if (endPixel.x() < 0)
+ {
+ need_x_left = std::max((int)(-endPixel.x()) + 10, need_x_left);
+ outside = true;
+ }
+ if (endPixel.y() >= m_metaData.height)
+ {
+ need_y_down =
+ std::max((int)(endPixel.y() - m_metaData.height) + 10, need_y_down);
+ outside = true;
+ }
+ if (endPixel.y() < 0)
+ {
+ need_y_up = std::max((int)(-endPixel.y()) + 10, need_y_up);
+ outside = true;
+ }
+ if (outside)
+ {
+ continue;
+ }
+ m_ChangedRegion.enclose(endPixel.x(), endPixel.y());
+
+ if (!ranges[i].free)
+ {
+ unsigned offset = endPixel.x() + m_metaData.width * endPixel.y();
+ if (ranges[i].range < 10)
+ {
+ m_MapPoints[offset].CurrentChange = ::OCCUPIED;
}
- 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) {
- // 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);
- }
+ else
+ {
+ m_MapPoints[offset].CurrentChange = ::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;
+ }
+ drawLine(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)
+ {
+ changeMapSize(need_x_left, need_y_up, need_x_right, need_y_down);
+ }
}
-double OccupancyMap::contrastFromProbability(int8_t prob) {
- // range from 0..126 (=127 values) and 128..255 (=128 values)
- double diff = ((double)prob - UNKNOWN);
- double contrast;
- if (prob <= UNKNOWN) {
- contrast = (diff / UNKNOWN); // 0..1
- } else {
- contrast = (diff / (UNKNOWN + 1)); // 0..1
- }
- return (contrast * contrast);
+double OccupancyMap::contrastFromProbability(int8_t prob)
+{
+ // range from 0..126 (=127 values) and 128..255 (=128 values)
+ double diff = ((double)prob - UNKNOWN);
+ double contrast;
+ if (prob <= UNKNOWN)
+ {
+ contrast = (diff / UNKNOWN); // 0..1
+ }
+ else
+ {
+ contrast = (diff / (UNKNOWN + 1)); // 0..1
+ }
+ return (contrast * contrast);
}
-double OccupancyMap::evaluateByContrast() {
- double contrastSum = 0.0;
- unsigned int contrastCnt = 0;
-
- for (int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++) {
- for (int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX();
- x++) {
- int i = x + y * m_metaData.width;
- if (m_MeasurementCount[i] > 1) {
- int prob = m_OccupancyProbability[i] * 100;
- if (prob != NOT_SEEN_YET) // ignore not yet seen cells
- {
- contrastSum += contrastFromProbability(prob);
- contrastCnt++;
- }
- }
+double OccupancyMap::evaluateByContrast()
+{
+ double contrastSum = 0.0;
+ unsigned int contrastCnt = 0;
+
+ for (int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++)
+ {
+ for (int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++)
+ {
+ int i = x + y * m_metaData.width;
+ if (m_MapPoints[i].MeasurementCount > 1)
+ {
+ int prob = m_MapPoints[i].OccupancyProbability * 100;
+ if (prob != NOT_SEEN_YET) // ignore not yet seen cells
+ {
+ contrastSum += contrastFromProbability(prob);
+ contrastCnt++;
}
- }
- if ((contrastCnt) > 0) {
- return ((contrastSum / contrastCnt) * 100);
- }
- return (0);
+ }
+ }
+ }
+ if ((contrastCnt) > 0)
+ {
+ return ((contrastSum / contrastCnt) * 100);
+ }
+ return (0);
}
-vector<MeasurePoint> OccupancyMap::getMeasurePoints(
- sensor_msgs::LaserScanConstPtr laserData) {
- vector<MeasurePoint> result;
- result.reserve(laserData->ranges.size());
-
- double minDist = m_MeasureSamplingStep;
-
- Point2D lastHitPos;
- Point2D lastUsedHitPos;
-
- // extract points for measuring
- for (unsigned int i = 0; i < laserData->ranges.size(); i++) {
- if (laserData->ranges[i] <= laserData->range_max &&
- laserData->ranges[i] >= laserData->range_min) {
- float alpha = laserData->angle_min + i * laserData->angle_increment;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX(cos(alpha) * laserData->ranges[i]);
- pin.setY(sin(alpha) * laserData->ranges[i]);
- pin.setZ(0);
-
- pout = m_laserTransform * pin;
-
- Point2D hitPos(pout.x(), pout.y());
-
- if (hitPos.distance(lastUsedHitPos) >= minDist) {
- MeasurePoint p;
- // preserve borders of segments
- if ((i != 0) && (lastUsedHitPos.distance(lastHitPos) >
- m_metaData.resolution * 0.5) &&
- (hitPos.distance(lastHitPos) >= minDist * 1.5)) {
- p.hitPos = lastHitPos;
- p.borderType = RightBorder;
- result.push_back(p);
- p.hitPos = hitPos;
- p.borderType = LeftBorder;
- result.push_back(p);
- lastUsedHitPos = hitPos;
- } else {
- // save current point
- p.hitPos = hitPos;
- p.borderType = NoBorder;
- result.push_back(p);
- lastUsedHitPos = hitPos;
- }
- }
- lastHitPos = hitPos;
+vector<MeasurePoint>
+OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData)
+{
+ vector<MeasurePoint> result;
+ result.reserve(laserData->ranges.size());
+
+ double minDist = m_MeasureSamplingStep;
+
+ Point2D lastHitPos;
+ Point2D lastUsedHitPos;
+
+ // extract points for measuring
+ for (unsigned int i = 0; i < laserData->ranges.size(); i++)
+ {
+ if (laserData->ranges[i] <= laserData->range_max &&
+ laserData->ranges[i] >= laserData->range_min)
+ {
+ float alpha = laserData->angle_min + i * laserData->angle_increment;
+ tf::Vector3 pin;
+ tf::Vector3 pout;
+ pin.setX(cos(alpha) * laserData->ranges[i]);
+ pin.setY(sin(alpha) * laserData->ranges[i]);
+ pin.setZ(0);
+
+ pout = m_laserTransform * pin;
+
+ Point2D hitPos(pout.x(), pout.y());
+
+ if (hitPos.distance(lastUsedHitPos) >= minDist)
+ {
+ MeasurePoint p;
+ // preserve borders of segments
+ if ((i != 0) && (lastUsedHitPos.distance(lastHitPos) >
+ m_metaData.resolution * 0.5) &&
+ (hitPos.distance(lastHitPos) >= minDist * 1.5))
+ {
+ p.hitPos = lastHitPos;
+ p.borderType = RightBorder;
+ result.push_back(p);
+ p.borderType = LeftBorder;
}
- }
-
- // the first and last one are border pixels
- if (result.size() > 0) {
- result[0].borderType = LeftBorder;
- result[result.size() - 1].borderType = RightBorder;
- }
-
- // calculate front check points
- for (unsigned i = 0; i < result.size(); i++) {
- CVec2 diff;
-
- switch (result[i].borderType) {
- case NoBorder:
- diff = result[i - 1].hitPos - result[i + 1].hitPos;
- break;
- case LeftBorder:
- diff = result[i].hitPos - result[i + 1].hitPos;
- break;
- case RightBorder:
- diff = result[i - 1].hitPos - result[i].hitPos;
- break;
+ else
+ {
+ p.borderType = NoBorder;
}
-
- CVec2 normal = diff.rotate(Math::Pi * 0.5);
- normal.normalize();
- normal *= m_metaData.resolution * sqrt(2.0) * 10.0;
-
- result[i].frontPos = result[i].hitPos + normal;
- }
-
- return result;
+ p.hitPos = hitPos;
+ result.push_back(p);
+ lastUsedHitPos = hitPos;
+ }
+ lastHitPos = hitPos;
+ }
+ }
+
+ // the first and last one are border pixels
+ if (result.size() > 0)
+ {
+ result[0].borderType = LeftBorder;
+ result[result.size() - 1].borderType = RightBorder;
+ }
+
+ // calculate front check points
+ for (unsigned i = 0; i < result.size(); i++)
+ {
+ CVec2 diff;
+
+ switch (result[i].borderType)
+ {
+ case NoBorder:
+ diff = result[i - 1].hitPos - result[i + 1].hitPos;
+ break;
+ case LeftBorder:
+ diff = result[i].hitPos - result[i + 1].hitPos;
+ break;
+ case RightBorder:
+ diff = result[i - 1].hitPos - result[i].hitPos;
+ break;
+ }
+
+ CVec2 normal = diff.rotate(Math::Pi * 0.5);
+ normal.normalize();
+ result[i].normal = normal;
+ normal *= m_metaData.resolution * sqrt(2.0) * 10.0;
+
+ result[i].frontPos = result[i].hitPos + normal;
+ }
+
+ return result;
}
float OccupancyMap::computeScore(Pose robotPose,
- std::vector<MeasurePoint> measurePoints) {
- // This is a very simple implementation, using only the end point of the
- // beam.
- // For every beam the end cell is computed and tested if the cell is
- // occupied.
- unsigned lastOffset = 0;
- unsigned hitOffset = 0;
- unsigned frontOffset = 0;
- float fittingFactor = 0.0;
-
- float sinTheta = sin(robotPose.theta());
- float cosTheta = cos(robotPose.theta());
-
- for (unsigned int i = 0; i < measurePoints.size(); i++) {
- // fast variant:
- float x = cosTheta * measurePoints[i].hitPos.x() -
- sinTheta * measurePoints[i].hitPos.y() + robotPose.x();
- float y = sinTheta * measurePoints[i].hitPos.x() +
- cosTheta * measurePoints[i].hitPos.y() + robotPose.y();
- geometry_msgs::Point hitPos;
- hitPos.x = x;
- hitPos.y = y;
-
- Eigen::Vector2i hitPixel = map_tools::toMapCoords(
- hitPos, m_metaData.origin, m_metaData.resolution);
- hitOffset = hitPixel.x() + m_metaData.width * hitPixel.y();
-
- // avoid multiple measuring of same pixel or unknown pixel
- if ((hitOffset == lastOffset) || (hitOffset >= unsigned(m_ByteSize)) ||
- (m_MeasurementCount[hitOffset] == 0)) {
- continue;
- }
-
- if (m_BacksideChecking) {
- // avoid matching of back and front pixels of obstacles
- x = cosTheta * measurePoints[i].frontPos.x() -
- sinTheta * measurePoints[i].frontPos.y() + robotPose.x();
- y = sinTheta * measurePoints[i].frontPos.x() +
- cosTheta * measurePoints[i].frontPos.y() + robotPose.y();
- geometry_msgs::Point frontPos;
- frontPos.x = x;
- frontPos.y = y;
-
- Eigen::Vector2i frontPixel = map_tools::toMapCoords(
- frontPos, m_metaData.origin, m_metaData.resolution);
- frontOffset = frontPixel.x() + m_metaData.width * frontPixel.y();
-
- if ((frontOffset >= unsigned(m_ByteSize)) ||
- (m_MeasurementCount[frontOffset] == 0)) {
- continue;
- }
- }
-
- lastOffset = hitOffset;
- // fittingFactor += m_SmoothOccupancyProbability[ offset ];
- fittingFactor += m_OccupancyProbability[hitOffset];
- }
- return fittingFactor;
+ std::vector<MeasurePoint> measurePoints)
+{
+ // This is a very simple implementation, using only the end point of the
+ // beam.
+ // For every beam the end cell is computed and tested if the cell is
+ // occupied.
+ unsigned lastOffset = 0;
+ unsigned hitOffset = 0;
+ unsigned frontOffset = 0;
+ float fittingFactor = 0.0;
+
+ float sinTheta = sin(robotPose.theta());
+ float cosTheta = cos(robotPose.theta());
+
+ for (unsigned int i = 0; i < measurePoints.size(); i++)
+ {
+ // fast variant:
+ float x = cosTheta * measurePoints[i].hitPos.x() -
+ sinTheta * measurePoints[i].hitPos.y() + robotPose.x();
+ float y = sinTheta * measurePoints[i].hitPos.x() +
+ cosTheta * measurePoints[i].hitPos.y() + robotPose.y();
+ geometry_msgs::Point hitPos;
+ hitPos.x = x;
+ hitPos.y = y;
+
+ Eigen::Vector2i hitPixel = map_tools::toMapCoords(hitPos, m_metaData.origin,
+ m_metaData.resolution);
+ hitOffset = hitPixel.x() + m_metaData.width * hitPixel.y();
+
+ // avoid multiple measuring of same pixel or unknown pixel
+ if ((hitOffset == lastOffset) || (hitOffset >= unsigned(m_ByteSize)) ||
+ (m_MapPoints[hitOffset].MeasurementCount == 0))
+ {
+ continue;
+ }
+
+ if (m_BacksideChecking)
+ {
+ // avoid matching of back and front pixels of obstacles
+ x = cosTheta * measurePoints[i].frontPos.x() -
+ sinTheta * measurePoints[i].frontPos.y() + robotPose.x();
+ y = sinTheta * measurePoints[i].frontPos.x() +
+ cosTheta * measurePoints[i].frontPos.y() + robotPose.y();
+ geometry_msgs::Point frontPos;
+ frontPos.x = x;
+ frontPos.y = y;
+
+ Eigen::Vector2i frontPixel = map_tools::toMapCoords(
+ frontPos, m_metaData.origin, m_metaData.resolution);
+ frontOffset = frontPixel.x() + m_metaData.width * frontPixel.y();
+
+ if ((frontOffset >= unsigned(m_ByteSize)) ||
+ (m_MapPoints[frontOffset].MeasurementCount == 0))
+ {
+ continue;
+ }
+ }
+
+ lastOffset = hitOffset;
+ fittingFactor += m_MapPoints[hitOffset].OccupancyProbability;
+ }
+ return fittingFactor;
}
-template <class DataT>
-void OccupancyMap::drawLine(DataT* data, Eigen::Vector2i& startPixel,
- Eigen::Vector2i& endPixel, char value) {
- // bresenham algorithm
- int xstart = startPixel.x();
- int ystart = startPixel.y();
- int xend = endPixel.x();
- int yend = endPixel.y();
-
- int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
- // compute distances
- dx = xend - xstart;
- dy = yend - ystart;
-
- // compute increment
- if (dx < 0) {
- incx = -1;
- dx = -dx;
- } else {
- incx = dx ? 1 : 0;
- }
-
- if (dy < 0) {
- incy = -1;
- dy = -dy;
- } else {
- incy = dy ? 1 : 0;
- }
-
- // which distance is greater?
- dist = (dx > dy) ? dx : dy;
- // initializing
- x = xstart;
- y = ystart;
- xerr = dx;
- yerr = dy;
-
- // compute cells
- for (t = 0; t < dist; t++) {
- int index = x + m_metaData.width * y;
- // set flag to free if no flag is set
- // (do not overwrite occupied cells)
- if (index < 0 || index > m_ByteSize) {
- continue;
- }
- if (data[index] == NO_CHANGE) {
- data[index] = value;
- }
- if (data[index] == OCCUPIED || data[index] == SAFETY_BORDER) {
- return;
- }
- xerr += dx;
- yerr += dy;
- if (xerr > dist) {
- xerr -= dist;
- x += incx;
- }
- if (yerr > dist) {
- yerr -= dist;
- y += incy;
- }
- }
+void OccupancyMap::drawLine(Eigen::Vector2i& startPixel,
+ Eigen::Vector2i& endPixel, char value)
+{
+ // bresenham algorithm
+ int xstart = startPixel.x();
+ int ystart = startPixel.y();
+ int xend = endPixel.x();
+ int yend = endPixel.y();
+
+ int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
+ // compute distances
+ dx = xend - xstart;
+ dy = yend - ystart;
+
+ // compute increment
+ if (dx < 0)
+ {
+ incx = -1;
+ dx = -dx;
+ }
+ else
+ {
+ incx = dx ? 1 : 0;
+ }
+
+ if (dy < 0)
+ {
+ incy = -1;
+ dy = -dy;
+ }
+ else
+ {
+ incy = dy ? 1 : 0;
+ }
+
+ // which distance is greater?
+ dist = (dx > dy) ? dx : dy;
+ // initializing
+ x = xstart;
+ y = ystart;
+ xerr = dx;
+ yerr = dy;
+
+ // compute cells
+ for (t = 0; t < dist; t++)
+ {
+ int index = x + m_metaData.width * y;
+ // set flag to free if no flag is set
+ // (do not overwrite occupied cells)
+ if (index < 0 || index > m_ByteSize)
+ {
+ continue;
+ }
+ if (m_MapPoints[index].CurrentChange == NO_CHANGE)
+ {
+ m_MapPoints[index].CurrentChange = value;
+ }
+ if (m_MapPoints[index].CurrentChange == ::OCCUPIED ||
+ m_MapPoints[index].CurrentChange == ::SAFETY_BORDER ||
+ m_MapPoints[index].CurrentChange == ::CONTRAST)
+ {
+ return;
+ }
+ xerr += dx;
+ yerr += dy;
+ if (xerr > dist)
+ {
+ xerr -= dist;
+ x += incx;
+ }
+ if (yerr > dist)
+ {
+ yerr -= dist;
+ y += incy;
+ }
+ }
}
-void OccupancyMap::applyChanges() {
- for (int y = m_ChangedRegion.minY() + 1; y <= m_ChangedRegion.maxY() - 1;
- y++) {
- int yOffset = m_metaData.width * y;
- for (int x = m_ChangedRegion.minX() + 1;
- x <= m_ChangedRegion.maxX() - 1; x++) {
- int i = x + yOffset;
- if ((m_CurrentChanges[i] == ::FREE ||
- m_CurrentChanges[i] == ::OCCUPIED) &&
- m_MeasurementCount[i] < SHRT_MAX) {
- m_MeasurementCount[i]++;
- }
- if (m_CurrentChanges[i] == ::OCCUPIED &&
- m_OccupancyCount[i] < USHRT_MAX) {
- // if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1)
- // m_MeasurementCount[x + m_metaData.width * (y+1)]++;
- // if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1)
- // m_MeasurementCount[x + m_metaData.width * (y-1)]++;
- // if(m_MeasurementCount[i-1] > 1)
- // m_MeasurementCount[i-1]++;
- // if(m_MeasurementCount[i+1] > 1)
- // m_MeasurementCount[i+1]++;
- m_OccupancyCount[i]++;
- }
- }
- }
- for (int y = m_ChangedRegion.minY() + 1; y <= m_ChangedRegion.maxY() - 1;
- y++) {
- int yOffset = m_metaData.width * y;
- for (int x = m_ChangedRegion.minX() + 1;
- x <= m_ChangedRegion.maxX() - 1; x++) {
- int i = x + yOffset;
- if (m_OccupancyCount[i] > m_MeasurementCount[i])
- m_OccupancyCount[i] = m_MeasurementCount[i];
- }
- }
+void OccupancyMap::applyChanges()
+{
+ for (int y = m_ChangedRegion.minY() + 1; y <= m_ChangedRegion.maxY() - 1; y++)
+ {
+ int yOffset = m_metaData.width * y;
+ for (int x = m_ChangedRegion.minX() + 1; x <= m_ChangedRegion.maxX() - 1;
+ x++)
+ {
+ int i = x + yOffset;
+
+ if ((m_MapPoints[i].CurrentChange == ::FREE ||
+ m_MapPoints[i].CurrentChange == ::OCCUPIED ||
+ m_MapPoints[i].CurrentChange == ::CONTRAST) &&
+ m_MapPoints[i].MeasurementCount < SHRT_MAX)
+ {
+ m_MapPoints[i].MeasurementCount++;
+ }
+ if (m_MapPoints[i].CurrentChange == ::OCCUPIED &&
+ m_MapPoints[i].OccupancyCount < SHRT_MAX)
+ {
+ m_MapPoints[i].OccupancyCount += 4;
+ }
+ }
+ }
+ for (int y = m_ChangedRegion.minY() + 1; y <= m_ChangedRegion.maxY() - 1; y++)
+ {
+ int yOffset = m_metaData.width * y;
+ for (int x = m_ChangedRegion.minX() + 1; x <= m_ChangedRegion.maxX() - 1;
+ x++)
+ {
+ int i = x + yOffset;
+ if (m_MapPoints[i].OccupancyCount > m_MapPoints[i].MeasurementCount)
+ m_MapPoints[i].OccupancyCount = m_MapPoints[i].MeasurementCount;
+ }
+ }
}
-void OccupancyMap::clearChanges() {
- m_ChangedRegion.expand(2);
- m_ChangedRegion.clip(
- Box2D<int>(0, 0, m_metaData.width - 1, m_metaData.height - 1));
- for (int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++) {
- int yOffset = m_metaData.width * y;
- for (int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++) {
- int i = x + yOffset;
- m_CurrentChanges[i] = NO_CHANGE;
- }
- }
- m_ChangedRegion =
- Box2D<int>(m_metaData.width - 1, m_metaData.height - 1, 0, 0);
+void OccupancyMap::clearChanges()
+{
+ m_ChangedRegion.expand(2);
+ m_ChangedRegion.clip(
+ Box2D<int>(0, 0, m_metaData.width - 1, m_metaData.height - 1));
+ for (int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++)
+ {
+ int yOffset = m_metaData.width * y;
+ for (int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++)
+ {
+ int i = x + yOffset;
+ m_MapPoints[i].CurrentChange = NO_CHANGE;
+ }
+ }
+ m_ChangedRegion =
+ Box2D<int>(m_metaData.width - 1, m_metaData.height - 1, 0, 0);
}
-void OccupancyMap::incrementMeasurementCount(Eigen::Vector2i p) {
- if (p.x() >= m_metaData.width || p.y() >= m_metaData.height) {
- return;
- }
- unsigned index = p.x() + m_metaData.width * p.y();
- if (m_CurrentChanges[index] == NO_CHANGE &&
- m_MeasurementCount[index] < USHRT_MAX) {
- m_CurrentChanges[index] = ::FREE;
- m_MeasurementCount[index]++;
- }
+void OccupancyMap::incrementMeasurementCount(Eigen::Vector2i p)
+{
+ if (p.x() >= m_metaData.width || p.y() >= m_metaData.height)
+ {
+ return;
+ }
+ unsigned i = p.x() + m_metaData.width * p.y();
+ if (m_MapPoints[i].CurrentChange == NO_CHANGE &&
+ m_MapPoints[i].MeasurementCount < USHRT_MAX)
+ {
+ m_MapPoints[i].CurrentChange = ::FREE;
+ m_MapPoints[i].MeasurementCount++;
+ }
}
-void OccupancyMap::incrementOccupancyCount(Eigen::Vector2i p) {
- if (p.x() >= m_metaData.width || p.y() >= m_metaData.height) {
- return;
- }
- unsigned index = p.x() + m_metaData.width * p.y();
- if ((m_CurrentChanges[index] == NO_CHANGE ||
- m_CurrentChanges[index] == ::FREE) &&
- m_MeasurementCount[index] < USHRT_MAX) {
- m_CurrentChanges[index] = ::OCCUPIED;
- m_OccupancyCount[index]++;
- }
+void OccupancyMap::incrementOccupancyCount(Eigen::Vector2i p)
+{
+ if (p.x() >= m_metaData.width || p.y() >= m_metaData.height)
+ {
+ return;
+ }
+ unsigned i = p.x() + m_metaData.width * p.y();
+ if ((m_MapPoints[i].CurrentChange == NO_CHANGE ||
+ m_MapPoints[i].CurrentChange == ::FREE) &&
+ m_MapPoints[i].MeasurementCount < USHRT_MAX)
+ {
+ m_MapPoints[i].CurrentChange = ::OCCUPIED;
+ m_MapPoints[i].OccupancyCount++;
+ }
}
-void OccupancyMap::scaleDownCounts(int maxCount) {
- clearChanges();
- if (maxCount <= 0) {
- ROS_WARN(
- "WARNING: argument maxCount is choosen to small, resetting map.");
- memset(m_MeasurementCount, 0, m_ByteSize);
- memset(m_OccupancyCount, 0, m_ByteSize);
- } else {
- for (unsigned i = 0; i < m_ByteSize; i++) {
- int scalingFactor = m_MeasurementCount[i] / maxCount;
- if (scalingFactor != 0) {
- m_MeasurementCount[i] /= scalingFactor;
- m_OccupancyCount[i] /= scalingFactor;
- }
- }
- }
- maximizeChangedRegion();
- applyChanges();
- computeOccupancyProbabilities();
+void OccupancyMap::scaleDownCounts(int maxCount)
+{
+ clearChanges();
+ if (maxCount > 0)
+ {
+ for (unsigned i = 0; i < m_ByteSize; i++)
+ {
+ int scalingFactor = m_MapPoints[i].MeasurementCount / maxCount;
+ if (scalingFactor != 0)
+ {
+ m_MapPoints[i].MeasurementCount /= scalingFactor;
+ m_MapPoints[i].OccupancyCount /= scalingFactor;
+ }
+ }
+ }
+ maximizeChangedRegion();
+ applyChanges();
+ computeOccupancyProbabilities();
}
-void OccupancyMap::markRobotPositionFree() {
- geometry_msgs::Point point;
- point.x = 0;
- point.y = 0;
- point.z = 0;
- geometry_msgs::Point endPosWorld =
- map_tools::transformPoint(point, m_latestMapTransform);
- Eigen::Vector2i robotPixel = map_tools::toMapCoords(
- endPosWorld, m_metaData.origin, m_metaData.resolution);
-
- int width = 0.35 / m_metaData.resolution;
- for (int i = robotPixel.y() - width; i <= robotPixel.y() + width; i++) {
- for (int j = robotPixel.x() - width; j <= robotPixel.x() + width; j++) {
- incrementMeasurementCount(Eigen::Vector2i(j, i));
- }
- }
- Box2D<int> robotBox(robotPixel.x() - width, robotPixel.y() - width,
- robotPixel.x() + width, robotPixel.y() + width);
- m_ChangedRegion.enclose(robotBox);
- m_ExploredRegion.enclose(robotBox);
+void OccupancyMap::markRobotPositionFree()
+{
+ geometry_msgs::Point point;
+ point.x = 0;
+ point.y = 0;
+ point.z = 0;
+ geometry_msgs::Point endPosWorld =
+ map_tools::transformPoint(point, m_latestMapTransform);
+ Eigen::Vector2i robotPixel = map_tools::toMapCoords(
+ endPosWorld, m_metaData.origin, m_metaData.resolution);
+
+ int width = 0.25 / m_metaData.resolution;
+ for (int i = robotPixel.y() - width; i <= robotPixel.y() + width; i++)
+ {
+ for (int j = robotPixel.x() - width; j <= robotPixel.x() + width; j++)
+ {
+ incrementMeasurementCount(Eigen::Vector2i(j, i));
+ }
+ }
+ Box2D<int> robotBox(robotPixel.x() - width, robotPixel.y() - width,
+ robotPixel.x() + width, robotPixel.y() + width);
+ m_ChangedRegion.enclose(robotBox);
+ m_ExploredRegion.enclose(robotBox);
}
QImage OccupancyMap::getProbabilityQImage(int trancparencyThreshold,
- bool showInaccessible) const {
- QImage retImage(m_metaData.width, m_metaData.height, QImage::Format_RGB32);
- for (int y = 0; y < m_metaData.height; y++) {
- for (int x = 0; x < m_metaData.width; x++) {
- int index = x + y * m_metaData.width;
- int value = UNKNOWN;
- if (m_MeasurementCount[index] > 0) {
- value = static_cast<int>((1.0 - m_OccupancyProbability[index]) *
- 255);
- if (m_MeasurementCount[index] < trancparencyThreshold) {
- value = static_cast<int>(
- (0.75 + 0.025 * m_MeasurementCount[index]) *
- (1.0 - m_OccupancyProbability[index]) * 255);
- }
- }
- retImage.setPixel(x, y, qRgb(value, value, value));
+ bool showInaccessible) const
+{
+ QImage retImage(m_metaData.width, m_metaData.height, QImage::Format_RGB32);
+ for (int y = 0; y < m_metaData.height; y++)
+ {
+ for (int x = 0; x < m_metaData.width; x++)
+ {
+ int i = x + y * m_metaData.width;
+ int value = UNKNOWN;
+ if (m_MapPoints[i].MeasurementCount > 0)
+ {
+ value =
+ static_cast<int>((1.0 - m_MapPoints[i].OccupancyProbability) * 255);
+ if (m_MapPoints[i].MeasurementCount < trancparencyThreshold)
+ {
+ value = static_cast<int>(
+ (0.75 + 0.025 * m_MapPoints[i].MeasurementCount) *
+ (1.0 - m_MapPoints[i].OccupancyProbability) * 255);
}
+ }
+ retImage.setPixel(x, y, qRgb(value, value, value));
}
- return retImage;
+ }
+ return retImage;
}
-void OccupancyMap::getOccupancyProbabilityImage(
- vector<int8_t>& data, nav_msgs::MapMetaData& metaData) {
- metaData = m_metaData;
- data.resize(m_metaData.width * m_metaData.height);
- std::fill(data.begin(), data.end(),
- (int8_t)NOT_SEEN_YET); // note: linker error without strange cast
- // from int8_t to int8_t
- for (int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++) {
- int yOffset = m_metaData.width * y;
- for (int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX();
- x++) {
- int i = x + yOffset;
- if (m_MeasurementCount[i] < 1) {
- continue;
- }
- if (m_OccupancyProbability[i] == UNKNOWN_LIKELIHOOD) {
- data[i] = NOT_SEEN_YET;
- } else {
- data[i] = (int)(m_OccupancyProbability[i] *
- 99); // TODO maybe - 2 (or *0.99 or smth)
- }
- }
- }
-}
-
-void OccupancyMap::maximizeChangedRegion() {
- m_ChangedRegion = m_ExploredRegion;
+void OccupancyMap::getOccupancyProbabilityImage(vector<int8_t>& data,
+ nav_msgs::MapMetaData& metaData)
+{
+ metaData = m_metaData;
+ data.resize(m_metaData.width * m_metaData.height);
+ std::fill(data.begin(), data.end(),
+ (int8_t)NOT_SEEN_YET); // note: linker error without strange cast
+ // from int8_t to int8_t
+ for (int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++)
+ {
+ int yOffset = m_metaData.width * y;
+ for (int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++)
+ {
+ int i = x + yOffset;
+ if (m_MapPoints[i].MeasurementCount < 1)
+ {
+ continue;
+ }
+ if (m_MapPoints[i].OccupancyProbability == UNKNOWN_LIKELIHOOD)
+ {
+ data[i] = NOT_SEEN_YET;
+ }
+ else
+ {
+ data[i] = (int)(m_MapPoints[i].OccupancyProbability *
+ 99); // TODO maybe - 2 (or *0.99 or smth)
+ }
+ }
+ }
}
-void OccupancyMap::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- if (msg->data.size() != m_ByteSize) {
- ROS_ERROR_STREAM("Size Mismatch between SLAM map ("
- << m_ByteSize << ") and masking map ("
- << msg->data.size() << ")");
- return;
- }
- for (size_t y = 0; y < msg->info.height; y++) {
- int yOffset = msg->info.width * y;
- for (size_t x = 0; x < msg->info.width; x++) {
- int i = yOffset + x;
-
- switch (msg->data[i]) {
- case homer_mapnav_msgs::ModifyMap::BLOCKED:
- case homer_mapnav_msgs::ModifyMap::OBSTACLE:
- // increase measure count of cells which were not yet
- // visible to be able to modify unknown areas
- if (m_MeasurementCount[i] == 0) m_MeasurementCount[i] = 10;
-
- m_OccupancyCount[i] = m_MeasurementCount[i];
- m_OccupancyProbability[i] = 1.0;
- m_ExploredRegion.enclose(x, y);
- break;
- case homer_mapnav_msgs::ModifyMap::FREE:
- // see comment above
- if (m_MeasurementCount[i] == 0) m_MeasurementCount[i] = 10;
-
- m_OccupancyCount[i] = 0;
- m_OccupancyProbability[i] = 0.0;
- m_ExploredRegion.enclose(x, y);
- break;
- case homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV:
- m_HighSensitive[i] = 1;
- break;
- }
- }
- }
+void OccupancyMap::maximizeChangedRegion()
+{
+ m_ChangedRegion = m_ExploredRegion;
}
-void OccupancyMap::cleanUp() {
- if (m_OccupancyProbability) {
- delete[] m_OccupancyProbability;
- }
- if (m_MeasurementCount) {
- delete[] m_MeasurementCount;
- }
- if (m_OccupancyCount) {
- delete[] m_OccupancyCount;
- }
- if (m_CurrentChanges) {
- delete[] m_CurrentChanges;
- }
- if (m_HighSensitive) {
- delete[] m_HighSensitive;
- }
+void OccupancyMap::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+ if (msg->data.size() != m_ByteSize)
+ {
+ ROS_ERROR_STREAM("Size Mismatch between SLAM map ("
+ << m_ByteSize << ") and masking map (" << msg->data.size()
+ << ")");
+ return;
+ }
+ for (size_t y = 0; y < msg->info.height; y++)
+ {
+ int yOffset = msg->info.width * y;
+ for (size_t x = 0; x < msg->info.width; x++)
+ {
+ int i = yOffset + x;
+
+ switch (msg->data[i])
+ {
+ case homer_mapnav_msgs::ModifyMap::BLOCKED:
+ case homer_mapnav_msgs::ModifyMap::OBSTACLE:
+ // increase measure count of cells which were not yet
+ // visible to be able to modify unknown areas
+ if (m_MapPoints[i].MeasurementCount == 0)
+ m_MapPoints[i].MeasurementCount = 10;
+
+ m_MapPoints[i].OccupancyCount = m_MapPoints[i].MeasurementCount;
+ m_MapPoints[i].OccupancyProbability = 1.0;
+ m_ExploredRegion.enclose(x, y);
+ break;
+ case homer_mapnav_msgs::ModifyMap::FREE:
+ // see comment above
+ if (m_MapPoints[i].MeasurementCount == 0)
+ m_MapPoints[i].MeasurementCount = 10;
+
+ m_MapPoints[i].OccupancyCount = 0;
+ m_MapPoints[i].OccupancyProbability = 0.0;
+ m_ExploredRegion.enclose(x, y);
+ break;
+ case homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV:
+ m_MapPoints[i].HighSensitive = 1;
+ break;
+ }
+ }
+ }
}
diff --git a/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp b/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
index 7265b0ea..e2b1b458 100755
--- a/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
+++ b/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
@@ -88,14 +88,6 @@ void HyperSlamFilter::setMoveJitterWhileTurning ( float mPerDegree )
}
}
-void HyperSlamFilter::setScanMatchingClusterSize ( float minClusterSize )
-{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->setScanMatchingClusterSize( minClusterSize );
- }
-}
-
void HyperSlamFilter::resetHigh()
{
for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
@@ -125,7 +117,7 @@ void HyperSlamFilter::setRobotPose ( Pose pose, double scatterVarXY, double scat
}
}
-void HyperSlamFilter::filter ( Pose currentPose, sensor_msgs::LaserScanConstPtr laserData, ros::Time measurementTime, ros::Duration &filterDuration)
+void HyperSlamFilter::filter ( Transformation2D trans, sensor_msgs::LaserScanConstPtr laserData)
{
//call filter methods of all particle filters
for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
@@ -142,7 +134,7 @@ void HyperSlamFilter::filter ( Pose currentPose, sensor_msgs::LaserScanConstPtr
randOnOff = (rand() % 100) < 80;
}
m_SlamFilters[i]->setMapping( m_DoMapping && randOnOff );
- m_SlamFilters[i]->filter(currentPose, laserData, measurementTime, filterDuration);
+ m_SlamFilters[i]->filter(trans, laserData);
}
if(m_SlamFilters.size() != 1)
diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
index 459bfca4..c77cebe7 100644
--- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
@@ -9,374 +9,323 @@ const float MIN_TURN_DISTANCE2 = 0.1 * 0.01;
const float M_2PI = 2 * M_PI;
SlamFilter::SlamFilter(int particleNum)
- : ParticleFilter<SlamParticle>(particleNum) {
- m_OccupancyMap = new OccupancyMap();
- // generate initial particles
- for (int i = 0; i < m_ParticleNum; i++) {
- m_CurrentList[i] = new SlamParticle();
- m_LastList[i] = new SlamParticle();
- }
-
- float rotationErrorRotating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_rotating",
- rotationErrorRotating);
- float rotationErrorTranslating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_translating",
- rotationErrorTranslating);
- float translationErrorTranslating = 0.0;
- ros::param::get(
- "/particlefilter/error_values/translation_error_translating",
- translationErrorTranslating);
- float translationErrorRotating = 0.0;
- ros::param::get(
- "/particlefilter/error_values/translation_error_translating",
- translationErrorRotating);
- float moveJitterWhileTurning = 0.0;
- ros::param::get("/particlefilter/error_values/move_jitter_while_turning",
- moveJitterWhileTurning);
- ros::param::get("/particlefilter/max_rotation_per_second",
- m_MaxRotationPerSecond);
-
- int updateMinMoveAngleDegrees;
- ros::param::get("/particlefilter/update_min_move_angle",
- updateMinMoveAngleDegrees);
- m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
- ros::param::get("/particlefilter/update_min_move_dist",
- m_UpdateMinMoveDistance);
- double maxUpdateInterval;
- ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
- m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
-
- setRotationErrorRotating(rotationErrorRotating);
- setRotationErrorTranslating(rotationErrorTranslating);
- setTranslationErrorTranslating(translationErrorTranslating);
- setTranslationErrorRotating(translationErrorRotating);
- setMoveJitterWhileTurning(moveJitterWhileTurning);
-
- m_FirstRun = true;
- m_DoMapping = true;
-
- m_EffectiveParticleNum = m_ParticleNum;
- m_LastUpdateTime = ros::Time(0);
- m_LastMoveTime = ros::Time::now();
+ : ParticleFilter<SlamParticle>(particleNum)
+{
+ m_OccupancyMap = new OccupancyMap();
+ // generate initial particles
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ m_CurrentList[i] = new SlamParticle();
+ m_LastList[i] = new SlamParticle();
+ }
+
+ float rotationErrorRotating = 0.0;
+ ros::param::get("/particlefilter/error_values/rotation_error_rotating",
+ rotationErrorRotating);
+ float rotationErrorTranslating = 0.0;
+ ros::param::get("/particlefilter/error_values/rotation_error_translating",
+ rotationErrorTranslating);
+ float translationErrorTranslating = 0.0;
+ ros::param::get("/particlefilter/error_values/translation_error_translating",
+ translationErrorTranslating);
+ float translationErrorRotating = 0.0;
+ ros::param::get("/particlefilter/error_values/translation_error_translating",
+ translationErrorRotating);
+ float moveJitterWhileTurning = 0.0;
+ ros::param::get("/particlefilter/error_values/move_jitter_while_turning",
+ moveJitterWhileTurning);
+
+ setRotationErrorRotating(rotationErrorRotating);
+ setRotationErrorTranslating(rotationErrorTranslating);
+ setTranslationErrorTranslating(translationErrorTranslating);
+ setTranslationErrorRotating(translationErrorRotating);
+ setMoveJitterWhileTurning(moveJitterWhileTurning);
+
+ m_FirstRun = true;
+ m_DoMapping = true;
+
+ m_EffectiveParticleNum = m_ParticleNum;
+ m_LastMoveTime = ros::Time::now();
}
SlamFilter::SlamFilter(SlamFilter& slamFilter)
- : ParticleFilter<SlamParticle>(slamFilter.m_ParticleNum) {
- m_OccupancyMap = new OccupancyMap(*(slamFilter.m_OccupancyMap));
- // generate initial particles
- for (int i = 0; i < m_ParticleNum; i++) {
- if (slamFilter.m_CurrentList[i] == 0) {
- m_CurrentList[i] = 0;
- } else {
- m_CurrentList[i] = new SlamParticle(*(slamFilter.m_CurrentList[i]));
- }
- if (slamFilter.m_LastList[i] == 0) {
- m_LastList[i] = 0;
- } else {
- m_LastList[i] = new SlamParticle(*(slamFilter.m_LastList[i]));
- }
+ : ParticleFilter<SlamParticle>(slamFilter.m_ParticleNum)
+{
+ m_OccupancyMap = new OccupancyMap(*(slamFilter.m_OccupancyMap));
+ // generate initial particles
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ if (slamFilter.m_CurrentList[i] == 0)
+ {
+ m_CurrentList[i] = 0;
}
-
- float rotationErrorRotating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_rotating",
- rotationErrorRotating);
- float rotationErrorTranslating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_translating",
- rotationErrorTranslating);
- float translationErrorTranslating = 0.0;
- ros::param::get(
- "/particlefilter/error_values/translation_error_translating",
- translationErrorTranslating);
- float translationErrorRotating = 0.0;
- ros::param::get(
- "/particlefilter/error_values/translation_error_translating",
- translationErrorRotating);
- float moveJitterWhileTurning = 0.0;
- ros::param::get("/particlefilter/error_values/move_jitter_while_turning",
- moveJitterWhileTurning);
- ros::param::get("/particlefilter/max_rotation_per_second",
- m_MaxRotationPerSecond);
-
- int updateMinMoveAngleDegrees;
- ros::param::get("/particlefilter/update_min_move_angle",
- updateMinMoveAngleDegrees);
- m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
- ros::param::get("/particlefilter/update_min_move_dist",
- m_UpdateMinMoveDistance);
- double maxUpdateInterval;
- ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
- m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
-
- setRotationErrorRotating(rotationErrorRotating);
- setRotationErrorTranslating(rotationErrorTranslating);
- setTranslationErrorTranslating(translationErrorTranslating);
- setTranslationErrorRotating(translationErrorRotating);
- setMoveJitterWhileTurning(moveJitterWhileTurning);
-
- m_FirstRun = slamFilter.m_FirstRun;
- m_DoMapping = slamFilter.m_DoMapping;
-
- m_EffectiveParticleNum = slamFilter.m_EffectiveParticleNum;
-
- m_LastUpdateTime = slamFilter.m_LastUpdateTime;
-
- m_ReferencePoseOdometry = slamFilter.m_ReferencePoseOdometry;
- m_ReferenceMeasurementTime = slamFilter.m_ReferenceMeasurementTime;
+ else
+ {
+ m_CurrentList[i] = new SlamParticle(*(slamFilter.m_CurrentList[i]));
+ }
+ if (slamFilter.m_LastList[i] == 0)
+ {
+ m_LastList[i] = 0;
+ }
+ else
+ {
+ m_LastList[i] = new SlamParticle(*(slamFilter.m_LastList[i]));
+ }
+ }
+
+ float rotationErrorRotating = 0.0;
+ ros::param::get("/particlefilter/error_values/rotation_error_rotating",
+ rotationErrorRotating);
+ float rotationErrorTranslating = 0.0;
+ ros::param::get("/particlefilter/error_values/rotation_error_translating",
+ rotationErrorTranslating);
+ float translationErrorTranslating = 0.0;
+ ros::param::get("/particlefilter/error_values/translation_error_translating",
+ translationErrorTranslating);
+ float translationErrorRotating = 0.0;
+ ros::param::get("/particlefilter/error_values/translation_error_translating",
+ translationErrorRotating);
+ float moveJitterWhileTurning = 0.0;
+ ros::param::get("/particlefilter/error_values/move_jitter_while_turning",
+ moveJitterWhileTurning);
+
+ setRotationErrorRotating(rotationErrorRotating);
+ setRotationErrorTranslating(rotationErrorTranslating);
+ setTranslationErrorTranslating(translationErrorTranslating);
+ setTranslationErrorRotating(translationErrorRotating);
+ setMoveJitterWhileTurning(moveJitterWhileTurning);
+
+ m_FirstRun = slamFilter.m_FirstRun;
+ m_DoMapping = slamFilter.m_DoMapping;
+
+ m_EffectiveParticleNum = slamFilter.m_EffectiveParticleNum;
}
-SlamFilter::~SlamFilter() {
- if (m_OccupancyMap) {
- delete m_OccupancyMap;
+SlamFilter::~SlamFilter()
+{
+ if (m_OccupancyMap)
+ {
+ delete m_OccupancyMap;
+ }
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ if (m_CurrentList[i])
+ {
+ delete m_CurrentList[i];
+ m_CurrentList[i] = 0;
}
- for (int i = 0; i < m_ParticleNum; i++) {
- if (m_CurrentList[i]) {
- delete m_CurrentList[i];
- m_CurrentList[i] = 0;
- }
- if (m_LastList[i]) {
- delete m_LastList[i];
- m_LastList[i] = 0;
- }
+ if (m_LastList[i])
+ {
+ delete m_LastList[i];
+ m_LastList[i] = 0;
}
+ }
}
-void SlamFilter::setRotationErrorRotating(float percent) {
- m_Alpha1 = percent / 100.0;
+void SlamFilter::setRotationErrorRotating(float percent)
+{
+ m_Alpha1 = percent / 100.0;
}
-void SlamFilter::resetHigh() { m_OccupancyMap->resetHighSensitive(); }
-
-void SlamFilter::setRotationErrorTranslating(float degreePerMeter) {
- m_Alpha2 = degreePerMeter / 180.0 * M_PI;
+void SlamFilter::resetHigh()
+{
+ m_OccupancyMap->resetHighSensitive();
}
-void SlamFilter::setTranslationErrorTranslating(float percent) {
- m_Alpha3 = percent / 100.0;
+void SlamFilter::setRotationErrorTranslating(float degreePerMeter)
+{
+ m_Alpha2 = degreePerMeter / 180.0 * M_PI;
}
-void SlamFilter::setTranslationErrorRotating(float mPerDegree) {
- m_Alpha4 = mPerDegree / 180.0 * M_PI;
+void SlamFilter::setTranslationErrorTranslating(float percent)
+{
+ m_Alpha3 = percent / 100.0;
}
-void SlamFilter::setMoveJitterWhileTurning(float mPerDegree) {
- m_Alpha5 = mPerDegree / 180.0 * M_PI;
+void SlamFilter::setTranslationErrorRotating(float mPerDegree)
+{
+ m_Alpha4 = mPerDegree / 180.0 * M_PI;
}
-void SlamFilter::setScanMatchingClusterSize(float minClusterSize) {
- minClusterSize = minClusterSize;
+void SlamFilter::setMoveJitterWhileTurning(float mPerDegree)
+{
+ m_Alpha5 = mPerDegree / 180.0 * M_PI;
}
-void SlamFilter::setMapping(bool doMapping) { m_DoMapping = doMapping; }
+void SlamFilter::setMapping(bool doMapping)
+{
+ m_DoMapping = doMapping;
+}
-void SlamFilter::setOccupancyMap(OccupancyMap* occupancyMap) {
- // delete old
- if (m_OccupancyMap) {
- delete m_OccupancyMap;
- }
- // copy
- m_OccupancyMap = occupancyMap;
+void SlamFilter::setOccupancyMap(OccupancyMap* occupancyMap)
+{
+ // delete old
+ if (m_OccupancyMap)
+ {
+ delete m_OccupancyMap;
+ }
+ // copy
+ m_OccupancyMap = occupancyMap;
}
-vector<Pose>* SlamFilter::getParticlePoses() const {
- vector<Pose>* particlePoses = new vector<Pose>();
- for (int i = 0; i < m_ParticleNum; i++) {
- float robotX, robotY, robotTheta;
- SlamParticle* particle = m_CurrentList[i];
- particle->getRobotPose(robotX, robotY, robotTheta);
- particlePoses->push_back(Pose(robotX, robotY, robotTheta));
- }
- return particlePoses;
+vector<Pose>* SlamFilter::getParticlePoses() const
+{
+ vector<Pose>* particlePoses = new vector<Pose>();
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ particlePoses->push_back(m_CurrentList[i]->getRobotPose());
+ }
+ return particlePoses;
}
-vector<SlamParticle*>* SlamFilter::getParticles() const {
- vector<SlamParticle*>* particles = new vector<SlamParticle*>();
- for (int i = 0; i < m_ParticleNum; i++) {
- SlamParticle* particle = m_CurrentList[i];
- particles->push_back(particle);
- }
- return particles;
+vector<SlamParticle*>* SlamFilter::getParticles() const
+{
+ vector<SlamParticle*>* particles = new vector<SlamParticle*>();
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ SlamParticle* particle = m_CurrentList[i];
+ particles->push_back(particle);
+ }
+ return particles;
}
void SlamFilter::setRobotPose(Pose pose, double scatterVarXY,
- double scatterVarTheta) {
- // set first particle to exact position
- m_CurrentList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
- m_LastList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
- // scatter remaining particles
- for (int i = 1; i < m_ParticleNum; ++i) {
- const double scatterX = randomGauss() * scatterVarXY;
- const double scatterY = randomGauss() * scatterVarXY;
- const double scatterTheta = randomGauss() * scatterVarTheta;
-
- m_CurrentList[i]->setRobotPose(pose.x() + scatterX, pose.y() + scatterY,
- pose.theta() + scatterTheta);
- m_LastList[i]->setRobotPose(pose.x() + scatterX, pose.y() + scatterY,
- pose.theta() + scatterTheta);
- }
+ double scatterVarTheta)
+{
+ // set first particle to exact position
+ m_CurrentList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
+ m_LastList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
+ // scatter remaining particles
+ for (int i = 1; i < m_ParticleNum; ++i)
+ {
+ const double scatterX = randomGauss() * scatterVarXY;
+ const double scatterY = randomGauss() * scatterVarXY;
+ const double scatterTheta = randomGauss() * scatterVarTheta;
+
+ m_CurrentList[i]->setRobotPose(pose.x() + scatterX, pose.y() + scatterY,
+ pose.theta() + scatterTheta);
+ m_LastList[i]->setRobotPose(pose.x() + scatterX, pose.y() + scatterY,
+ pose.theta() + scatterTheta);
+ }
}
-vector<float> SlamFilter::getParticleWeights() const {
- vector<float> particleWeights(m_ParticleNum);
- for (int i = 0; i < m_ParticleNum; i++) {
- particleWeights[i] = m_CurrentList[i]->getWeight();
- }
- return particleWeights;
+vector<float> SlamFilter::getParticleWeights() const
+{
+ vector<float> particleWeights(m_ParticleNum);
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ particleWeights[i] = m_CurrentList[i]->getWeight();
+ }
+ return particleWeights;
}
-double SlamFilter::randomGauss(float variance) const {
- if (variance < 0) {
- variance = -variance;
- }
- double x1, x2, w, y1;
- do {
- x1 = 2.0 * random01() - 1.0;
- x2 = 2.0 * random01() - 1.0;
- w = x1 * x1 + x2 * x2;
- } while (w >= 1.0);
-
- w = sqrt((-2.0 * log(w)) / w);
- y1 = x1 * w;
- // now y1 is uniformly distributed
- return sqrt(variance) * y1;
+double SlamFilter::randomGauss(float variance) const
+{
+ if (variance < 0)
+ {
+ variance = -variance;
+ }
+ double x1, x2, w, y1;
+ do
+ {
+ x1 = 2.0 * random01() - 1.0;
+ x2 = 2.0 * random01() - 1.0;
+ w = x1 * x1 + x2 * x2;
+ } while (w >= 1.0);
+
+ w = sqrt((-2.0 * log(w)) / w);
+ y1 = x1 * w;
+ // now y1 is uniformly distributed
+ return sqrt(variance) * y1;
}
vector<float> SlamFilter::filterOutliers(sensor_msgs::LaserScanConstPtr rawData,
- float maxDiff) {
- if (rawData->ranges.size() < 2) {
- return rawData->ranges;
- }
- vector<float> filteredData = rawData->ranges;
- for (unsigned int i = 1; i < filteredData.size() - 1; i++) {
- if (abs((float)(rawData->ranges[i - 1] - rawData->ranges[i] * 2 +
- rawData->ranges[i + 1])) > maxDiff * 2) {
- filteredData[i] = 0;
- }
- }
- if (fabs(rawData->ranges[0] - rawData->ranges[1]) > maxDiff) {
- filteredData[0] = 0;
- }
- if (fabs(rawData->ranges[rawData->ranges.size() - 1] -
- rawData->ranges[rawData->ranges.size() - 2]) > maxDiff) {
- filteredData[rawData->ranges.size() - 1] = 0;
+ float maxDiff)
+{
+ if (rawData->ranges.size() < 2)
+ {
+ return rawData->ranges;
+ }
+ vector<float> filteredData = rawData->ranges;
+ for (unsigned int i = 1; i < filteredData.size() - 1; i++)
+ {
+ if (abs((float)(rawData->ranges[i - 1] - rawData->ranges[i] * 2 +
+ rawData->ranges[i + 1])) > maxDiff * 2)
+ {
+ filteredData[i] = 0;
}
-
- return filteredData;
+ }
+ if (fabs(rawData->ranges[0] - rawData->ranges[1]) > maxDiff)
+ {
+ filteredData[0] = 0;
+ }
+ if (fabs(rawData->ranges[rawData->ranges.size() - 1] -
+ rawData->ranges[rawData->ranges.size() - 2]) > maxDiff)
+ {
+ filteredData[rawData->ranges.size() - 1] = 0;
+ }
+
+ return filteredData;
}
-void SlamFilter::filter(Pose currentPose,
- sensor_msgs::LaserScanConstPtr laserData,
- ros::Time measurementTime,
- ros::Duration& FilterDuration) {
- // if first run, initialize data
- if (m_FirstRun) {
- m_FirstRun = false;
- // only do mapping, save first pose as reference
- if (m_DoMapping) {
- tf::Transform transform(tf::createQuaternionFromYaw(0.0),
- tf::Vector3(0.0, 0.0, 0.0));
- m_OccupancyMap->insertLaserData(laserData, transform);
- }
- m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(
- *laserData); // copy const ptr to be able to change values; //test
- m_ReferencePoseOdometry = currentPose;
- m_ReferenceMeasurementTime = measurementTime;
-
- resample();
- measure();
- ROS_INFO_STREAM("first run!");
- normalize();
- sort(0, m_ParticleNum - 1);
- return;
- }
- // m_CurrentLaserConfig = laserConf;
- m_CurrentPoseOdometry = currentPose;
- m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(
- *laserData); // copy const ptr to be able to change values
- m_CurrentLaserData->ranges = filterOutliers(laserData, 0.3);
-
- Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
-
- 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() > 0.5)
- // if(false)
+void SlamFilter::filter(Transformation2D trans,
+ sensor_msgs::LaserScanConstPtr laserData)
+{
+ sensor_msgs::LaserScanPtr laserScan =
+ boost::make_shared<sensor_msgs::LaserScan>(
+ *laserData); // copy const ptr to be able to change values
+ laserScan->ranges = filterOutliers(laserData, 0.3);
+
+ if (m_FirstRun)
+ {
+ ROS_INFO_STREAM("first run!");
+ m_FirstRun = false;
+ // only do mapping, save first pose as reference
+ if (m_DoMapping)
{
- ROS_DEBUG_STREAM("Move too small, will not resample.");
- if (m_EffectiveParticleNum < m_ParticleNum / 10) {
- resample();
- ROS_INFO_STREAM("Particles too scattered, resampling.");
- // filter steps
- drift();
- measure();
- normalize();
-
- sort(0, m_ParticleNum - 1);
- }
- } else {
- if (moving) {
- m_LastMoveTime = ros::Time::now();
- }
- resample();
- // filter steps
- drift();
- measure();
- normalize();
-
- sort(0, m_ParticleNum - 1);
+ tf::Transform transform(tf::createQuaternionFromYaw(0.0),
+ tf::Vector3(0.0, 0.0, 0.0));
+ m_OccupancyMap->insertLaserData(laserScan, transform);
}
- Pose likeliestPose = getLikeliestPose(measurementTime); // test
- Transformation2D transSinceLastUpdate = likeliestPose - m_LastUpdatePose;
-
- ostringstream stream;
- stream.precision(2);
- stream << "Transformation since last update: angle="
- << Math::rad2Deg(transSinceLastUpdate.theta())
- << " dist=" << transSinceLastUpdate.magnitude() << "m" << endl;
-
- bool update =
- ((std::fabs(transSinceLastUpdate.theta()) > m_UpdateMinMoveAngle) ||
- (transSinceLastUpdate.magnitude() > m_UpdateMinMoveDistance)) ||
- ((measurementTime - m_LastUpdateTime) > m_MaxUpdateInterval);
-
- if (m_DoMapping && update) {
- stream << "Updating map.";
- double elapsedSeconds =
- (measurementTime - m_ReferenceMeasurementTime).toSec();
- double thetaPerSecond;
- if (elapsedSeconds == 0.0) {
- thetaPerSecond = trans.theta();
- } else {
- thetaPerSecond = trans.theta() / elapsedSeconds;
- }
- 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_DEBUG_STREAM("No mapping performed - variance to high");
- }
- } else {
- stream << "No map update performed.";
- }
- ROS_DEBUG_STREAM(stream.str());
- // safe last used pose and laserdata as reference
+ resample();
+ measure(laserScan);
+ normalize();
+ sort(0, m_ParticleNum - 1);
+
+ m_LastMoveTime = ros::Time::now();
+ return;
+ }
+
+ bool moving = sqr(trans.x()) + sqr(trans.y()) > 0.0000001 ||
+ sqr(trans.theta()) > 0.000001;
+
+ if (moving)
+ {
+ m_LastMoveTime = ros::Time::now();
+ }
- m_ReferencePoseOdometry = m_CurrentPoseOdometry;
- m_ReferenceMeasurementTime = measurementTime;
+ bool particlesScattered = m_EffectiveParticleNum < m_ParticleNum / 10;
+
+ bool justMoved = (ros::Time::now() - m_LastMoveTime).toSec() < 0.5;
+
+ if (justMoved || particlesScattered)
+ {
+ resample();
+ drift(trans);
+ measure(laserScan);
+ normalize();
+ sort(0, m_ParticleNum - 1);
+ }
+
+ if (m_DoMapping && justMoved)
+ {
+ Pose likeliestPose = getLikeliestPose();
+ tf::Transform transform(
+ tf::createQuaternionFromYaw(likeliestPose.theta()),
+ tf::Vector3(likeliestPose.x(), likeliestPose.y(), 0.0));
+ m_OccupancyMap->insertLaserData(laserScan, transform);
+ }
}
/**
@@ -395,265 +344,178 @@ void SlamFilter::filter(Pose currentPose,
* The expected value of the errors are zero.
*/
-void SlamFilter::drift() {
- float rx = m_ReferencePoseOdometry.x();
- float ry = m_ReferencePoseOdometry.y();
- float rt = m_ReferencePoseOdometry.theta();
- float cx = m_CurrentPoseOdometry.x();
- float cy = m_CurrentPoseOdometry.y();
- float ct = m_CurrentPoseOdometry.theta();
-
- Transformation2D odoTrans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
- 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;
- //}
- float distance = sqrt(sqr(odoTrans.x()) + sqr(odoTrans.y()));
- // float deltaRot1, deltaTrans, deltaRot2;
- // if (distance < sqrt(MIN_MOVE_DISTANCE2)) {
- // deltaRot1 = odoTrans.theta();
- // deltaTrans = 0.0;
- // deltaRot2 = 0.0;
- //} else if (backwardMove) {
- // deltaRot1 = atan2(ry - cy, rx - cx) - rt;
- // deltaTrans = -distance;
- // deltaRot2 = ct - rt - deltaRot1;
- //} else {
- // deltaRot1 = atan2(odoTrans.y(), odoTrans.x()) - rt;
- // deltaTrans = distance;
- // deltaRot2 = ct - rt - deltaRot1;
- //}
-
- // while (deltaRot1 >= M_PI) deltaRot1 -= M_2PI;
- // while (deltaRot1 < -M_PI) deltaRot1 += M_2PI;
- // while (deltaRot2 >= M_PI) deltaRot2 -= M_2PI;
- // while (deltaRot2 < -M_PI) deltaRot2 += M_2PI;
-
- // always leave one particle with pure displacement
- SlamParticle* particle = m_CurrentList[0];
+void SlamFilter::drift(Transformation2D odoTrans)
+{
+ SlamParticle* particle = m_CurrentList[0];
+ // get stored pose
+ Pose pose = particle->getRobotPose();
+
+ float deltaX =
+ odoTrans.x() * cos(pose.theta()) - odoTrans.y() * sin(pose.theta());
+ float deltaY =
+ odoTrans.x() * sin(pose.theta()) + odoTrans.y() * cos(pose.theta());
+ float deltaS = std::fabs(deltaX) + std::fabs(deltaY);
+
+ // always leave one particle with pure displacement
+ // move pose
+ float posX = pose.x() + deltaX;
+ float posY = pose.y() + deltaY;
+ float theta = pose.theta() + odoTrans.theta();
+ // save new pose
+ particle->setRobotPose(posX, posY, theta);
+
+ for (int i = 1; i < m_ParticleNum; i++)
+ {
+ SlamParticle* particle = m_CurrentList[i];
// get stored pose
- float robotX, robotY, robotTheta;
- particle->getRobotPose(robotX, robotY, robotTheta);
- Pose pose(robotX, robotY, robotTheta);
- // move pose
- // float 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;
+ Pose pose = particle->getRobotPose();
+
+ float posX =
+ pose.x() + deltaX + randomGauss(m_Alpha3 * std::fabs(deltaX) +
+ m_Alpha5 * std::fabs(odoTrans.theta()));
+ float posY =
+ pose.y() + deltaY + randomGauss(m_Alpha3 * std::fabs(deltaY) +
+ m_Alpha5 * std::fabs(odoTrans.theta()));
+ float theta = pose.theta() + odoTrans.theta() +
+ randomGauss(m_Alpha1 * std::fabs(odoTrans.theta()) +
+ m_Alpha2 * std::fabs(deltaS));
+
// save new pose
- particle->setRobotPose(posX, posY, theta);
+ while (theta > M_PI)
+ theta -= M_2PI;
+ while (theta <= -M_PI)
+ theta += M_2PI;
- 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(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 * 0.5 * 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(2 * m_Alpha1 * fabs(poseRotation)) +
- randomGauss(0.00005);
-
- // save new pose
- while (theta > M_PI) theta -= M_2PI;
- while (theta <= -M_PI) theta += M_2PI;
-
- particle->setRobotPose(posX, posY, theta);
- }
+ particle->setRobotPose(posX, posY, theta);
+ }
}
-void SlamFilter::measure() {
- if (m_OccupancyMap) {
- m_MeasurePoints = m_OccupancyMap->getMeasurePoints(m_CurrentLaserData);
-
- // calculating parallel on 8 threats
- omp_set_num_threads(8);
- int i = 0;
- //#pragma omp parallel for
- for (i = 0; i < m_ParticleNum; i++) {
- SlamParticle* particle = m_CurrentList[i];
- if (!particle) {
- ROS_ERROR_STREAM("ERROR: Particle is NULL-pointer!");
- } else {
- // calculate weights
- float robotX, robotY, robotTheta;
- particle->getRobotPose(robotX, robotY, robotTheta);
- Pose pose(robotX, robotY, robotTheta);
- float weight =
- m_OccupancyMap->computeScore(pose, m_MeasurePoints);
- particle->setWeight(weight);
- }
- }
- }
- m_EffectiveParticleNum = getEffectiveParticleNum2();
-}
+void SlamFilter::measure(sensor_msgs::LaserScanPtr laserData)
+{
+ if (m_OccupancyMap)
+ {
+ std::vector<MeasurePoint> measurePoints =
+ m_OccupancyMap->getMeasurePoints(laserData);
-void SlamFilter::updateMap() {
- m_OccupancyMap->insertLaserData(m_CurrentLaserData, m_latestTransform);
-}
-
-void SlamFilter::printParticles() const {
- cout << endl << "### PARTICLE LIST ###" << endl;
- cout << right << fixed;
- cout.width(5);
- for (int i = 0; i < m_ParticleNum; i++) {
- SlamParticle* p_particle = m_CurrentList[i];
- if (p_particle) {
- float robotX, robotY, robotTheta;
- p_particle->getRobotPose(robotX, robotY, robotTheta);
- cout << "Particle " << i << ": (" << robotX << "," << robotY << ","
- << robotTheta * 180.0 / M_PI << "), weight:\t"
- << p_particle->getWeight() << endl;
- }
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ SlamParticle* particle = m_CurrentList[i];
+ if (particle)
+ {
+ particle->setWeight(m_OccupancyMap->computeScore(
+ particle->getRobotPose(), measurePoints));
+ }
}
- cout << "### END OF LIST ###" << endl;
+ }
+ m_EffectiveParticleNum = getEffectiveParticleNum2();
}
-void SlamFilter::reduceParticleNumber(int newParticleNum) {
- if (newParticleNum < m_ParticleNum) {
- SlamParticle** newCurrentList = new SlamParticle*[newParticleNum];
- SlamParticle** newLastList = new SlamParticle*[newParticleNum];
+void SlamFilter::reduceParticleNumber(int newParticleNum)
+{
+ if (newParticleNum < m_ParticleNum)
+ {
+ SlamParticle** newCurrentList = new SlamParticle*[newParticleNum];
+ SlamParticle** newLastList = new SlamParticle*[newParticleNum];
- for (int i = 0; i < newParticleNum; i++) {
- newCurrentList[i] = m_CurrentList[i];
- newLastList[i] = m_LastList[i];
- }
+ for (int i = 0; i < newParticleNum; i++)
+ {
+ newCurrentList[i] = m_CurrentList[i];
+ newLastList[i] = m_LastList[i];
+ }
- for (int i = newParticleNum + 1; i < m_ParticleNum; i++) {
- delete m_CurrentList[i];
- delete m_LastList[i];
- }
- delete[] m_CurrentList;
- delete[] m_LastList;
+ for (int i = newParticleNum + 1; i < m_ParticleNum; i++)
+ {
+ delete m_CurrentList[i];
+ delete m_LastList[i];
+ }
+ delete[] m_CurrentList;
+ delete[] m_LastList;
- m_CurrentList = newCurrentList;
- m_LastList = newLastList;
+ m_CurrentList = newCurrentList;
+ m_LastList = newLastList;
- m_ParticleNum = newParticleNum;
- normalize();
- }
+ m_ParticleNum = newParticleNum;
+ normalize();
+ }
}
-Pose SlamFilter::getLikeliestPose(ros::Time poseTime) {
- 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) {
- numParticles = 1;
- }
- for (int i = 0; i < numParticles; i++) {
- float robotX, robotY, robotTheta;
- m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
- sumX += robotX;
- sumY += robotY;
- // calculate sum of vectors in unit circle
- sumDirX += cos(robotTheta);
- sumDirY += sin(robotTheta);
- }
- float meanTheta = atan2(sumDirY, sumDirX);
- float meanX = sumX / numParticles;
- float meanY = sumY / numParticles;
- // broadcast transform map -> base_link
- tf::Transform transform(
- tf::createQuaternionFromYaw(meanTheta),
- tf::Vector3(sumX / numParticles, sumY / numParticles, 0.0));
- tf::TransformBroadcaster tfBroadcaster;
- m_latestTransform = transform;
-
- tfBroadcaster.sendTransform(tf::StampedTransform(
- m_latestTransform, poseTime, "/map", "/base_link"));
- return Pose(meanX, meanY, meanTheta);
+Pose SlamFilter::getLikeliestPose()
+{
+ float percentage = 2;
+ float sumX = 0, sumY = 0, sumDirX = 0, sumDirY = 0;
+ int numParticles = static_cast<int>(percentage / 100.0 * m_ParticleNum);
+ if (0 == numParticles)
+ {
+ numParticles = 1;
+ }
+ for (int i = 0; i < numParticles; i++)
+ {
+ Pose pose = m_CurrentList[i]->getRobotPose();
+ sumX += pose.x();
+ sumY += pose.y();
+ // calculate sum of vectors in unit circle
+ sumDirX += cos(pose.theta());
+ sumDirY += sin(pose.theta());
+ }
+ float meanTheta = atan2(sumDirY, sumDirX);
+ float meanX = sumX / numParticles;
+ float meanY = sumY / numParticles;
+ return Pose(meanX, meanY, meanTheta);
}
-OccupancyMap* SlamFilter::getLikeliestMap() const { return m_OccupancyMap; }
+OccupancyMap* SlamFilter::getLikeliestMap() const
+{
+ return m_OccupancyMap;
+}
void SlamFilter::getPoseVariances(int particleNum, float& poseVarianceX,
- float& poseVarianceY, float& poseVarianceT) {
- // the particles of m_CurrentList are sorted by their weights
- if (particleNum > m_ParticleNum || particleNum <= 0) {
- particleNum = m_ParticleNum;
- }
- // calculate average pose
- float averagePoseX = 0;
- float averagePoseY = 0;
- float averagePoseT = 0;
- float robotX = 0.0;
- float robotY = 0.0;
- float robotT = 0.0;
- for (int i = 0; i < particleNum; i++) {
- m_CurrentList[i]->getRobotPose(robotX, robotY, robotT);
- averagePoseX += robotX;
- averagePoseY += robotY;
- averagePoseT += robotT;
- }
- 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, robotT);
- poseVarianceX += (averagePoseX - robotX) * (averagePoseX - robotX);
- poseVarianceY += (averagePoseY - robotY) * (averagePoseY - robotY);
- poseVarianceT += (averagePoseT - robotT) * (averagePoseT - robotT);
- }
- poseVarianceX /= (float)particleNum;
- poseVarianceY /= (float)particleNum;
- poseVarianceT /= (float)particleNum;
+ float& poseVarianceY, float& poseVarianceT)
+{
+ // the particles of m_CurrentList are sorted by their weights
+ if (particleNum > m_ParticleNum || particleNum <= 0)
+ {
+ particleNum = m_ParticleNum;
+ }
+ // calculate average pose
+ float averagePoseX = 0;
+ float averagePoseY = 0;
+ float averagePoseT = 0;
+ float robotX = 0.0;
+ float robotY = 0.0;
+ float robotT = 0.0;
+ for (int i = 0; i < particleNum; i++)
+ {
+ m_CurrentList[i]->getRobotPose(robotX, robotY, robotT);
+ averagePoseX += robotX;
+ averagePoseY += robotY;
+ averagePoseT += robotT;
+ }
+ 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, robotT);
+ poseVarianceX += (averagePoseX - robotX) * (averagePoseX - robotX);
+ poseVarianceY += (averagePoseY - robotY) * (averagePoseY - robotY);
+ poseVarianceT += (averagePoseT - robotT) * (averagePoseT - robotT);
+ }
+ poseVarianceX /= (float)particleNum;
+ poseVarianceY /= (float)particleNum;
+ poseVarianceT /= (float)particleNum;
}
-double SlamFilter::evaluateByContrast() {
- return m_OccupancyMap->evaluateByContrast();
+double SlamFilter::evaluateByContrast()
+{
+ return m_OccupancyMap->evaluateByContrast();
}
-void SlamFilter::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- m_OccupancyMap->applyMasking(msg);
+void SlamFilter::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+ m_OccupancyMap->applyMasking(msg);
}
diff --git a/homer_mapping/src/ParticleFilter/SlamParticle.cpp b/homer_mapping/src/ParticleFilter/SlamParticle.cpp
index c0024dec..3d564cde 100644
--- a/homer_mapping/src/ParticleFilter/SlamParticle.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamParticle.cpp
@@ -22,9 +22,20 @@ void SlamParticle::setRobotPose(float robotX, float robotY, float robotTheta) {
m_RobotOrientation = robotTheta;
}
+void SlamParticle::setRobotPose(Pose pose){
+ m_RobotPositionX = pose.x();
+ m_RobotPositionY = pose.y();
+ m_RobotOrientation = pose.theta();
+}
+
void SlamParticle::getRobotPose(float& robotX, float& robotY, float& robotTheta) {
robotX = m_RobotPositionX;
robotY = m_RobotPositionY;
robotTheta = m_RobotOrientation;
}
+Pose SlamParticle::getRobotPose(){
+ return Pose(m_RobotPositionX, m_RobotPositionY, m_RobotOrientation);
+}
+
+
diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp
index 704b5498..edf5e40d 100644
--- a/homer_mapping/src/slam_node.cpp
+++ b/homer_mapping/src/slam_node.cpp
@@ -1,410 +1,433 @@
#include <homer_mapping/slam_node.h>
-SlamNode::SlamNode(ros::NodeHandle* nh) : m_HyperSlamFilter(0) {
- init();
-
- // subscribe to topics
- m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>(
- "/scan", 1, &SlamNode::callbackLaserScan, this);
- m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>(
- "/odom", 1, &SlamNode::callbackOdometry, this);
- m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>(
- "/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this);
- m_InitialPoseSubscriber =
- nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>(
- "/initialpose", 1, &SlamNode::callbackInitialPose, this);
- m_DoMappingSubscriber = nh->subscribe<std_msgs::Bool>(
- "/homer_mapping/do_mapping", 1, &SlamNode::callbackDoMapping, this);
- m_ResetMapSubscriber = nh->subscribe<std_msgs::Empty>(
- "/map_manager/reset_maps", 1, &SlamNode::callbackResetMap, this);
- m_LoadMapSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
- "/map_manager/loaded_map", 1, &SlamNode::callbackLoadedMap, this);
- m_MaskingSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
- "/map_manager/mask_slam", 1, &SlamNode::callbackMasking, this);
- m_ResetHighSubscriber = nh->subscribe<std_msgs::Empty>(
- "/map_manager/reset_high", 1, &SlamNode::callbackResetHigh, this);
-
- // advertise topics
- m_PoseStampedPublisher =
- nh->advertise<geometry_msgs::PoseStamped>("/pose", 2);
- m_PoseArrayPublisher =
- nh->advertise<geometry_msgs::PoseArray>("/pose_array", 2);
- m_SLAMMapPublisher =
- nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
-
- geometry_msgs::PoseStamped poseMsg;
- poseMsg.header.stamp = ros::Time(0);
- poseMsg.header.frame_id = "map";
- poseMsg.pose.position.x = 0.0;
- poseMsg.pose.position.y = 0.0;
- poseMsg.pose.position.z = 0.0;
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
- geometry_msgs::Quaternion quatMsg;
- tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
- // to
- poseMsg.pose.orientation = quatMsg;
- m_PoseStampedPublisher.publish(poseMsg);
-
- //// //broadcast transform map -> base_link
- tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
- m_tfBroadcaster.sendTransform(tf::StampedTransform(
- transform, poseMsg.header.stamp, "map", "base_link"));
- Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
- tf::getYaw(poseMsg.pose.orientation));
- m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
- m_ScatterVarTheta);
+SlamNode::SlamNode(ros::NodeHandle* nh) : m_HyperSlamFilter(0)
+{
+ init();
+
+ // subscribe to topics
+ m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>(
+ "/scan", 1, &SlamNode::callbackLaserScan, this);
+ m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>(
+ "/odom", 1, &SlamNode::callbackOdometry, this);
+ m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>(
+ "/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this);
+ m_InitialPoseSubscriber =
+ nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>(
+ "/initialpose", 1, &SlamNode::callbackInitialPose, this);
+ m_DoMappingSubscriber = nh->subscribe<std_msgs::Bool>(
+ "/homer_mapping/do_mapping", 1, &SlamNode::callbackDoMapping, this);
+ m_ResetMapSubscriber = nh->subscribe<std_msgs::Empty>(
+ "/map_manager/reset_maps", 1, &SlamNode::callbackResetMap, this);
+ m_LoadMapSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
+ "/map_manager/loaded_map", 1, &SlamNode::callbackLoadedMap, this);
+ m_MaskingSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
+ "/map_manager/mask_slam", 1, &SlamNode::callbackMasking, this);
+ m_ResetHighSubscriber = nh->subscribe<std_msgs::Empty>(
+ "/map_manager/reset_high", 1, &SlamNode::callbackResetHigh, this);
+
+ // advertise topics
+ m_PoseStampedPublisher =
+ nh->advertise<geometry_msgs::PoseStamped>("/pose", 2);
+ m_PoseArrayPublisher =
+ nh->advertise<geometry_msgs::PoseArray>("/pose_array", 2);
+ m_SLAMMapPublisher =
+ nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
+
+ geometry_msgs::PoseStamped poseMsg;
+ poseMsg.header.stamp = ros::Time(0);
+ poseMsg.header.frame_id = "map";
+ poseMsg.pose.position.x = 0.0;
+ poseMsg.pose.position.y = 0.0;
+ poseMsg.pose.position.z = 0.0;
+ tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
+ geometry_msgs::Quaternion quatMsg;
+ tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
+ // to
+ poseMsg.pose.orientation = quatMsg;
+ m_PoseStampedPublisher.publish(poseMsg);
+
+ //// //broadcast transform map -> base_link
+ tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
+ m_tfBroadcaster.sendTransform(tf::StampedTransform(
+ transform, poseMsg.header.stamp, "map", "base_link"));
+ Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
+ tf::getYaw(poseMsg.pose.orientation));
+ m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
+ m_ScatterVarTheta);
}
-void SlamNode::init() {
- double waitTime;
- ros::param::get("/particlefilter/wait_time", waitTime);
- m_WaitDuration = ros::Duration(waitTime);
- ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY);
- ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
-
- m_DoMapping = true;
-
- int particleNum;
- ros::param::get("/particlefilter/particle_num", particleNum);
- int particleFilterNum;
- ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num",
- particleFilterNum);
- m_HyperSlamFilter = new HyperSlamFilter(particleFilterNum, particleNum);
-
- m_ReferenceOdometryTime = ros::Time(0);
- m_LastMapSendTime = ros::Time(0);
- m_LastPositionSendTime = ros::Time(0);
- m_LastMappingTime = ros::Time(0);
- m_ReferenceOdometryTime = ros::Time(0);
-
- m_laser_queue.clear();
- m_odom_queue.clear();
-}
+void SlamNode::init()
+{
+ double waitTime;
+ ros::param::get("/particlefilter/wait_time", waitTime);
+ m_WaitDuration = ros::Duration(waitTime);
+ ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY);
+ ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
-SlamNode::~SlamNode() { delete m_HyperSlamFilter; }
+ m_DoMapping = true;
-void SlamNode::resetMaps() {
- ROS_INFO("Resetting maps..");
+ int particleNum;
+ ros::param::get("/particlefilter/particle_num", particleNum);
+ int particleFilterNum;
+ ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num",
+ particleFilterNum);
+ m_HyperSlamFilter = new HyperSlamFilter(particleFilterNum, particleNum);
- delete m_HyperSlamFilter;
- m_HyperSlamFilter = 0;
+ m_ReferenceOdometryTime = ros::Time(0);
+ m_LastMapSendTime = ros::Time(0);
- init();
- geometry_msgs::PoseStamped poseMsg;
- poseMsg.header.stamp = ros::Time::now();
- poseMsg.header.frame_id = "map";
- poseMsg.pose.position.x = 0.0;
- poseMsg.pose.position.y = 0.0;
- poseMsg.pose.position.z = 0.0;
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
- geometry_msgs::Quaternion quatMsg;
- tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
- // to
- poseMsg.pose.orientation = quatMsg;
- m_PoseStampedPublisher.publish(poseMsg);
-
- m_LastLikeliestPose.set(0.0, 0.0);
- m_LastLikeliestPose.setTheta(0.0f);
-
- //// //broadcast transform map -> base_link
- tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
- m_tfBroadcaster.sendTransform(tf::StampedTransform(
- transform, poseMsg.header.stamp, "map", "base_link"));
- Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
- tf::getYaw(poseMsg.pose.orientation));
- m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
- m_ScatterVarTheta);
-
- // sendMapDataMessage();
+ m_laser_queue.clear();
+ m_odom_queue.clear();
+}
+
+SlamNode::~SlamNode()
+{
+ delete m_HyperSlamFilter;
+}
+
+void SlamNode::resetMaps()
+{
+ ROS_INFO("Resetting maps..");
+
+ delete m_HyperSlamFilter;
+ m_HyperSlamFilter = 0;
+
+ init();
+ geometry_msgs::PoseStamped poseMsg;
+ poseMsg.header.stamp = ros::Time::now();
+ poseMsg.header.frame_id = "map";
+ poseMsg.pose.position.x = 0.0;
+ poseMsg.pose.position.y = 0.0;
+ poseMsg.pose.position.z = 0.0;
+ tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
+ geometry_msgs::Quaternion quatMsg;
+ tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
+ // to
+ poseMsg.pose.orientation = quatMsg;
+ m_PoseStampedPublisher.publish(poseMsg);
+
+ m_LastLikeliestPose.set(0.0, 0.0);
+ m_LastLikeliestPose.setTheta(0.0f);
+
+ //// //broadcast transform map -> base_link
+ tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
+ m_tfBroadcaster.sendTransform(tf::StampedTransform(
+ transform, poseMsg.header.stamp, "map", "base_link"));
+ Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
+ tf::getYaw(poseMsg.pose.orientation));
+ m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
+ m_ScatterVarTheta);
+
+ // sendMapDataMessage();
}
-void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg) {
- m_HyperSlamFilter->resetHigh();
+void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg)
+{
+ m_HyperSlamFilter->resetHigh();
}
-void SlamNode::sendMapDataMessage(ros::Time mapTime) {
- std::vector<int8_t> mapData;
- nav_msgs::MapMetaData metaData;
+void SlamNode::sendMapDataMessage(ros::Time mapTime)
+{
+ std::vector<int8_t> mapData;
+ nav_msgs::MapMetaData metaData;
- OccupancyMap* occMap =
- m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
- occMap->getOccupancyProbabilityImage(mapData, metaData);
+ OccupancyMap* occMap =
+ m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
+ occMap->getOccupancyProbabilityImage(mapData, metaData);
- nav_msgs::OccupancyGrid mapMsg;
- std_msgs::Header header;
- header.stamp = mapTime;
- header.frame_id = "map";
- mapMsg.header = header;
- mapMsg.info = metaData;
- mapMsg.data = mapData;
+ nav_msgs::OccupancyGrid mapMsg;
+ std_msgs::Header header;
+ header.stamp = mapTime;
+ header.frame_id = "map";
+ mapMsg.header = header;
+ mapMsg.info = metaData;
+ mapMsg.data = mapData;
- m_SLAMMapPublisher.publish(mapMsg);
+ m_SLAMMapPublisher.publish(mapMsg);
}
-void SlamNode::callbackUserDefPose(const geometry_msgs::Pose::ConstPtr& msg) {
- Pose userdef_pose(msg->position.x, msg->position.y,
- tf::getYaw(msg->orientation));
- m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
- m_ScatterVarTheta);
+void SlamNode::callbackUserDefPose(const geometry_msgs::Pose::ConstPtr& msg)
+{
+ Pose userdef_pose(msg->position.x, msg->position.y,
+ tf::getYaw(msg->orientation));
+ m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
+ m_ScatterVarTheta);
}
void SlamNode::callbackInitialPose(
- const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) {
- Pose userdef_pose(msg->pose.pose.position.x, msg->pose.pose.position.y,
- tf::getYaw(msg->pose.pose.orientation));
- m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
- m_ScatterVarTheta);
+ const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
+{
+ Pose userdef_pose(msg->pose.pose.position.x, msg->pose.pose.position.y,
+ tf::getYaw(msg->pose.pose.orientation));
+ m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
+ m_ScatterVarTheta);
}
-void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg) {
- m_laser_queue.push_back(msg);
- if (m_laser_queue.size() > 5) // Todo param
- {
- m_laser_queue.erase(m_laser_queue.begin());
- }
+void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg)
+{
+ m_laser_queue.push_back(msg);
+ if (m_laser_queue.size() > 5) // Todo param
+ {
+ m_laser_queue.erase(m_laser_queue.begin());
+ }
}
-void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg) {
- m_odom_queue.push_back(msg);
- static int last_i = 0;
- if (m_odom_queue.size() > 5) // Todo param
+void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
+{
+ m_odom_queue.push_back(msg);
+ static int last_i = 0;
+ if (m_odom_queue.size() > 5) // Todo param
+ {
+ last_i--;
+ if (last_i < 0)
{
- last_i--;
- if (last_i < 0) {
- last_i = 0;
- }
- m_odom_queue.erase(m_odom_queue.begin());
+ last_i = 0;
}
+ m_odom_queue.erase(m_odom_queue.begin());
+ }
- static Pose last_interpolatedPose(0.0, 0.0, 0.0);
- ros::Time currentOdometryTime = msg->header.stamp;
- if ((ros::Time::now() - m_LastMappingTime > m_WaitDuration) &&
- m_odom_queue.size() > 1 && m_laser_queue.size() > 0) {
- int i, j;
- bool got_match = false;
-
- for (i = m_odom_queue.size() - 1; i > 0; i--) {
- // Check if we would repeat a calculation which is already done but
- // still in the queue
- if (m_odom_queue.at(i - 1)->header.stamp ==
- m_ReferenceOdometryTime) {
- break;
- }
-
- for (j = m_laser_queue.size() - 1; j > -1; j--) {
- // find a laserscan in between two odometry readings (or at
- // the same time)
- if ((m_laser_queue.at(j)->header.stamp >=
- m_odom_queue.at(i - 1)->header.stamp) &&
- (m_odom_queue.at(i)->header.stamp >=
- m_laser_queue.at(j)->header.stamp)) {
- got_match = true;
- break;
- }
- }
- if (got_match) {
- break;
- }
- }
+ static Pose last_interpolatedPose(0.0, 0.0, 0.0);
+ ros::Time currentOdometryTime = msg->header.stamp;
+ if (m_odom_queue.size() > 1 && m_laser_queue.size() > 0)
+ {
+ int i, j;
+ bool got_match = false;
- if (got_match) {
- last_i = i;
- m_LastLaserData = m_laser_queue.at(j);
- m_LastMappingTime = m_LastLaserData->header.stamp;
-
- float odoX = m_odom_queue.at(i)->pose.pose.position.x;
- float odoY = m_odom_queue.at(i)->pose.pose.position.y;
- float odoTheta =
- tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation);
- Pose currentOdometryPose(odoX, odoY, odoTheta);
- currentOdometryTime = m_odom_queue.at(i)->header.stamp;
-
- odoX = m_odom_queue.at(i - 1)->pose.pose.position.x;
- odoY = m_odom_queue.at(i - 1)->pose.pose.position.y;
- odoTheta =
- tf::getYaw(m_odom_queue.at(i - 1)->pose.pose.orientation);
- Pose lastOdometryPose(odoX, odoY, odoTheta);
- m_ReferenceOdometryPose = lastOdometryPose;
- m_ReferenceOdometryTime = m_odom_queue.at(i - 1)->header.stamp;
-
- ros::Time startTime = ros::Time::now();
- // laserscan in between current odometry reading and
- // m_ReferenceOdometry
- // -> calculate pose of robot during laser scan
- ros::Duration d1 =
- m_LastLaserData->header.stamp - m_ReferenceOdometryTime;
- ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime;
-
- float timeFactor;
- if (d1.toSec() == 0.0) {
- timeFactor = 0.0f;
- } else if (d2.toSec() == 0.0) {
- timeFactor = 1.0f;
- } else {
- timeFactor = d1.toSec() / d2.toSec();
- }
- ros::Duration duration = ros::Duration(0);
-
- last_interpolatedPose = m_ReferenceOdometryPose.interpolate(
- currentOdometryPose, timeFactor);
- m_HyperSlamFilter->filter(last_interpolatedPose, m_LastLaserData,
- m_LastLaserData->header.stamp, duration);
- ros::Time finishTime = ros::Time::now();
-
- m_LastLikeliestPose =
- m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose(
- m_LastLaserData->header.stamp);
-
- // TODO löschen
- m_LastPositionSendTime = m_LastLaserData->header.stamp;
- // send map max. every 500 ms
- if ((m_LastLaserData->header.stamp - m_LastMapSendTime).toSec() >
- 0.5) {
- sendMapDataMessage(m_LastLaserData->header.stamp);
- m_LastMapSendTime = m_LastLaserData->header.stamp;
- }
-
- ROS_DEBUG_STREAM("Pos. data calc time: "
- << (finishTime - startTime).toSec() << "s");
- ROS_DEBUG_STREAM("Map send Interval: "
- << (finishTime - m_LastPositionSendTime).toSec()
- << "s");
+ for (i = m_odom_queue.size() - 1; i > 0; i--)
+ {
+ // Check if we would repeat a calculation which is already done but
+ // still in the queue
+ if (m_odom_queue.at(i - 1)->header.stamp == m_ReferenceOdometryTime)
+ {
+ break;
+ }
+
+ for (j = m_laser_queue.size() - 1; j > -1; j--)
+ {
+ // find a laserscan in between two odometry readings (or at
+ // the same time)
+ if ((m_laser_queue.at(j)->header.stamp >=
+ m_odom_queue.at(i - 1)->header.stamp) &&
+ (m_odom_queue.at(i)->header.stamp >=
+ m_laser_queue.at(j)->header.stamp))
+ {
+ got_match = true;
+ break;
}
+ }
+ if (got_match)
+ {
+ break;
+ }
}
- Pose currentPose = m_LastLikeliestPose;
- // currentPose.setX(currentPose.x() - last_interpolatedPose.x());
- // currentPose.setY(currentPose.y() - last_interpolatedPose.y());
- // currentPose.setTheta(currentPose.theta() -
- // last_interpolatedPose.theta());
- for (int i = (last_i - 1 < 0 ? 0 : last_i - 1); i < m_odom_queue.size();
- i++) {
- currentPose.setX(currentPose.x() +
- m_odom_queue.at(i)->twist.twist.linear.x);
- currentPose.setY(currentPose.y() +
- m_odom_queue.at(i)->twist.twist.linear.y);
- currentPose.setTheta(currentPose.theta() +
- m_odom_queue.at(i)->twist.twist.angular.z);
- }
-
- geometry_msgs::PoseStamped poseMsg;
- // header
- poseMsg.header.stamp = msg->header.stamp;
- poseMsg.header.frame_id = "map";
-
- // position and orientation
- poseMsg.pose.position.x = currentPose.x();
- poseMsg.pose.position.y = currentPose.y();
- poseMsg.pose.position.z = 0.0;
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta());
- geometry_msgs::Quaternion quatMsg;
- tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
- // to geometry_msgs::Quaternion
- poseMsg.pose.orientation = quatMsg;
- m_PoseStampedPublisher.publish(poseMsg);
- 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);
+ if (got_match)
+ {
+ last_i = i;
+ sensor_msgs::LaserScan::ConstPtr laserData = m_laser_queue.at(j);
+
+ float odoX = m_odom_queue.at(i)->pose.pose.position.x;
+ float odoY = m_odom_queue.at(i)->pose.pose.position.y;
+ float odoTheta = tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation);
+ Pose currentOdometryPose(odoX, odoY, odoTheta);
+ currentOdometryTime = m_odom_queue.at(i)->header.stamp;
+
+ odoX = m_odom_queue.at(i - 1)->pose.pose.position.x;
+ odoY = m_odom_queue.at(i - 1)->pose.pose.position.y;
+ odoTheta = tf::getYaw(m_odom_queue.at(i - 1)->pose.pose.orientation);
+ Pose lastOdometryPose(odoX, odoY, odoTheta);
+ m_ReferenceOdometryPose = lastOdometryPose;
+ m_ReferenceOdometryTime = m_odom_queue.at(i - 1)->header.stamp;
+
+ // laserscan in between current odometry reading and
+ // m_ReferenceOdometry
+ // -> calculate pose of robot during laser scan
+ ros::Duration d1 =
+ laserData->header.stamp - m_ReferenceOdometryTime;
+ ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime;
+
+ float timeFactor;
+ if (d1.toSec() == 0.0)
+ {
+ timeFactor = 0.0f;
+ }
+ else if (d2.toSec() == 0.0)
+ {
+ timeFactor = 1.0f;
+ }
+ else
+ {
+ timeFactor = d1.toSec() / d2.toSec();
+ }
+
+ last_interpolatedPose =
+ m_ReferenceOdometryPose.interpolate(currentOdometryPose, timeFactor);
+ Transformation2D trans = last_interpolatedPose - m_lastUsedPose;
+
+ float x = trans.x() * cos(-last_interpolatedPose.theta()) -
+ trans.y() * sin(-last_interpolatedPose.theta());
+ float y = trans.y() * cos(-last_interpolatedPose.theta()) +
+ trans.x() * sin(-last_interpolatedPose.theta());
+
+ Transformation2D newTrans(x, y, trans.theta());
+
+ // SLAM STEP
+ m_HyperSlamFilter->filter(newTrans, laserData);
+
+ m_lastUsedPose = last_interpolatedPose;
+ m_LastLikeliestPose =
+ m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose();
+
+ // broadcast transform map -> base_link
+ tf::Transform transform(
+ tf::createQuaternionFromYaw(m_LastLikeliestPose.theta()),
+ tf::Vector3(m_LastLikeliestPose.x(), m_LastLikeliestPose.y(), 0.0));
+ tf::TransformBroadcaster tfBroadcaster;
+
+ tfBroadcaster.sendTransform(tf::StampedTransform(
+ transform, laserData->header.stamp, "/map", "/base_link"));
+
+ // send map max. every 500 ms
+ if ((laserData->header.stamp - m_LastMapSendTime).toSec() > 0.5)
+ {
+ sendMapDataMessage(laserData->header.stamp);
+ m_LastMapSendTime = laserData->header.stamp;
+ }
}
- delete poses;
-
- m_PoseArrayPublisher.publish(poseArray);
-
- // broadcast transform map -> base_link
- // tf::Transform transform(quatTF,tf::Vector3(currentPose.x(),
- // currentPose.y(), 0.0));
- // m_tfBroadcaster.sendTransform(tf::StampedTransform(transform,
- // msg->header.stamp, "map", "base_link"));
+ }
+ Pose currentPose = m_LastLikeliestPose;
+ // currentPose.setX(currentPose.x() - last_interpolatedPose.x());
+ // currentPose.setY(currentPose.y() - last_interpolatedPose.y());
+ // currentPose.setTheta(currentPose.theta() -
+ // last_interpolatedPose.theta());
+
+ // Add up Transformations
+ // for (int i = (last_i - 1 < 0 ? 0 : last_i - 1); i < m_odom_queue.size();
+ // i++)
+ //{
+ // currentPose.setX(currentPose.x() +
+ // m_odom_queue.at(i)->twist.twist.linear.x);
+ // currentPose.setY(currentPose.y() +
+ // m_odom_queue.at(i)->twist.twist.linear.y);
+ // currentPose.setTheta(currentPose.theta() +
+ // m_odom_queue.at(i)->twist.twist.angular.z);
+ //}
+
+ // Pose Publishing
+ geometry_msgs::PoseStamped poseMsg;
+ poseMsg.header.stamp = msg->header.stamp;
+ poseMsg.header.frame_id = "map";
+ poseMsg.pose.position.x = currentPose.x();
+ poseMsg.pose.position.y = currentPose.y();
+ poseMsg.pose.position.z = 0.0;
+ tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta());
+ geometry_msgs::Quaternion quatMsg;
+ tf::quaternionTFToMsg(quatTF, quatMsg);
+ poseMsg.pose.orientation = quatMsg;
+ m_PoseStampedPublisher.publish(poseMsg);
+
+ // Pose Array publishing
+ 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);
+ tmpPose.orientation = quatMsg;
+ poseArray.poses.push_back(tmpPose);
+ }
+ delete poses;
+ m_PoseArrayPublisher.publish(poseArray);
}
-void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr& msg) {
- m_DoMapping = msg->data;
- m_HyperSlamFilter->setMapping(m_DoMapping);
- ROS_INFO_STREAM("Do mapping is set to " << (m_DoMapping));
+void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr& msg)
+{
+ m_DoMapping = msg->data;
+ m_HyperSlamFilter->setMapping(m_DoMapping);
+ ROS_INFO_STREAM("Do mapping is set to " << (m_DoMapping));
}
-void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr& msg) {
- resetMaps();
+void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr& msg)
+{
+ resetMaps();
}
-void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- float res = msg->info.resolution;
- int height = msg->info.height; // cell size
- int width = msg->info.width; // cell size
- // if(height!=width) {
- // ROS_ERROR("Height != width in loaded map");
- // return;
- //}
-
- // convert map vector from ros format to robbie probability array
- float* map = new float[msg->data.size()];
- // generate exploredRegion
- int minX = INT_MIN;
- int minY = INT_MIN;
- int maxX = INT_MAX;
- int maxY = INT_MAX;
- for (size_t y = 0; y < msg->info.height; y++) {
- int yOffset = msg->info.width * y;
- for (size_t x = 0; x < msg->info.width; x++) {
- int i = yOffset + x;
- if (msg->data[i] == -1)
- map[i] = 0.5;
- else
- map[i] = msg->data[i] / 100.0;
-
- if (map[i] != 0.5) {
- if (minX == INT_MIN || minX > (int)x) minX = (int)x;
- if (minY == INT_MIN || minY > (int)y) minY = (int)y;
- if (maxX == INT_MAX || maxX < (int)x) maxX = (int)x;
- if (maxY == INT_MAX || maxY < (int)y) maxY = (int)y;
- }
- }
+void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+ float res = msg->info.resolution;
+ int height = msg->info.height; // cell size
+ int width = msg->info.width; // cell size
+ // if(height!=width) {
+ // ROS_ERROR("Height != width in loaded map");
+ // return;
+ //}
+
+ // convert map vector from ros format to robbie probability array
+ float* map = new float[msg->data.size()];
+ // generate exploredRegion
+ int minX = INT_MIN;
+ int minY = INT_MIN;
+ int maxX = INT_MAX;
+ int maxY = INT_MAX;
+ for (size_t y = 0; y < msg->info.height; y++)
+ {
+ int yOffset = msg->info.width * y;
+ for (size_t x = 0; x < msg->info.width; x++)
+ {
+ int i = yOffset + x;
+ if (msg->data[i] == -1)
+ map[i] = 0.5;
+ else
+ map[i] = msg->data[i] / 100.0;
+
+ if (map[i] != 0.5)
+ {
+ if (minX == INT_MIN || minX > (int)x)
+ minX = (int)x;
+ if (minY == INT_MIN || minY > (int)y)
+ minY = (int)y;
+ if (maxX == INT_MAX || maxX < (int)x)
+ maxX = (int)x;
+ if (maxY == INT_MAX || maxY < (int)y)
+ maxY = (int)y;
+ }
}
- Box2D<int> exploredRegion = Box2D<int>(minX, minY, maxX, maxY);
- OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width,
- height, exploredRegion);
- m_HyperSlamFilter->setOccupancyMap(occMap);
- m_HyperSlamFilter->setMapping(
- false); // is this already done by gui message?
- ROS_INFO_STREAM("Replacing occupancy map");
+ }
+ Box2D<int> exploredRegion = Box2D<int>(minX, minY, maxX, maxY);
+ OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width,
+ height, exploredRegion);
+ m_HyperSlamFilter->setOccupancyMap(occMap);
+ m_HyperSlamFilter->setMapping(false); // is this already done by gui message?
+ ROS_INFO_STREAM("Replacing occupancy map");
}
-void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- m_HyperSlamFilter->applyMasking(msg);
+void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+ m_HyperSlamFilter->applyMasking(msg);
}
/**
* @brief main function
*/
-int main(int argc, char** argv) {
- ros::init(argc, argv, "homer_mapping");
- ros::NodeHandle nh;
-
- SlamNode slamNode(&nh);
-
- ros::Rate loop_rate(160);
- while (ros::ok()) {
- ros::spinOnce();
- loop_rate.sleep();
- }
- return 0;
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "homer_mapping");
+ ros::NodeHandle nh;
+
+ SlamNode slamNode(&nh);
+
+ ros::Rate loop_rate(160);
+ while (ros::ok())
+ {
+ ros::spinOnce();
+ loop_rate.sleep();
+ }
+ return 0;
}
--
GitLab