summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorOlivier Lanneluc2013-04-25 20:27:40 +0200
committerNicolas Schodet2013-04-27 00:24:09 +0200
commit33a67790aa0df5a0461dc3c5425d5c2a14d282b8 (patch)
tree753f275241934e06ef441d930e75e49b6287bec0 /digital
parent4e67c3e2e763eab19d253fad78331a0b4ac6625a (diff)
digital/io-hub/src/common-cc: enlarge the obstacles clearance radiusoliv-path
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/common-cc/path.cc21
-rw-r--r--digital/io-hub/src/common-cc/path.hh14
2 files changed, 21 insertions, 14 deletions
diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc
index 8d709c9a..fe62496a 100644
--- a/digital/io-hub/src/common-cc/path.cc
+++ b/digital/io-hub/src/common-cc/path.cc
@@ -49,6 +49,9 @@ extern "C" {
/** Check for vectors equality */
#define PATH_VECT_EQUAL(v1, v2) ((v1)->x==(v2)->x && (v1)->y==(v2)->y)
+/** Check if point is inside a circle */
+#define PATH_IN_CIRCLE(pOINT, cENTER, rADIUS) (distance_point_point((pOINT), (cENTER)) <= (rADIUS))
+
/** Static nodes index for the endpoints */
enum {
PATH_NAVPOINT_SRC_IDX = 0,
@@ -83,20 +86,20 @@ void Path::reset()
#ifdef playground_2013_hh
/* Declare the cake as an obstacle */
- add_obstacle(pg_cake_pos, pg_cake_radius, PATH_CAKE_NAVPOINTS_NB);
+ add_obstacle(pg_cake_pos, pg_cake_radius, PATH_CAKE_NAVPOINTS_NB, PATH_CAKE_NAVPOINTS_LAYERS, 0 /* no extra clearance radius */);
#endif
}
-void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes)
+void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int nlayers, const uint16_t clearance)
{
uint32_t rot_a, rot_b, nr;
uint32_t x, y, nx;
int npt, layer;
- DPRINTF("Add obstacle c=(%u;%u) r=%u num=%u\n",c.x, c.y, r, nodes);
+ DPRINTF("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*/ + Obstacles::clearance_mm;
+ r += BOT_SIZE_RADIUS /*BOT_SIZE_SIDE*/ + clearance;
/* Store obstacle */
//assert(obstacles_nb < PATH_OBSTACLES_NB);
@@ -117,8 +120,8 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes)
y = 0;
/* Add a number of sets of navigation points with different weights */
- //for(weight=PATH_NAVPOINTS_LAYERS; weight>0; weight--)
- for(layer=PATH_NAVPOINTS_LAYERS-1; layer>=0; layer--)
+ //for(weight=PATH_OBSTACLES_NAVPOINTS_LAYERS; weight>0; weight--)
+ for(layer=nlayers-1; layer>=0; layer--)
{
/* Compute obstacle points positions around a circle */
for (npt=0; npt<nodes; npt++)
@@ -188,7 +191,9 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors)
continue;
}
/* Collision while trying to escape the source point */
- else if (i==PATH_NAVPOINT_SRC_IDX)
+ /* if and only if the source point is in this obstacle */
+ else if (i==PATH_NAVPOINT_SRC_IDX &&
+ PATH_IN_CIRCLE(&navpoints[i], &obstacles[j].c, obstacles[j].r))
{
/* Allow this navigation point with an extra cost */
weight *= escape_factor;
@@ -268,7 +273,7 @@ void Path::compute(weight_t escape)
void Path::obstacle(int index, const vect_t &c, uint16_t r, int f)
{
- add_obstacle(c, r, PATH_OBSTACLES_NAVPOINTS_NB);
+ add_obstacle(c, r, PATH_OBSTACLES_NAVPOINTS_NB, PATH_OBSTACLES_NAVPOINTS_LAYERS, Obstacles::clearance_mm + 100/* extra clearance radius */);
}
void Path::endpoints(const vect_t &src, const vect_t &dst, const bool force_move)
diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh
index 8003f32b..e683b83a 100644
--- a/digital/io-hub/src/common-cc/path.hh
+++ b/digital/io-hub/src/common-cc/path.hh
@@ -73,30 +73,32 @@ class Path
private:
/** Add an obstacle on the field */
- void add_obstacle(const vect_t &c, uint16_t r, const int nodes);
+ void add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int nlayers, const uint16_t clearance);
/** Number of possible obstacles. */
static const int PATH_OBSTACLES_NB = (4+1/*cake*/);
/** Number of points per standard obstacle. */
- static const int PATH_OBSTACLES_NAVPOINTS_NB = 8;
+ static const int PATH_OBSTACLES_NAVPOINTS_NB = 12;
#ifdef playground_2013_hh
/** Number of points for the cake */
static const int PATH_CAKE_NAVPOINTS_NB = 14;
#endif
/** Number of reserved points for the 2 endpoints */
static const int PATH_RESERVED_NAVPOINTS_NB = 2;
+ /** Number of navigation points layers for the cake. */
+ static const int PATH_CAKE_NAVPOINTS_LAYERS = 1;
/** Number of navigation points layers for each obstacle. */
- static const int PATH_NAVPOINTS_LAYERS = 2;
+ static const int PATH_OBSTACLES_NAVPOINTS_LAYERS = 2;
/** Number of navigation points. */
static const int PATH_NAVPOINTS_NB = (PATH_RESERVED_NAVPOINTS_NB +
#ifdef playground_2013_hh
- PATH_NAVPOINTS_LAYERS * (PATH_CAKE_NAVPOINTS_NB - PATH_OBSTACLES_NAVPOINTS_NB) +
+ PATH_CAKE_NAVPOINTS_LAYERS * (PATH_CAKE_NAVPOINTS_NB - PATH_OBSTACLES_NAVPOINTS_NB) +
#endif
- PATH_NAVPOINTS_LAYERS * (PATH_OBSTACLES_NB * PATH_OBSTACLES_NAVPOINTS_NB));
+ PATH_OBSTACLES_NAVPOINTS_LAYERS * (PATH_OBSTACLES_NB * PATH_OBSTACLES_NAVPOINTS_NB));
/** Navigation points weight precision (2^-n). */
static const int PATH_WEIGHT_PRECISION = 4;
/** Navigation points weight step (2^-n). */
- static const int PATH_WEIGHT_STEP = 5;
+ static const int PATH_WEIGHT_STEP = 6;
/** Borders, any point outside borders is eliminated. */
const uint16_t border_xmin, border_ymin, border_xmax, border_ymax;