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 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) (limited to 'digital/io-hub/src/common-cc/path.cc') 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); } -- cgit v1.2.3