summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub
diff options
context:
space:
mode:
authorOlivier Lanneluc2013-05-03 16:13:51 +0200
committerNicolas Schodet2013-05-03 23:07:15 +0200
commitfd7fc5373ddf2abc42750ac87749f7a374f11fdf (patch)
tree93525b7ac7d1bc482619f0757751ddfb8ec19012 /digital/io-hub
parent93614a3099bda4413d6baf1fc0a4ee3533ca7476 (diff)
digital/io-hub/src/common-cc/path: prevent a possible overflow about navpoints weights
Also increase the weight of navpoints around the first layer to tend to avoid them.
Diffstat (limited to 'digital/io-hub')
-rw-r--r--digital/io-hub/src/common-cc/path.cc16
-rw-r--r--digital/io-hub/src/common-cc/path.hh7
2 files changed, 13 insertions, 10 deletions
diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc
index 198b269b..d1d1f132 100644
--- a/digital/io-hub/src/common-cc/path.cc
+++ b/digital/io-hub/src/common-cc/path.cc
@@ -156,7 +156,7 @@ void Path::add_obstacle( const vect_t &c,
&& navpoints[navpoints_nb].y <= border_ymax)
{
/* Accept point */
- navweights[navpoints_nb] = (1 << PATH_WEIGHT_PRECISION) + (layer * PATH_WEIGHT_STEP);
+ navweights[navpoints_nb] = (layer * PATH_WEIGHT_STEP);
host_debug("Add point %u (%u;%u) w=%u\n",
navpoints_nb, navpoints[navpoints_nb].x, navpoints[navpoints_nb].y, navweights[navpoints_nb]);
navpoints_nb++;
@@ -187,15 +187,17 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors)
int neighbors_nb = 0;
ucoo::assert(cur_point<navpoints_nb && neighbors!=NULL);
- /* Parse all nodes */
+ /* Parse all navigation points */
for(int i=0; i<navpoints_nb; i++)
{
/* Except the current one */
if (i!=cur_point)
{
- /* Get segment length */
- weight_t weight = navweights[i] * (weight_t)distance_point_point(&navpoints[cur_point], &navpoints[i]);
- weight >>= PATH_WEIGHT_PRECISION;
+ /* Compute the segment weight */
+ /* 1st: compute the distance to go */
+ weight_t weight = (weight_t)distance_point_point(&navpoints[cur_point], &navpoints[i]);
+ /* 2nd: Add the target navpoint extra weigth */
+ weight += (weight * navweights[cur_point]) >> PATH_WEIGHT_PRECISION;
host_debug("- Node %u (%u;%u) w=%u (%u) ", i, navpoints[i].x, navpoints[i].y, weight, navweights[i]);
/* Check every obstacle */
@@ -320,8 +322,8 @@ void Path::endpoints(const vect_t &src, const vect_t &dst)
navpoints[PATH_NAVPOINT_DST_IDX] = dst;
/* Init endpoints weights */
- navweights[PATH_NAVPOINT_SRC_IDX] = (1<<PATH_WEIGHT_PRECISION);
- navweights[PATH_NAVPOINT_DST_IDX] = (1<<PATH_WEIGHT_PRECISION);
+ navweights[PATH_NAVPOINT_SRC_IDX] = 0;
+ navweights[PATH_NAVPOINT_DST_IDX] = 0;
}
bool Path::get_next(vect_t &p)
diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh
index b25d4235..beb3a3e5 100644
--- a/digital/io-hub/src/common-cc/path.hh
+++ b/digital/io-hub/src/common-cc/path.hh
@@ -104,10 +104,11 @@ class Path
PATH_CAKE_NAVPOINTS_LAYERS * (PATH_CAKE_NAVPOINTS_NB - PATH_OBSTACLES_NAVPOINTS_NB) +
#endif
PATH_OBSTACLES_NAVPOINTS_LAYERS * (PATH_OBSTACLES_NB * PATH_OBSTACLES_NAVPOINTS_NB));
- /** Navigation points weight precision (2^-n). */
+ /** Navigation points weight precision (2^-n).
+ * Pay attention to overflow on weight_t variables */
static const int PATH_WEIGHT_PRECISION = 4;
- /** Navigation points weight step (2^-n). */
- static const int PATH_WEIGHT_STEP = 6;
+ /** Navigation points weight step * (2^-n). */
+ static const int PATH_WEIGHT_STEP = 8;
/** 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;