From 06a8ecd50af9289e2de49e5e95cfcf3fc617f61e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 May 2013 23:03:28 +0200 Subject: digital/io-hub/src/common-cc/path: disable debug code --- digital/io-hub/src/common-cc/path.cc | 62 ++++++++++++++++++++---------------- 1 file changed, 34 insertions(+), 28 deletions(-) (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index 77ab1869..64d56f12 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -38,7 +38,13 @@ extern "C" { } /** Enable path finding debug code */ -#define PATH_DEBUG 1 +#define PATH_DEBUG 0 + +#if PATH_DEBUG +# define path_debug host_debug +#else +# define path_debug(args...) do { } while (0) +#endif /** Define path finding plot ID for the python simu framework */ #define PATH_PLOT_ID 2 @@ -79,13 +85,13 @@ Path::Path( const uint16_t border_xmin, navpoints_nb(0), next_node(0) { - host_debug("Path constructor\n"); + path_debug("Path constructor\n"); } void Path::reset() { vect_t nul = {0,0}; - host_debug("Path reset\n"); + path_debug("Path reset\n"); obstacles_nb = 0; navpoints_nb = CC_RESERVED_NAVPOINTS_NB; navpoints[CC_NAVPOINT_SRC_IDX] = nul; @@ -105,7 +111,7 @@ void Path::add_obstacle( const vect_t &c, int32_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); + path_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 */ r += BOT_SIZE_RADIUS; @@ -150,7 +156,7 @@ void Path::add_obstacle( const vect_t &c, { /* Accept point */ navweights[navpoints_nb] = (layer * CC_NAVWEIGHT_STEP); - host_debug("Add point %u (%u;%u) w=%u\n", + path_debug("Add point %u (%u;%u) w=%u\n", navpoints_nb, navpoints[navpoints_nb].x, navpoints[navpoints_nb].y, navweights[navpoints_nb]); navpoints_nb++; } @@ -169,7 +175,7 @@ void Path::add_obstacle( const vect_t &c, y += BOT_SIZE_RADIUS; x = 0; } -#ifdef HOST +#ifdef TARGET_host /* Plot obstacle points */ robot->hardware.simu_report.pos( &navpoints[CC_RESERVED_NAVPOINTS_NB], navpoints_nb-CC_RESERVED_NAVPOINTS_NB, PATH_PLOT_ID); #endif @@ -191,7 +197,7 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors) weight_t weight = (weight_t)distance_point_point(&navpoints[cur_point], &navpoints[next_point]); /* 2nd: Add the target navpoint extra weigth */ weight += (weight * navweights[cur_point]) >> CC_NAVWEIGHT_PRECISION; - host_debug("- Node %u (%u;%u) w=%u (%u) ", next_point, navpoints[next_point].x, navpoints[next_point].y, weight, navweights[next_point]); + path_debug("- Node %u (%u;%u) w=%u (%u) ", next_point, navpoints[next_point].x, navpoints[next_point].y, weight, navweights[next_point]); /* Check every obstacle */ for(int j=0; j validated w=%u\n", weight); + path_debug("=> validated w=%u\n", weight); neighbors[neighbors_nb].node = next_point; neighbors[neighbors_nb].weight = weight; neighbors_nb++; } else { - host_debug("=> skipped\n"); + path_debug("=> skipped\n"); } } } -#ifdef PATH_DEBUG - host_debug("\tFound %u neighbors: ", neighbors_nb); +#if PATH_DEBUG + path_debug("\tFound %u neighbors: ", neighbors_nb); for(int i=0;i> Path found: "); + path_debug(">> Path found: "); while(node!=CC_NAVPOINT_DST_IDX) { - host_debug("%u (%u), ", node, navweights[node]); + path_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); - } + path_debug("%u\n", node); #endif + } - host_debug("** Path compute(end) found=%u escape=%u\n", path_found, escape); + path_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) @@ -309,7 +315,7 @@ void Path::obstacle(const int index, const vect_t &c, const uint16_t r, const in 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", + path_debug("Set path endpoints src=(%u;%u) dst=(%u;%u)\n", src.x, src.y, dst.x, dst.y); navpoints[CC_NAVPOINT_SRC_IDX] = src; navpoints[CC_NAVPOINT_DST_IDX] = dst; @@ -348,7 +354,7 @@ int Path::get_point_index(const vect_t& point) void Path::prepare_score(const vect_t &src, weight_t escape) { - host_debug("Path prepare score from src=(%u;%u) escape=%u\n", src.x, src.y, escape); + path_debug("Path prepare score from src=(%u;%u) escape=%u\n", src.x, src.y, escape); escape_factor = escape; astar_dijkstra_prepare(astar_nodes, navpoints_nb, get_point_index(src), CC_NAVPOINT_DST_IDX); } @@ -356,16 +362,16 @@ void Path::prepare_score(const vect_t &src, weight_t escape) weight_t Path::get_score(const vect_t &dst) { uint16_t score = astar_dijkstra_finish(astar_nodes, navpoints_nb, get_point_index(dst)); - host_debug("Path get score=%u for dst=(%u;%u)\n", score, dst.x, dst.y); + path_debug("Path get score=%u for dst=(%u;%u)\n", score, dst.x, dst.y); return score; } extern "C" uint8_t AC_ASTAR_NEIGHBOR_CALLBACK (uint8_t node, struct astar_neighbor_t *neighbors) { -#ifdef PATH_DEBUG +#if PATH_DEBUG vect_t point_v = robot->path.get_point_vect(node); - host_debug("AC_ASTAR_NEIGHBOR_CALLBACK node=%u (%u;%u)\n", node, point_v.x, point_v.y); + path_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); } @@ -376,6 +382,6 @@ 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(CC_NAVPOINT_SRC_IDX); int16_t dist = distance_point_point(&point_a, &point_b); - host_debug("Heuristic node=%u dist=%u\n", node, dist); + path_debug("Heuristic node=%u dist=%u\n", node, dist); return dist; } -- cgit v1.2.3