summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorOlivier Lanneluc2013-04-16 16:21:54 +0200
committerNicolas Schodet2013-04-27 00:21:08 +0200
commit8f63fa287cf35d7c960ce8d2ec4fc3a498967d17 (patch)
tree7f07f5e50b37bea7a59c4faee00a725a5b674eb7 /digital
parentbda9896e4205537fbf5d124e36f8ca27e9665292 (diff)
Do not allow to go into obstacles exclusion circle, except when target is the center of the cake.
More dev required to make it more generic.
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/common-cc/obstacles.cc2
-rw-r--r--digital/io-hub/src/common-cc/path.cc26
-rw-r--r--digital/io-hub/src/common-cc/path.hh1
3 files changed, 20 insertions, 9 deletions
diff --git a/digital/io-hub/src/common-cc/obstacles.cc b/digital/io-hub/src/common-cc/obstacles.cc
index fc8a5204..5e20a611 100644
--- a/digital/io-hub/src/common-cc/obstacles.cc
+++ b/digital/io-hub/src/common-cc/obstacles.cc
@@ -154,7 +154,7 @@ Obstacles::add_obstacles (Path &path) const
if (obstacles_[i].valid)
{
path.obstacle (index++, obstacles_[i].pos,
- obstacle_radius_mm + clearance_mm /*+ BOT_SIZE_SIDE*/);
+ obstacle_radius_mm /*+ clearance_mm + BOT_SIZE_SIDE*/);
}
}
}
diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc
index f83a670f..cc5e290a 100644
--- a/digital/io-hub/src/common-cc/path.cc
+++ b/digital/io-hub/src/common-cc/path.cc
@@ -2,6 +2,7 @@
// io-hub - Modular Input/Output. {{{
//
// Copyright (C) 2013 Nicolas Schodet
+// Copyright (C) 2013 Olivier Lanneluc
//
// APBTeam:
// Web: http://apbteam.org/
@@ -88,8 +89,8 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, int num)
DPRINTF("Add obstacle c=(%u;%u) r=%u num=%u\n",c.x, c.y, r, num);
- /* Enlarge the obstacle radius by at least the robot size */
- r += BOT_SIZE_RADIUS * 4 / 3;
+ /* Enlarge the obstacle radius by the robot size and clearance area width */
+ r += BOT_SIZE_SIDE + Obstacles::clearance_mm;
/* Store obstacle */
//assert(obstacles_nb < PATH_OBSTACLES_NB);
@@ -134,10 +135,10 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, int num)
}
#ifdef HOST
- // Plot obstacle points
+ /* Plot obstacle points */
robot->hardware.simu_report.pos( &navpoints[PATH_RESERVED_NAVPOINTS_NB], navpoints_nb-PATH_RESERVED_NAVPOINTS_NB, PATH_PLOT_ID);
#if 0
- // Draw the last obstacle
+ /* Draw the last obstacle */
navpoints[navpoints_nb] = navpoints[navpoints_nb - num];
robot->hardware.simu_report.path( &navpoints[navpoints_nb - num], num + 1);
#endif
@@ -236,19 +237,28 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors)
/* Check every obstacle */
for(int j=0; j<obstacles_nb; j++)
{
- // Check for intersection with obstacle
+ /* Check for intersection with obstacle */
uint16_t d = distance_segment_point(&navpoints[cur_point], &navpoints[i], &obstacles[j].c);
if (d < obstacles[j].r)
{
- /* Collision: apply the escape factor if node is the source point, invalidate node otherwise */
- weight *= (i==PATH_NAVPOINT_SRC_IDX || cur_point==PATH_NAVPOINT_DST_IDX ? escape_factor : 0);
+ /* Collision: try to escape when node is the source point */
+ if (i==PATH_NAVPOINT_SRC_IDX &&
+ (cur_point!=PATH_NAVPOINT_DST_IDX ||
+ (navpoints[cur_point].x==pg_cake_pos.x && navpoints[cur_point].y==pg_cake_pos.y)))
+ {
+ weight *= escape_factor; /* allow this navigation point, but it costs */
+ }
+ else
+ {
+ weight = 0; /* disable this navigation point */
+ }
DPRINTF("in collision with c=(%u;%u) r=%u w=%u ",
obstacles[j].c.x, obstacles[j].c.y, obstacles[j].r, weight);
break; /* Stop checking obstacle for this node */
}
}
- // Add neighbor if valid
+ /* Add neighbor if valid */
if (weight)
{
DPRINTF("=> validated w=%u\n", weight);
diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh
index 54822e22..61326aac 100644
--- a/digital/io-hub/src/common-cc/path.hh
+++ b/digital/io-hub/src/common-cc/path.hh
@@ -3,6 +3,7 @@
// io-hub - Modular Input/Output. {{{
//
// Copyright (C) 2013 Nicolas Schodet
+// Copyright (C) 2013 Olivier Lanneluc
//
// APBTeam:
// Web: http://apbteam.org/