Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13207] pkg/trunk
|
From: <ei...@us...> - 2009-04-01 02:23:13
|
Revision: 13207
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13207&view=rev
Author: eitanme
Date: 2009-04-01 01:32:45 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r20002@cib: eitan | 2009-03-24 17:02:20 -0700
Bug fix and optimization
Modified Paths:
--------------
pkg/trunk/world_models/new_costmap/CMakeLists.txt
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/manifest.xml
pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20001
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20002
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/world_models/new_costmap/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/new_costmap/CMakeLists.txt 2009-04-01 01:32:19 UTC (rev 13206)
+++ pkg/trunk/world_models/new_costmap/CMakeLists.txt 2009-04-01 01:32:45 UTC (rev 13207)
@@ -8,3 +8,6 @@
rospack_add_library(new_costmap_2d src/costmap_2d.cpp)
#rospack_link_boost(costmap_2d thread)
+
+rospack_add_executable(costmap_test src/costmap_test.cpp)
+target_link_libraries(costmap_test new_costmap_2d)
Modified: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:32:19 UTC (rev 13206)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:32:45 UTC (rev 13207)
@@ -58,7 +58,7 @@
unsigned int src_x_, src_y_;
};
- bool operator<(const CellData &a, const CellData &b){
+ inline bool operator<(const CellData &a, const CellData &b){
return a.distance_ < b.distance_;
}
@@ -70,17 +70,6 @@
public:
/**
* @brief Constructor for a costmap
- * @param meters_size_x The x size of the map in meters
- * @param meters_size_y The y size of the map in meters
- * @param resolution The resolution of the map in meters/cell
- * @param origin_x The x origin of the map
- * @param origin_y The y origin of the map
- */
- Costmap2D(double meters_size_x, double meters_size_y,
- double resolution, double origin_x, double origin_y);
-
- /**
- * @brief Constructor for a costmap
* @param cells_size_x The x size of the map in cells
* @param cells_size_y The y size of the map in cells
* @param resolution The resolution of the map in meters/cell
@@ -103,6 +92,23 @@
const std::vector<unsigned char>& static_data, unsigned char lethal_threshold);
/**
+ * @brief Revert to the static map outside of a specified window centered at a world coordinate
+ * @param wx The x coordinate of the center point of the window in world space (meters)
+ * @param wy The y coordinate of the center point of the window in world space (meters)
+ * @param w_size_x The x size of the window in meters
+ * @param w_size_y The y size of the window in meters
+ */
+ void resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y);
+
+ /**
+ * @brief Update the costmap with new observations
+ * @param obstacles The point clouds of obstacles to insert into the map
+ * @param clearing_observations The set of observations to use for raytracing
+ */
+ void updateWorld(double robot_x, double robot_y,
+ const std::vector<Observation>& observations, const std::vector<Observation>& clearing_observations);
+
+ /**
* @brief Get the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
@@ -125,52 +131,32 @@
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
- * @return true if the conversion was successful (legal bounds) false otherwise
+ * @return True if the conversion was successful (legal bounds) false otherwise
*/
bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
+ /**
+ * @brief Given two map coordinates... compute the associated index
+ * @param mx The x coordinate
+ * @param my The y coordinate
+ * @return The associated index
+ */
inline unsigned int getIndex(unsigned int mx, unsigned int my) const{
return my * size_x_ + mx;
}
+ /**
+ * @brief Given an index... compute the associated map coordinates
+ * @param index The index
+ * @param mx Will be set to the x coordinate
+ * @param my Will be set to the y coordinate
+ */
inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const{
my = index / size_x_;
mx = index - (my * size_x_);
}
- inline void updateCellCost(unsigned int index, unsigned char cost){
- unsigned char* cell_cost = &cost_map_[index];
- if(*cell_cost == NO_INFORMATION)
- *cell_cost = cost;
- else
- *cell_cost = std::max(cost, *cell_cost);
- }
- inline unsigned char computeCost(double distance) const {
- unsigned char cost = 0;
- if(distance == 0)
- cost = LETHAL_OBSTACLE;
- else if(distance <= cell_inscribed_radius_)
- cost = INSCRIBED_INFLATED_OBSTACLE;
- else {
- double factor = weight_ / (1 + pow(distance - cell_inscribed_radius_, 2));
- cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
- }
- return cost;
- }
-
- inline char costLookup(int mx, int my, int src_x, int src_y){
- unsigned int dx = abs(mx - src_x);
- unsigned int dy = abs(my - src_y);
- return cached_costs_[dx][dy];
- }
-
- inline double distanceLookup(int mx, int my, int src_x, int src_y){
- unsigned int dx = abs(mx - src_x);
- unsigned int dy = abs(my - src_y);
- return cached_distances_[dx][dy];
- }
-
/**
* @brief Convert from map coordinates to world coordinates without checking for legal bounds
* @param wx The x world coordinate
@@ -180,54 +166,8 @@
*/
void worldToMapNoBounds(double wx, double wy, unsigned int& mx, unsigned int& my) const;
- /**
- * @brief Revert to the static map outside of a specified window centered at a world coordinate
- * @param wx The x coordinate of the center point of the window in world space (meters)
- * @param wy The y coordinate of the center point of the window in world space (meters)
- * @param w_size_x The x size of the window in meters
- * @param w_size_y The y size of the window in meters
- */
- void resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y);
/**
- * @brief Update the costmap with new observations
- * @param obstacles The point clouds of obstacles to insert into the map
- * @param clearing_observations The set of observations to use for raytracing
- */
- void updateWorld(double robot_x, double robot_y,
- const std::vector<Observation>& observations, const std::vector<Observation>& clearing_observations);
-
- /**
- * @brief Insert new obstacles into the cost map
- * @param obstacles The point clouds of obstacles to insert into the map
- * @param inflation_queue The queue to place the obstacles into for inflation
- */
- void updateObstacles(const std::vector<Observation>& observations, std::priority_queue<CellData>& inflation_queue);
-
- /**
- * @brief Clear freespace based on any number of observations
- * @param clearing_observations The observations used to raytrace
- */
- void raytraceFreespace(const std::vector<Observation>& clearing_observations);
-
- /**
- * @brief Clear freespace from an observation
- * @param clearing_observation The observation used to raytrace
- */
- void raytraceFreespace(const Observation& clearing_observation);
-
- /**
- * @brief Provides support for re-inflating obstacles within a certain window (used after raytracing)
- * @param wx The x coordinate of the center point of the window in world space (meters)
- * @param wy The y coordinate of the center point of the window in world space (meters)
- * @param w_size_x The x size of the window in meters
- * @param w_size_y The y size of the window in meters
- * @param inflation_queue The priority queue to push items back onto for propogation
- */
- void resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y,
- std::priority_queue<CellData>& inflation_queue );
-
- /**
* @brief Accessor for the x size of the costmap in cells
* @return The x size of the costmap
*/
@@ -278,7 +218,6 @@
* @param src_x The x index of the obstacle point inflation started at
* @param src_y The y index of the obstacle point inflation started at
* @param inflation_queue The priority queue to insert into
- * @return
*/
inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y, std::priority_queue<CellData>& inflation_queue){
@@ -293,7 +232,7 @@
return;
//assign the cost associated with the distance from an obstacle to the cell
- cost_map_[index] = costLookup(mx, my, src_x, src_y);
+ updateCellCost(index, costLookup(mx, my, src_x, src_y));
//push the cell data onto the queue and mark
inflation_queue.push(CellData(distance, index, mx, my, src_x, src_y));
@@ -301,10 +240,46 @@
}
}
- inline int sign(int x){
- return x > 0 ? 1.0 : -1.0;
- }
+ /**
+ * @brief Insert new obstacles into the cost map
+ * @param obstacles The point clouds of obstacles to insert into the map
+ * @param inflation_queue The queue to place the obstacles into for inflation
+ */
+ void updateObstacles(const std::vector<Observation>& observations, std::priority_queue<CellData>& inflation_queue);
+ /**
+ * @brief Clear freespace based on any number of observations
+ * @param clearing_observations The observations used to raytrace
+ */
+ void raytraceFreespace(const std::vector<Observation>& clearing_observations);
+
+ /**
+ * @brief Clear freespace from an observation
+ * @param clearing_observation The observation used to raytrace
+ */
+ void raytraceFreespace(const Observation& clearing_observation);
+
+ /**
+ * @brief Provides support for re-inflating obstacles within a certain window (used after raytracing)
+ * @param wx The x coordinate of the center point of the window in world space (meters)
+ * @param wy The y coordinate of the center point of the window in world space (meters)
+ * @param w_size_x The x size of the window in meters
+ * @param w_size_y The y size of the window in meters
+ * @param inflation_queue The priority queue to push items back onto for propogation
+ */
+ void resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y,
+ std::priority_queue<CellData>& inflation_queue );
+
+
+ /**
+ * @brief Raytrace a line and apply some action at each step
+ * @param at The action to take... a functor
+ * @param x0 The starting x coordinate
+ * @param y0 The starting y coordinate
+ * @param x1 The ending x coordinate
+ * @param y1 The ending y coordinate
+ * @return
+ */
template <class ActionType>
inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1){
int dx = x1 - x0;
@@ -331,6 +306,9 @@
}
+ /**
+ * @brief A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step
+ */
template <class ActionType>
inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset){
for(unsigned int i = 0; i < abs_da; ++i){
@@ -351,10 +329,61 @@
*/
void inflateObstacles(std::priority_queue<CellData>& inflation_queue);
+ /**
+ * @brief Takes the max of existing cost and the new cost... keeps static map obstacles from being overridden prematurely
+ * @param index The index od the cell to assign a cost to
+ * @param cost The cost
+ */
+ inline void updateCellCost(unsigned int index, unsigned char cost){
+ unsigned char* cell_cost = &cost_map_[index];
+ if(*cell_cost == NO_INFORMATION)
+ *cell_cost = cost;
+ else
+ *cell_cost = std::max(cost, *cell_cost);
+ }
+
+ /**
+ * @brief Given distance in the world... convert it to cells
+ * @param world_dist The world distance
+ * @return The equivalent cell distance
+ */
unsigned int cellDistance(double world_dist);
- double worldDistance(unsigned int cell_dist);
+ /**
+ * @brief Given a distance... compute a cost
+ * @param distance The distance from an obstacle in cells
+ * @return A cost value for the distance
+ */
+ inline unsigned char computeCost(double distance) const {
+ unsigned char cost = 0;
+ if(distance == 0)
+ cost = LETHAL_OBSTACLE;
+ else if(distance <= cell_inscribed_radius_)
+ cost = INSCRIBED_INFLATED_OBSTACLE;
+ else {
+ double factor = weight_ / (1 + pow(distance - cell_inscribed_radius_, 2));
+ cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
+ }
+ return cost;
+ }
+ inline char costLookup(int mx, int my, int src_x, int src_y){
+ unsigned int dx = abs(mx - src_x);
+ unsigned int dy = abs(my - src_y);
+ return cached_costs_[dx][dy];
+ }
+
+ inline double distanceLookup(int mx, int my, int src_x, int src_y){
+ unsigned int dx = abs(mx - src_x);
+ unsigned int dy = abs(my - src_y);
+ return cached_distances_[dx][dy];
+ }
+
+ inline int sign(int x){
+ return x > 0 ? 1.0 : -1.0;
+ }
+
+
unsigned int size_x_;
unsigned int size_y_;
double resolution_;
Modified: pkg/trunk/world_models/new_costmap/manifest.xml
===================================================================
--- pkg/trunk/world_models/new_costmap/manifest.xml 2009-04-01 01:32:19 UTC (rev 13206)
+++ pkg/trunk/world_models/new_costmap/manifest.xml 2009-04-01 01:32:45 UTC (rev 13207)
@@ -12,7 +12,8 @@
<depend package="tf" />
<depend package="robot_filter" />
<depend package="robot_srvs" />
+<depend package="costmap_2d" />
<export>
- <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lnew_costmap"/>
+ <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lnew_costmap_2d"/>
</export>
</package>
Modified: pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:32:19 UTC (rev 13206)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:32:45 UTC (rev 13207)
@@ -40,17 +40,6 @@
using namespace robot_msgs;
namespace costmap_2d{
-
- Costmap2D::Costmap2D(double meters_size_x, double meters_size_y,
- double resolution, double origin_x, double origin_y) : resolution_(resolution),
- origin_x_(origin_x), origin_y_(origin_y) {
- //make sure that we have a legal sized cost_map
- ROS_ASSERT_MSG(meters_size_x > 0 && meters_size_y > 0, "Both the x and y dimensions of the costmap must be greater than 0.");
-
- size_x_ = (unsigned int) ceil(meters_size_x / resolution);
- size_y_ = (unsigned int) ceil(meters_size_y / resolution);
- }
-
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
double resolution, double origin_x, double origin_y, double inscribed_radius,
double circumscribed_radius, double inflation_radius, double obstacle_range,
@@ -61,6 +50,12 @@
max_obstacle_height_(max_obstacle_height), raytrace_range_(raytrace_range), cached_costs_(NULL), cached_distances_(NULL),
inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), inflation_radius_(inflation_radius),
weight_(weight){
+ //creat the cost_map, static_map, and markers
+ cost_map_ = new unsigned char[size_x_ * size_y_];
+ static_map_ = new unsigned char[size_x_ * size_y_];
+ markers_ = new unsigned char[size_x_ * size_y_];
+ memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
+
//convert our inflations from world to cell distance
cell_inscribed_radius_ = cellDistance(inscribed_radius);
cell_circumscribed_radius_ = cellDistance(circumscribed_radius);
@@ -72,14 +67,14 @@
for(unsigned int i = 0; i <= cell_inflation_radius_; ++i){
cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 1];
cached_distances_[i] = new double[cell_inflation_radius_ + 1];
- for(unsigned int j = 0; i <= cell_inflation_radius_; ++j){
+ for(unsigned int j = 0; j <= cell_inflation_radius_; ++j){
cached_distances_[i][j] = sqrt(i*i + j*j);
cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
}
}
if(!static_data.empty()){
- ROS_ASSERT_MSG(size_x_ * size_y_ == static_data_.size(), "If you want to initialize a costmap with static data, their sizes must match.");
+ ROS_ASSERT_MSG(size_x_ * size_y_ == static_data.size(), "If you want to initialize a costmap with static data, their sizes must match.");
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
@@ -110,11 +105,6 @@
return (unsigned int) cells_dist;
}
- double Costmap2D::worldDistance(unsigned int cell_dist){
- double world_dist = cell_dist * resolution_;
- return world_dist;
- }
-
unsigned char Costmap2D::getCellCost(unsigned int mx, unsigned int my) const {
ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot get the cost of a cell that is outside the bounds of the costmap");
return cost_map_[my * size_x_ + mx];
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
