summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorOlivier Lanneluc2013-05-03 16:11:39 +0200
committerNicolas Schodet2013-05-03 23:07:15 +0200
commit93614a3099bda4413d6baf1fc0a4ee3533ca7476 (patch)
tree582e87ff6f8a0f2cf42f17b1e6bef18771b8eb30
parent496c1b542fcc390aa1cd142a5b86aa82ae1b3c25 (diff)
digital/io-hub/src/common-cc/path: clarify the clearance margins applied around obstacles
-rw-r--r--digital/io-hub/src/common-cc/path.cc14
-rw-r--r--digital/io-hub/src/common-cc/path.hh6
2 files changed, 15 insertions, 5 deletions
diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc
index c34dd6a4..198b269b 100644
--- a/digital/io-hub/src/common-cc/path.cc
+++ b/digital/io-hub/src/common-cc/path.cc
@@ -104,7 +104,7 @@ void Path::add_obstacle( const vect_t &c,
uint16_t r,
int nodes,
const int nlayers,
- const uint16_t clearance,
+ const uint16_t nav_clearance,
const bool target)
{
uint32_t rot_a, rot_b, nr;
@@ -113,8 +113,8 @@ void Path::add_obstacle( const vect_t &c,
host_debug("Add obstacle c=(%u;%u) r=%u nodes=%u layers=%u\n",c.x, c.y, r, nodes, nlayers);
- /* Enlarge the obstacle radius by the robot size and clearance area width */
- r += BOT_SIZE_RADIUS + clearance;
+ /* Enlarge the obstacle radius by the robot size */
+ r += BOT_SIZE_RADIUS;
/* Store obstacle */
//assert(obstacles_nb < PATH_OBSTACLES_NB);
@@ -123,6 +123,10 @@ void Path::add_obstacle( const vect_t &c,
obstacles[obstacles_nb].target_allowed = target;
obstacles_nb++;
+ /* Enlarge the navigation points circle radius by */
+ /* the additionnal clearance area width */
+ r += nav_clearance;
+
/* Extend the points radius to allow the robot to go */
/* from one to another without collision with the */
/* obstacle circle. New radius is r / cos(angle/2) */
@@ -300,10 +304,10 @@ void Path::compute(weight_t escape)
void Path::obstacle(const int index, const vect_t &c, const uint16_t r, const int f, const bool target)
{
add_obstacle( c,
- r,
+ (r + Obstacles::clearance_mm + PATH_OBSTACLES_CLEARANCE),
PATH_OBSTACLES_NAVPOINTS_NB,
PATH_OBSTACLES_NAVPOINTS_LAYERS,
- Obstacles::clearance_mm + 100/* extra clearance radius */,
+ PATH_NAVPOINTS_CLEARANCE,
target);
}
diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh
index fc743291..b25d4235 100644
--- a/digital/io-hub/src/common-cc/path.hh
+++ b/digital/io-hub/src/common-cc/path.hh
@@ -108,6 +108,12 @@ class Path
static const int PATH_WEIGHT_PRECISION = 4;
/** Navigation points weight step (2^-n). */
static const int PATH_WEIGHT_STEP = 6;
+ /** Extra clearance area added to the radius of the mobile obstacles
+ * to counter the imprecision of the sonic sensors when the robot brakes */
+ static const uint16_t PATH_OBSTACLES_CLEARANCE = 60;
+ /** Extra clearance area added to the radius of the navigation points
+ * circle to move the navpoints away from the obstacle circle */
+ static const uint16_t PATH_NAVPOINTS_CLEARANCE = 40;
/** Borders, any point outside borders is eliminated. */
const uint16_t border_xmin, border_ymin, border_xmax, border_ymax;