Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13205] pkg/trunk
|
From: <ei...@us...> - 2009-04-01 02:22:11
|
Revision: 13205
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13205&view=rev
Author: eitanme
Date: 2009-04-01 01:32:03 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r19995@cib: eitan | 2009-03-24 11:27:42 -0700
Put in raytracing that should be more efficient than before and also will be compatible with rolling window costmaps
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: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
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:19995
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:24:04 UTC (rev 13204)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:32:03 UTC (rev 13205)
@@ -91,6 +91,7 @@
* @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 raytrace_range The maximum distance we'll raytrace out to
* @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
@@ -98,7 +99,7 @@
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,
- double max_obstacle_height, double weight,
+ double max_obstacle_height, double raytrace_range, double weight,
const std::vector<unsigned char>& static_data, unsigned char lethal_threshold);
/**
@@ -283,6 +284,50 @@
}
}
+ inline int sign(int x){
+ return x > 0 ? 1.0 : -1.0;
+ }
+
+ template <class ActionType>
+ inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1){
+ int dx = x1 - x0;
+ int dy = y1 - y0;
+
+ unsigned int abs_dx = abs(dx);
+ unsigned int abs_dy = abs(dy);
+
+ int offset_dx = sign(dx);
+ int offset_dy = sign(dy) * size_x_;
+
+ unsigned int offset = y0 * size_x_ + x0;
+
+ //if x is dominant
+ if(abs_dx >= abs_dy){
+ int error_y = abs_dx / 2;
+ bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset);
+ return;
+ }
+
+ //otherwise y is dominant
+ int error_x = abs_dy / 2;
+ bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset);
+
+ }
+
+ 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){
+ at(offset);
+ offset += offset_a;
+ error_b += abs_db;
+ if((unsigned int)error_b >= abs_da){
+ offset += offset_b;
+ error_b -= abs_da;
+ }
+ }
+ at(offset);
+ }
+
/**
* @brief Given a priority queue with the actual obstacles, compute the inflated costs for the costmap
* @param inflation_queue A priority queue contatining the cell data for the actual obstacles
@@ -301,10 +346,22 @@
unsigned char* markers_;
double sq_obstacle_range_;
double max_obstacle_height_;
+ double raytrace_range_;
unsigned char** cached_costs_;
double** cached_distances_;
unsigned int inscribed_radius_, circumscribed_radius_, inflation_radius_;
double weight_;
+
+ //functors for raytracing actions
+ class ClearCell {
+ public:
+ ClearCell(unsigned char* cost_map) : cost_map_(cost_map) {}
+ inline void operator()(unsigned int offset){
+ cost_map_[offset] = 0;
+ }
+ private:
+ unsigned char* cost_map_;
+ };
};
};
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:24:04 UTC (rev 13204)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:32:03 UTC (rev 13205)
@@ -54,11 +54,11 @@
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,
- double max_obstacle_height, double weight,
+ double max_obstacle_height, double raytrace_range, 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),
+ 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){
//convert our inflations from world to cell distance
@@ -260,8 +260,77 @@
}
}
- void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){}
+ void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){
+ //pre-comput the squared raytrace range... saves a sqrt in an inner loop
+ double sq_raytrace_range = raytrace_range_ * raytrace_range_;
+
+ //create the functor that we'll use to clear cells from the costmap
+ ClearCell clearer(cost_map_);
+
+ for(unsigned int i = 0; i < clearing_scans.size(); ++i){
+ double ox = clearing_scans[i].origin_.x;
+ double oy = clearing_scans[i].origin_.y;
+ PointCloud* cloud = clearing_scans[i].cloud_;
+
+ //get the map coordinates of the origin of the sensor
+ unsigned int x0, y0;
+ if(!worldToMap(ox, oy, x0, y0))
+ return;
+
+ //we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
+ double end_x = origin_x_ + metersSizeX();
+ double end_y = origin_y_ + metersSizeY();
+
+ //for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
+ for(unsigned int j = 0; j < cloud->pts.size(); ++j){
+ double wx = cloud->pts[j].x;
+ double wy = cloud->pts[i].y;
+
+ //we want to check that we scale the vector so that we only trace to the desired distance
+ double sq_distance = (wx - ox) * (wx - ox) + (wy - oy) * (wy - oy);
+ double scaling_fact = sq_raytrace_range / sq_distance;
+ scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
+
+ //now we also need to make sure that the enpoint we're raytracing
+ //to isn't off the costmap and scale if necessary
+ double a = wx - ox;
+ double b = wy - oy;
+
+ //the minimum value to raytrace from is the origin
+ if(wx < origin_x_){
+ double t = (origin_x_ - ox) / a;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
+ if(wy < origin_y_){
+ double t = (origin_y_ - oy) / b;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
+
+ //the maximum value to raytrace to is the end of the map
+ if(wx > end_x){
+ double t = (end_x - ox) / a;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
+ if(wy > end_y){
+ double t = (end_y - oy) / b;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
+
+ //now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
+ unsigned int x1, y1;
+ worldToMap(wx, wy, x1, y1);
+
+ //and finally... we can execute our trace to clear obstacles along that line
+ raytraceLine(clearer, x0, y0, x1, y1);
+ }
+ }
+ }
+
void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
