summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/common-cc/path.cc
diff options
context:
space:
mode:
authorOlivier Lanneluc2013-05-01 17:01:29 +0200
committerNicolas Schodet2013-05-03 23:07:14 +0200
commitce62267e8872db43c2da008361e8d014601f7b5a (patch)
treec6b77163967728ef61ffba1194ac2be8a6fd1626 /digital/io-hub/src/common-cc/path.cc
parent13a7c0ff52ee38e464f6db3d0556d0569ae5ed6f (diff)
digital/io-hub/src/common-cc/path.cc: fix bug about the escape factor + fix rounding error in the radius of the first navigation points layer + remove commented code
Diffstat (limited to 'digital/io-hub/src/common-cc/path.cc')
-rw-r--r--digital/io-hub/src/common-cc/path.cc7
1 files changed, 4 insertions, 3 deletions
diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc
index 226b00ec..32aed5f0 100644
--- a/digital/io-hub/src/common-cc/path.cc
+++ b/digital/io-hub/src/common-cc/path.cc
@@ -103,7 +103,7 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int
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 /*BOT_SIZE_SIDE*/ + clearance;
+ r += BOT_SIZE_RADIUS + clearance;
/* Store obstacle */
//assert(obstacles_nb < PATH_OBSTACLES_NB);
@@ -120,11 +120,10 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int
/* from one to another without collision with the */
/* obstacle circle. New radius is r / cos(angle/2) */
nr = PATH_ANGLE_824(nodes*2);
- x = fixed_div_f824(r, fixed_cos_f824(nr)) + 1 /* margin */;
+ x = fixed_div_f824(r, fixed_cos_f824(nr)) + 2 /* margin for the unprecise fixed point computation */;
y = 0;
/* Add a number of sets of navigation points with different weights */
- //for(weight=PATH_OBSTACLES_NAVPOINTS_LAYERS; weight>0; weight--)
for(layer=nlayers-1; layer>=0; layer--)
{
/* Compute obstacle points positions around a circle */
@@ -196,7 +195,9 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors)
}
/* Collision while trying to escape the source point */
/* if and only if the source point is in this obstacle */
+ /* and the distance is less than the obstacle radius */
else if (i==PATH_NAVPOINT_SRC_IDX &&
+ distance_point_point(&navpoints[cur_point], &navpoints[i]) < obstacles[j].r &&
PATH_IN_CIRCLE(&navpoints[i], &obstacles[j].c, obstacles[j].r))
{
/* Allow this navigation point with an extra cost */