// path.cc // io-hub - Modular Input/Output. {{{ // // Copyright (C) 2013 Nicolas Schodet // Copyright (C) 2013 Olivier Lanneluc // // APBTeam: // Web: http://apbteam.org/ // Email: team AT apbteam DOT org // // This program is free software; you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation; either version 2 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. // // }}} #include "path.hh" #include "bot.hh" #include "robot.hh" #include "playground.hh" #ifdef HOST #include "debug.host.hh" #endif extern "C" { #include "modules/math/fixed/fixed.h" #include "modules/math/geometry/distance.h" #include "modules/utils/utils.h" } /** Enable path finding debug code */ #define PATH_DEBUG 1 /** Define path finding plot ID for the python simu framework */ #define PATH_PLOT_ID 2 /** Angle between obstacles points. */ #define PATH_ANGLE_824(pOINTS_NB) \ ((1L << 24) / (pOINTS_NB)) /** Check for vectors equality */ #define PATH_VECT_EQUAL(v1, v2) \ ((v1)->x==(v2)->x && (v1)->y==(v2)->y) /** Check if point is inside a circle */ #define PATH_IN_CIRCLE(pOINT, cENTER, rADIUS) \ (distance_point_point((pOINT), (cENTER)) <= (rADIUS)) /** Static nodes index for the endpoints */ enum { PATH_NAVPOINT_SRC_IDX = 0, PATH_NAVPOINT_DST_IDX }; Path::Path() : border_xmin(pg_border_distance + pg_plate_size_border*2 + BOT_SIZE_RADIUS/2), border_ymin(pg_border_distance), border_xmax(pg_width - pg_border_distance - pg_plate_size_border*2 - BOT_SIZE_RADIUS/2), border_ymax(pg_length - pg_border_distance), escape_factor(0), obstacles_nb(0), navpoints_nb(0), next_node(0) { host_debug("Path constructor\n"); } void Path::reset() { vect_t nul = {0,0}; /* Reset everything */ host_debug("Path reset\n"); obstacles_nb = 0; navpoints_nb = PATH_RESERVED_NAVPOINTS_NB; navpoints[PATH_NAVPOINT_SRC_IDX] = nul; navpoints[PATH_NAVPOINT_DST_IDX] = nul; next_node = 0; escape_factor = 0; #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 */, 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, const bool target) { uint32_t rot_a, rot_b, nr; uint32_t rot_c, rot_d; uint32_t x, y, nx; int npt, layer; host_debug("Add obstacle c=(%u;%u) r=%u nodes=%u layers=%u\n",c.x, c.y, r, nodes, nlayers); /* Enlarge the obstacle radius by the robot size and clearance area width */ r += BOT_SIZE_RADIUS + clearance; /* Store obstacle */ //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) */ nr = PATH_ANGLE_824(nodes); rot_a = fixed_cos_f824(nr); rot_b = fixed_sin_f824(nr); /* Complex number B = cos(angle/2) + i sin(angle/2) */ nr = PATH_ANGLE_824(nodes*2); rot_c = fixed_cos_f824(nr); rot_d = fixed_sin_f824(nr); /* Extend the points radius to allow the robot to go */ /* from one to another without collision with the */ /* obstacle circle. New radius is r / cos(angle/2) */ x = fixed_div_f824(r, rot_c) + 2 /* margin for the unprecise fixed point computation */; y = 0; /* Add a number of sets of navigation points with different weights */ for(layer=nlayers-1; layer>=0; layer--) { /* Compute obstacle points positions around a circle */ for (npt=0; npt= border_xmin && navpoints[navpoints_nb].y >= border_ymin && navpoints[navpoints_nb].x <= border_xmax && navpoints[navpoints_nb].y <= border_ymax) { /* Accept point */ navweights[navpoints_nb] = (1 << PATH_WEIGHT_PRECISION) + (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++; } /* 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; } /* Prepare the next layer */ /* Enlarge the circle */ x += BOT_SIZE_RADIUS; y = 0; /* Rotate the next point by (angle/2) */ /* This interleaves the new navpoints with the previous ones */ nx = fixed_mul_f824(x, rot_c) - fixed_mul_f824(y, rot_d); y = fixed_mul_f824(y, rot_c) + fixed_mul_f824(x, rot_d); x = nx; } #ifdef HOST /* Plot obstacle points */ robot->hardware.simu_report.pos( &navpoints[PATH_RESERVED_NAVPOINTS_NB], navpoints_nb-PATH_RESERVED_NAVPOINTS_NB, PATH_PLOT_ID); #endif } int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors) { int neighbors_nb = 0; /* Parse all nodes */ for(int i=0; i>= 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 */ for(int j=0; j validated w=%u\n", weight); neighbors[neighbors_nb].node = i; neighbors[neighbors_nb].weight = weight; neighbors_nb++; } else { host_debug("=> skipped\n"); } } } #ifdef PATH_DEBUG host_debug("\tFound %u neighbors: ", neighbors_nb); for(int i=0;i> Path found: "); while(node!=PATH_NAVPOINT_DST_IDX) { host_debug("%u (%u), ", node, navweights[node]); path[path_nb++] = navpoints[node]; node = astar_nodes[node].prev; } path[path_nb++] = navpoints[node]; robot->hardware.simu_report.path(path, path_nb); host_debug("%u\n", node); } #endif host_debug("** Path compute(end) found=%u escape=%u\n", path_found, escape); } 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 */, target); } 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", src.x, src.y, dst.x, dst.y); navpoints[PATH_NAVPOINT_SRC_IDX] = src; navpoints[PATH_NAVPOINT_DST_IDX] = dst; /* Init endpoints weights */ navweights[PATH_NAVPOINT_SRC_IDX] = (1<path.get_point_vect(node); host_debug("AC_ASTAR_NEIGHBOR_CALLBACK node=%u (%u;%u)\n", node, point_v.x, point_v.y); #endif return robot->path.find_neighbors(node, neighbors); } extern "C" uint16_t AC_ASTAR_HEURISTIC_CALLBACK (uint8_t node) { vect_t point_a = robot->path.get_point_vect(node); vect_t point_b = robot->path.get_point_vect(PATH_NAVPOINT_SRC_IDX); int16_t dist = distance_point_point(&point_a, &point_b); host_debug("Heuristic node=%u dist=%u\n", node, dist); return dist; }