summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorOlivier Lanneluc2013-04-16 01:10:46 +0200
committerNicolas Schodet2013-04-27 00:19:08 +0200
commit2a01637d453ffcfbc3bb182ec2bef13d25e82781 (patch)
tree60d4d4764dd69e91a6c525a0c40d62e763e5befb /digital
parent9dee862008b43919c90f5ebe5b95bc307bd1600f (diff)
Fix the obstacle avoidance circle radius computation
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.cc64
-rw-r--r--digital/io-hub/src/common-cc/path.hh43
3 files changed, 43 insertions, 66 deletions
diff --git a/digital/io-hub/src/common-cc/obstacles.cc b/digital/io-hub/src/common-cc/obstacles.cc
index af6d1a79..fc8a5204 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 01a6821d..2672426f 100644
--- a/digital/io-hub/src/common-cc/path.cc
+++ b/digital/io-hub/src/common-cc/path.cc
@@ -43,6 +43,9 @@ extern "C" {
#define PATH_PLOT_ID 2
+/** Angle between obstacles points. */
+#define PATH_ANGLE_824(pOINTS_NB) ((1L << 24) / (pOINTS_NB))
+
enum {
PATH_POINT_SRC_IDX = 0,
PATH_POINT_DST_IDX
@@ -75,44 +78,39 @@ void Path::reset()
escape_factor = 0;
/* Declare the cake as an obstacle */
- add_obstacle(pg_cake_pos, pg_cake_radius + BOT_SIZE_SIDE * 4 / 3, 0, PATH_CAKE_POINTS_NB);
+ add_obstacle(pg_cake_pos, pg_cake_radius, PATH_CAKE_POINTS_NB);
}
-void Path::add_obstacle(const vect_t &c, const int r, const int factor, const int num)
+void Path::add_obstacle(const vect_t &c, int r, int num)
{
- uint32_t rot_a, rot_b, fact_r;
+ uint32_t rot_a, rot_b, nr;
uint32_t x, y, nx;
- int j;
- DPRINTF("Add obstacle c=(%u;%u) r=%u factor=%u points_nb=%u\n",c.x,c.y,r,factor,num);
+ DPRINTF("Add obstacle c=(%u;%u) r=%u num=%u\n",c.x, c.y, r, num);
+
+ /* Enlarge the exclusion radius */
+ r += BOT_SIZE_RADIUS * 4 / 3;
/* Store obstacle */
//assert(obstacles_nb < PATH_OBSTACLES_NB);
obstacles[obstacles_nb].c = c;
obstacles[obstacles_nb].r = r;
- obstacles[obstacles_nb].factor = factor;
obstacles_nb++;
/* Complex number of modulus 1 and rotation angle */
- rot_a = fixed_cos_f824(PATH_OBSTACLES_POINTS_ANGLE(num));
- rot_b = fixed_sin_f824(PATH_OBSTACLES_POINTS_ANGLE(num));
-
- /* Additional radius factor to allow the robot to go */
- /* from one obstacle point to its neighbor point */
- /* without collision with the obstacle avoidance circle */
- /* Radius factor is sqrt( sin(angle)^2 + 1^2 ) */
- fact_r = fixed_sqrt_uf248(
- (fixed_mul_f824(rot_b,rot_b) >> 16) +
- (1L<<8) );
- fact_r += 1; /* To counter rounding operations */
- //fact_r += 13; /* Add 10% margin */
-
- /* First point is at the relative position (r;0) */
- x = ((r * fact_r) >> 8);
+ nr = PATH_ANGLE_824(num);
+ rot_a = fixed_cos_f824(nr);
+ rot_b = fixed_sin_f824(nr);
+
+ /* Extend the points radius to allow the robot to go */
+ /* from one to another without collision with the */
+ /* obstacle avoidance circle. New radius is r / cos(angle/2) */
+ nr = PATH_ANGLE_824(2*num);
+ x = fixed_div_f824(r, fixed_cos_f824(nr)) + 1 /* margin */;
y = 0;
/* Compute obstacle points positions around a circle */
- for (j = 0; j<num; j++)
+ for (; num>0; num--)
{
/* Compute the point absolute position */
points[points_nb].x = c.x + (vect_value_t)x;
@@ -130,8 +128,8 @@ void Path::add_obstacle(const vect_t &c, const int r, const int factor, const in
}
/* Complex multiply with A = cos(angle) + i sin(angle) */
- nx = fixed_mul_f824(x, rot_a) - fixed_mul_f824 (y, rot_b);
- y = fixed_mul_f824 (y, rot_a) + fixed_mul_f824 (x, rot_b);
+ nx = fixed_mul_f824(x, rot_a) - fixed_mul_f824(y, rot_b);
+ y = fixed_mul_f824(y, rot_a) + fixed_mul_f824(x, rot_b);
x = nx;
}
@@ -146,9 +144,9 @@ void Path::add_obstacle(const vect_t &c, const int r, const int factor, const in
#endif
}
-void Path::obstacle(int index, const vect_t &c, int r, int factor)
+void Path::obstacle(int index, const vect_t &c, int r, int f)
{
- add_obstacle(c, r, factor, PATH_OBSTACLES_POINTS_NB);
+ add_obstacle(c, r, PATH_OBSTACLES_POINTS_NB);
}
void Path::endpoints(const vect_t &src, const vect_t &dst)
@@ -159,12 +157,12 @@ void Path::endpoints(const vect_t &src, const vect_t &dst)
points[PATH_POINT_DST_IDX] = dst;
}
-void Path::compute(int factor)
+void Path::compute(int escape)
{
- DPRINTF("** Path compute(start) factor=%u\n", factor);
+ DPRINTF("** Path compute(start) escape=%u\n", escape);
/* Store the escape factor */
- escape_factor = factor;
+ escape_factor = escape;
/* Call the A* algorithm */
path_found = (bool)astar(astar_nodes, PATH_NODES_NB, PATH_POINT_DST_IDX, PATH_POINT_SRC_IDX);
@@ -192,7 +190,7 @@ void Path::compute(int factor)
}
#endif
- DPRINTF("** Path compute(end) found=%u\n", path_found);
+ DPRINTF("** Path compute(end) found=%u escape=%u\n", path_found, escape);
}
bool Path::get_next(vect_t &p)
@@ -275,10 +273,10 @@ uint8_t Path::find_neighbors(uint8_t cur_point, struct astar_neighbor_t *neighbo
return neighbors_nb;
}
-void Path::prepare_score(const vect_t &src, int factor)
+void Path::prepare_score(const vect_t &src, int escape)
{
- DPRINTF("Path prepare score from src=(%u;%u) factor=%u\n", src.x, src.y, factor);
- escape_factor = factor;
+ DPRINTF("Path prepare score from src=(%u;%u) escape=%u\n", src.x, src.y, escape);
+ escape_factor = escape;
astar_dijkstra_prepare(astar_nodes, PATH_POINTS_NB, get_point_index(src), PATH_POINT_DST_IDX);
}
diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh
index 36fde87e..37f1c77d 100644
--- a/digital/io-hub/src/common-cc/path.hh
+++ b/digital/io-hub/src/common-cc/path.hh
@@ -29,26 +29,6 @@ extern "C" {
#include "modules/path/astar/astar.h"
}
-#if 0
- /** Number of possible obstacles. */
- static const uint8_t PATH_OBSTACLES_NB = (4+1/*cake*/);
-/** Number of points for the cake */
-#define PATH_CAKE_POINTS_NB 10
-/** Number of points per standard obstacle. */
-#define PATH_OBSTACLES_POINTS_NB 6
-#define PATH_RESERVED_POINTS_NB 2
-/** Number of points. */
-#define PATH_POINTS_NB (PATH_RESERVED_POINTS_NB + \
- (PATH_OBSTACLES_NB * PATH_OBSTACLES_POINTS_NB) + \
- (PATH_CAKE_POINTS_NB - PATH_OBSTACLES_POINTS_NB) + \
- 1 /*debug*/)
-/** Number of nodes. */
-#define PATH_NODES_NB PATH_POINTS_NB
-#endif
-
-/** Angle between obstacles points. */
-#define PATH_OBSTACLES_POINTS_ANGLE(pOINTS_NB) ((1L << 24) / (pOINTS_NB))
-
/** Obstacle */
typedef struct path_obstacle_t
{
@@ -56,8 +36,6 @@ typedef struct path_obstacle_t
vect_t c;
/** Radius. */
uint16_t r;
- /** factor */
- int factor;
} path_obstacle_t;
/** Path finding class */
@@ -68,14 +46,12 @@ class Path
Path();
/** Reset path computation, remove every obstacles */
void reset(void);
- /** Add an obstacle on the field */
- void add_obstacle(const vect_t &c, const int r, const int factor, const int points_nb);
/** Set a moving obstacle position, radius and factor*/
- void obstacle(int index, const vect_t &c, int r, int factor = 0);
+ void obstacle(int index, const vect_t &c, int r, int f = 0);
/** Set path source and destination */
void endpoints(const vect_t &src, const vect_t &dst);
/** Compute path with the given escape factor */
- void compute(int factor = 0);
+ void compute(int escape = 0);
/** Return a point vector by index */
vect_t& get_point_vect(const uint8_t index);
/** Return a point index */
@@ -86,36 +62,39 @@ class Path
/** Find all neighbors of a given node, fill the astar_neighbor structure */
uint8_t find_neighbors(uint8_t node, struct astar_neighbor_t *neighbors);
/** Prepare score computation for the given source, with given escape factor */
- void prepare_score(const vect_t &src, int factor = 0);
+ void prepare_score(const vect_t &src, int escape = 0);
/** Return score for a given destination, using a previous preparation
(also reuse previously given escape factor) */
uint16_t get_score(const vect_t &dst);
private:
+ /** Add an obstacle on the field */
+ void add_obstacle(const vect_t &c, int r, int num);
+
/** Number of possible obstacles. */
static const uint8_t PATH_OBSTACLES_NB = (4+1/*cake*/);
/** Number of points per standard obstacle. */
static const uint8_t PATH_OBSTACLES_POINTS_NB = 8;
/** Number of points for the cake */
- static const uint8_t PATH_CAKE_POINTS_NB = 12;
+ static const uint8_t PATH_CAKE_POINTS_NB = 14;
/** Number of reserved points for the 2 endpoints */
static const uint8_t PATH_RESERVED_POINTS_NB = 2;
/** Number of points. */
static const uint8_t PATH_POINTS_NB = (PATH_RESERVED_POINTS_NB +
(PATH_OBSTACLES_NB * PATH_OBSTACLES_POINTS_NB) +
- (PATH_CAKE_POINTS_NB - PATH_OBSTACLES_POINTS_NB) +
- 1 /*debug*/);
+ (PATH_CAKE_POINTS_NB - PATH_OBSTACLES_POINTS_NB));
/** Number of nodes. */
static const uint8_t PATH_NODES_NB = PATH_POINTS_NB;
+
/** Borders, any point outside borders is eliminated. */
const int16_t border_xmin, border_ymin, border_xmax, border_ymax;
/** Escape factor, 0 if none. */
- uint8_t escape_factor;
+ int escape_factor;
/** List of obstacles. */
path_obstacle_t obstacles[PATH_OBSTACLES_NB];
/** Number of obstacles */
uint8_t obstacles_nb;
- /** List of navigation points */
+ /** List of navigation points coordonates */
vect_t points[PATH_POINTS_NB];
/** Number of navigation points */
uint8_t points_nb;