From 93614a3099bda4413d6baf1fc0a4ee3533ca7476 Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Fri, 3 May 2013 16:11:39 +0200 Subject: digital/io-hub/src/common-cc/path: clarify the clearance margins applied around obstacles --- digital/io-hub/src/common-cc/path.cc | 14 +++++++++----- digital/io-hub/src/common-cc/path.hh | 6 ++++++ 2 files changed, 15 insertions(+), 5 deletions(-) (limited to 'digital/io-hub') 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; -- cgit v1.2.3