diff --git a/homer_nav_libs/include/homer_nav_libs/Explorer/Explorer.h b/homer_nav_libs/include/homer_nav_libs/Explorer/Explorer.h
index 2d34991270d1f94d5fcac6e24417744cd1ed25de..d39dd791a9000330ffe3a88f0126233e7f768ffb 100644
--- a/homer_nav_libs/include/homer_nav_libs/Explorer/Explorer.h
+++ b/homer_nav_libs/include/homer_nav_libs/Explorer/Explorer.h
@@ -1,23 +1,22 @@
#ifndef EXPLORER_H
#define EXPLORER_H
-#include <vector>
+#include <geometry_msgs/Pose.h>
#include <cmath>
#include <iostream>
#include <queue>
#include <sstream>
-#include <geometry_msgs/Pose.h>
+#include <vector>
#include <homer_nav_libs/Explorer/GridMap.h>
#include <homer_nav_libs/tools.h>
-namespace ExplorerConstants
-{
- static int8_t UNKNOWN;
- static const int8_t NOT_SEEN_YET = -1;
- static const double MAX_DISTANCE = DBL_MAX;
- static const double MAX_COST = DBL_MAX;
- static const int OBSTACLE = INT_MAX;
+namespace ExplorerConstants {
+static int8_t UNKNOWN;
+static const int8_t NOT_SEEN_YET = -1;
+static const double MAX_DISTANCE = DBL_MAX;
+static const double MAX_COST = DBL_MAX;
+static const int OBSTACLE = INT_MAX;
}
/**
@@ -45,136 +44,167 @@ namespace ExplorerConstants
*
* This class uses a couple of "maps" for computation and storing data:
*
- * - m_OccupancyMap stores the occupancy probabilities in double values. A value of 100 means
+ * - m_OccupancyMap stores the occupancy probabilities in double values. A value
+ * of 100 means
* totally occupied, 0 totally free.
- * - m_ObstacleDistanceMap stores in each cell the distance (one unit = one cell) to the nearest obstacle.
- * This map is computed by an eucledian distance transformation from m_OccupancyMap.
- * - m_FrontierMap is a bool map which has 1 in frontier cells and 0 in all others. A frontier
- * is defined as a free cell that has one of its four direct neighbours in unknown space and is "safe" for
+ * - m_ObstacleDistanceMap stores in each cell the distance (one unit = one
+ * cell) to the nearest obstacle.
+ * This map is computed by an eucledian distance transformation from
+ * m_OccupancyMap.
+ * - m_FrontierMap is a bool map which has 1 in frontier cells and 0 in all
+ * others. A frontier
+ * is defined as a free cell that has one of its four direct neighbours in
+ * unknown space and is "safe" for
* the robot (m_ObstacleDistanceMap is used for that).
- * - m_DrivingDistanceMap is a double map that stores for each cell the distance to m_Start. It is computed
- * by a flood-fill (seed-fill) algorithm. The values are therefor only an approximation and not exact.
- * m_DrivingDistanceMap is used to search the nearest frontier when requesting an auto target.
- * - m_TargetMap is a double map that stores for each cell the distance to m_Target. It is computed
- * like m_DrivingDistanceMap. This map is used as heuristic for the A*-Pathfinding algorithm.
- * - m_NavigationMap is used to mark the cells that are touched by the A*-Pathfinding algorithm.
+ * - m_DrivingDistanceMap is a double map that stores for each cell the distance
+ * to m_Start. It is computed
+ * by a flood-fill (seed-fill) algorithm. The values are therefor only an
+ * approximation and not exact.
+ * m_DrivingDistanceMap is used to search the nearest frontier when
+ * requesting an auto target.
+ * - m_TargetMap is a double map that stores for each cell the distance to
+ * m_Target. It is computed
+ * like m_DrivingDistanceMap. This map is used as heuristic for the
+ * A*-Pathfinding algorithm.
+ * - m_NavigationMap is used to mark the cells that are touched by the
+ * A*-Pathfinding algorithm.
*
*
- * The coordinate system and units that are used in this class are based on map cells.
+ * The coordinate system and units that are used in this class are based on map
+ * cells.
* @see GridMap
*
*/
-class Explorer
-{
-
- public:
-
+class Explorer {
+ public:
/**
* @brief Default constructor.
- * @param minAllowedObstacleDistance,maxAllowedObstacleDistance Range of allowed distances to next obstacle [Pixels]
- * @param minSafeObstacleDistance,maxSafeObstacleDistance Range of distances to next obstacle considered as safe [Pixels]
+ * @param minAllowedObstacleDistance,maxAllowedObstacleDistance Range of
+ * allowed distances to next obstacle [Pixels]
+ * @param minSafeObstacleDistance,maxSafeObstacleDistance Range of distances
+ * to next obstacle considered as safe [Pixels]
* @param safePathWeight Weight for safer path
*/
- Explorer ( double minAllowedObstacleDistance, double maxAllowedObstacleDistance,
- double minSafeObstacleDistance, double maxSafeObstacleDistance,
- double safePathWeight, double frontierSafenessFactor=1.0, int unknownThreshold=50 );
+ Explorer(double minAllowedObstacleDistance,
+ double maxAllowedObstacleDistance, double minSafeObstacleDistance,
+ double maxSafeObstacleDistance, double safePathWeight,
+ double frontierSafenessFactor = 1.0, int unknownThreshold = 50);
/**
- * @brief Destructor deletes all dynamically allocated memory used by the maps
+ * @brief Destructor deletes all dynamically allocated memory used by the
+ * maps
*/
~Explorer();
void setUnknownThreshold(int unknownTresh);
- void setAllowedObstacleDistance ( double min, double max );
- void setSafeObstacleDistance ( double min, double max );
- void setFrontierSafenessFactor ( double frontierSafenessFactor );
- void setSafePathWeight ( double weight );
+ void setAllowedObstacleDistance(double min, double max);
+ void setSafeObstacleDistance(double min, double max);
+ void setFrontierSafenessFactor(double frontierSafenessFactor);
+ void setSafePathWeight(double weight);
/**
* @brief Copies and sets the occupancy map.
* @param width Width of the map
* @param height Height of the map
* @param origin Real-world pose of the cell (0,0) in the map
- * @param data GridMap-data (occupancy probabilities: 0 = free, 100 = occupied) of size width * height
+ * @param data GridMap-data (occupancy probabilities: 0 = free, 100 =
+ * occupied) of size width * height
*/
- void setOccupancyMap ( int width, int height, geometry_msgs::Pose origin, int8_t* mapData);
+ void setOccupancyMap(int width, int height, geometry_msgs::Pose origin,
+ int8_t* mapData);
+ void setOccupancyMap(const nav_msgs::OccupancyGrid::ConstPtr& cmap);
/** only update occupied areas in current occupancy map */
- void updateObstacles ( int width, int height, geometry_msgs::Pose origin, int8_t* mapData );
+ void updateObstacles(int width, int height, geometry_msgs::Pose origin,
+ int8_t* mapData);
/**
* @brief Sets the start position for the path finding algorithm.
* m_Start is set to the given value.
* If startPixel lies outside the map, m_Start remains untouched.
- * @param startPixel Start position for path finding in pixel (map-) coordinates.
+ * @param startPixel Start position for path finding in pixel (map-)
+ * coordinates.
*/
- void setStart ( Eigen::Vector2i start );
+ void setStart(Eigen::Vector2i start);
/**
* @brief Resets the internal state of the exploration mode.
- * Sets m_DesiredDistance to 0, such that getExplorationTransformPath() triggers
- * a frontier exploration if there is no prior call of setTarget(point, distance).
+ * Sets m_DesiredDistance to 0, such that getExplorationTransformPath()
+ * triggers
+ * a frontier exploration if there is no prior call of setTarget(point,
+ * distance).
* Call this method once before every exploration.
*/
void resetExploration();
/**
- * Sets the target position for path finding. m_Target is set to the given value.
+ * Sets the target position for path finding. m_Target is set to the given
+ * value.
* If endPixel lies outside of the map, m_Target remains untouched.
* computeTargetDistanceMap() is called at the end of this method. m
* @param targetPixel Target to reach from startPixel
*/
- void setTarget ( Eigen::Vector2i targetPixel );
+ void setTarget(Eigen::Vector2i targetPixel);
/**
- * Sets the target region for path finding. m_ExplorationMap is set to the given region.
+ * Sets the target region for path finding. m_ExplorationMap is set to the
+ * given region.
* If targetPixel lies outside of the map, the exploration map is set empty.
* @param targetPixel Center of the target region to reach from startPixel
* @param radius Radius of the target region in pixels
*/
- void setTarget( Eigen::Vector2i targetPixel, int radius );
+ void setTarget(Eigen::Vector2i targetPixel, int radius);
/**
- * @brief find the nearest position to target that is approachble from the start position
+ * @brief find the nearest position to target that is approachble from the
+ * start position
*/
- Eigen::Vector2i getNearestAccessibleTarget ( Eigen::Vector2i target );
+ Eigen::Vector2i getNearestAccessibleTarget(Eigen::Vector2i target);
/**
- * @brief find the nearest position to target surpassing the minimum obstacle distance
+ * @brief find the nearest position to target surpassing the minimum
+ * obstacle distance
*/
- Eigen::Vector2i getNearestWalkablePoint ( Eigen::Vector2i target );
+ Eigen::Vector2i getNearestWalkablePoint(Eigen::Vector2i target);
/**
* @brief Returns the map-coordinates of the nearest frontier to m_Start.
- * Uses m_DrivingDistanceMap and m_ObstacleDistanceMap. If there is no frontier left,
+ * Uses m_DrivingDistanceMap and m_ObstacleDistanceMap. If there is no
+ * frontier left,
* nextFrontier remains untouched.
* @param[out] nextFrontier Nearest frontier in map-coordinates.
- * @return true if frontier found and stored in nextFrontier, false if no frontier found (nextFrontier
+ * @return true if frontier found and stored in nextFrontier, false if no
+ * frontier found (nextFrontier
* remains untouched).
*/
- bool getNearestFrontier ( Eigen::Vector2i& nextFrontier );
+ bool getNearestFrontier(Eigen::Vector2i& nextFrontier);
/**
* Computes the path from m_Start to m_Target with path transform.
- * The result is returned. If the returned vector contains no elements, there is no path.
+ * The result is returned. If the returned vector contains no elements,
+ * there is no path.
* @return vector with path points
*/
- std::vector<Eigen::Vector2i> getPath( bool &success );
+ std::vector<Eigen::Vector2i> getPath(bool& success);
/**
- * Computes the path from m_Start to the next frontier using exploration transform.
- * The result is returned. If the returned vector contains no elements, there is no path.
+ * Computes the path from m_Start to the next frontier using exploration
+ * transform.
+ * The result is returned. If the returned vector contains no elements,
+ * there is no path.
* @return vector with path points
*/
- std::vector<Eigen::Vector2i> getExplorationTransformPath( bool &success );
+ std::vector<Eigen::Vector2i> getExplorationTransformPath(bool& success);
/**
* @brief Returns a version of the path that contains less vertices.
* @note The nearer the next obstacle, the more waypoints are created.
* @param path List of vertices to be simplified
- * @param treshold[0..1] a lower threshold results in more waypoints (default:1.0)
+ * @param treshold[0..1] a lower threshold results in more waypoints
+ * (default:1.0)
* @return Vector of (sampled) waypoints.
*/
- std::vector<Eigen::Vector2i> sampleWaypointsFromPath ( std::vector<Eigen::Vector2i> path, float threshold=1.0 );
+ std::vector<Eigen::Vector2i> sampleWaypointsFromPath(
+ std::vector<Eigen::Vector2i> path, float threshold = 1.0);
/**
* Getters for the different transforms (see constructor for description)
@@ -198,50 +228,49 @@ class Explorer
*/
Eigen::Vector2i getTarget() const;
- private:
-
+ private:
/** @brief Delete the given map and set pointer to 0 */
template <class T>
- void releaseMap ( GridMap<T>*& map )
- {
- if ( map )
- {
- delete map;
- map=0;
- }
+ void releaseMap(GridMap<T>*& map) {
+ if (map) {
+ delete map;
+ map = 0;
+ }
}
/** @brief Delete and re-create given map */
template <class T>
- void resetMap ( GridMap<T>*& map )
- {
- if ( !m_OccupancyMap )
- {
- ROS_ERROR ( "Occupancy map is missing." );
- return;
- }
- releaseMap ( map );
- map = new GridMap<T> ( m_OccupancyMap->width(), m_OccupancyMap->height() );
+ void resetMap(GridMap<T>*& map) {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
+ releaseMap(map);
+ map = new GridMap<T>(m_OccupancyMap->width(), m_OccupancyMap->height());
}
/**
- * @return true if the robot can stand on the given position without touching an obstacle, false otherwise
+ * @return true if the robot can stand on the given position without
+ * touching an obstacle, false otherwise
* @warning Call computeWalkableMaps before
*/
- inline bool isWalkable ( int x, int y ) const
- {
- return ( ( m_OccupancyMap->getValue ( x, y ) <= ExplorerConstants::UNKNOWN ) &&
- ( m_ObstacleTransform->getValue ( x, y ) > m_MinAllowedObstacleDistance ) );
+ inline bool isWalkable(int x, int y) const {
+ return (
+ (m_OccupancyMap->getValue(x, y) <= ExplorerConstants::UNKNOWN) &&
+ (m_ObstacleTransform->getValue(x, y) >
+ m_MinAllowedObstacleDistance));
}
/**
- * @return true if point is approachable from the current start position, false otherwise.
- * @warning m_OccupancyMap, m_ObstacleTransform and m_DrivingDistanceTransform have to be present!
+ * @return true if point is approachable from the current start position,
+ * false otherwise.
+ * @warning m_OccupancyMap, m_ObstacleTransform and
+ * m_DrivingDistanceTransform have to be present!
* @warning Call computeApproachableMaps before
*/
- inline bool isApproachable ( int x, int y ) const
- {
- return ( m_DrivingDistanceTransform->getValue ( x, y ) < ExplorerConstants::MAX_DISTANCE );
+ inline bool isApproachable(int x, int y) const {
+ return (m_DrivingDistanceTransform->getValue(x, y) <
+ ExplorerConstants::MAX_DISTANCE);
}
/** @brief Releases all memory of the member maps */
@@ -253,17 +282,21 @@ class Explorer
* @param n Number of elements in f
* @return Distance transformation of f
*/
- double* distanceTransform1D ( double *f, int n );
+ double* distanceTransform1D(double* f, int n);
/**
- * @brief Fills the given map from given start point with distance values to this point.
- * The filling will only be performed on cells that are marked as free in m_OccupancyMap and
- * that have an obstacle distance value between m_MinimumObstacleDistance and m_MaximumObstacleDistance.
- * The map that is passed as argument will be fully overwritten by this function.
+ * @brief Fills the given map from given start point with distance values to
+ * this point.
+ * The filling will only be performed on cells that are marked as free in
+ * m_OccupancyMap and
+ * that have an obstacle distance value between m_MinimumObstacleDistance
+ * and m_MaximumObstacleDistance.
+ * The map that is passed as argument will be fully overwritten by this
+ * function.
* @param map GridMap to fill
* @param start Start point for the fill algorithm
*/
- void distanceFloodFill ( GridMap<double>* map, Eigen::Vector2i start );
+ void distanceFloodFill(GridMap<double>* map, Eigen::Vector2i start);
/** @brief Compute map needed for path calculation */
void computePathTransform();
@@ -271,19 +304,23 @@ class Explorer
/** @brief Compute map needed for exploration path calculation */
void computeExplorationTransform();
- /** @brief Compute the distances to the next obstacle with eucledian distance transform from m_OccupancyMap. */
+ /** @brief Compute the distances to the next obstacle with eucledian
+ * distance transform from m_OccupancyMap. */
void computeObstacleTransform();
/** @brief Compute cost function based on obstacle transform */
void computeCostTransform();
- /** @brief Compute the frontiers between free and unknown space. Depends on OccupancyMap and ObstacleTransform. */
+ /** @brief Compute the frontiers between free and unknown space. Depends on
+ * OccupancyMap and ObstacleTransform. */
void computeFrontierMap();
- /** @brief Compute the target region (a circle of radius m_DesiredDistance around m_Target). */
+ /** @brief Compute the target region (a circle of radius m_DesiredDistance
+ * around m_Target). */
void computeRegionMap();
- /** @brief Compute the target map, which is either a frontier map or a region map. */
+ /** @brief Compute the target map, which is either a frontier map or a
+ * region map. */
void computeTargetMap();
/** @brief Compute a map of driving distances from the start point */
@@ -344,7 +381,8 @@ class Explorer
double m_SafePathWeight;
/**
- * Factor for minObstacleDistance that determines if a frontier pixel is valid
+ * Factor for minObstacleDistance that determines if a frontier pixel is
+ * valid
*/
double m_FrontierSafenessFactor;
@@ -352,8 +390,6 @@ class Explorer
* Real-world pose of the point (0,0) in the map
*/
geometry_msgs::Pose m_Origin;
-
};
#endif
-
diff --git a/homer_nav_libs/include/homer_nav_libs/Explorer/GridMap.h b/homer_nav_libs/include/homer_nav_libs/Explorer/GridMap.h
index 64b3469eb331afcb9ff24067564fa1ff479ce234..5ce49ae7aee3a74365671f5ed829197deafffc33 100644
--- a/homer_nav_libs/include/homer_nav_libs/Explorer/GridMap.h
+++ b/homer_nav_libs/include/homer_nav_libs/Explorer/GridMap.h
@@ -1,5 +1,5 @@
-#ifndef GridMap_H
-#define GridMap_H
+#ifndef GridMap_H
+#define GridMap_H
#include <float.h>
#include <iostream>
@@ -14,560 +14,513 @@
/**
* @class GridMap
* @author Malte Knauf, Stephan Wirth, David Gossow (RX)
- * @brief GridMap data structure. Implemeted as template class. The template type
+ * @brief GridMap data structure. Implemeted as template class. The template
+ * type
* defines the data type of each map cell.
*/
-template<class DataT>
-class GridMap
-{
-
- public:
-
- /// Initialize empty map
- GridMap();
-
- /**
- * @param width Width of the map.
- * @param height Height of the map.
- * @param data Pointer to map data, must be of size width*height.
- * @param copyData if true, the map data will be copied
- * if false, GridMap takes ownership of the pointer
- * @param cellSize physical size of each map cell [m]
- * @param centerX,centerY center of the map in world coordinates
- */
- GridMap ( int width, int height, DataT* data = 0, bool copyData = true, float cellSize = 1, float centerX = 0, float centerY = 0 );
-
- /// Copy data from given region
- GridMap ( int width, int height, DataT* data, Eigen::AlignedBox2i extractRegion );
-
- /// Copy data from given map
- GridMap<DataT> ( const GridMap<DataT>& other ) { m_Data=0; *this = other; }
-
- /// Copy data from given map
- GridMap<DataT>& operator= ( const GridMap<DataT>& other );
-
- ~GridMap();
+template <class DataT>
+class GridMap {
+ public:
+ /// Initialize empty map
+ GridMap();
+
+ /**
+ * @param width Width of the map.
+ * @param height Height of the map.
+ * @param data Pointer to map data, must be of size width*height.
+ * @param copyData if true, the map data will be copied
+ * if false, GridMap takes ownership of the pointer
+* @param cellSize physical size of each map cell [m]
+ * @param centerX,centerY center of the map in world coordinates
+ */
+ GridMap(int width, int height, DataT* data = 0, bool copyData = true,
+ float cellSize = 1, float centerX = 0, float centerY = 0);
+
+ /// Copy data from given region
+ GridMap(int width, int height, DataT* data,
+ Eigen::AlignedBox2i extractRegion);
+
+ /// Copy data from given map
+ GridMap<DataT>(const GridMap<DataT>& other) {
+ m_Data = 0;
+ *this = other;
+ }
- /// Convert map coordinates to world coordinates
- void mapToWorld ( int mapX, int mapY, float& worldX, float& worldY );
+ /// Copy data from given map
+ GridMap<DataT>& operator=(const GridMap<DataT>& other);
- /// Convert world coordinates to map coordinates
- void worldToMap ( float worldX, float worldY, int& mapX, int& mapY );
+ ~GridMap();
- /// @brief set value at given position
- inline void setValue ( int x, int y, DataT val );
+ /// Convert map coordinates to world coordinates
+ void mapToWorld(int mapX, int mapY, float& worldX, float& worldY);
- /// @brief replace content with given value
- void fill ( DataT val );
-
- /// @brief Draw a filled polygon into the map (world coords)
- void drawPolygon ( std::vector<Eigen::Vector2d> vertices, DataT value );
+ /// Convert world coordinates to map coordinates
+ void worldToMap(float worldX, float worldY, int& mapX, int& mapY);
- /// @brief Draw a filled circle into the map (world coords)
- void drawCircle( Eigen::Vector2d center, float radius, DataT value );
+ /// @brief set value at given position
+ inline void setValue(int x, int y, DataT val);
- /// @return Value at the given position.
- inline DataT getValue ( int x, int y ) const;
+ /// @brief replace content with given value
+ void fill(DataT val);
- /// @return Pointer to given pixel
- inline DataT* getDirectAccess ( int x, int y );
+ /// @brief Draw a filled polygon into the map (world coords)
+ void drawPolygon(std::vector<Eigen::Vector2d> vertices, DataT value);
- /// @return width in grid cells
- int width() const { return m_Width; }
+ /// @brief Draw a filled circle into the map (world coords)
+ void drawCircle(Eigen::Vector2d center, float radius, DataT value);
- /// @return height in grid cells
- int height() const { return m_Height; }
+ /// @return Value at the given position.
+ inline DataT getValue(int x, int y) const;
- /// @return center of the map in world coordinates
- Eigen::Vector2d center() const {return Eigen::Vector2d(m_CenterX,m_CenterY);}
+ /// @return Pointer to given pixel
+ inline DataT* getDirectAccess(int x, int y);
- /// @return side length of one cell in mm
- float cellSize() { return m_CellSize; }
+ /// @return width in grid cells
+ int width() const { return m_Width; }
- private:
+ /// @return height in grid cells
+ int height() const { return m_Height; }
- void drawLine ( DataT *data, int startX, int startY, int endX, int endY, DataT value );
- void fillPolygon ( DataT* data, int x, int y, char value );
+ /// @return center of the map in world coordinates
+ Eigen::Vector2d center() const {
+ return Eigen::Vector2d(m_CenterX, m_CenterY);
+ }
- int m_Width;
- int m_Height;
- int m_DataSize;
- DataT* m_Data;
- float m_CellSize;
- float m_CenterX;
- float m_CenterY;
+ /// @return side length of one cell in mm
+ float cellSize() { return m_CellSize; }
+
+ private:
+ void drawLine(DataT* data, int startX, int startY, int endX, int endY,
+ DataT value);
+ void fillPolygon(DataT* data, int x, int y, char value);
+
+ int m_Width;
+ int m_Height;
+ int m_DataSize;
+ DataT* m_Data;
+ float m_CellSize;
+ float m_CenterX;
+ float m_CenterY;
};
-
-template<class DataT>
-GridMap<DataT>::GridMap()
-{
- m_Width = 0;
- m_Height = 0;
- m_DataSize = 0;
- m_Data = 0;
- m_CellSize = 0;
- m_CenterX = 0;
- m_CenterY = 0;
+template <class DataT>
+GridMap<DataT>::GridMap() {
+ m_Width = 0;
+ m_Height = 0;
+ m_DataSize = 0;
+ m_Data = 0;
+ m_CellSize = 0;
+ m_CenterX = 0;
+ m_CenterY = 0;
}
-template<class DataT>
-GridMap<DataT>::GridMap ( int width, int height, DataT* data, bool copyData, float cellSize, float centerX, float centerY )
-{
- m_Width = width;
- m_Height = height;
- m_CellSize = cellSize;
- m_DataSize = width * height;
- m_CenterX = centerX;
- m_CenterY = centerY;
- m_Data = 0;
-
- if ( data )
- {
- if ( copyData )
- {
- m_Data = new DataT[m_DataSize];
-
- for ( int i = 0; i < m_DataSize; i++ )
- {
- m_Data[i] = data[i];
- }
- }
- else
- {
- m_Data = data;
- }
- }
- else
- {
- m_Data = new DataT[m_DataSize];
-
- for ( int i = 0; i < m_DataSize; i++ )
- {
- m_Data[i] = 0;
- }
- }
+template <class DataT>
+GridMap<DataT>::GridMap(int width, int height, DataT* data, bool copyData,
+ float cellSize, float centerX, float centerY) {
+ m_Width = width;
+ m_Height = height;
+ m_CellSize = cellSize;
+ m_DataSize = width * height;
+ m_CenterX = centerX;
+ m_CenterY = centerY;
+ m_Data = 0;
+
+ if (data) {
+ if (copyData) {
+ m_Data = new DataT[m_DataSize];
+
+ for (int i = 0; i < m_DataSize; i++) {
+ m_Data[i] = data[i];
+ }
+ } else {
+ m_Data = data;
+ }
+ } else {
+ m_Data = new DataT[m_DataSize];
+
+ for (int i = 0; i < m_DataSize; i++) {
+ m_Data[i] = 0;
+ }
+ }
}
-template<class DataT>
-GridMap<DataT>::GridMap (int width, int height, DataT* data, Eigen::AlignedBox2i extractRegion )
-{
+template <class DataT>
+GridMap<DataT>::GridMap(int width, int height, DataT* data,
+ Eigen::AlignedBox2i extractRegion) {
m_Width = extractRegion.sizes().x();
m_Height = extractRegion.sizes().y();
- m_DataSize = m_Width * m_Height;
- m_Data = new DataT[m_DataSize];
- m_CellSize = 1;
- m_CenterX = 0;
- m_CenterY = 0;
-
- for ( int y = extractRegion.min().y(); y <= extractRegion.max().y(); y++ )
- {
+ m_DataSize = m_Width * m_Height;
+ m_Data = new DataT[m_DataSize];
+ m_CellSize = 1;
+ m_CenterX = 0;
+ m_CenterY = 0;
+
+ for (int y = extractRegion.min().y(); y <= extractRegion.max().y(); y++) {
int yOffset = m_Width * y;
- for ( int x = extractRegion.min().x(); x <= extractRegion.max().x(); x++ )
- {
+ for (int x = extractRegion.min().x(); x <= extractRegion.max().x();
+ x++) {
int i = x + yOffset;
- m_Data[i] = data[i];
- }
- }
+ m_Data[i] = data[i];
+ }
+ }
}
-
-template<class DataT>
-inline DataT* GridMap<DataT>::getDirectAccess ( int x, int y )
-{
+template <class DataT>
+inline DataT* GridMap<DataT>::getDirectAccess(int x, int y) {
#ifdef GRIDMAP_SAFE_ACCESS
- if ( x >= 0 && x < m_Width && y >= 0 && y < m_Height )
- {
+ if (x >= 0 && x < m_Width && y >= 0 && y < m_Height) {
return &m_Data[y * m_Width + x];
- }
- else
- {
- throw;
- }
+ } else {
+ throw;
+ }
#else
return &m_Data[y * m_Width + x];
#endif
}
-
-template<class DataT>
-GridMap<DataT>& GridMap<DataT>::operator= ( const GridMap<DataT>& other )
-{
- delete[] m_Data;
- m_Width = other.m_Width;
- m_Height = other.m_Height;
- m_DataSize = other.m_DataSize;
- m_Data = new DataT[m_DataSize];
- memcpy ( m_Data, other.m_Data, sizeof ( DataT ) *m_DataSize );
- m_CellSize = other.m_CellSize;
- m_CenterX = other.m_CenterX;
- m_CenterY = other.m_CenterY;
- return *this;
+template <class DataT>
+GridMap<DataT>& GridMap<DataT>::operator=(const GridMap<DataT>& other) {
+ delete[] m_Data;
+ m_Width = other.m_Width;
+ m_Height = other.m_Height;
+ m_DataSize = other.m_DataSize;
+ m_Data = new DataT[m_DataSize];
+ memcpy(m_Data, other.m_Data, sizeof(DataT) * m_DataSize);
+ m_CellSize = other.m_CellSize;
+ m_CenterX = other.m_CenterX;
+ m_CenterY = other.m_CenterY;
+ return *this;
}
/* TODO
template<class DataT>
GridMap<DataT>::GridMap ( ExtendedInStream& strm )
{
- short version;
- strm >> version;
- strm >> m_Width;
- strm >> m_Height;
- strm >> m_CellSize;
- strm >> m_CenterX;
- strm >> m_CenterY;
- m_DataSize = m_Width * m_Height;
- m_Data = new DataT[m_DataSize];
- strm.get ( m_Data, m_DataSize );
+ short version;
+ strm >> version;
+ strm >> m_Width;
+ strm >> m_Height;
+ strm >> m_CellSize;
+ strm >> m_CenterX;
+ strm >> m_CenterY;
+ m_DataSize = m_Width * m_Height;
+ m_Data = new DataT[m_DataSize];
+ strm.get ( m_Data, m_DataSize );
}
*/
-template<class DataT>
-GridMap<DataT>::~GridMap()
-{
- if ( m_Data )
- {
- delete m_Data;
- m_Data = 0;
- }
+template <class DataT>
+GridMap<DataT>::~GridMap() {
+ if (m_Data) {
+ delete m_Data;
+ m_Data = 0;
+ }
}
/*
template<class DataT>
void GridMap<DataT>::storer ( ExtendedOutStream& strm ) const
{
- strm << short ( 12 );
- strm << m_Width;
- strm << m_Height;
- strm << m_CellSize;
- strm << m_CenterX;
- strm << m_CenterY;
- strm.put ( m_Data, m_DataSize );
+ strm << short ( 12 );
+ strm << m_Width;
+ strm << m_Height;
+ strm << m_CellSize;
+ strm << m_CenterX;
+ strm << m_CenterY;
+ strm.put ( m_Data, m_DataSize );
}
*/
-template<class DataT>
-void GridMap<DataT>::mapToWorld ( int mapX, int mapY, float& worldX, float& worldY )
-{
- worldX = m_CenterX + m_CellSize * ( mapX - m_Width / 2 );
- worldY = m_CenterY + m_CellSize * ( mapY - m_Height / 2 );
+template <class DataT>
+void GridMap<DataT>::mapToWorld(int mapX, int mapY, float& worldX,
+ float& worldY) {
+ worldX = m_CenterX + m_CellSize * (mapX - m_Width / 2);
+ worldY = m_CenterY + m_CellSize * (mapY - m_Height / 2);
}
-template<class DataT>
-void GridMap<DataT>::worldToMap ( float worldX, float worldY, int& mapX, int& mapY )
-{
- mapX = float ( m_Width ) / 2.0 - ( ( worldY - m_CenterY ) / m_CellSize + 0.5 );
- mapY = float ( m_Height ) / 2.0 - ( ( worldX - m_CenterX ) / m_CellSize + 0.5 );
-
- if ( mapX < 0 || mapX >= m_Width || mapY < 0 || mapY >= m_Height )
- {
- //ROS_WARN_STREAM ( "Index out of bounds: " << mapX << "," << mapY ); //TODO
-
- if ( mapX < 0 )
- {
- mapX = 0;
- }
-
- if ( mapX >= m_Width )
- {
- mapX = m_Width - 1;
- }
-
- if ( mapY < 0 )
- {
- mapY = 0;
- }
-
- if ( mapY >= m_Height )
- {
- mapY = m_Height - 1;
- }
- }
-}
+template <class DataT>
+void GridMap<DataT>::worldToMap(float worldX, float worldY, int& mapX,
+ int& mapY) {
+ mapX = float(m_Width) / 2.0 - ((worldY - m_CenterY) / m_CellSize + 0.5);
+ mapY = float(m_Height) / 2.0 - ((worldX - m_CenterX) / m_CellSize + 0.5);
+ if (mapX < 0 || mapX >= m_Width || mapY < 0 || mapY >= m_Height) {
+ // ROS_WARN_STREAM ( "Index out of bounds: " << mapX << "," << mapY );
+ // //TODO
-template<class DataT>
-inline void GridMap<DataT>::setValue ( int x, int y, DataT val )
-{
+ if (mapX < 0) {
+ mapX = 0;
+ }
+
+ if (mapX >= m_Width) {
+ mapX = m_Width - 1;
+ }
+
+ if (mapY < 0) {
+ mapY = 0;
+ }
+
+ if (mapY >= m_Height) {
+ mapY = m_Height - 1;
+ }
+ }
+}
+
+template <class DataT>
+inline void GridMap<DataT>::setValue(int x, int y, DataT val) {
#ifdef GRIDMAP_SAFE_ACCESS
- if ( x >= 0 && x < m_Width && y >= 0 && y < m_Height )
- {
+ if (x >= 0 && x < m_Width && y >= 0 && y < m_Height) {
m_Data[y * m_Width + x] = val;
- }
- else
- {
- throw;
- }
+ } else {
+ throw;
+ }
#else
m_Data[y * m_Width + x] = val;
#endif
}
-template<class DataT>
-inline DataT GridMap<DataT>::getValue ( int x, int y ) const
-{
+template <class DataT>
+inline DataT GridMap<DataT>::getValue(int x, int y) const {
#ifdef GRIDMAP_SAFE_ACCESS
- if ( x >= 0 && x < m_Width && y >= 0 && y < m_Height )
- {
- return m_Data[y * m_Width + x];
- }
- else
- {
- ROS_ERROR_STREAM( "Accessing map pixels " << x << "," << y << ": out of bounds (0,0," << m_Width-1 << "," << m_Height-1 << ")" ); //TODO
- throw;
- }
+ if (x >= 0 && x < m_Width && y >= 0 && y < m_Height) {
+ return m_Data[y * m_Width + x];
+ } else {
+ ROS_ERROR_STREAM("Accessing map pixels "
+ << x << "," << y << ": out of bounds (0,0,"
+ << m_Width - 1 << "," << m_Height - 1 << ")"); // TODO
+ throw;
+ }
#else
- return m_Data[y * m_Width + x];
+ return m_Data[y * m_Width + x];
#endif
}
-template<class DataT>
-void GridMap<DataT>::fill ( DataT val )
-{
- for ( int i = 0; i < m_DataSize; i++ )
- {
- m_Data[i] = val;
- }
+template <class DataT>
+void GridMap<DataT>::fill(DataT val) {
+ for (int i = 0; i < m_DataSize; i++) {
+ m_Data[i] = val;
+ }
}
/* TODO do we need image representation?
template<class DataT>
-puma2::ColorImageRGB8* GridMap<DataT>::getImage ( DataT specialValue, DataT clipRangeLow, DataT clipRangeHigh )
+puma2::ColorImageRGB8* GridMap<DataT>::getImage ( DataT specialValue, DataT
+clipRangeLow, DataT clipRangeHigh )
{
- puma2::ColorImageRGB8* image = new puma2::ColorImageRGB8 ( m_Width, m_Height );
- double maxVal = 0.0001;
- double minVal = 0.0;
+ puma2::ColorImageRGB8* image = new puma2::ColorImageRGB8 ( m_Width,
+m_Height );
+ double maxVal = 0.0001;
+ double minVal = 0.0;
- for ( int i = 0; i < m_DataSize; i++ )
+ for ( int i = 0; i < m_DataSize; i++ )
{
- if ( ( m_Data[i] < minVal ) && ( m_Data[i] != specialValue ) )
- {
- minVal = m_Data[i];
- }
-
- if ( ( m_Data[i] > maxVal ) && ( m_Data[i] != specialValue ) )
- {
- maxVal = m_Data[i];
- }
- }
-
- std::ostringstream stream;
-
- stream << " Min: " << minVal << "Max: " << maxVal;
- stream << " ClipMin: " << double ( clipRangeLow ) << " ClipMax: " << double ( clipRangeHigh );
+ if ( ( m_Data[i] < minVal ) && ( m_Data[i] != specialValue ) )
+ {
+ minVal = m_Data[i];
+ }
+
+ if ( ( m_Data[i] > maxVal ) && ( m_Data[i] != specialValue ) )
+ {
+ maxVal = m_Data[i];
+ }
+ }
+
+ std::ostringstream stream;
+
+ stream << " Min: " << minVal << "Max: " << maxVal;
+ stream << " ClipMin: " << double ( clipRangeLow ) << " ClipMax: " <<
+double ( clipRangeHigh );
ROS_DEBUG_STREAM ( stream.str() ); //TODO: was TRACE_SYSTEMINFO
- if ( maxVal > clipRangeHigh )
- {
- maxVal = clipRangeHigh;
- }
-
- if ( minVal < clipRangeLow )
- {
- minVal = clipRangeLow;
- }
-
- double range = maxVal - minVal;
-
- puma2::ColorImageRGB8::PixelType* imageData;
- imageData = image->unsafeRowPointerArray() [0];
-
- for ( int i = 0; i < m_DataSize; i++ )
- {
- DataT currentValue = m_Data[i];
-
- if ( currentValue == specialValue )
- {
- imageData[i][0] = 40;
- imageData[i][1] = 220;
- imageData[i][2] = 120;
- continue;
- }
-
- if ( currentValue > clipRangeHigh )
- {
- imageData[i][0] = 200;
- imageData[i][1] = 200;
- imageData[i][2] = 128;
- continue;
- }
-
- if ( currentValue < clipRangeLow )
- {
- imageData[i][0] = 40;
- imageData[i][1] = 40;
- imageData[i][2] = 180;
- continue;
- }
-
- double valueDouble = ( ( double ) ( currentValue - minVal ) ) / range;
-
- unsigned char value = ( unsigned char ) ( valueDouble * 255 );
-
- imageData[i][0] = value;
- imageData[i][1] = value;
- imageData[i][2] = value;
- }
-
- return image;
+ if ( maxVal > clipRangeHigh )
+ {
+ maxVal = clipRangeHigh;
+ }
+
+ if ( minVal < clipRangeLow )
+ {
+ minVal = clipRangeLow;
+ }
+
+ double range = maxVal - minVal;
+
+ puma2::ColorImageRGB8::PixelType* imageData;
+ imageData = image->unsafeRowPointerArray() [0];
+
+ for ( int i = 0; i < m_DataSize; i++ )
+ {
+ DataT currentValue = m_Data[i];
+
+ if ( currentValue == specialValue )
+ {
+ imageData[i][0] = 40;
+ imageData[i][1] = 220;
+ imageData[i][2] = 120;
+ continue;
+ }
+
+ if ( currentValue > clipRangeHigh )
+ {
+ imageData[i][0] = 200;
+ imageData[i][1] = 200;
+ imageData[i][2] = 128;
+ continue;
+ }
+
+ if ( currentValue < clipRangeLow )
+ {
+ imageData[i][0] = 40;
+ imageData[i][1] = 40;
+ imageData[i][2] = 180;
+ continue;
+ }
+
+ double valueDouble = ( ( double ) ( currentValue - minVal ) ) /
+range;
+
+ unsigned char value = ( unsigned char ) ( valueDouble * 255 );
+
+ imageData[i][0] = value;
+ imageData[i][1] = value;
+ imageData[i][2] = value;
+ }
+
+ return image;
}
*/
-
-
-template<class DataT>
-void GridMap<DataT>::drawCircle(Eigen::Vector2d center, float radius, DataT value )
-{
- int centerMapX,centerMapY;
- worldToMap( center.x(), center.y(), centerMapX, centerMapY );
-
- int radiusCells = radius / m_CellSize;
- int radiusCells2 = radiusCells*radiusCells;
-
- Eigen::AlignedBox2i bBox( Eigen::Vector2i(centerMapX - radiusCells, centerMapY - radiusCells), Eigen::Vector2i(centerMapX + radiusCells, centerMapY + radiusCells) );
- Eigen::AlignedBox2i bBoxGrid( Eigen::Vector2i(0,0), Eigen::Vector2i(m_Width-1,m_Height-1) );
- bBox.clamp( bBoxGrid );
-
- for ( int y = bBox.min().y(); y <= bBox.max().y(); y++ )
- {
- for ( int x = bBox.min().x(); x <= bBox.max().x(); x++ )
- {
- int xC = x-centerMapX;
- int yC = y-centerMapY;
- if ( xC*xC+yC*yC <= radiusCells2 )
- {
- setValue( x, y, value );
- }
- }
- }
+template <class DataT>
+void GridMap<DataT>::drawCircle(Eigen::Vector2d center, float radius,
+ DataT value) {
+ int centerMapX, centerMapY;
+ worldToMap(center.x(), center.y(), centerMapX, centerMapY);
+
+ int radiusCells = radius / m_CellSize;
+ int radiusCells2 = radiusCells * radiusCells;
+
+ Eigen::AlignedBox2i bBox(
+ Eigen::Vector2i(centerMapX - radiusCells, centerMapY - radiusCells),
+ Eigen::Vector2i(centerMapX + radiusCells, centerMapY + radiusCells));
+ Eigen::AlignedBox2i bBoxGrid(Eigen::Vector2i(0, 0),
+ Eigen::Vector2i(m_Width - 1, m_Height - 1));
+ bBox.clamp(bBoxGrid);
+
+ for (int y = bBox.min().y(); y <= bBox.max().y(); y++) {
+ for (int x = bBox.min().x(); x <= bBox.max().x(); x++) {
+ int xC = x - centerMapX;
+ int yC = y - centerMapY;
+ if (xC * xC + yC * yC <= radiusCells2) {
+ setValue(x, y, value);
+ }
+ }
+ }
}
+template <class DataT>
+void GridMap<DataT>::drawPolygon(std::vector<Eigen::Vector2d> vertices,
+ DataT value) {
+ if (vertices.size() == 0) {
+ ROS_INFO("No vertices given!");
+ return;
+ }
+ // make temp. map
+ DataT* data = new DataT[m_DataSize];
+ for (int i = 0; i < m_DataSize; i++) {
+ data[i] = 0;
+ }
-template<class DataT>
-void GridMap<DataT>::drawPolygon (std::vector<Eigen::Vector2d> vertices, DataT value )
-{
- if ( vertices.size() == 0 )
- {
- ROS_INFO( "No vertices given!" );
- return;
- }
- //make temp. map
- DataT* data = new DataT[ m_DataSize ];
- for ( int i = 0; i < m_DataSize; i++ )
- {
- data[i] = 0;
- }
-
- //draw the lines surrounding the polygon
- for ( unsigned int i = 0; i < vertices.size(); i++ )
- {
- int i2 = ( i+1 ) % vertices.size();
- int startX,startY,endX,endY;
- worldToMap( vertices[i].x(), vertices[i].y(), startX, startY );
- worldToMap( vertices[i2].x(), vertices[i2].y(), endX, endY );
- drawLine ( data, startX, startY, endX, endY, 1 );
- }
- //claculate a point in the middle of the polygon
- float midX = 0;
- float midY = 0;
- for ( unsigned int i = 0; i < vertices.size(); i++ )
- {
- midX += vertices[i].x();
- midY += vertices[i].y();
- }
- midX /= vertices.size();
- midY /= vertices.size();
- int midMapX,midMapY;
- worldToMap( midX, midY, midMapX, midMapY );
- //fill polygon
- fillPolygon ( data, midMapX, midMapY, 1 );
-
- //copy polygon to map
- for ( int i = 0; i < m_DataSize; i++ )
- {
- if ( data[i] != 0 )
- {
- m_Data[i] = value;
- }
- }
-
- delete[] data;
+ // draw the lines surrounding the polygon
+ for (unsigned int i = 0; i < vertices.size(); i++) {
+ int i2 = (i + 1) % vertices.size();
+ int startX, startY, endX, endY;
+ worldToMap(vertices[i].x(), vertices[i].y(), startX, startY);
+ worldToMap(vertices[i2].x(), vertices[i2].y(), endX, endY);
+ drawLine(data, startX, startY, endX, endY, 1);
+ }
+ // claculate a point in the middle of the polygon
+ float midX = 0;
+ float midY = 0;
+ for (unsigned int i = 0; i < vertices.size(); i++) {
+ midX += vertices[i].x();
+ midY += vertices[i].y();
+ }
+ midX /= vertices.size();
+ midY /= vertices.size();
+ int midMapX, midMapY;
+ worldToMap(midX, midY, midMapX, midMapY);
+ // fill polygon
+ fillPolygon(data, midMapX, midMapY, 1);
+
+ // copy polygon to map
+ for (int i = 0; i < m_DataSize; i++) {
+ if (data[i] != 0) {
+ m_Data[i] = value;
+ }
+ }
+
+ delete[] data;
}
-template<class DataT>
-void GridMap<DataT>::fillPolygon ( DataT* data, int x, int y, char value )
-{
- int index = x + m_Width * y;
- if ( value != data[index] )
- {
- data[index] = value;
- fillPolygon ( data, x + 1, y, value );
- fillPolygon ( data, x - 1, y, value );
- fillPolygon ( data, x, y + 1, value );
- fillPolygon ( data, x, y - 1, value );
- }
+template <class DataT>
+void GridMap<DataT>::fillPolygon(DataT* data, int x, int y, char value) {
+ int index = x + m_Width * y;
+ if (value != data[index]) {
+ data[index] = value;
+ fillPolygon(data, x + 1, y, value);
+ fillPolygon(data, x - 1, y, value);
+ fillPolygon(data, x, y + 1, value);
+ fillPolygon(data, x, y - 1, value);
+ }
}
+template <class DataT>
+void GridMap<DataT>::drawLine(DataT* data, int startX, int startY, int endX,
+ int endY, DataT value) {
+ // bresenham algorithm
+ int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
+ // compute distances
+ dx = endX - startX;
+ dy = endY - startY;
+
+ // compute increment
+ if (dx < 0) {
+ incx = -1;
+ dx = -dx;
+ } else {
+ incx = dx ? 1 : 0;
+ }
-template<class DataT>
-void GridMap<DataT>::drawLine ( DataT *data, int startX, int startY, int endX, int endY, DataT value )
-{
- //bresenham algorithm
- int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
- // compute distances
- dx = endX - startX;
- dy = endY - startY;
-
- // compute increment
- if ( dx < 0 )
- {
- incx = -1;
- dx = -dx;
- }
- else
- {
- incx = dx ? 1 : 0;
- }
-
- if ( dy < 0 )
- {
- incy = -1;
- dy = -dy;
- }
- else
- {
- incy = dy ? 1 : 0;
- }
-
- // which distance is greater?
- dist = ( dx > dy ) ? dx : dy;
- // initializing
- x = startX;
- y = startY;
- xerr = dx;
- yerr = dy;
-
- // compute cells
- for ( t = 0; t < dist; t++ )
- {
- data[x + m_Width * y] = value;
-
- xerr += dx;
- yerr += dy;
- if ( xerr > dist )
- {
- xerr -= dist;
- x += incx;
+ if (dy < 0) {
+ incy = -1;
+ dy = -dy;
+ } else {
+ incy = dy ? 1 : 0;
}
- if ( yerr > dist )
- {
- yerr -= dist;
- y += incy;
+
+ // which distance is greater?
+ dist = (dx > dy) ? dx : dy;
+ // initializing
+ x = startX;
+ y = startY;
+ xerr = dx;
+ yerr = dy;
+
+ // compute cells
+ for (t = 0; t < dist; t++) {
+ data[x + m_Width * y] = value;
+
+ xerr += dx;
+ yerr += dy;
+ if (xerr > dist) {
+ xerr -= dist;
+ x += incx;
+ }
+ if (yerr > dist) {
+ yerr -= dist;
+ y += incy;
+ }
}
- }
}
-
-
#endif
#ifdef GRIDMAP_SAFE_ACCESS
diff --git a/homer_nav_libs/include/homer_nav_libs/tools.h b/homer_nav_libs/include/homer_nav_libs/tools.h
index 112c22f3fed568bc5b919311d777798ea2d36dbf..e5a30b006d07cc306e2650e7982593350aa836c6 100644
--- a/homer_nav_libs/include/homer_nav_libs/tools.h
+++ b/homer_nav_libs/include/homer_nav_libs/tools.h
@@ -1,357 +1,397 @@
#ifndef TOOLS_H
#define TOOLS_H
-#include <Eigen/Geometry>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_listener.h>
-#include <ros/ros.h>
+#include <Eigen/Geometry>
#include <vector>
/**
* @author Malte Knauf (2014)
- * Convenience functions that are often used in the mapping and navigation process
+ * Convenience functions that are often used in the mapping and navigation
+ * process
*/
-namespace map_tools
-{
-
- /**
- * @brief Converts a point p in world frame /map to the respective cell position in the map
- * @param p Point in world frame
- * @param origin Origin of the map
- * @param resolution Resolution of the map
- * @return Cell position of the point
- */
- Eigen::Vector2i toMapCoords(geometry_msgs::Point p, geometry_msgs::Pose origin, float resolution)
- {
- int x_idx = (p.x - origin.position.x)/resolution + 0.51;
- int y_idx = (p.y - origin.position.y)/resolution + 0.51;
- Eigen::Vector2i ret(x_idx, y_idx);
- return ret;
- }
+namespace map_tools {
- /**
- * @brief Converts the cell position of a point to its respective position in the world frame
- * @param idx Cell position of the point
- * @param origin Origin of the map
- * @param resolution Resolution of the map
- * @return Point in world frame
- */
- geometry_msgs::Point fromMapCoords(Eigen::Vector2i idx, geometry_msgs::Pose origin, float resolution)
- {
- geometry_msgs::Point ret;
- ret.x = origin.position.x + (idx.x() - 0.5) * resolution;
- ret.y = origin.position.y + (idx.y() - 0.5) * resolution;
- return ret;
- }
+/**
+ * @brief Converts a point p in world frame /map to the respective cell position
+ * in the map
+ * @param p Point in world frame
+ * @param origin Origin of the map
+ * @param resolution Resolution of the map
+ * @return Cell position of the point
+ */
+Eigen::Vector2i toMapCoords(geometry_msgs::Point p, geometry_msgs::Pose origin,
+ float resolution) {
+ int x_idx = (p.x - origin.position.x) / resolution + 0.51;
+ int y_idx = (p.y - origin.position.y) / resolution + 0.51;
+ Eigen::Vector2i ret(x_idx, y_idx);
+ return ret;
+}
- /**
- * @brief Converts the QT pixel position of a point to its respective position in the world frame
- * @param idx Cell position of the point
- * @param origin Origin of the map
- * @param resolution Resolution of the map
- * @return Point in world frame
- */
- geometry_msgs::Point qtFromMapCoords(Eigen::Vector2i idx, geometry_msgs::Pose origin, float resolution)
- {
- geometry_msgs::Point ret;
- ret.x = -(origin.position.x + idx.y()) * resolution;
- ret.y = -(origin.position.y + idx.x()) * resolution;
- return ret;
+/**
+ * @brief Converts a point p in world frame /map to the respective cell position
+ * in the map
+ * @param p Point in world frame
+ * @param origin Origin of the map
+ * @param resolution Resolution of the map
+ * @return Cell position of the point
+ */
+Eigen::Vector2i toMapCoords(const geometry_msgs::Point p,
+ const nav_msgs::OccupancyGrid::ConstPtr& cmap) {
+ int x_idx =
+ (p.x - cmap->info.origin.position.x) / cmap->info.resolution + 0.51;
+ int y_idx =
+ (p.y - cmap->info.origin.position.y) / cmap->info.resolution + 0.51;
+ Eigen::Vector2i ret(x_idx, y_idx);
+ return ret;
+}
+
+/**
+ * @brief Converts the cell position of a point to its respective position in
+ * the world frame
+ * @param idx Cell position of the point
+ * @param origin Origin of the map
+ * @param resolution Resolution of the map
+ * @return Point in world frame
+ */
+geometry_msgs::Point fromMapCoords(
+ const Eigen::Vector2i idx, const nav_msgs::OccupancyGrid::ConstPtr& cmap) {
+ geometry_msgs::Point ret;
+ ret.x =
+ cmap->info.origin.position.x + (idx.x() - 0.5) * cmap->info.resolution;
+ ret.y =
+ cmap->info.origin.position.y + (idx.y() - 0.5) * cmap->info.resolution;
+ return ret;
+}
+
+/**
+ * @brief Converts the cell position of a point to its respective position in
+ * the world frame
+ * @param idx Cell position of the point
+ * @param origin Origin of the map
+ * @param resolution Resolution of the map
+ * @return Point in world frame
+ */
+geometry_msgs::Point fromMapCoords(Eigen::Vector2i idx,
+ geometry_msgs::Pose origin,
+ float resolution) {
+ geometry_msgs::Point ret;
+ ret.x = origin.position.x + (idx.x() - 0.5) * resolution;
+ ret.y = origin.position.y + (idx.y() - 0.5) * resolution;
+ return ret;
+}
+
+/**
+ * @brief Converts the QT pixel position of a point to its respective position
+ * in the world frame
+ * @param idx Cell position of the point
+ * @param origin Origin of the map
+ * @param resolution Resolution of the map
+ * @return Point in world frame
+ */
+geometry_msgs::Point qtFromMapCoords(Eigen::Vector2i idx,
+ geometry_msgs::Pose origin,
+ float resolution) {
+ geometry_msgs::Point ret;
+ ret.x = -(origin.position.x + idx.y()) * resolution;
+ ret.y = -(origin.position.y + idx.x()) * resolution;
+ return ret;
+}
+
+/**
+ * @brief map_index returns for a given point in the map real-world frame the
+ * respective index in the map
+ * @param p Point in the real-world frame (usually the frame /map or /world)
+ * @param origin Pose of the point (0,0) of the map in the real-world frame
+ * @param width Width of the map
+ * @param resolution Resolution in meters/cell of the map
+ * @return index of point in the map
+ */
+int map_index(geometry_msgs::Point p, geometry_msgs::Pose origin, float width,
+ float resolution) {
+ return (int)(width * ((p.y - origin.position.y) / resolution + 0.51) +
+ ((p.x - origin.position.x) / resolution + 0.51));
+}
+
+/**
+ * @brief point_in_map returns true if given point is in the map. False
+ * otherwise
+ * @param p Point in the real-world frame (usually the frame /map or /world)
+ * @param origin Pose of the point (0,0) of the map in the real-world frame
+ * @param width Width of the map
+ * @param resolution Resolution in meters/cell of the map
+ * @return true or false
+ */
+bool point_in_map(geometry_msgs::Point p, geometry_msgs::Pose origin,
+ float width, float resolution) {
+ int x_idx = (p.x - origin.position.x) / resolution + 0.51;
+ int y_idx = (p.y - origin.position.y) / resolution + 0.51;
+ if (x_idx < 0 || y_idx < 0 || x_idx >= width || y_idx >= width)
+ return false;
+ return true;
+}
+
+/**
+ * @brief transformPoint wrapper to transform points between coordinate frames
+ * @param point input point in from_frame
+ * @param listener transform listener
+ * @param from_frame input frame
+ * @param to_frame output frame
+ * @return transformed point in to_frame
+ */
+geometry_msgs::Point transformPoint(geometry_msgs::Point point,
+ tf::TransformListener& listener,
+ std::string from_frame,
+ std::string to_frame,
+ ros::Time stamp = ros::Time(0)) {
+ geometry_msgs::PointStamped pin;
+ geometry_msgs::PointStamped pout;
+ pin.header.frame_id = from_frame;
+ pin.point = point;
+ try {
+ listener.transformPoint(to_frame, stamp, pin, "/map", pout);
+ return pout.point;
+ } catch (tf::TransformException ex) {
+ ROS_ERROR("%s", ex.what());
}
+}
+
+geometry_msgs::Point transformPoint(const geometry_msgs::Point point,
+ tf::StampedTransform transform) {
+ geometry_msgs::Point point_out;
+ tf::Vector3 pin;
+ tf::Vector3 pout;
+ pin.setX(point.x);
+ pin.setY(point.y);
+ pin.setZ(point.z);
+
+ pout = transform * pin;
+
+ point_out.x = pout.x();
+ point_out.y = pout.y();
+
+ return point_out;
+}
+geometry_msgs::Point transformPoint(const geometry_msgs::Point point,
+ tf::Transform transform) {
+ geometry_msgs::Point point_out;
+ tf::Vector3 pin;
+ tf::Vector3 pout;
+ pin.setX(point.x);
+ pin.setY(point.y);
+ pin.setZ(point.z);
+
+ pout = transform * pin;
- /**
- * @brief map_index returns for a given point in the map real-world frame the respective index in the map
- * @param p Point in the real-world frame (usually the frame /map or /world)
- * @param origin Pose of the point (0,0) of the map in the real-world frame
- * @param width Width of the map
- * @param resolution Resolution in meters/cell of the map
- * @return index of point in the map
- */
- int map_index(geometry_msgs::Point p, geometry_msgs::Pose origin, float width, float resolution)
- {
- return (int)(width *
- ((p.y - origin.position.y)/resolution + 0.51) +
- ((p.x - origin.position.x)/resolution + 0.51));
+ point_out.x = pout.x();
+ point_out.y = pout.y();
+ point_out.z = pout.z();
+
+ return point_out;
+}
+
+/**
+ * @brief transformPoint wrapper to transform points between coordinate frames
+ * at specific time
+ * @param point input point in from_frame
+ * @param listener transform listener
+ * @param Time to look for transform
+ * @param from_frame input frame
+ * @param to_frame output frame
+ * @return transformed point in to_frame
+ */
+geometry_msgs::Point transformPoint(geometry_msgs::Point point,
+ tf::TransformListener& listener,
+ const ros::Time& time,
+ std::string from_frame,
+ std::string to_frame) {
+ geometry_msgs::PointStamped pin;
+ geometry_msgs::PointStamped pout;
+ pin.header.frame_id = from_frame;
+ pin.point = point;
+ try {
+ listener.transformPoint(to_frame, time, pin, "/map", pout);
+ return pout.point;
+ } catch (tf::TransformException ex) {
+ ROS_ERROR("%s", ex.what());
}
+}
- /**
- * @brief point_in_map returns true if given point is in the map. False otherwise
- * @param p Point in the real-world frame (usually the frame /map or /world)
- * @param origin Pose of the point (0,0) of the map in the real-world frame
- * @param width Width of the map
- * @param resolution Resolution in meters/cell of the map
- * @return true or false
- */
- bool point_in_map(geometry_msgs::Point p, geometry_msgs::Pose origin, float width, float resolution)
- {
- int x_idx = (p.x - origin.position.x)/resolution + 0.51;
- int y_idx = (p.y - origin.position.y)/resolution + 0.51;
- if(x_idx < 0 || y_idx < 0 || x_idx >= width || y_idx >= width) return false;
- return true;
+/**
+ * @brief laser_range_to_point converts a single given laser scan range in polar
+ * coordinates
+ * to the respective point in euclidean coordinates in the target frame
+ * @param laser_range range of the laser point to convert
+ * @param index
+ * @param start_angle
+ * @param angle_step
+ * @param listener
+ * @param from_frame
+ * @param to_frame
+ * @return
+ */
+geometry_msgs::Point laser_range_to_point(
+ float laser_range, int index, float start_angle, float angle_step,
+ tf::TransformListener& listener, std::string from_frame,
+ std::string to_frame, ros::Time stamp = ros::Time(0), float time_inc = 0) {
+ float alpha = start_angle + index * angle_step;
+ geometry_msgs::PointStamped pin;
+ geometry_msgs::PointStamped pout;
+ pin.header.frame_id = from_frame;
+ pin.point.x = cos(alpha) * laser_range;
+ pin.point.y = sin(alpha) * laser_range;
+
+ try {
+ listener.transformPoint(to_frame, stamp, pin, "/map", pout);
+ return pout.point;
+ } catch (tf::TransformException ex) {
+ // ROS_ERROR("%s",ex.what());
}
+}
- /**
- * @brief transformPoint wrapper to transform points between coordinate frames
- * @param point input point in from_frame
- * @param listener transform listener
- * @param from_frame input frame
- * @param to_frame output frame
- * @return transformed point in to_frame
- */
- geometry_msgs::Point transformPoint(geometry_msgs::Point point, tf::TransformListener &listener,
- std::string from_frame, std::string to_frame, ros::Time stamp = ros::Time(0))
- {
- geometry_msgs::PointStamped pin;
- geometry_msgs::PointStamped pout;
- pin.header.frame_id = from_frame;
- pin.point = point;
- try
- {
- listener.transformPoint(to_frame, stamp, pin, "/map",pout);
- return pout.point;
- }
- catch (tf::TransformException ex){
- ROS_ERROR("%s",ex.what());
+/**
+ * @brief laser_ranges_to_points converts a given laser scan in polar
+ * coordinates
+ * to the respective points in euclidean coordinates in the target
+ * frame
+ * @param laser_data laser data ranges
+ * @param start_angle angle of the first measurement
+ * @param angle_step angle increment between two consecutive laser measurements
+ * @param range_min minimum valid range
+ * @param range_max maximum valid range
+ * @return vector containing the laser measurements in euclidean points
+ */
+std::vector<geometry_msgs::Point> laser_ranges_to_points(
+ const std::vector<float>& laser_data, float start_angle, float angle_step,
+ float range_min, float range_max, tf::TransformListener& listener,
+ std::string from_frame, std::string to_frame,
+ ros::Time stamp = ros::Time(0), float time_inc = 0) {
+ std::vector<geometry_msgs::Point> ret;
+ float alpha = start_angle;
+ for (int i = 0; i < laser_data.size(); i++) {
+ if (laser_data[i] < range_min || laser_data[i] > range_max) {
+ alpha += angle_step;
+ continue;
}
- }
-
- geometry_msgs::Point transformPoint(const geometry_msgs::Point point, tf::StampedTransform transform)
- {
- geometry_msgs::Point point_out;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX(point.x);
- pin.setY(point.y);
- pin.setZ(point.z);
-
- pout = transform* pin;
-
- point_out.x = pout.x();
- point_out.y = pout.y();
-
- return point_out;
- }
- geometry_msgs::Point transformPoint(const geometry_msgs::Point point, tf::Transform transform)
- {
- geometry_msgs::Point point_out;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX(point.x);
- pin.setY(point.y);
- pin.setZ(point.z);
-
- pout = transform* pin;
-
- point_out.x = pout.x();
- point_out.y = pout.y();
- point_out.z = pout.z();
-
- return point_out;
- }
-
- /**
- * @brief transformPoint wrapper to transform points between coordinate frames at specific time
- * @param point input point in from_frame
- * @param listener transform listener
- * @param Time to look for transform
- * @param from_frame input frame
- * @param to_frame output frame
- * @return transformed point in to_frame
- */
- geometry_msgs::Point transformPoint(geometry_msgs::Point point, tf::TransformListener &listener, const ros::Time & time ,
- std::string from_frame, std::string to_frame)
- {
+ geometry_msgs::Point point;
+ point.x = cos(alpha) * laser_data.at(i);
+ point.y = sin(alpha) * laser_data.at(i);
+
geometry_msgs::PointStamped pin;
- geometry_msgs::PointStamped pout;
pin.header.frame_id = from_frame;
pin.point = point;
- try
- {
- listener.transformPoint(to_frame, time, pin, "/map" ,pout);
- return pout.point;
- }
- catch (tf::TransformException ex){
- ROS_ERROR("%s",ex.what());
+ geometry_msgs::PointStamped pout;
+ try {
+ listener.transformPoint(to_frame, stamp, pin, "/map", pout);
+ ret.push_back(pout.point);
+ } catch (tf::TransformException ex) {
+ // ROS_ERROR("%s",ex.what());
}
+
+ alpha += angle_step;
}
+ return ret;
+}
+
+/**
+* @brief laser_msg_to_points converts a given laser scan in polar coordinates
+* to the respective points in euclidean coordinates in the target frame
+* at a specific time
+* @param scan laser data msg
+* @param listener TransformListener
+* @param to_frame target frame
+* @return vector containing the laser measurements in euclidean points
+*/
+std::vector<geometry_msgs::Point> laser_msg_to_points(
+ const sensor_msgs::LaserScan::ConstPtr& scan,
+ tf::TransformListener& listener, std::string to_frame,
+ ros::Time stamp = ros::Time(0)) {
+ std::vector<geometry_msgs::Point> ret;
+ float alpha = scan->angle_min;
+ if (!listener.waitForTransform(scan->header.frame_id, to_frame, stamp,
+ ros::Duration(0.3))) {
+ return ret;
+ }
+ for (int i = 0; i < scan->ranges.size(); i++) {
+ if (scan->ranges[i] < scan->range_min ||
+ scan->ranges[i] > scan->range_max) {
+ alpha += scan->angle_increment;
+ continue;
+ }
+ geometry_msgs::Point point;
+ point.x = cos(alpha) * scan->ranges.at(i);
+ point.y = sin(alpha) * scan->ranges.at(i);
- /**
- * @brief laser_range_to_point converts a single given laser scan range in polar coordinates
- * to the respective point in euclidean coordinates in the target frame
- * @param laser_range range of the laser point to convert
- * @param index
- * @param start_angle
- * @param angle_step
- * @param listener
- * @param from_frame
- * @param to_frame
- * @return
- */
- geometry_msgs::Point laser_range_to_point(float laser_range, int index, float start_angle,
- float angle_step, tf::TransformListener &listener,
- std::string from_frame, std::string to_frame, ros::Time stamp = ros::Time(0), float time_inc = 0)
- {
- float alpha = start_angle + index * angle_step;
geometry_msgs::PointStamped pin;
+ pin.header.frame_id = scan->header.frame_id;
+ pin.point = point;
geometry_msgs::PointStamped pout;
- pin.header.frame_id = from_frame;
- pin.point.x = cos(alpha) * laser_range;
- pin.point.y = sin(alpha) * laser_range;
-
- try
- {
+ try {
+ // listener.transformPoint(to_frame, (stamp + ros::Duration( i *
+ // scan->time_increment)), pin, "/map" ,pout);
listener.transformPoint(to_frame, stamp, pin, "/map", pout);
- return pout.point;
- }
- catch (tf::TransformException ex){
- //ROS_ERROR("%s",ex.what());
+ ret.push_back(pout.point);
+ } catch (tf::TransformException ex) {
+ ROS_ERROR("%s", ex.what());
}
+
+ alpha += scan->angle_increment;
}
+ return ret;
+}
- /**
- * @brief laser_ranges_to_points converts a given laser scan in polar coordinates
- * to the respective points in euclidean coordinates in the target frame
- * @param laser_data laser data ranges
- * @param start_angle angle of the first measurement
- * @param angle_step angle increment between two consecutive laser measurements
- * @param range_min minimum valid range
- * @param range_max maximum valid range
- * @return vector containing the laser measurements in euclidean points
- */
- std::vector<geometry_msgs::Point> laser_ranges_to_points(const std::vector<float>& laser_data, float start_angle,
- float angle_step, float range_min, float range_max,
- tf::TransformListener &listener,
- std::string from_frame, std::string to_frame, ros::Time stamp = ros::Time(0), float time_inc = 0)
- {
- std::vector<geometry_msgs::Point> ret;
- float alpha = start_angle;
- for (int i = 0; i < laser_data.size(); i++) {
- if(laser_data[i] < range_min || laser_data[i] > range_max)
- {
- alpha += angle_step;
- continue;
- }
- geometry_msgs::Point point;
- point.x = cos(alpha) * laser_data.at(i);
- point.y = sin(alpha) * laser_data.at(i);
-
- geometry_msgs::PointStamped pin;
- pin.header.frame_id = from_frame;
- pin.point = point;
- geometry_msgs::PointStamped pout;
- try
- {
- listener.transformPoint(to_frame, stamp, pin, "/map", pout);
- ret.push_back(pout.point);
- }
- catch (tf::TransformException ex){
- //ROS_ERROR("%s",ex.what());
- }
-
- alpha += angle_step;
+/**
+* @brief get_max_move_distance searches for the nearest values in an defined
+* area
+* @param vector points: laserPoints in base_link
+* @return float distance to nearest Point
+*/
+float get_max_move_distance(std::vector<geometry_msgs::Point> points,
+ float min_x, float min_y) {
+ float minDistance = 30;
+ for (unsigned int i = 0; i < points.size(); i++) {
+ if (std::fabs(points[i].y) < min_y && points[i].x > min_x) {
+ float distance =
+ sqrt((points[i].x * points[i].x) + (points[i].y * points[i].y));
+ if (distance < minDistance) {
+ minDistance = distance;
+ }
}
- return ret;
}
-
-
- /**
- * @brief laser_msg_to_points converts a given laser scan in polar coordinates
- * to the respective points in euclidean coordinates in the target frame
- * at a specific time
- * @param scan laser data msg
- * @param listener TransformListener
- * @param to_frame target frame
- * @return vector containing the laser measurements in euclidean points
- */
- std::vector<geometry_msgs::Point> laser_msg_to_points(const sensor_msgs::LaserScan::ConstPtr& scan,
- tf::TransformListener &listener, std::string to_frame, ros::Time stamp = ros::Time(0))
- {
- std::vector<geometry_msgs::Point> ret;
- float alpha = scan->angle_min;
- if(!listener.waitForTransform(scan->header.frame_id, to_frame, stamp, ros::Duration(0.3)))
- {
- return ret;
- }
- for (int i = 0; i < scan->ranges.size(); i++) {
- if(scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max)
- {
- alpha += scan->angle_increment;
- continue;
- }
- geometry_msgs::Point point;
- point.x = cos(alpha) * scan->ranges.at(i);
- point.y = sin(alpha) * scan->ranges.at(i);
-
- geometry_msgs::PointStamped pin;
- pin.header.frame_id = scan->header.frame_id;
- pin.point = point;
- geometry_msgs::PointStamped pout;
- try
- {
- //listener.transformPoint(to_frame, (stamp + ros::Duration( i * scan->time_increment)), pin, "/map" ,pout);
- listener.transformPoint(to_frame, stamp, pin, "/map" ,pout);
- ret.push_back(pout.point);
- }
- catch (tf::TransformException ex){
- ROS_ERROR("%s",ex.what());
- }
-
- alpha += scan->angle_increment;
- }
- return ret;
+ float maxMoveDist = minDistance - min_x;
+ if (maxMoveDist < 0) {
+ maxMoveDist = 0.0;
}
-
- /**
- * @brief get_max_move_distance searches for the nearest values in an defined area
- * @param vector points: laserPoints in base_link
- * @return float distance to nearest Point
- */
-float get_max_move_distance(std::vector<geometry_msgs::Point> points, float min_x, float min_y)
-{
- float minDistance = 30;
- for (unsigned int i = 0; i < points.size(); i++)
- {
- if(std::fabs(points[i].y) < min_y && points[i].x > min_x)
- {
- float distance = sqrt((points[i].x * points[i].x) + (points[i].y * points[i].y));
- if (distance < minDistance)
- {
- minDistance = distance;
- }
- }
- }
- float maxMoveDist = minDistance - min_x;
- if (maxMoveDist < 0)
- {
- maxMoveDist = 0.0;
- }
- return maxMoveDist;
+ return maxMoveDist;
}
-
-
- /**
- * @brief Calculates the euclidean distance (in cells) between to points in the map
- * @param a Point a
- * @param b point b
- * @return euclidean distance in cells
- */
- double distance(const Eigen::Vector2i& a, const Eigen::Vector2i& b)
- {
- return sqrt((a.x() - b.x()) * (a.x() - b.x()) + (a.y() - b.y()) * (a.y() - b.y()));
- }
- /**
- * @brief Calculates the euclidean distance (in m) between to points in the world
- * @param a Point a
- * @param b point b
- * @return euclidean distance in m
- */
- double distance(const geometry_msgs::Point& a, const geometry_msgs::Point& b)
- {
- return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
- }
+/**
+ * @brief Calculates the euclidean distance (in cells) between to points in the
+ * map
+ * @param a Point a
+ * @param b point b
+ * @return euclidean distance in cells
+ */
+double distance(const Eigen::Vector2i& a, const Eigen::Vector2i& b) {
+ return sqrt((a.x() - b.x()) * (a.x() - b.x()) +
+ (a.y() - b.y()) * (a.y() - b.y()));
+}
+
+/**
+ * @brief Calculates the euclidean distance (in m) between to points in the
+ * world
+ * @param a Point a
+ * @param b point b
+ * @return euclidean distance in m
+ */
+double distance(const geometry_msgs::Point& a, const geometry_msgs::Point& b) {
+ return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
+}
/**
* @brief findValue
@@ -362,35 +402,44 @@ float get_max_move_distance(std::vector<geometry_msgs::Point> points, float min_
* @param center_y /
* @param value Value to search for in given map
* @param radius Radius of the circle
- * @return true if the given value could be found within the given radius around (x,y)
+ * @return true if the given value could be found within the given radius around
+ * (x,y)
*/
- bool findValue(const std::vector<int8_t>* map, int width, int height, int center_x, int center_y, unsigned char value, float radius)
- {
-
- int start_x = int ( center_x - radius );
- int start_y = int ( center_y - radius );
- int end_x = int ( center_x + radius );
- int end_y = int ( center_y + radius );
-
- if ( start_x < 0 ) { start_x = 0; }
- if ( start_y < 0 ) { start_y = 0; }
- if ( end_x >= int ( width ) ) { end_x = width - 1; }
- if ( end_y >= int ( height ) ) { end_y = height - 1; }
-
- float sqr_radius = radius*radius;
-
- for ( int y = start_y; y <= end_y; y++ )
- for ( int x = start_x; x <= end_x; x++ )
- {
- if ( map->at(x+width*y) > value )
- {
- float sqr_dist = float ( x - center_x ) * float ( x - center_x ) + float ( y - center_y ) * float ( y - center_y );
- if ( sqr_dist <= sqr_radius ) { return true; }
- }
- }
+bool findValue(const std::vector<int8_t>* map, int width, int height,
+ int center_x, int center_y, unsigned char value, float radius) {
+ int start_x = int(center_x - radius);
+ int start_y = int(center_y - radius);
+ int end_x = int(center_x + radius);
+ int end_y = int(center_y + radius);
- return false;
+ if (start_x < 0) {
+ start_x = 0;
+ }
+ if (start_y < 0) {
+ start_y = 0;
+ }
+ if (end_x >= int(width)) {
+ end_x = width - 1;
}
+ if (end_y >= int(height)) {
+ end_y = height - 1;
+ }
+
+ float sqr_radius = radius * radius;
+
+ for (int y = start_y; y <= end_y; y++)
+ for (int x = start_x; x <= end_x; x++) {
+ if (map->at(x + width * y) > value) {
+ float sqr_dist = float(x - center_x) * float(x - center_x) +
+ float(y - center_y) * float(y - center_y);
+ if (sqr_dist <= sqr_radius) {
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
}
-#endif // TOOLS_H
+#endif // TOOLS_H
diff --git a/homer_nav_libs/src/Explorer/Explorer.cpp b/homer_nav_libs/src/Explorer/Explorer.cpp
index e57d72266ad0c9fef0db4ec816a3f5ccbf72a119..2eaec66ca4594f8b7eef08267b828abbcd961c67 100644
--- a/homer_nav_libs/src/Explorer/Explorer.cpp
+++ b/homer_nav_libs/src/Explorer/Explorer.cpp
@@ -3,1133 +3,1027 @@
using namespace std;
using namespace ExplorerConstants;
-Explorer::Explorer ( double minAllowedObstacleDistance, double maxAllowedObstacleDistance,
- double minSafeObstacleDistance, double maxSafeObstacleDistance,
- double safePathWeight, double frontierSafenessFactor, int unknownThreshold )
-{
- ExplorerConstants::UNKNOWN = unknownThreshold;
-
- m_MinAllowedObstacleDistance = minAllowedObstacleDistance;
- m_MaxAllowedObstacleDistance = maxAllowedObstacleDistance;
-
- m_MinSafeObstacleDistance = minSafeObstacleDistance;
- m_MaxSafeObstacleDistance = maxSafeObstacleDistance;
-
- m_SafePathWeight = safePathWeight;
- m_FrontierSafenessFactor = frontierSafenessFactor;
-
- m_OccupancyMap = 0;
- m_ObstacleTransform = 0;
- m_CostTransform = 0;
- m_TargetMap = 0;
- m_DrivingDistanceTransform = 0;
- m_TargetDistanceTransform = 0;
- m_PathTransform = 0;
- m_ExplorationTransform = 0;
- m_DesiredDistance = 0;
+Explorer::Explorer(double minAllowedObstacleDistance,
+ double maxAllowedObstacleDistance,
+ double minSafeObstacleDistance,
+ double maxSafeObstacleDistance, double safePathWeight,
+ double frontierSafenessFactor, int unknownThreshold) {
+ ExplorerConstants::UNKNOWN = unknownThreshold;
+
+ m_MinAllowedObstacleDistance = minAllowedObstacleDistance;
+ m_MaxAllowedObstacleDistance = maxAllowedObstacleDistance;
+
+ m_MinSafeObstacleDistance = minSafeObstacleDistance;
+ m_MaxSafeObstacleDistance = maxSafeObstacleDistance;
+
+ m_SafePathWeight = safePathWeight;
+ m_FrontierSafenessFactor = frontierSafenessFactor;
+
+ m_OccupancyMap = 0;
+ m_ObstacleTransform = 0;
+ m_CostTransform = 0;
+ m_TargetMap = 0;
+ m_DrivingDistanceTransform = 0;
+ m_TargetDistanceTransform = 0;
+ m_PathTransform = 0;
+ m_ExplorationTransform = 0;
+ m_DesiredDistance = 0;
}
-Explorer::~Explorer()
-{
- releaseMaps();
- releaseMap( m_OccupancyMap );
+Explorer::~Explorer() {
+ releaseMaps();
+ releaseMap(m_OccupancyMap);
}
-void Explorer::releaseMaps()
-{
- releaseMap( m_TargetMap );
- releaseMap( m_ObstacleTransform );
- releaseMap( m_CostTransform );
- releaseMap( m_DrivingDistanceTransform );
- releaseMap( m_TargetDistanceTransform );
- releaseMap( m_PathTransform );
- releaseMap( m_ExplorationTransform );
+void Explorer::releaseMaps() {
+ releaseMap(m_TargetMap);
+ releaseMap(m_ObstacleTransform);
+ releaseMap(m_CostTransform);
+ releaseMap(m_DrivingDistanceTransform);
+ releaseMap(m_TargetDistanceTransform);
+ releaseMap(m_PathTransform);
+ releaseMap(m_ExplorationTransform);
}
-// SETTERS ////////////////////////////////////////////////////////////////////////////////////////////////
+// SETTERS
+// ////////////////////////////////////////////////////////////////////////////////////////////////
-void Explorer::setUnknownThreshold(int unknownTresh)
-{
+void Explorer::setUnknownThreshold(int unknownTresh) {
ExplorerConstants::UNKNOWN = unknownTresh;
}
-void Explorer::setAllowedObstacleDistance ( double min, double max )
-{
- m_MinAllowedObstacleDistance = min;
- m_MaxAllowedObstacleDistance = max;
- releaseMaps();
+void Explorer::setAllowedObstacleDistance(double min, double max) {
+ m_MinAllowedObstacleDistance = min;
+ m_MaxAllowedObstacleDistance = max;
+ releaseMaps();
}
-void Explorer::setSafeObstacleDistance ( double min, double max )
-{
- m_MinSafeObstacleDistance = min;
- m_MaxSafeObstacleDistance = max;
- releaseMaps();
+void Explorer::setSafeObstacleDistance(double min, double max) {
+ m_MinSafeObstacleDistance = min;
+ m_MaxSafeObstacleDistance = max;
+ releaseMaps();
}
-void Explorer::setSafePathWeight ( double weight )
-{
- m_SafePathWeight = weight;
- releaseMaps();
+void Explorer::setSafePathWeight(double weight) {
+ m_SafePathWeight = weight;
+ releaseMaps();
}
-void Explorer::setFrontierSafenessFactor( double frontierSafenessFactor )
-{
- m_FrontierSafenessFactor = frontierSafenessFactor;
- releaseMaps();
+void Explorer::setFrontierSafenessFactor(double frontierSafenessFactor) {
+ m_FrontierSafenessFactor = frontierSafenessFactor;
+ releaseMaps();
}
-void Explorer::setOccupancyMap ( int width, int height, geometry_msgs::Pose origin, int8_t* data )
-{
- if ( !data ) {
- ROS_ERROR( "Received 0-pointer." );
- return;
- }
- releaseMaps();
- releaseMap( m_OccupancyMap );
- //m_OccupancyMap = new GridMap<unsigned char> ( width, height, data, exploredRegion );
- m_OccupancyMap = new GridMap<int8_t> ( width, height, data );
- m_Origin = origin;
-}
-
-
-void Explorer::updateObstacles(int width, int height, geometry_msgs::Pose origin, int8_t* mapData)
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
- if ( (width != m_OccupancyMap->width()) || (height != m_OccupancyMap->height()) ) {
- ROS_ERROR_STREAM( "Wrong map size!" );
- return;
- }
- for ( unsigned i=0; i<m_OccupancyMap->width()*m_OccupancyMap->height(); i++ )
- {
- int8_t* myMapData=m_OccupancyMap->getDirectAccess(0,0);
- if ( myMapData[i] != UNKNOWN )
- {
- myMapData[i]=mapData[i];
+void Explorer::setOccupancyMap(int width, int height,
+ geometry_msgs::Pose origin, int8_t* data) {
+ if (!data) {
+ ROS_ERROR("Received 0-pointer.");
+ return;
}
- }
- releaseMaps();
+ releaseMaps();
+ releaseMap(m_OccupancyMap);
+ // m_OccupancyMap = new GridMap<unsigned char> ( width, height, data,
+ // exploredRegion );
+ m_OccupancyMap = new GridMap<int8_t>(width, height, data);
+ m_Origin = origin;
}
-void Explorer::resetExploration()
-{
- m_DesiredDistance = 0;
+void Explorer::setOccupancyMap(const nav_msgs::OccupancyGrid::ConstPtr& cmap) {
+ releaseMaps();
+ releaseMap(m_OccupancyMap);
+ // m_OccupancyMap = new GridMap<unsigned char> ( width, height, data,
+ // exploredRegion );
+ nav_msgs::OccupancyGrid temp_map = *cmap;
+ m_OccupancyMap = new GridMap<int8_t>(cmap->info.width, cmap->info.height,
+ &(temp_map.data)[0]);
+ m_Origin = cmap->info.origin;
}
-void Explorer::setStart ( Eigen::Vector2i start )
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR_STREAM( "Occupancy map is missing." );
- return;
- }
- if (( start.x() <= 1 ) || ( start.y() <= 1 ) || ( start.x() >= m_OccupancyMap->width()-1 ) || ( start.y() >= m_OccupancyMap->height()-1 ) )
- {
- ROS_ERROR_STREAM( "Invalid position!" );
- return;
- }
- computeWalkableMaps();
-
- if ( !isWalkable( start.x(), start.y() ) )
- {
- Eigen::Vector2i correctedStart=getNearestWalkablePoint( start );
- if ( !isWalkable( correctedStart.x(), correctedStart.y() ) )
- {
- ROS_ERROR_STREAM( "No walkable position was found on the map!" );
- }
- else
- {
- ROS_INFO_STREAM("Start position " << start.x() << "," << start.y() << " was corrected to " << correctedStart.x() << "," << correctedStart.y());
+void Explorer::updateObstacles(int width, int height,
+ geometry_msgs::Pose origin, int8_t* mapData) {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
+ if ((width != m_OccupancyMap->width()) ||
+ (height != m_OccupancyMap->height())) {
+ ROS_ERROR_STREAM("Wrong map size!");
+ return;
+ }
+ for (unsigned i = 0; i < m_OccupancyMap->width() * m_OccupancyMap->height();
+ i++) {
+ int8_t* myMapData = m_OccupancyMap->getDirectAccess(0, 0);
+ if (myMapData[i] != UNKNOWN) {
+ myMapData[i] = mapData[i];
+ }
}
- m_Start = correctedStart;
- return;
- }
- m_Start = start;
+ releaseMaps();
}
+void Explorer::resetExploration() { m_DesiredDistance = 0; }
-Eigen::Vector2i Explorer::getNearestAccessibleTarget(Eigen::Vector2i target )
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return target;
- }
- if ( ( target.x() <= 1 ) || ( target.y() <= 1 ) || ( target.x() >= m_OccupancyMap->width()-1 ) || ( target.y() >= m_OccupancyMap->height()-1 ) )
- {
- ROS_ERROR( "Invalid position!" );
- return target;
- }
- computeApproachableMaps();
- computeWalkableMaps();
- Eigen::Vector2i correctTarget=target;
-
- if ( !isApproachable( target.x(), target.y() ) )
- {
- ROS_INFO_STREAM("target cell in drivingdistancetransform: " << m_DrivingDistanceTransform->getValue ( target.x(), target.y() ));
- ROS_INFO_STREAM("target " << target << " is not approachable. Correcting target...");
- int minSqrDist=INT_MAX;
- for ( int x = 0; x < m_ObstacleTransform->width(); x++ )
- {
- for ( int y = 0; y < m_ObstacleTransform->height(); y++ )
- {
- bool isSafe = m_ObstacleTransform->getValue ( x, y ) > m_FrontierSafenessFactor * m_MinAllowedObstacleDistance;
- if ( isApproachable ( x,y ) && isWalkable( x , y) && isSafe )
- {
- int xDiff = target.x() - x;
- int yDiff = target.y() - y;
- int sqrDist = xDiff*xDiff + yDiff*yDiff;
- if ( sqrDist < minSqrDist )
- {
- correctTarget.x() = x;
- correctTarget.y() = y;
- minSqrDist = sqrDist;
- }
+void Explorer::setStart(Eigen::Vector2i start) {
+ if (!m_OccupancyMap) {
+ ROS_ERROR_STREAM("Occupancy map is missing.");
+ return;
+ }
+ if ((start.x() <= 1) || (start.y() <= 1) ||
+ (start.x() >= m_OccupancyMap->width() - 1) ||
+ (start.y() >= m_OccupancyMap->height() - 1)) {
+ ROS_ERROR_STREAM("Invalid position!");
+ return;
+ }
+ computeWalkableMaps();
+
+ if (!isWalkable(start.x(), start.y())) {
+ Eigen::Vector2i correctedStart = getNearestWalkablePoint(start);
+ if (!isWalkable(correctedStart.x(), correctedStart.y())) {
+ ROS_ERROR_STREAM("No walkable position was found on the map!");
+ } else {
+ ROS_INFO_STREAM("Start position " << start.x() << "," << start.y()
+ << " was corrected to "
+ << correctedStart.x() << ","
+ << correctedStart.y());
}
- }
+ m_Start = correctedStart;
+ return;
}
- }
- ROS_DEBUG_STREAM("Target position " << target.x() << "," << target.y() << " was corrected to " << correctTarget.x() << "," << correctTarget.y());
-
- return correctTarget;
+ m_Start = start;
}
+Eigen::Vector2i Explorer::getNearestAccessibleTarget(Eigen::Vector2i target) {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return target;
+ }
+ if ((target.x() <= 1) || (target.y() <= 1) ||
+ (target.x() >= m_OccupancyMap->width() - 1) ||
+ (target.y() >= m_OccupancyMap->height() - 1)) {
+ ROS_ERROR("Invalid position!");
+ return target;
+ }
+ computeApproachableMaps();
+ computeWalkableMaps();
+ Eigen::Vector2i correctTarget = target;
+
+ if (!isApproachable(target.x(), target.y())) {
+ ROS_INFO_STREAM(
+ "target cell in drivingdistancetransform: "
+ << m_DrivingDistanceTransform->getValue(target.x(), target.y()));
+ ROS_INFO_STREAM("target "
+ << target
+ << " is not approachable. Correcting target...");
+ int minSqrDist = INT_MAX;
+ for (int x = 0; x < m_ObstacleTransform->width(); x++) {
+ for (int y = 0; y < m_ObstacleTransform->height(); y++) {
+ bool isSafe =
+ m_ObstacleTransform->getValue(x, y) >
+ m_FrontierSafenessFactor * m_MinAllowedObstacleDistance;
+ if (isApproachable(x, y) && isWalkable(x, y) && isSafe) {
+ int xDiff = target.x() - x;
+ int yDiff = target.y() - y;
+ int sqrDist = xDiff * xDiff + yDiff * yDiff;
+ if (sqrDist < minSqrDist) {
+ correctTarget.x() = x;
+ correctTarget.y() = y;
+ minSqrDist = sqrDist;
+ }
+ }
+ }
+ }
+ }
+ ROS_DEBUG_STREAM("Target position "
+ << target.x() << "," << target.y() << " was corrected to "
+ << correctTarget.x() << "," << correctTarget.y());
-Eigen::Vector2i Explorer::getNearestWalkablePoint( Eigen::Vector2i target )
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return target;
- }
- if (( target.x() <= 1 ) || ( target.y() <= 1 ) || ( target.x() >= m_OccupancyMap->width()-1 ) || ( target.y() >= m_OccupancyMap->height()-1 ) )
- {
- ROS_ERROR( "Invalid position!" );
- return target;
- }
-
- computeWalkableMaps();
- Eigen::Vector2i correctTarget=target;
+ return correctTarget;
+}
+Eigen::Vector2i Explorer::getNearestWalkablePoint(Eigen::Vector2i target) {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return target;
+ }
+ if ((target.x() <= 1) || (target.y() <= 1) ||
+ (target.x() >= m_OccupancyMap->width() - 1) ||
+ (target.y() >= m_OccupancyMap->height() - 1)) {
+ ROS_ERROR("Invalid position!");
+ return target;
+ }
- if ( !isWalkable( target.x(), target.y() ) )
- {
- int minSqrDist=INT_MAX;
- for ( int x = 0; x < m_ObstacleTransform->width(); x++ )
- {
- for ( int y = 0; y < m_ObstacleTransform->height(); y++ )
- {
- if ( isWalkable ( x,y ) )
- {
- int xDiff = target.x() - x;
- int yDiff = target.y() - y;
- int sqrDist = xDiff*xDiff + yDiff*yDiff;
- if ( sqrDist < minSqrDist )
- {
- correctTarget.x() = x;
- correctTarget.y() = y;
- minSqrDist = sqrDist;
- }
+ computeWalkableMaps();
+ Eigen::Vector2i correctTarget = target;
+
+ if (!isWalkable(target.x(), target.y())) {
+ int minSqrDist = INT_MAX;
+ for (int x = 0; x < m_ObstacleTransform->width(); x++) {
+ for (int y = 0; y < m_ObstacleTransform->height(); y++) {
+ if (isWalkable(x, y)) {
+ int xDiff = target.x() - x;
+ int yDiff = target.y() - y;
+ int sqrDist = xDiff * xDiff + yDiff * yDiff;
+ if (sqrDist < minSqrDist) {
+ correctTarget.x() = x;
+ correctTarget.y() = y;
+ minSqrDist = sqrDist;
+ }
+ }
+ }
}
- }
}
- }
- ROS_DEBUG_STREAM("Position " << target.x() << "," << target.y() << " was corrected to " << correctTarget.x() << "," << correctTarget.y());
+ ROS_DEBUG_STREAM("Position " << target.x() << "," << target.y()
+ << " was corrected to " << correctTarget.x()
+ << "," << correctTarget.y());
- return correctTarget;
+ return correctTarget;
}
-
-void Explorer::setTarget (Eigen::Vector2i target )
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
- if ( ( target.x() <= 1 ) || ( target.y() <= 1 ) || ( target.x() >= m_OccupancyMap->width()-1 ) || ( target.y() >= m_OccupancyMap->height()-1 ) )
- {
- ROS_ERROR( "Invalid position!" );
- return;
- }
- computeApproachableMaps();
- if ( !isApproachable ( target.x(), target.y() ) )
- {
- ROS_WARN( "Target position is not approachable. Path computation will possibly fail." );
- }
- m_Target = target;
- m_DesiredDistance = 0;
+void Explorer::setTarget(Eigen::Vector2i target) {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
+ if ((target.x() <= 1) || (target.y() <= 1) ||
+ (target.x() >= m_OccupancyMap->width() - 1) ||
+ (target.y() >= m_OccupancyMap->height() - 1)) {
+ ROS_ERROR("Invalid position!");
+ return;
+ }
+ computeApproachableMaps();
+ if (!isApproachable(target.x(), target.y())) {
+ ROS_WARN(
+ "Target position is not approachable. Path computation will "
+ "possibly fail.");
+ }
+ m_Target = target;
+ m_DesiredDistance = 0;
}
+void Explorer::setTarget(Eigen::Vector2i target, int desiredDistance) {
+ ROS_ERROR_STREAM("setTarget still in use!!");
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
-void Explorer::setTarget (Eigen::Vector2i target, int desiredDistance )
-{
- ROS_ERROR_STREAM("setTarget still in use!!");
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
-
- if ( desiredDistance < 1 )
- {
- setTarget( target );
- return;
- }
-
- if ( target.x() + desiredDistance <= 1 || target.x() - desiredDistance >= m_OccupancyMap->width()-1 ||
- target.y() + desiredDistance <= 1 || target.y() - desiredDistance >= m_OccupancyMap->height()-1 )
- {
- ROS_ERROR( "Invalid position" );
- return;
- }
- computeApproachableMaps();
- // TODO: check if region is approachable
- m_Target = target;
- m_DesiredDistance = desiredDistance;
-}
+ if (desiredDistance < 1) {
+ setTarget(target);
+ return;
+ }
+ if (target.x() + desiredDistance <= 1 ||
+ target.x() - desiredDistance >= m_OccupancyMap->width() - 1 ||
+ target.y() + desiredDistance <= 1 ||
+ target.y() - desiredDistance >= m_OccupancyMap->height() - 1) {
+ ROS_ERROR("Invalid position");
+ return;
+ }
+ computeApproachableMaps();
+ // TODO: check if region is approachable
+ m_Target = target;
+ m_DesiredDistance = desiredDistance;
+}
-// GETTERS ////////////////////////////////////////////////////////////////////////////////////////////////
+// GETTERS
+// ////////////////////////////////////////////////////////////////////////////////////////////////
-Eigen::Vector2i Explorer::getStart() const
-{
- return m_Start;
-}
+Eigen::Vector2i Explorer::getStart() const { return m_Start; }
-Eigen::Vector2i Explorer::getTarget() const
-{
- return m_Target;
-}
+Eigen::Vector2i Explorer::getTarget() const { return m_Target; }
-GridMap<int8_t>* Explorer::getOccupancyMap()
-{
- return m_OccupancyMap;
-}
+GridMap<int8_t>* Explorer::getOccupancyMap() { return m_OccupancyMap; }
-GridMap<double>* Explorer::getObstacleTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computeObstacleTransform();
- return m_ObstacleTransform;
+GridMap<double>* Explorer::getObstacleTransform() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computeObstacleTransform();
+ return m_ObstacleTransform;
}
-GridMap<double>* Explorer::getCostTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computeCostTransform();
- return m_CostTransform;
+GridMap<double>* Explorer::getCostTransform() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computeCostTransform();
+ return m_CostTransform;
}
-GridMap<bool>* Explorer::getTargetMap()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
+GridMap<bool>* Explorer::getTargetMap() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
- computeTargetMap();
- return m_TargetMap;
+ computeTargetMap();
+ return m_TargetMap;
}
-GridMap<double>* Explorer::getDrivingDistanceTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computeDrivingDistanceTransform();
- return m_DrivingDistanceTransform;
+GridMap<double>* Explorer::getDrivingDistanceTransform() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computeDrivingDistanceTransform();
+ return m_DrivingDistanceTransform;
}
-GridMap<double>* Explorer::getTargetDistanceTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computeTargetDistanceTransform();
- return m_TargetDistanceTransform;
+GridMap<double>* Explorer::getTargetDistanceTransform() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computeTargetDistanceTransform();
+ return m_TargetDistanceTransform;
}
-GridMap<double>* Explorer::getPathTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computePathTransform();
- return m_PathTransform;
+GridMap<double>* Explorer::getPathTransform() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computePathTransform();
+ return m_PathTransform;
}
-GridMap<double>* Explorer::getExplorationTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return 0;
- }
- computeExplorationTransform();
- return m_ExplorationTransform;
+GridMap<double>* Explorer::getExplorationTransform() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computeExplorationTransform();
+ return m_ExplorationTransform;
}
+// MAP GENERATION
+// ////////////////////////////////////////////////////////////////////////////////////////////////7
-
-// MAP GENERATION ////////////////////////////////////////////////////////////////////////////////////////////////7
-
-
-void Explorer::computeApproachableMaps()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
- computeDrivingDistanceTransform();
+void Explorer::computeApproachableMaps() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
+ computeDrivingDistanceTransform();
}
-
-void Explorer::computeWalkableMaps()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
- computeObstacleTransform();
+void Explorer::computeWalkableMaps() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
+ computeObstacleTransform();
}
+void Explorer::computeDrivingDistanceTransform() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
-void Explorer::computeDrivingDistanceTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
-
- if ( m_DrivingDistanceTransform ) { return; }
+ if (m_DrivingDistanceTransform) {
+ return;
+ }
- ROS_DEBUG( "Computing drivingDistanceTransform..." );
- resetMap( m_DrivingDistanceTransform );
- distanceFloodFill ( m_DrivingDistanceTransform, m_Start );
+ ROS_DEBUG("Computing drivingDistanceTransform...");
+ resetMap(m_DrivingDistanceTransform);
+ distanceFloodFill(m_DrivingDistanceTransform, m_Start);
}
+void Explorer::computeTargetDistanceTransform() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
-void Explorer::computeTargetDistanceTransform()
-{
- if ( !m_OccupancyMap )
- {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
-
- if ( m_TargetDistanceTransform ) { return; }
+ if (m_TargetDistanceTransform) {
+ return;
+ }
- ROS_DEBUG( "Computing targetDistanceTransform..." );
- resetMap( m_TargetDistanceTransform );
- distanceFloodFill ( m_TargetDistanceTransform, m_Target );
+ ROS_DEBUG("Computing targetDistanceTransform...");
+ resetMap(m_TargetDistanceTransform);
+ distanceFloodFill(m_TargetDistanceTransform, m_Target);
}
+void Explorer::computeRegionMap() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
-void Explorer::computeRegionMap()
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
-
- resetMap( m_TargetMap );
- ROS_DEBUG( "Computing target region map..." );
-
- m_TargetMap->fill( false );
- const int desiredDistanceSquared = m_DesiredDistance * m_DesiredDistance;
- int height = m_OccupancyMap->height();
- int width = m_OccupancyMap->width();
-
- // draw a circle onto the ExplorationMap
- const int firstX = m_Target.x() - m_DesiredDistance <= 1 ? 2 : m_Target.x() - m_DesiredDistance;
- const int firstY = m_Target.y() - m_DesiredDistance <= 1 ? 2 : m_Target.y() - m_DesiredDistance;
- const int lastX = m_Target.x() + m_DesiredDistance >= width-1 ? width-2 : m_Target.x() + m_DesiredDistance;
- const int lastY = m_Target.y() + m_DesiredDistance >= height-1 ? height-2 : m_Target.y() + m_DesiredDistance;
-
- for ( int y = firstY; y <= lastY; ++y )
- {
- for ( int x = firstX; x <= lastX; ++x )
- {
- const int dx = x - m_Target.x();
- const int dy = y - m_Target.y();
-
- if ( dx*dx + dy*dy <= desiredDistanceSquared )
- {
- m_TargetMap->setValue( x, y, true );
- }
+ resetMap(m_TargetMap);
+ ROS_DEBUG("Computing target region map...");
+
+ m_TargetMap->fill(false);
+ const int desiredDistanceSquared = m_DesiredDistance * m_DesiredDistance;
+ int height = m_OccupancyMap->height();
+ int width = m_OccupancyMap->width();
+
+ // draw a circle onto the ExplorationMap
+ const int firstX = m_Target.x() - m_DesiredDistance <= 1
+ ? 2
+ : m_Target.x() - m_DesiredDistance;
+ const int firstY = m_Target.y() - m_DesiredDistance <= 1
+ ? 2
+ : m_Target.y() - m_DesiredDistance;
+ const int lastX = m_Target.x() + m_DesiredDistance >= width - 1
+ ? width - 2
+ : m_Target.x() + m_DesiredDistance;
+ const int lastY = m_Target.y() + m_DesiredDistance >= height - 1
+ ? height - 2
+ : m_Target.y() + m_DesiredDistance;
+
+ for (int y = firstY; y <= lastY; ++y) {
+ for (int x = firstX; x <= lastX; ++x) {
+ const int dx = x - m_Target.x();
+ const int dy = y - m_Target.y();
+
+ if (dx * dx + dy * dy <= desiredDistanceSquared) {
+ m_TargetMap->setValue(x, y, true);
+ }
+ }
}
- }
}
-void Explorer::computeFrontierMap()
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Occupancy map is missing." );
- return;
- }
-
- // if ( m_FrontierMap ) { return; }
-
- resetMap( m_TargetMap );
-
- ROS_DEBUG( "Computing frontier map..." );
- m_TargetMap->fill ( 0 );
- // extract borders
- for ( int y = 1; y < m_OccupancyMap->height() - 1; y++ )
- {
- for ( int x = 1; x < m_OccupancyMap->width() - 1; x++ )
- {
- int value = m_OccupancyMap->getValue ( x, y );
- int value_u = m_OccupancyMap->getValue ( x, y - 1 );
- int value_d = m_OccupancyMap->getValue ( x, y + 1 );
- int value_l = m_OccupancyMap->getValue ( x - 1, y );
- int value_r = m_OccupancyMap->getValue ( x + 1, y );
- bool isFree = value < UNKNOWN && value != NOT_SEEN_YET;
- bool upUnknown = (value_u == UNKNOWN || value_u == NOT_SEEN_YET);
- bool downUnknown = (value_d == UNKNOWN || value_u == NOT_SEEN_YET);
- bool leftUnknown = (value_l == UNKNOWN || value_u == NOT_SEEN_YET);
- bool rightUnknown = (value_r == UNKNOWN || value_u == NOT_SEEN_YET);
- bool hasUnknownNeighbour = upUnknown || downUnknown || leftUnknown || rightUnknown;
- bool isSafe = m_ObstacleTransform->getValue ( x, y ) > m_FrontierSafenessFactor * m_MinAllowedObstacleDistance;
- if ( isFree && hasUnknownNeighbour && isSafe )
- {
- m_TargetMap->setValue ( x, y, 1 );
- }
- else
- {
- m_TargetMap->setValue ( x, y, 0 );
- }
+void Explorer::computeFrontierMap() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
}
- }
-}
-void Explorer::computeTargetMap()
-{
- ROS_ERROR_STREAM("target Map shouldn't be used anymore!");
- if ( m_DesiredDistance < 1 )
- {
- computeFrontierMap();
- }
- else
- {
- computeRegionMap();
- }
+ // if ( m_FrontierMap ) { return; }
+
+ resetMap(m_TargetMap);
+
+ ROS_DEBUG("Computing frontier map...");
+ m_TargetMap->fill(0);
+ // extract borders
+ for (int y = 1; y < m_OccupancyMap->height() - 1; y++) {
+ for (int x = 1; x < m_OccupancyMap->width() - 1; x++) {
+ int value = m_OccupancyMap->getValue(x, y);
+ int value_u = m_OccupancyMap->getValue(x, y - 1);
+ int value_d = m_OccupancyMap->getValue(x, y + 1);
+ int value_l = m_OccupancyMap->getValue(x - 1, y);
+ int value_r = m_OccupancyMap->getValue(x + 1, y);
+ bool isFree = value < UNKNOWN && value != NOT_SEEN_YET;
+ bool upUnknown = (value_u == UNKNOWN || value_u == NOT_SEEN_YET);
+ bool downUnknown = (value_d == UNKNOWN || value_u == NOT_SEEN_YET);
+ bool leftUnknown = (value_l == UNKNOWN || value_u == NOT_SEEN_YET);
+ bool rightUnknown = (value_r == UNKNOWN || value_u == NOT_SEEN_YET);
+ bool hasUnknownNeighbour =
+ upUnknown || downUnknown || leftUnknown || rightUnknown;
+ bool isSafe =
+ m_ObstacleTransform->getValue(x, y) >
+ m_FrontierSafenessFactor * m_MinAllowedObstacleDistance;
+ if (isFree && hasUnknownNeighbour && isSafe) {
+ m_TargetMap->setValue(x, y, 1);
+ } else {
+ m_TargetMap->setValue(x, y, 0);
+ }
+ }
+ }
}
-void Explorer::computeObstacleTransform()
-{
- if ( !m_OccupancyMap)
- {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return;
- }
+void Explorer::computeTargetMap() {
+ ROS_ERROR_STREAM("target Map shouldn't be used anymore!");
+ if (m_DesiredDistance < 1) {
+ computeFrontierMap();
+ } else {
+ computeRegionMap();
+ }
+}
- if ( m_ObstacleTransform )
- {
- return;
- }
+void Explorer::computeObstacleTransform() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return;
+ }
- resetMap( m_ObstacleTransform );
+ if (m_ObstacleTransform) {
+ return;
+ }
- ROS_DEBUG( "Computing obstacle transform..." );
- for ( int x = 0; x < m_ObstacleTransform->width(); x++ )
- {
- for ( int y = 0; y < m_ObstacleTransform->height(); y++ )
- {
- if ( m_OccupancyMap->getValue(x, y) > UNKNOWN ||
- m_OccupancyMap->getValue(x, y) == NOT_SEEN_YET)
- {
- m_ObstacleTransform->setValue ( x, y, 0 ); // Obstacle
- }
- else
- {
- m_ObstacleTransform->setValue ( x, y, INT_MAX ); // Free
- }
+ resetMap(m_ObstacleTransform);
+
+ ROS_DEBUG("Computing obstacle transform...");
+ for (int x = 0; x < m_ObstacleTransform->width(); x++) {
+ for (int y = 0; y < m_ObstacleTransform->height(); y++) {
+ if (m_OccupancyMap->getValue(x, y) > UNKNOWN ||
+ m_OccupancyMap->getValue(x, y) == NOT_SEEN_YET) {
+ m_ObstacleTransform->setValue(x, y, 0); // Obstacle
+ } else {
+ m_ObstacleTransform->setValue(x, y, INT_MAX); // Free
+ }
+ }
}
- }
- int width = m_ObstacleTransform->width();
- int height = m_ObstacleTransform->height();
- double* f = new double[width > height ? width : height];
+ int width = m_ObstacleTransform->width();
+ int height = m_ObstacleTransform->height();
+ double* f = new double[width > height ? width : height];
- // transform along columns
- for ( int x = 0; x < width; x++ )
- {
- for ( int y = 0; y < height; y++ )
- {
- // copy column
- f[y] = m_ObstacleTransform->getValue ( x, y );
- }
- // 1-D transform of column
- double* d = distanceTransform1D ( f, height );
- // copy transformed 1-D to output image
- for ( int y = 0; y < height; y++ )
- {
- m_ObstacleTransform->setValue ( x, y, d[y] );
+ // transform along columns
+ for (int x = 0; x < width; x++) {
+ for (int y = 0; y < height; y++) {
+ // copy column
+ f[y] = m_ObstacleTransform->getValue(x, y);
+ }
+ // 1-D transform of column
+ double* d = distanceTransform1D(f, height);
+ // copy transformed 1-D to output image
+ for (int y = 0; y < height; y++) {
+ m_ObstacleTransform->setValue(x, y, d[y]);
+ }
+ delete[] d;
}
- delete [] d;
- }
- // transform along rows
- for ( int y = 0; y < height; y++ )
- {
- for ( int x = 0; x < width; x++ )
- {
- f[x] = m_ObstacleTransform->getValue ( x, y );
- }
- double* d = distanceTransform1D ( f, width );
- for ( int x = 0; x < width; x++ )
- {
- m_ObstacleTransform->setValue ( x, y, d[x] );
+ // transform along rows
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ f[x] = m_ObstacleTransform->getValue(x, y);
+ }
+ double* d = distanceTransform1D(f, width);
+ for (int x = 0; x < width; x++) {
+ m_ObstacleTransform->setValue(x, y, d[x]);
+ }
+ delete[] d;
}
- delete [] d;
- }
- delete f;
-
- // take square roots
- for ( int y = 0; y < m_ObstacleTransform->height(); y++ )
- {
- for ( int x = 0; x < m_ObstacleTransform->width(); x++ )
- {
- if ( isWalkable( x,y ) )
- {
- float value = sqrt ( m_ObstacleTransform->getValue ( x, y ) );
- m_ObstacleTransform->setValue ( x, y, value );
- }
+ delete f;
+
+ // take square roots
+ for (int y = 0; y < m_ObstacleTransform->height(); y++) {
+ for (int x = 0; x < m_ObstacleTransform->width(); x++) {
+ if (isWalkable(x, y)) {
+ float value = sqrt(m_ObstacleTransform->getValue(x, y));
+ m_ObstacleTransform->setValue(x, y, value);
+ }
+ }
}
- }
}
+void Explorer::computeCostTransform() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return;
+ }
-void Explorer::computeCostTransform()
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return;
- }
-
- if ( m_CostTransform ) { return; }
-
- computeObstacleTransform();
- computeApproachableMaps();
-
- resetMap( m_CostTransform );
- m_CostTransform->fill( ExplorerConstants::MAX_COST );
+ if (m_CostTransform) {
+ return;
+ }
- for ( unsigned y=0; y<m_CostTransform->height(); y++)
- {
- for ( unsigned x=0; x<m_CostTransform->width(); x++)
- {
- if ( !isApproachable( x, y ) ) {
- continue;
- }
- double dist = m_ObstacleTransform->getValue(x, y);
- double cost = 0;
- if ( dist < m_MinSafeObstacleDistance ) {
- cost = m_MinSafeObstacleDistance - dist;
- }
-// if ( dist > m_MaxSafeObstacleDistance ) {
-// cost = dist - m_MaxSafeObstacleDistance;
-// }
- m_CostTransform->setValue( x, y, cost * cost );
+ computeObstacleTransform();
+ computeApproachableMaps();
+
+ resetMap(m_CostTransform);
+ m_CostTransform->fill(ExplorerConstants::MAX_COST);
+
+ for (unsigned y = 0; y < m_CostTransform->height(); y++) {
+ for (unsigned x = 0; x < m_CostTransform->width(); x++) {
+ if (!isApproachable(x, y)) {
+ continue;
+ }
+ double dist = m_ObstacleTransform->getValue(x, y);
+ double cost = 0;
+ if (dist < m_MinSafeObstacleDistance) {
+ cost = m_MinSafeObstacleDistance - dist;
+ }
+ // if ( dist > m_MaxSafeObstacleDistance ) {
+ // cost = dist - m_MaxSafeObstacleDistance;
+ // }
+ m_CostTransform->setValue(x, y, cost * cost);
+ }
}
- }
}
+void Explorer::computePathTransform() {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return;
+ }
-void Explorer::computePathTransform()
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return;
- }
-
- if ( m_PathTransform ) { return; }
-
- computeObstacleTransform();
- computeCostTransform();
-
- resetMap( m_PathTransform );
-
- ROS_DEBUG( "Computing path transform..." );
- GridMap<double>* map = m_PathTransform;
- int width = map->width();
- int height = map->height();
- double maxDistance = MAX_DISTANCE;
- map->fill ( maxDistance );
-
- int fromX = m_Target.x();
- int fromY = m_Target.y();
- map->setValue ( fromX, fromY, 0 );
-
- queue<int> xQueue;
- queue<int> yQueue;
- xQueue.push ( fromX + 1 );
- yQueue.push ( fromY );
- xQueue.push ( fromX - 1 );
- yQueue.push ( fromY );
- xQueue.push ( fromX );
- yQueue.push ( fromY - 1 );
- xQueue.push ( fromX );
- yQueue.push ( fromY + 1 );
- int xVal, yVal;
- while ( !xQueue.empty() )
- {
- xVal = xQueue.front();
- yVal = yQueue.front();
- xQueue.pop();
- yQueue.pop();
- if ( xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 && isWalkable( xVal, yVal ) )
- {
- float value = map->getValue ( xVal, yVal );
- float value_u = map->getValue ( xVal, yVal - 1 ) + 1;
- float value_d = map->getValue ( xVal, yVal + 1 ) + 1;
- float value_l = map->getValue ( xVal - 1, yVal ) + 1;
- float value_r = map->getValue ( xVal + 1, yVal ) + 1;
-
- float value_ur = map->getValue ( xVal + 1, yVal - 1 ) + 1.4142;
- float value_ul = map->getValue ( xVal - 1, yVal - 1 ) + 1.4142;
- float value_ll = map->getValue ( xVal - 1, yVal + 1 ) + 1.4142;
- float value_lr = map->getValue ( xVal + 1, yVal + 1 ) + 1.4142;
-
- float min1 = value_u < value_d ? value_u : value_d;
- float min2 = value_l < value_r ? value_l : value_r;
- float min3 = value_ur < value_ul ? value_ur : value_ul;
- float min4 = value_ll < value_lr ? value_ll : value_lr;
- float min12 = min1 < min2 ? min1 : min2;
- float min34 = min3 < min4 ? min3 : min4;
- float min = min12 < min34 ? min12 : min34;
- float newVal = min + m_SafePathWeight * m_CostTransform->getValue( xVal, yVal );
- if ( value > newVal )
- {
- map->setValue ( xVal, yVal, newVal );
- if ( map->getValue ( xVal, yVal + 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal + 1 );
- }
- if ( map->getValue ( xVal, yVal - 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal );
- }
- if ( map->getValue ( xVal - 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal );
- }
- if ( map->getValue ( xVal + 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal - 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal + 1 );
- }
- if ( map->getValue ( xVal - 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal + 1 );
+ if (m_PathTransform) {
+ return;
+ }
+
+ computeObstacleTransform();
+ computeCostTransform();
+
+ resetMap(m_PathTransform);
+
+ ROS_DEBUG("Computing path transform...");
+ GridMap<double>* map = m_PathTransform;
+ int width = map->width();
+ int height = map->height();
+ double maxDistance = MAX_DISTANCE;
+ map->fill(maxDistance);
+
+ int fromX = m_Target.x();
+ int fromY = m_Target.y();
+ map->setValue(fromX, fromY, 0);
+
+ queue<int> xQueue;
+ queue<int> yQueue;
+ xQueue.push(fromX + 1);
+ yQueue.push(fromY);
+ xQueue.push(fromX - 1);
+ yQueue.push(fromY);
+ xQueue.push(fromX);
+ yQueue.push(fromY - 1);
+ xQueue.push(fromX);
+ yQueue.push(fromY + 1);
+ int xVal, yVal;
+ while (!xQueue.empty()) {
+ xVal = xQueue.front();
+ yVal = yQueue.front();
+ xQueue.pop();
+ yQueue.pop();
+ if (xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 &&
+ isWalkable(xVal, yVal)) {
+ float value = map->getValue(xVal, yVal);
+ float value_u = map->getValue(xVal, yVal - 1) + 1;
+ float value_d = map->getValue(xVal, yVal + 1) + 1;
+ float value_l = map->getValue(xVal - 1, yVal) + 1;
+ float value_r = map->getValue(xVal + 1, yVal) + 1;
+
+ float value_ur = map->getValue(xVal + 1, yVal - 1) + 1.4142;
+ float value_ul = map->getValue(xVal - 1, yVal - 1) + 1.4142;
+ float value_ll = map->getValue(xVal - 1, yVal + 1) + 1.4142;
+ float value_lr = map->getValue(xVal + 1, yVal + 1) + 1.4142;
+
+ float min1 = value_u < value_d ? value_u : value_d;
+ float min2 = value_l < value_r ? value_l : value_r;
+ float min3 = value_ur < value_ul ? value_ur : value_ul;
+ float min4 = value_ll < value_lr ? value_ll : value_lr;
+ float min12 = min1 < min2 ? min1 : min2;
+ float min34 = min3 < min4 ? min3 : min4;
+ float min = min12 < min34 ? min12 : min34;
+ float newVal =
+ min + m_SafePathWeight * m_CostTransform->getValue(xVal, yVal);
+ if (value > newVal) {
+ map->setValue(xVal, yVal, newVal);
+ if (map->getValue(xVal, yVal + 1) > newVal + 1) {
+ xQueue.push(xVal);
+ yQueue.push(yVal + 1);
+ }
+ if (map->getValue(xVal, yVal - 1) > newVal + 1) {
+ xQueue.push(xVal);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal + 1, yVal) > newVal + 1) {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal - 1, yVal) > newVal + 1) {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal + 1, yVal - 1) > newVal + 1.4142) {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal - 1, yVal - 1) > newVal + 1.4142) {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal + 1, yVal + 1) > newVal + 1.4142) {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal + 1);
+ }
+ if (map->getValue(xVal - 1, yVal + 1) > newVal + 1.4142) {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal + 1);
+ }
+ }
}
- }
}
- }
}
+void Explorer::computeExplorationTransform() {
+ ROS_ERROR_STREAM("Exploration Transform shouldn't be used!");
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return;
+ }
-void Explorer::computeExplorationTransform()
-{
- ROS_ERROR_STREAM("Exploration Transform shouldn't be used!");
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return;
- }
-
- if ( m_ExplorationTransform ) { return; }
-
- ROS_DEBUG_STREAM("computeExplorationTransform: before obstacle transform");
- computeObstacleTransform();
- ROS_DEBUG_STREAM("computeExplorationTransform: before cost transform");
- computeCostTransform();
- ROS_DEBUG_STREAM("computeExplorationTransform: before target map");
- computeTargetMap();
- ROS_DEBUG_STREAM("computeExplorationTransform: before walkable maps");
- computeWalkableMaps();
- ROS_DEBUG_STREAM("computeExplorationTransform: before exploration transform");
- resetMap( m_ExplorationTransform );
-
- ROS_DEBUG( "Computing exploration transform..." );
- GridMap<double>* map = m_ExplorationTransform;
- int width = map->width();
- int height = map->height();
- double maxDistance = MAX_DISTANCE;
- map->fill ( maxDistance );
- queue<int> xQueue;
- queue<int> yQueue;
- // fill seeds: Mark the frontiers as targets
- ROS_DEBUG_STREAM("computeExplorationTransform: before first loop");
- for ( int y = 0; y < m_TargetMap->height(); y++ )
- {
- for ( int x = 0; x < m_TargetMap->width(); x++ )
- {
- if ( m_TargetMap->getValue ( x, y ) == 1 )
- {
- map->setValue ( x, y, 0 );
- xQueue.push ( x + 1 );
- yQueue.push ( y );
- xQueue.push ( x - 1 );
- yQueue.push ( y );
- xQueue.push ( x );
- yQueue.push ( y - 1 );
- xQueue.push ( x );
- yQueue.push ( y + 1 );
- }
+ if (m_ExplorationTransform) {
+ return;
}
- }
- ROS_DEBUG_STREAM("computeExplorationTransform: After first looop");
- // Now go through the coordinates in the queue
- int xVal, yVal;
- ROS_DEBUG_STREAM("computeExplorationTransform: before while loop");
- while ( !xQueue.empty() )
- {
- xVal = xQueue.front();
- yVal = yQueue.front();
- xQueue.pop();
- yQueue.pop();
- if ( xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 && isWalkable ( xVal, yVal ) )
- {
- // Get own cost and the cost of the 8 neighbor cells (neighbors plus the cost to go there)
- float value = map->getValue ( xVal, yVal );
- float value_u = map->getValue ( xVal, yVal - 1 ) + 1;
- float value_d = map->getValue ( xVal, yVal + 1 ) + 1;
- float value_l = map->getValue ( xVal - 1, yVal ) + 1;
- float value_r = map->getValue ( xVal + 1, yVal ) + 1;
- float value_ur = map->getValue ( xVal + 1, yVal - 1 ) + 1.4142;
- float value_ul = map->getValue ( xVal - 1, yVal - 1 ) + 1.4142;
- float value_ll = map->getValue ( xVal - 1, yVal + 1 ) + 1.4142;
- float value_lr = map->getValue ( xVal + 1, yVal + 1 ) + 1.4142;
- float min1 = value_u < value_d ? value_u : value_d;
- float min2 = value_l < value_r ? value_l : value_r;
- float min3 = value_ur < value_ul ? value_ur : value_ul;
- float min4 = value_ll < value_lr ? value_ll : value_lr;
- float min12 = min1 < min2 ? min1 : min2;
- float min34 = min3 < min4 ? min3 : min4;
- float min = min12 < min34 ? min12 : min34;
- float newVal = min + m_SafePathWeight * m_CostTransform->getValue( xVal, yVal );
- if ( value > newVal )
- {
- // Cost is lower then the currently known cost: Reduce cost here
- map->setValue ( xVal, yVal, newVal );
- // Add the neighbours that might profit in the queue
- if ( map->getValue ( xVal, yVal + 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal + 1 );
- }
- if ( map->getValue ( xVal, yVal - 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal );
- }
- if ( map->getValue ( xVal - 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal );
- }
- if ( map->getValue ( xVal + 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal - 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal + 1 );
+
+ ROS_DEBUG_STREAM("computeExplorationTransform: before obstacle transform");
+ computeObstacleTransform();
+ ROS_DEBUG_STREAM("computeExplorationTransform: before cost transform");
+ computeCostTransform();
+ ROS_DEBUG_STREAM("computeExplorationTransform: before target map");
+ computeTargetMap();
+ ROS_DEBUG_STREAM("computeExplorationTransform: before walkable maps");
+ computeWalkableMaps();
+ ROS_DEBUG_STREAM(
+ "computeExplorationTransform: before exploration transform");
+ resetMap(m_ExplorationTransform);
+
+ ROS_DEBUG("Computing exploration transform...");
+ GridMap<double>* map = m_ExplorationTransform;
+ int width = map->width();
+ int height = map->height();
+ double maxDistance = MAX_DISTANCE;
+ map->fill(maxDistance);
+ queue<int> xQueue;
+ queue<int> yQueue;
+ // fill seeds: Mark the frontiers as targets
+ ROS_DEBUG_STREAM("computeExplorationTransform: before first loop");
+ for (int y = 0; y < m_TargetMap->height(); y++) {
+ for (int x = 0; x < m_TargetMap->width(); x++) {
+ if (m_TargetMap->getValue(x, y) == 1) {
+ map->setValue(x, y, 0);
+ xQueue.push(x + 1);
+ yQueue.push(y);
+ xQueue.push(x - 1);
+ yQueue.push(y);
+ xQueue.push(x);
+ yQueue.push(y - 1);
+ xQueue.push(x);
+ yQueue.push(y + 1);
+ }
}
- if ( map->getValue ( xVal - 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal + 1 );
+ }
+ ROS_DEBUG_STREAM("computeExplorationTransform: After first looop");
+ // Now go through the coordinates in the queue
+ int xVal, yVal;
+ ROS_DEBUG_STREAM("computeExplorationTransform: before while loop");
+ while (!xQueue.empty()) {
+ xVal = xQueue.front();
+ yVal = yQueue.front();
+ xQueue.pop();
+ yQueue.pop();
+ if (xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 &&
+ isWalkable(xVal, yVal)) {
+ // Get own cost and the cost of the 8 neighbor cells (neighbors plus
+ // the cost to go there)
+ float value = map->getValue(xVal, yVal);
+ float value_u = map->getValue(xVal, yVal - 1) + 1;
+ float value_d = map->getValue(xVal, yVal + 1) + 1;
+ float value_l = map->getValue(xVal - 1, yVal) + 1;
+ float value_r = map->getValue(xVal + 1, yVal) + 1;
+ float value_ur = map->getValue(xVal + 1, yVal - 1) + 1.4142;
+ float value_ul = map->getValue(xVal - 1, yVal - 1) + 1.4142;
+ float value_ll = map->getValue(xVal - 1, yVal + 1) + 1.4142;
+ float value_lr = map->getValue(xVal + 1, yVal + 1) + 1.4142;
+ float min1 = value_u < value_d ? value_u : value_d;
+ float min2 = value_l < value_r ? value_l : value_r;
+ float min3 = value_ur < value_ul ? value_ur : value_ul;
+ float min4 = value_ll < value_lr ? value_ll : value_lr;
+ float min12 = min1 < min2 ? min1 : min2;
+ float min34 = min3 < min4 ? min3 : min4;
+ float min = min12 < min34 ? min12 : min34;
+ float newVal =
+ min + m_SafePathWeight * m_CostTransform->getValue(xVal, yVal);
+ if (value > newVal) {
+ // Cost is lower then the currently known cost: Reduce cost here
+ map->setValue(xVal, yVal, newVal);
+ // Add the neighbours that might profit in the queue
+ if (map->getValue(xVal, yVal + 1) > newVal + 1) {
+ xQueue.push(xVal);
+ yQueue.push(yVal + 1);
+ }
+ if (map->getValue(xVal, yVal - 1) > newVal + 1) {
+ xQueue.push(xVal);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal + 1, yVal) > newVal + 1) {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal - 1, yVal) > newVal + 1) {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal + 1, yVal - 1) > newVal + 1.4142) {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal - 1, yVal - 1) > newVal + 1.4142) {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal + 1, yVal + 1) > newVal + 1.4142) {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal + 1);
+ }
+ if (map->getValue(xVal - 1, yVal + 1) > newVal + 1.4142) {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal + 1);
+ }
+ }
}
- }
}
- }
- ROS_DEBUG_STREAM("computeExplorationTransform: after exploration transform");
+ ROS_DEBUG_STREAM(
+ "computeExplorationTransform: after exploration transform");
}
+vector<Eigen::Vector2i> Explorer::sampleWaypointsFromPath(
+ std::vector<Eigen::Vector2i> pathPoints, float threshold) {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return pathPoints;
+ }
+ if (pathPoints.size() < 3) {
+ return pathPoints;
+ }
-vector<Eigen::Vector2i> Explorer::sampleWaypointsFromPath ( std::vector<Eigen::Vector2i> pathPoints, float threshold )
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return pathPoints;
- }
- if ( pathPoints.size() < 3 ) {
- return pathPoints;
- }
-
- computeObstacleTransform();
+ computeObstacleTransform();
+
+ vector<Eigen::Vector2i> simplifiedPath;
+ simplifiedPath.reserve(pathPoints.size());
+
+ Eigen::Vector2i lastAddedPoint = pathPoints[0];
+ simplifiedPath.push_back(lastAddedPoint);
+
+ for (unsigned int i = 1; i < pathPoints.size() - 1; i++) {
+ double distanceToNextPoint =
+ map_tools::distance(lastAddedPoint, pathPoints.at(i));
+ double obstacleDistanceLastAddedPoint = m_ObstacleTransform->getValue(
+ lastAddedPoint.x(), lastAddedPoint.y());
+ double obstacleDistancePossibleNextPoint =
+ m_ObstacleTransform->getValue(pathPoints[i].x(), pathPoints[i].y());
+ if ((distanceToNextPoint >=
+ obstacleDistanceLastAddedPoint * threshold) ||
+ (distanceToNextPoint >=
+ obstacleDistancePossibleNextPoint * threshold)) {
+ simplifiedPath.push_back(pathPoints[i]);
+ lastAddedPoint = pathPoints[i];
+ }
+ }
+ simplifiedPath.push_back(pathPoints[pathPoints.size() - 1]);
+ return simplifiedPath;
+}
- vector<Eigen::Vector2i> simplifiedPath;
- simplifiedPath.reserve( pathPoints.size() );
+std::vector<Eigen::Vector2i> Explorer::getPath(bool& success) {
+ success = false;
- Eigen::Vector2i lastAddedPoint = pathPoints[0];
- simplifiedPath.push_back ( lastAddedPoint );
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return vector<Eigen::Vector2i>();
+ }
- for ( unsigned int i = 1; i < pathPoints.size() - 1; i++ )
- {
- double distanceToNextPoint = map_tools::distance(lastAddedPoint, pathPoints.at(i));
- double obstacleDistanceLastAddedPoint = m_ObstacleTransform->getValue ( lastAddedPoint.x(), lastAddedPoint.y() );
- double obstacleDistancePossibleNextPoint = m_ObstacleTransform->getValue ( pathPoints[i].x(), pathPoints[i].y() );
- if (( distanceToNextPoint >= obstacleDistanceLastAddedPoint*threshold ) ||
- ( distanceToNextPoint >= obstacleDistancePossibleNextPoint*threshold ) )
- {
- simplifiedPath.push_back ( pathPoints[i] );
- lastAddedPoint = pathPoints[i];
+ if (m_DesiredDistance > 0) {
+ // we are actually performing an exploration since the target
+ // is a region.
+ ROS_ERROR_STREAM(
+ "Desired Distance > 0: Executing getExplorationTransformPath");
+ return getExplorationTransformPath(success);
}
- }
- simplifiedPath.push_back ( pathPoints[pathPoints.size() - 1] );
- return simplifiedPath;
-}
+ ROS_DEBUG_STREAM("Computing Path Transform");
+ computePathTransform();
+ ROS_DEBUG_STREAM("Finished Path Transform");
+ vector<Eigen::Vector2i> path;
-std::vector<Eigen::Vector2i> Explorer::getPath(bool& success)
-{
- success = false;
-
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return vector<Eigen::Vector2i>();
- }
-
- if ( m_DesiredDistance > 0 ) {
- // we are actually performing an exploration since the target
- // is a region.
- ROS_ERROR_STREAM("Desired Distance > 0: Executing getExplorationTransformPath");
- return getExplorationTransformPath( success );
- }
- ROS_DEBUG_STREAM("Computing Path Transform");
- computePathTransform();
- ROS_DEBUG_STREAM("Finished Path Transform");
-
- vector<Eigen::Vector2i> path;
-
- int x = m_Start.x();
- int y = m_Start.y();
-
- int width = m_OccupancyMap->width();
- int height = m_OccupancyMap->height();
-
- //special case: start and end point are equal, return single waypoint
- if ( map_tools::distance( m_Start, m_Target ) < 2.0 )
- {
- success = true;
- path.push_back ( Eigen::Vector2i( m_Start.x(), m_Start.y() ) );
- return path;
- }
+ int x = m_Start.x();
+ int y = m_Start.y();
- while ( x != m_Target.x() || y != m_Target.y() )
- {
- path.push_back ( Eigen::Vector2i( x, y ) );
- int minPosX = x;
- int minPosY = y;
- double min = m_PathTransform->getValue ( x, y );
+ int width = m_OccupancyMap->width();
+ int height = m_OccupancyMap->height();
- if ( ( x <= 1 ) || ( y <= 1 ) || ( x >= width-1 ) || ( y >= height-1 ) )
- {
- ROS_ERROR( "Out of map bounds" );
- return vector<Eigen::Vector2i>();
+ // special case: start and end point are equal, return single waypoint
+ if (map_tools::distance(m_Start, m_Target) < 2.0) {
+ success = true;
+ path.push_back(Eigen::Vector2i(m_Start.x(), m_Start.y()));
+ return path;
}
- for ( int i = -1; i <= 1; i++ )
- {
- for ( int j = -1; j <= 1; j++ )
- {
- double pt = m_PathTransform->getValue ( x + i, y + j );
- if ( pt < min )
- {
- min = pt;
- minPosX = x + i;
- minPosY = y + j;
+ while (x != m_Target.x() || y != m_Target.y()) {
+ path.push_back(Eigen::Vector2i(x, y));
+ int minPosX = x;
+ int minPosY = y;
+ double min = m_PathTransform->getValue(x, y);
+
+ if ((x <= 1) || (y <= 1) || (x >= width - 1) || (y >= height - 1)) {
+ ROS_ERROR("Out of map bounds");
+ return vector<Eigen::Vector2i>();
+ }
+
+ for (int i = -1; i <= 1; i++) {
+ for (int j = -1; j <= 1; j++) {
+ double pt = m_PathTransform->getValue(x + i, y + j);
+ if (pt < min) {
+ min = pt;
+ minPosX = x + i;
+ minPosY = y + j;
+ }
+ }
+ }
+ if (minPosX == x && minPosY == y) {
+ ROS_WARN("Target is unreachable!");
+ return vector<Eigen::Vector2i>();
+ } else {
+ x = minPosX;
+ y = minPosY;
}
- }
- }
- if ( minPosX == x && minPosY == y )
- {
- ROS_WARN( "Target is unreachable!" );
- return vector<Eigen::Vector2i>();
- }
- else
- {
- x = minPosX;
- y = minPosY;
}
- }
- success = true;
+ success = true;
- return path;
+ return path;
}
-vector<Eigen::Vector2i> Explorer::getExplorationTransformPath(bool& success)
-{
- success = false;
-
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return vector<Eigen::Vector2i>();
- }
-
- ROS_DEBUG_STREAM("Exploration Transform: Before obstacle transform");
- computeObstacleTransform();
- ROS_DEBUG_STREAM("Exploration Transform: Before exploration transform");
- computeExplorationTransform();
- ROS_DEBUG_STREAM("Exploration Transform: after obstacle transform");
-
- //check if we are already there
- if ( m_TargetMap->getValue ( m_Start.x(), m_Start.y() ) )
- {
- success = true;
- vector<Eigen::Vector2i> path;
- path.push_back ( Eigen::Vector2i ( m_Start.x(), m_Start.y() ) );
- return path;
- }
+vector<Eigen::Vector2i> Explorer::getExplorationTransformPath(bool& success) {
+ success = false;
- int width = m_OccupancyMap->width();
- int height = m_OccupancyMap->height();
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return vector<Eigen::Vector2i>();
+ }
- vector<Eigen::Vector2i> path;
- int x = m_Start.x();
- int y = m_Start.y();
+ ROS_DEBUG_STREAM("Exploration Transform: Before obstacle transform");
+ computeObstacleTransform();
+ ROS_DEBUG_STREAM("Exploration Transform: Before exploration transform");
+ computeExplorationTransform();
+ ROS_DEBUG_STREAM("Exploration Transform: after obstacle transform");
+
+ // check if we are already there
+ if (m_TargetMap->getValue(m_Start.x(), m_Start.y())) {
+ success = true;
+ vector<Eigen::Vector2i> path;
+ path.push_back(Eigen::Vector2i(m_Start.x(), m_Start.y()));
+ return path;
+ }
- if ( m_ObstacleTransform->getValue ( x, y ) < m_MinAllowedObstacleDistance )
- {
- // robot got stuck!
- // find way out using ObstacleTransform...
- int maxPosX = x;
- int maxPosY = y;
+ int width = m_OccupancyMap->width();
+ int height = m_OccupancyMap->height();
- if ( ( x <= 1 ) || ( y <= 1 ) || ( x >= width-1 ) || ( y >= height-1 ) )
- {
- ROS_ERROR( "Out of map bounds" );
- return vector<Eigen::Vector2i>();
- }
+ vector<Eigen::Vector2i> path;
+ int x = m_Start.x();
+ int y = m_Start.y();
+
+ if (m_ObstacleTransform->getValue(x, y) < m_MinAllowedObstacleDistance) {
+ // robot got stuck!
+ // find way out using ObstacleTransform...
+ int maxPosX = x;
+ int maxPosY = y;
+
+ if ((x <= 1) || (y <= 1) || (x >= width - 1) || (y >= height - 1)) {
+ ROS_ERROR("Out of map bounds");
+ return vector<Eigen::Vector2i>();
+ }
- while ( m_ObstacleTransform->getValue ( maxPosX, maxPosY ) < m_MinAllowedObstacleDistance )
- {
- double max = m_ObstacleTransform->getValue ( x, y );
- for ( int i = -1; i <= 1; i++ )
- {
- for ( int j = -1; j <= 1; j++ )
- {
- double pt = m_ObstacleTransform->getValue ( x + i, y + j );
- if ( pt > max )
- {
- max = pt;
- maxPosX = x + i;
- maxPosY = y + j;
- }
+ while (m_ObstacleTransform->getValue(maxPosX, maxPosY) <
+ m_MinAllowedObstacleDistance) {
+ double max = m_ObstacleTransform->getValue(x, y);
+ for (int i = -1; i <= 1; i++) {
+ for (int j = -1; j <= 1; j++) {
+ double pt = m_ObstacleTransform->getValue(x + i, y + j);
+ if (pt > max) {
+ max = pt;
+ maxPosX = x + i;
+ maxPosY = y + j;
+ }
+ }
+ }
+ if (maxPosX == x && maxPosY == y) // no ascentFound
+ {
+ break;
+ } else {
+ path.push_back(Eigen::Vector2i(maxPosX, maxPosY));
+ x = maxPosX;
+ y = maxPosY;
+ }
}
- }
- if ( maxPosX == x && maxPosY == y ) // no ascentFound
- {
- break;
- }
- else
- {
- path.push_back ( Eigen::Vector2i ( maxPosX, maxPosY ) );
- x = maxPosX;
- y = maxPosY;
- }
- }
- }
- // now path is "free"
- bool descentFound = true;
- while ( descentFound )
- {
- descentFound = false;
- int minPosX = x;
- int minPosY = y;
- double min = m_ExplorationTransform->getValue ( x, y );
- if ( ( x <= 1 ) || ( y <= 1 ) || ( x >= width-1 ) || ( y >= height-1 ) )
- {
- ROS_ERROR( "Out of map bounds" );
- return vector<Eigen::Vector2i>();
}
+ // now path is "free"
+ bool descentFound = true;
+ while (descentFound) {
+ descentFound = false;
+ int minPosX = x;
+ int minPosY = y;
+ double min = m_ExplorationTransform->getValue(x, y);
+ if ((x <= 1) || (y <= 1) || (x >= width - 1) || (y >= height - 1)) {
+ ROS_ERROR("Out of map bounds");
+ return vector<Eigen::Vector2i>();
+ }
- for ( int i = -1; i <= 1; i++ )
- {
- for ( int j = -1; j <= 1; j++ )
- {
- double pt = m_ExplorationTransform->getValue ( x + i, y + j );
- if ( pt < min )
+ for (int i = -1; i <= 1; i++) {
+ for (int j = -1; j <= 1; j++) {
+ double pt = m_ExplorationTransform->getValue(x + i, y + j);
+ if (pt < min) {
+ min = pt;
+ minPosX = x + i;
+ minPosY = y + j;
+ }
+ }
+ }
+ if (minPosX == x && minPosY == y) // no descentFound
{
- min = pt;
- minPosX = x + i;
- minPosY = y + j;
+ descentFound = false;
+ } else {
+ descentFound = true;
+ path.push_back(Eigen::Vector2i(minPosX, minPosY));
+ x = minPosX;
+ y = minPosY;
}
- }
- }
- if ( minPosX == x && minPosY == y ) // no descentFound
- {
- descentFound = false;
- }
- else
- {
- descentFound = true;
- path.push_back ( Eigen::Vector2i ( minPosX, minPosY ) );
- x = minPosX;
- y = minPosY;
}
- }
- success = true;
+ success = true;
- ROS_INFO_STREAM("Exploration Transform: End of function");
- return path;
+ ROS_INFO_STREAM("Exploration Transform: End of function");
+ return path;
#if 0
// START P2AT HACK
@@ -1212,197 +1106,174 @@ vector<Eigen::Vector2i> Explorer::getExplorationTransformPath(bool& success)
#endif
}
+bool Explorer::getNearestFrontier(Eigen::Vector2i& nextFrontier) {
+ if (!m_OccupancyMap) {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return false;
+ }
-bool Explorer::getNearestFrontier ( Eigen::Vector2i& nextFrontier )
-{
- if ( !m_OccupancyMap) {
- ROS_ERROR( "Missing occupancy map. Aborting." );
- return false;
- }
-
- computeFrontierMap();
- computeDrivingDistanceTransform();
-
- bool found = false;
- int distXPos = -1;
- int distYPos = -1;
- double dist = 10000000;
- for ( int y = 0; y < m_TargetMap->height(); y++ )
- {
- for ( int x = 0; x < m_TargetMap->width(); x++ )
- {
- if ( m_TargetMap->getValue ( x, y ) == 1 && m_DrivingDistanceTransform->getValue ( x, y ) < 999999 )
- {
- if ( m_DrivingDistanceTransform->getValue ( x, y ) < dist )
- {
- found = true;
- dist = m_DrivingDistanceTransform->getValue ( x, y );
- distXPos = x;
- distYPos = y;
+ computeFrontierMap();
+ computeDrivingDistanceTransform();
+
+ bool found = false;
+ int distXPos = -1;
+ int distYPos = -1;
+ double dist = 10000000;
+ for (int y = 0; y < m_TargetMap->height(); y++) {
+ for (int x = 0; x < m_TargetMap->width(); x++) {
+ if (m_TargetMap->getValue(x, y) == 1 &&
+ m_DrivingDistanceTransform->getValue(x, y) < 999999) {
+ if (m_DrivingDistanceTransform->getValue(x, y) < dist) {
+ found = true;
+ dist = m_DrivingDistanceTransform->getValue(x, y);
+ distXPos = x;
+ distYPos = y;
+ }
+ }
}
- }
}
- }
- if ( found )
- {
- nextFrontier.x() = distXPos;
- nextFrontier.y() = distYPos;
- return true;
- }
- else
- {
- return false;
- }
+ if (found) {
+ nextFrontier.x() = distXPos;
+ nextFrontier.y() = distYPos;
+ return true;
+ } else {
+ return false;
+ }
}
+// HELPERS
+// //////////////////////////////////////////////////////////////////////////////////////////////////////////
+void Explorer::distanceFloodFill(GridMap<double>* map, Eigen::Vector2i start) {
+ if (!map) {
+ ROS_ERROR("Received 0-pointer!");
+ }
-// HELPERS //////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-
-void Explorer::distanceFloodFill ( GridMap<double>* map, Eigen::Vector2i start )
-{
- if ( !map ) {
- ROS_ERROR( "Received 0-pointer!" );
- }
-
- computeObstacleTransform();
-
- int width = map->width();
- int height = map->height();
- map->fill ( MAX_DISTANCE );
-
- int fromX = start.x();
- int fromY = start.y();
- map->setValue ( fromX, fromY, 0 );
-
- queue<int> xQueue;
- queue<int> yQueue;
- xQueue.push ( fromX + 1 );
- yQueue.push ( fromY );
- xQueue.push ( fromX - 1 );
- yQueue.push ( fromY );
- xQueue.push ( fromX );
- yQueue.push ( fromY - 1 );
- xQueue.push ( fromX );
- yQueue.push ( fromY + 1 );
- int xVal, yVal;
- while ( !xQueue.empty() )
- {
- xVal = xQueue.front();
- yVal = yQueue.front();
- xQueue.pop();
- yQueue.pop();
- bool isFree = (m_OccupancyMap->getValue ( xVal, yVal ) < UNKNOWN ||
- m_OccupancyMap->getValue ( xVal, yVal ) != NOT_SEEN_YET); // only fill free cells
- bool isSafe = m_ObstacleTransform->getValue ( xVal, yVal ) > m_MinAllowedObstacleDistance;
- if ( xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 && isFree && isSafe )
- {
- float value = map->getValue ( xVal, yVal );
- float value_u = map->getValue ( xVal, yVal - 1 ) + 1;
- float value_d = map->getValue ( xVal, yVal + 1 ) + 1;
- float value_l = map->getValue ( xVal - 1, yVal ) + 1;
- float value_r = map->getValue ( xVal + 1, yVal ) + 1;
-
- float value_ur = map->getValue ( xVal + 1, yVal - 1 ) + 1.4142;
- float value_ul = map->getValue ( xVal - 1, yVal - 1 ) + 1.4142;
- float value_ll = map->getValue ( xVal - 1, yVal + 1 ) + 1.4142;
- float value_lr = map->getValue ( xVal + 1, yVal + 1 ) + 1.4142;
-
- float min1 = value_u < value_d ? value_u : value_d;
- float min2 = value_l < value_r ? value_l : value_r;
- float min3 = value_ur < value_ul ? value_ur : value_ul;
- float min4 = value_ll < value_lr ? value_ll : value_lr;
- float min12 = min1 < min2 ? min1 : min2;
- float min34 = min3 < min4 ? min3 : min4;
- float min = min12 < min34 ? min12 : min34;
- float newVal = min;
- if ( value > newVal )
- {
- map->setValue ( xVal, yVal, newVal );
- if ( map->getValue ( xVal, yVal + 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal + 1 );
- }
- if ( map->getValue ( xVal, yVal - 1 ) > newVal + 1 )
- {
- xQueue.push ( xVal );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal );
+ computeObstacleTransform();
+
+ int width = map->width();
+ int height = map->height();
+ map->fill(MAX_DISTANCE);
+
+ int fromX = start.x();
+ int fromY = start.y();
+ map->setValue(fromX, fromY, 0);
+
+ queue<int> xQueue;
+ queue<int> yQueue;
+ xQueue.push(fromX + 1);
+ yQueue.push(fromY);
+ xQueue.push(fromX - 1);
+ yQueue.push(fromY);
+ xQueue.push(fromX);
+ yQueue.push(fromY - 1);
+ xQueue.push(fromX);
+ yQueue.push(fromY + 1);
+ int xVal, yVal;
+ while (!xQueue.empty()) {
+ xVal = xQueue.front();
+ yVal = yQueue.front();
+ xQueue.pop();
+ yQueue.pop();
+ bool isFree = (m_OccupancyMap->getValue(xVal, yVal) < UNKNOWN ||
+ m_OccupancyMap->getValue(xVal, yVal) !=
+ NOT_SEEN_YET); // only fill free cells
+ bool isSafe = m_ObstacleTransform->getValue(xVal, yVal) >
+ m_MinAllowedObstacleDistance;
+ if (xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 &&
+ isFree && isSafe) {
+ float value = map->getValue(xVal, yVal);
+ float value_u = map->getValue(xVal, yVal - 1) + 1;
+ float value_d = map->getValue(xVal, yVal + 1) + 1;
+ float value_l = map->getValue(xVal - 1, yVal) + 1;
+ float value_r = map->getValue(xVal + 1, yVal) + 1;
+
+ float value_ur = map->getValue(xVal + 1, yVal - 1) + 1.4142;
+ float value_ul = map->getValue(xVal - 1, yVal - 1) + 1.4142;
+ float value_ll = map->getValue(xVal - 1, yVal + 1) + 1.4142;
+ float value_lr = map->getValue(xVal + 1, yVal + 1) + 1.4142;
+
+ float min1 = value_u < value_d ? value_u : value_d;
+ float min2 = value_l < value_r ? value_l : value_r;
+ float min3 = value_ur < value_ul ? value_ur : value_ul;
+ float min4 = value_ll < value_lr ? value_ll : value_lr;
+ float min12 = min1 < min2 ? min1 : min2;
+ float min34 = min3 < min4 ? min3 : min4;
+ float min = min12 < min34 ? min12 : min34;
+ float newVal = min;
+ if (value > newVal) {
+ map->setValue(xVal, yVal, newVal);
+ if (map->getValue(xVal, yVal + 1) > newVal + 1) {
+ xQueue.push(xVal);
+ yQueue.push(yVal + 1);
+ }
+ if (map->getValue(xVal, yVal - 1) > newVal + 1) {
+ xQueue.push(xVal);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal + 1, yVal) > newVal + 1) {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal - 1, yVal) > newVal + 1) {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal + 1, yVal - 1) > newVal + 1.4142) {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal - 1, yVal - 1) > newVal + 1.4142) {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal + 1, yVal + 1) > newVal + 1.4142) {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal + 1);
+ }
+ if (map->getValue(xVal - 1, yVal + 1) > newVal + 1.4142) {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal + 1);
+ }
+ }
}
- if ( map->getValue ( xVal - 1, yVal ) > newVal + 1 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal );
- }
- if ( map->getValue ( xVal + 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal - 1, yVal - 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal - 1 );
- }
- if ( map->getValue ( xVal + 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal + 1 );
- yQueue.push ( yVal + 1 );
- }
- if ( map->getValue ( xVal - 1, yVal + 1 ) > newVal + 1.4142 )
- {
- xQueue.push ( xVal - 1 );
- yQueue.push ( yVal + 1 );
- }
- }
}
- }
}
-
// Implementation taken from http://www.cs.cmu.edu/~cil/vnew.html
-double* Explorer::distanceTransform1D ( double *f, int n )
-{
- //int width = m_OccupancyMap->width();
- //int height = m_OccupancyMap->height();
- //double maxDistance = height > width ? height : width;
-
- double *d = new double[n];
- int *v = new int[n];
- double *z = new double[n+1];
- int k = 0;
- v[0] = 0;
- z[0] = -INT_MAX;
- z[1] = INT_MAX;
- for ( int q = 1; q <= n-1; q++ )
- {
- double s = ( ( f[q]+ ( q*q ) )- ( f[v[k]]+ ( v[k]*v[k] ) ) ) / ( 2*q-2*v[k] );
- while ( s <= z[k] )
- {
- k--;
- s = ( ( f[q]+ ( q*q ) )- ( f[v[k]]+ ( v[k]*v[k] ) ) ) / ( 2*q-2*v[k] );
+double* Explorer::distanceTransform1D(double* f, int n) {
+ // int width = m_OccupancyMap->width();
+ // int height = m_OccupancyMap->height();
+ // double maxDistance = height > width ? height : width;
+
+ double* d = new double[n];
+ int* v = new int[n];
+ double* z = new double[n + 1];
+ int k = 0;
+ v[0] = 0;
+ z[0] = -INT_MAX;
+ z[1] = INT_MAX;
+ for (int q = 1; q <= n - 1; q++) {
+ double s =
+ ((f[q] + (q * q)) - (f[v[k]] + (v[k] * v[k]))) / (2 * q - 2 * v[k]);
+ while (s <= z[k]) {
+ k--;
+ s = ((f[q] + (q * q)) - (f[v[k]] + (v[k] * v[k]))) /
+ (2 * q - 2 * v[k]);
+ }
+ k++;
+ v[k] = q;
+ z[k] = s;
+ z[k + 1] = INT_MAX;
}
- k++;
- v[k] = q;
- z[k] = s;
- z[k+1] = INT_MAX;
- }
- k = 0;
- for ( int q = 0; q <= n-1; q++ )
- {
- while ( z[k+1] < q )
- k++;
- d[q] = ( ( q-v[k] ) * ( q-v[k] ) ) + f[v[k]];
- }
-
- delete [] v;
- delete [] z;
- return d;
+ k = 0;
+ for (int q = 0; q <= n - 1; q++) {
+ while (z[k + 1] < q) k++;
+ d[q] = ((q - v[k]) * (q - v[k])) + f[v[k]];
+ }
+
+ delete[] v;
+ delete[] z;
+ return d;
}