From 2a01637d453ffcfbc3bb182ec2bef13d25e82781 Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Tue, 16 Apr 2013 01:10:46 +0200 Subject: Fix the obstacle avoidance circle radius computation --- digital/io-hub/src/common-cc/obstacles.cc | 2 +- digital/io-hub/src/common-cc/path.cc | 64 +++++++++++++++---------------- digital/io-hub/src/common-cc/path.hh | 43 ++++++--------------- 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; j0; 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; -- cgit v1.2.3