From 53534d968ac93bc8d908c3a7e80c0afd26baa15b Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Thu, 2 May 2013 23:30:24 +0200 Subject: digital/io-hub/src/common-cc/path.cc: new way to allow robot to target the cake Remove the parameter force_move in method set_endpoint, add it in the structure obstacle_t, and set it to true statically for the cake obstacle, false for the other ones. --- digital/io-hub/src/common-cc/path.cc | 47 +++++++++++++++++++++--------------- digital/io-hub/src/common-cc/path.hh | 14 +++++------ 2 files changed, 34 insertions(+), 27 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 bb947212..a9cb5948 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -90,11 +90,21 @@ 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, PATH_CAKE_NAVPOINTS_LAYERS, 0 /* no extra clearance radius */); + add_obstacle( pg_cake_pos, + pg_cake_radius, + PATH_CAKE_NAVPOINTS_NB, + PATH_CAKE_NAVPOINTS_LAYERS, + 0 /* no extra clearance radius */, + true /*target the center is allowed*/); #endif } -void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int nlayers, const uint16_t clearance) +void Path::add_obstacle( const vect_t &c, + uint16_t r, + const int nodes, + const int nlayers, + const uint16_t clearance, + const bool target) { uint32_t rot_a, rot_b, nr; uint32_t rot_c, rot_d; @@ -110,6 +120,7 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int //assert(obstacles_nb < PATH_OBSTACLES_NB); obstacles[obstacles_nb].c = c; obstacles[obstacles_nb].r = r; + obstacles[obstacles_nb].target_allowed = target; obstacles_nb++; /* Complex number A = cos(angle) + i sin(angle) */ @@ -196,8 +207,12 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors) uint16_t d = distance_segment_point(&navpoints[cur_point], &navpoints[i], &obstacles[j].c); if (d < obstacles[j].r) { - /* Collision while forcing the last move to the destination */ - if (force_move && cur_point==PATH_NAVPOINT_DST_IDX && + /* Collision while planing the last move to the */ + /* the center of an obstacle. This is useful to */ + /* target the center of an obstacle and stop */ + /* in front of it (ex, the cake in apbirthday 2013) */ + if (obstacles[j].target_allowed && + cur_point==PATH_NAVPOINT_DST_IDX && PATH_VECT_EQUAL(&navpoints[cur_point], &obstacles[j].c)) { /* Skip this obstacle */ @@ -288,12 +303,17 @@ void Path::compute(weight_t escape) host_debug("** Path compute(end) found=%u escape=%u\n", path_found, escape); } -void Path::obstacle(int index, const vect_t &c, uint16_t r, int f) +void Path::obstacle(const int index, const vect_t &c, const uint16_t r, const int f, const bool target) { - add_obstacle(c, r, PATH_OBSTACLES_NAVPOINTS_NB, PATH_OBSTACLES_NAVPOINTS_LAYERS, Obstacles::clearance_mm + 100/* extra clearance radius */); + add_obstacle( c, + r, + PATH_OBSTACLES_NAVPOINTS_NB, + PATH_OBSTACLES_NAVPOINTS_LAYERS, + Obstacles::clearance_mm + 100/* extra clearance radius */, + target); } -void Path::endpoints(const vect_t &src, const vect_t &dst, const bool force_move) +void Path::endpoints(const vect_t &src, const vect_t &dst) { /* Store endpoints location */ host_debug("Set path endpoints src=(%u;%u) dst=(%u;%u)\n", @@ -304,19 +324,6 @@ void Path::endpoints(const vect_t &src, const vect_t &dst, const bool force_move /* Init endpoints weights */ navweights[PATH_NAVPOINT_SRC_IDX] = (1<force_move = force_move; - -#ifdef playground_2013_hh - /* Temporary code for the cake */ - if (PATH_VECT_EQUAL(&dst, &pg_cake_pos)) - { - this->force_move = true; - } -#endif } 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 e683b83a..1232cda1 100644 --- a/digital/io-hub/src/common-cc/path.hh +++ b/digital/io-hub/src/common-cc/path.hh @@ -38,6 +38,10 @@ typedef struct path_obstacle_t vect_t c; /** Radius. */ uint16_t r; + /** Allow the robot to target the obstacle + * in order to go in front of it, while stopping + * at a given distance, before the collision happens */ + bool target_allowed; } path_obstacle_t; typedef uint16_t weight_t; @@ -51,9 +55,9 @@ class Path /** Reset path computation, remove every obstacles */ void reset(void); /** Set a moving obstacle position, radius and factor*/ - void obstacle(int index, const vect_t &c, uint16_t r, int f = 0); + void obstacle(const int index, const vect_t &c, const uint16_t r, const int f = 0, const bool target = false); /** Set path source and destination */ - void endpoints(const vect_t &src, const vect_t &dst, const bool force_move=false); + void endpoints(const vect_t &src, const vect_t &dst); /** Compute path with the given escape factor */ void compute(weight_t escape = 0); /** Return a point vector by index */ @@ -73,7 +77,7 @@ class Path private: /** Add an obstacle on the field */ - void add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int nlayers, const uint16_t clearance); + void add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int nlayers, const uint16_t clearance, const bool target); /** Number of possible obstacles. */ static const int PATH_OBSTACLES_NB = (4+1/*cake*/); @@ -120,10 +124,6 @@ class Path int next_node; /** TRUE when a path has been found */ bool path_found; - /** TRUE to allow last movement to enter an obstacle - * This may be used to target the center of an obstacle - * and stop closed to it */ - bool force_move; }; #endif // path_hh -- cgit v1.2.3