From fb7fa96504d6e81a24fe139e9701ea173e05bb0a Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Fri, 12 Apr 2013 23:54:46 +0200 Subject: digital/io-hub/src/common-cc: first commit - testing the escape factor --- digital/io-hub/src/common-cc/path.cc | 252 ++++++++++++++++++++++++++++++++++- digital/io-hub/src/common-cc/path.hh | 118 +++++++++++++--- 2 files changed, 347 insertions(+), 23 deletions(-) (limited to 'digital/io-hub/src/common-cc') diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index ef7787cc..78011eb3 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -22,23 +22,265 @@ // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. // // }}} + #include "path.hh" +#include "bot.hh" +#include "robot.hh" +#include "playground.hh" extern "C" { -#include "modules/path/astar/astar.h" +#include "modules/math/fixed/fixed.h" +#include "modules/math/geometry/distance.h" +#include "modules/utils/utils.h" +} + +#define PATH_DEBUG 1 + +#ifdef HOST +#include "../../digital/ai/src/common/debug.host.h" +#endif + +#define PATH_PLOT_ID 2 + +enum { + PATH_POINT_SRC_IDX = 0, + PATH_POINT_DST_IDX +}; + +Path::Path() : + border_xmin(pg_border_distance), + border_ymin(pg_border_distance), + border_xmax(pg_width - pg_border_distance), + border_ymax(pg_length - pg_border_distance), + escape_factor(0), + obstacles_nb(0), + points_nb(0), + next_node(0) +{ + DPRINTF("Path constructor\n"); +} + +void Path::reset() +{ + vect_t nul = {0,0}; + vect_t cake = {pg_width>>1, pg_length}; + + /* Reset everything */ + DPRINTF( "Path reset\n"); + obstacles_nb = 0; + points_nb = PATH_FIXED_POINTS_NB; + points[PATH_POINT_SRC_IDX] = nul; + points[PATH_POINT_DST_IDX] = nul; + next_node = 0; + escape_factor = 0; + + /* Declare the cake as an obstacle */ + add_obstacle(cake, 500 + BOT_SIZE_SIDE, 0, PATH_CAKE_POINTS_NB); +} + +void Path::add_obstacle(const vect_t &c, const int r, const int factor, const int num) +{ + uint32_t rot_a, rot_b, fact_r; + 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); + + /* 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); + y = 0; + + /* Compute obstacle points positions around a circle */ + for (j = 0; j= border_xmin + && points[points_nb].y >= border_ymin + && points[points_nb].x <= border_xmax + && points[points_nb].y <= border_ymax) + { + /* Accept point */ + DPRINTF("Add point %u (%u;%u)\n", points_nb, points[points_nb].x, points[points_nb].y); + points_nb++; + } + + /* 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); + x = nx; + } + +#ifdef HOST + // Plot obstacle points + robot->hardware.simu_report.pos( &points[PATH_FIXED_POINTS_NB], points_nb-PATH_FIXED_POINTS_NB, PATH_PLOT_ID); +#if 0 + // Draw the last obstacle + points[points_nb] = points[points_nb - num]; + robot->hardware.simu_report.path( &points[points_nb - num], points_nb + 1); +#endif +#endif +} + +void Path::obstacle(int index, const vect_t &c, int r, int factor) +{ + add_obstacle(c, r, factor, PATH_OBSTACLES_POINTS_NB); +} + +void Path::endpoints(const vect_t &src, const vect_t &dst) +{ + DPRINTF("Set path endpoints src=(%u;%u) dst=(%u;%u)\n", + src.x, src.y, dst.x, dst.y); + points[PATH_POINT_SRC_IDX] = src; + points[PATH_POINT_DST_IDX] = dst; +} + +void Path::compute(int factor) +{ + DPRINTF("** Path compute(start) factor=%u\n", factor); + + /* Store the escape factor */ + escape_factor = factor; + + /* Call the A* algorithm */ + path_found = (bool)astar(astar_nodes, PATH_NODES_NB, PATH_POINT_DST_IDX, PATH_POINT_SRC_IDX); + if (path_found) + { + /* Store next node to go to */ + next_node = astar_nodes[0].prev; + +#if PATH_DEBUG + /* Log and display the path found */ + vect_t path[PATH_POINTS_NB]; + uint8_t node = PATH_POINT_SRC_IDX; + uint8_t path_nb = 0; + + DPRINTF(">> Path found: "); + while(node!=PATH_POINT_DST_IDX) + { + DPRINTF("%u, ", node); + path[path_nb++] = points[node]; + node = astar_nodes[node].prev; + } + path[path_nb++] = points[node]; + robot->hardware.simu_report.path(path, path_nb); + DPRINTF("%u\n", node); + } +#endif + + DPRINTF("** Path compute(end) found=%u\n", path_found); +} + +bool Path::get_next(vect_t &p) +{ + if (path_found) + { + p = points[next_node]; + next_node = astar_nodes[next_node].prev; + } + return path_found; +} + +vect_t& Path::get_point(uint8_t index) +{ + return points[index]; +} + +uint8_t Path::find_neighbors(uint8_t cur_point, struct astar_neighbor_t *neighbors) +{ + uint8_t i, j; + uint8_t neighbors_nb = 0; + + /* Parse all nodes */ + for(i=0; i validated w=%u\n", weight); + neighbors[neighbors_nb].node = i; + neighbors[neighbors_nb].weight = (uint16_t)UTILS_MIN(weight,0xFFFF); + neighbors_nb++; + } + else + { + DPRINTF("=> skipped\n"); + } + } + } + +#if PATH_DEBUG + DPRINTF("\tFound %u neighbors: ", neighbors_nb); + for(i=0;ipath.get_point(node); + DPRINTF("AC_ASTAR_NEIGHBOR_CALLBACK node=%u (%u;%u)\n", node, node_v.x, node_v.y); +#endif + return robot->path.find_neighbors(node, neighbors); } extern "C" uint16_t AC_ASTAR_HEURISTIC_CALLBACK (uint8_t node) { - // TODO: dummy implementation. - return 0; + vect_t node_a = robot->path.get_point(node); + vect_t node_b = robot->path.get_point(PATH_POINT_SRC_IDX); + int16_t dist = distance_point_point(&node_a, &node_b); + DPRINTF("Heuristic node=%u dist=%u\n", node, dist); + return dist; } diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh index 3c281e46..19533593 100644 --- a/digital/io-hub/src/common-cc/path.hh +++ b/digital/io-hub/src/common-cc/path.hh @@ -25,30 +25,112 @@ // }}} #include "defs.hh" -/// Path finding class. -/// TODO: dummy implementation. +extern "C" { +#include "modules/path/astar/astar.h" +} + +/** Number of possible obstacles. */ +#define 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_FIXED_POINTS_NB 2 + +/** Number of points. */ +#define PATH_POINTS_NB (PATH_FIXED_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 + +/** Angle between obstacles points. */ +#define PATH_OBSTACLES_POINTS_ANGLE(pOINTS_NB) ((1L << 24) / (pOINTS_NB)) + +/** Obstacle */ +typedef struct path_obstacle_t +{ + /** Center. */ + vect_t c; + /** Radius. */ + uint16_t r; + /** factor */ + int factor; +} path_obstacle_t; + +// Path finding class. class Path { public: - /// Reset path computation, remove every obstacles. - void reset () { } - /// Set an obstacle position, radius and factor. - void obstacle (int index, const vect_t &c, int r, int factor = 0) { } - /// Set path source and destination. - void endpoints (const vect_t &src, const vect_t &dst) { dst_ = dst; } - /// Compute path with the given escape factor. - void compute (int factor = 0) { } - /// Get next point in computed path, return false if none (no path found - /// or last point given yet). - bool get_next (vect_t &p) { p = dst_; return true; } - /// Prepare score compuration for the given source, with given escape - /// factor. + // Initialise 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); + + // 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); + + // Return a point by index + vect_t& get_point(uint8_t index); + + // Get next point in computed path, return false if none (no path found + // or last point given yet). + bool get_next(vect_t &p); + + //Find all neighbors of a given node, fill the astar_neighbor structure + uint8_t find_neighbors(uint8_t node, struct astar_neighbor_t *neighbors); + +#if 0 + // Prepare score computation for the given source, with given escape factor. void prepare_score (const vect_t &src, int factor = 0); - /// Return score for a given destination, using a previous preparation - /// (also reuse previously given escape factor). + + // Return score for a given destination, using a previous preparation + // (also reuse previously given escape factor). int get_score (const vect_t &dst); +#endif + private: - vect_t dst_; + + /** 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; + + /** List of obstacles. */ + path_obstacle_t obstacles[PATH_OBSTACLES_NB]; + + uint8_t obstacles_nb; + + /** List of navigation points */ + vect_t points[PATH_POINTS_NB]; + + /** Number of navigation points */ + uint8_t points_nb; + + /** List of nodes used for A*. */ + struct astar_node_t astar_nodes[PATH_NODES_NB]; + + /** Which node to look at for next step. */ + uint8_t next_node; + + /** TRUE when a path has been found */ + bool path_found; }; #endif // path_hh -- cgit v1.2.3