Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13204] pkg/trunk
|
From: <ei...@us...> - 2009-04-01 01:24:13
|
Revision: 13204
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13204&view=rev
Author: eitanme
Date: 2009-04-01 01:24:04 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r19889@cib: eitan | 2009-03-23 18:32:05 -0700
Getting closer
Modified Paths:
--------------
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
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:19857
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:19889
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/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:23:08 UTC (rev 13203)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:24:04 UTC (rev 13204)
@@ -86,9 +86,20 @@
* @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
+ * @param inscribed_radius The inscribed radius of the robot
+ * @param circumscribed_radius The circumscribed radius of the robot
+ * @param inflation_radius How far out to inflate obstacles
+ * @param obstacle_range The maximum range at which obstacles will be put into the costmap
+ * @param max_obstacle_height The maximum height of obstacles that will be considered
+ * @param weight The scaling factor for the cost function. Should be 0 < weight <= 1. Lower values reduce effective cost.
+ * @param static_data Data used to initialize the costmap
+ * @param lethal_threshold The cost threshold at which a point in the static data is considered a lethal obstacle
*/
Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
- double resolution, double origin_x, double origin_y);
+ double resolution, double origin_x, double origin_y, double inscribed_radius,
+ double circumscribed_radius, double inflation_radius, double obstacle_range,
+ double max_obstacle_height, double weight,
+ const std::vector<unsigned char>& static_data, unsigned char lethal_threshold);
/**
* @brief Get the cost of a cell in the costmap
@@ -121,6 +132,11 @@
return my * size_x_ + mx;
}
+ 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)
@@ -273,6 +289,8 @@
*/
void inflateObstacles(std::priority_queue<CellData*>& inflation_queue);
+ unsigned int cellDistance(double world_dist);
+
unsigned int size_x_;
unsigned int size_y_;
double resolution_;
@@ -285,7 +303,7 @@
double max_obstacle_height_;
unsigned char** cached_costs_;
double** cached_distances_;
- double inscribed_radius_, circumscribed_radius, inflation_radius_;
+ unsigned int inscribed_radius_, circumscribed_radius_, inflation_radius_;
double weight_;
};
};
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:23:08 UTC (rev 13203)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:24:04 UTC (rev 13204)
@@ -41,21 +41,75 @@
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) (meters_size_x / resolution);
- size_y_ = (unsigned int) (meters_size_y / resolution);
+ 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) : size_x_(cells_size_x),
- size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y) {}
+ double resolution, double origin_x, double origin_y, double inscribed_radius,
+ double circumscribed_radius, double inflation_radius, double obstacle_range,
+ double max_obstacle_height, double weight,
+ const std::vector<unsigned char>& static_data, unsigned char lethal_threshold) : size_x_(cells_size_x),
+ size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y), static_map_(NULL),
+ cost_map_(NULL), markers_(NULL), sq_obstacle_range_(obstacle_range * obstacle_range),
+ max_obstacle_height_(max_obstacle_height), cached_costs_(NULL), cached_distances_(NULL),
+ inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), inflation_radius_(inflation_radius),
+ weight_(weight){
+ //convert our inflations from world to cell distance
+ inscribed_radius_ = cellDistance(inscribed_radius);
+ circumscribed_radius_ = cellDistance(circumscribed_radius);
+ inflation_radius_ = cellDistance(inflation_radius);
+ //based on the inflation radius... compute distance and cost caches
+ cached_costs_ = new unsigned char*[inflation_radius_ + 1];
+ cached_distances_ = new double*[inflation_radius_ + 1];
+ for(unsigned int i = 0; i <= inflation_radius_; ++i){
+ cached_costs_[i] = new unsigned char[inflation_radius_ + 1];
+ cached_distances_[i] = new double[inflation_radius_ + 1];
+ for(unsigned int j = 0; i <= 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.");
+
+ //we'll need a priority queue for inflation
+ std::priority_queue<CellData*> inflation_queue;
+
+ //initialize the costmap with static data
+ for(unsigned int i = 0; i < size_x_; ++i){
+ for(unsigned int j = 0; j < size_y_; ++j){
+ unsigned int index = getIndex(i, j);
+ //if the static value is above the threshold... it is a lethal obstacle... otherwise just take the cost
+ cost_map_[index] = static_data[index] >= lethal_threshold ? LETHAL_OBSTACLE : static_data[index];
+ if(cost_map_[index] == LETHAL_OBSTACLE){
+ unsigned int mx, my;
+ indexToCells(index, mx, my);
+ enqueue(index, mx, my, mx, my, inflation_queue);
+ }
+ }
+ }
+ //now... let's inflate the obstacles
+ inflateObstacles(inflation_queue);
+
+ //we also want to keep a copy of the current costmap as the static map
+ memcpy(static_map_, cost_map_, size_x_ * size_y_);
+ }
+ }
+
+ unsigned int Costmap2D::cellDistance(double world_dist){
+ double cells_dist = max(0.0, ceil(world_dist / resolution_));
+ return (unsigned int) cells_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];
@@ -208,7 +262,8 @@
void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){}
- void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){}
+ void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){
+ }
unsigned int Costmap2D::cellSizeX() const{}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
