diff --git a/art_autonomous_exploration/src/navigation.py b/art_autonomous_exploration/src/navigation.py index d550414..113f978 100644 --- a/art_autonomous_exploration/src/navigation.py +++ b/art_autonomous_exploration/src/navigation.py @@ -215,6 +215,25 @@ def selectTarget(self): # Reverse the path to start from the robot self.path = self.path[::-1] + ######################################### + # Extra challenge #1 + # A. Smooth path planner + if len(self.path) > 3: + x = np.array(self.path) + y = np.copy(x) + a = 0.5 + b = 0.1 + + # FORMULA : y_i = y_i + a * (x_i - y_i) + b * (y_i+1 - 2 * y_i + y_i+1) + + epsilon = np.sum(np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :]))) + while epsilon > 1e-3: + y[1:-1, :] += a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :]) + epsilon = np.sum(np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :]))) + + # Copy the smoother path + self.path = y.tolist() + # Break the path to subgoals every 2 pixels (1m = 20px) step = 1 n_subgoals = (int) (len(self.path)/step) @@ -231,6 +250,7 @@ def selectTarget(self): # may not be desired for coverage-based exploration ######################################################################## + self.counter_to_next_sub = self.count_limit # Publish the path for visualization purposes diff --git a/art_autonomous_exploration/src/target_selection.py b/art_autonomous_exploration/src/target_selection.py index 0048821..1417eff 100755 --- a/art_autonomous_exploration/src/target_selection.py +++ b/art_autonomous_exploration/src/target_selection.py @@ -44,30 +44,26 @@ def selectTarget(self, init_ogm, coverage, robot_pose, origin, \ # Find only the useful boundaries of OGM. Only there calculations # have meaning ogm_limits = OgmOperations.findUsefulBoundaries(init_ogm, origin, resolution) - #print "ogm_limits", ogm_limits + # Blur the OGM to erase discontinuities due to laser rays ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits) - #print "ogm", ogm # Calculate Brushfire field tinit = time.time() brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE) - #print "brush", brush # Calculate skeletonization tinit = time.time() skeleton = self.topo.skeletonizationCffi(ogm, \ origin, resolution, ogm_limits) Print.art_print("Skeletonization time: " + str(time.time() - tinit), Print.ORANGE) - #print "skeleton", skeleton # Find topological graph tinit = time.time() nodes = self.topo.topologicalNodes(ogm, skeleton, coverage, origin, \ resolution, brush, ogm_limits) Print.art_print("Topo nodes time: " + str(time.time() - tinit), Print.ORANGE) - #print "nodes", nodes # Visualization of topological nodes vis_nodes = [] @@ -88,38 +84,66 @@ def selectTarget(self, init_ogm, coverage, robot_pose, origin, \ # SMART TARGET SELECTION USING: # 1. Brush-fire field - # 2. OMG Skeleton + # 2. OGM Skeleton # 3. Topological Nodes # 4. Coverage field - max_brush = -999 - min_dist = 999 + # 5. OGM Limits + + # Next subtarget is selected based on a + # weighted-calculated score for each node. The score + # is calculated using normalized values of the brush + # field and the number of branches. The weight values + # are defined experimentaly through the tuning method. + temp_score = 0 + max_score = 0 + best_node = nodes[0] + + # the max-min boundaries are set arbitarily + BRUSH_MAX = 17 + BRUSH_MIN = 1 + BRUSH_WEIGHT = 2.5 + BRANCH_MAX = 10 + BRANCH_MIN = 0 + BRANCH_WEIGHT = 2.5 + DISTANCE_MIN = 0 + DISTANCE_MAX = 40 + DISTANCE_WEIGHT = 0.5 + for n in nodes: - # Find the node with max brush value -> - # which tends to have max distance from the obstacles - if brush[n[0]][n[1]] > max_brush: - max_brush = brush[n[0]][n[1]] - brush_x = n[0] - brush_y = n[1] + # Use brushfire to increase temp_score + temp_score = (brush[n[0]][n[1]] - BRUSH_MIN) / (BRUSH_MAX - BRUSH_MIN) * BRUSH_WEIGHT - # Use OMG Skeleton to find potential + # Use OGM Skeleton to find potential # branches following the target branches = 0 for i in range(-1,2): for j in range(-1,2): if (i != 0 or j != 0): - branches += skeleton[brush_x+i][brush_y+j] - # If branches >= 2 --> path continues beyond the target - if branches >= 2: - final_x = brush_x - final_y = brush_y + branches += skeleton[n[0]+i][n[1]+j] + + # Use OGM-Skeleton to increase temp_score (select a goal with more future options) + temp_score += (branches - BRANCH_MIN) / (BRANCH_MAX - BRUSH_MIN) * BRANCH_WEIGHT + + # Use OGM-Limits to decrease temp_score + # the goal closest to OGM limits is best exploration option + distance = math.sqrt((ogm_limits['max_x'] - n[0]) ** 2 + (ogm_limits['max_y'] - n[1]) ** 2) + temp_score -= (distance - DISTANCE_MIN) / (DISTANCE_MAX - DISTANCE_MIN) * DISTANCE_WEIGHT + # If temp_score is higher than current max + # score, then max score is updated and current node + # becomes next goal - target + if temp_score > max_score: + max_score = temp_score + best_node = n + final_x = best_node[0] + final_y = best_node[1] target = [final_x, final_y] # Random point - #if self.method == 'random' or force_random == True: - # target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) + # if self.method == 'random' or force_random == True: + # target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) ######################################################################## return target diff --git a/art_autonomous_exploration/src/utilities.py b/art_autonomous_exploration/src/utilities.py index d2be6f3..f6c863d 100755 --- a/art_autonomous_exploration/src/utilities.py +++ b/art_autonomous_exploration/src/utilities.py @@ -39,12 +39,16 @@ def brushfireFromObstacles(ogm, brush, ogml): for i in range(len(y)): yi[i] = ffi.cast("int *", y[i].ctypes.data) - br_c = lib.brushfireFromObstacles(xi, yi, len(x), len(x[0]), + br_c = lib.brushfireFromObstacles(xi, yi, len(x), len(x[0]), ogml['min_x'], ogml['max_x'], ogml['min_y'], ogml['max_y']) - # TODO: Must be faster! - for i in range(ogm.shape[0]): - for j in range(ogm.shape[1]): - brush[i][j] = yi[i][j] + # # TODO: Must be faster! + # for i in range(ogm.shape[0]): + # for j in range(ogm.shape[1]): + # brush[i][j] = yi[i][j] + # + # Extra challenge #2 + # Optimize targer selection calculations + brush[:ogm.shape[0], :ogm.shape[1]] = np.array(y) return brush @@ -69,6 +73,12 @@ def thinning(skeleton, ogml): for i in range(skeleton.shape[0]): for j in range(skeleton.shape[1]): skeleton[i][j] = yi[i][j] + + # Extra Challenge #2 + # Optimize target selection calculations + # itime = time.time() + # skeleton = np.array(y) + Print.art_print("Skeletonization final copy: " + str(time.time() - itime), Print.BLUE) return skeleton @@ -87,7 +97,7 @@ def prune(skeleton, ogml, iterations): br_c = lib.prune(xi, yi, len(x), len(x[0]), ogml['min_x'], ogml['max_x'], ogml['min_y'], ogml['max_y'], iterations) - + # TODO: Must be faster! for i in range(skeleton.shape[0]): for j in range(skeleton.shape[1]): @@ -118,7 +128,7 @@ def findUsefulBoundaries(ogm, origin, resolution): min_y = origin['y'] / resolution max_x = origin['x'] / resolution max_y = origin['y'] / resolution - + x = ogm.shape[0] y = ogm.shape[1] @@ -175,15 +185,15 @@ def findUsefulBoundaries(ogm, origin, resolution): min_y * resolution + origin['y'] ], [ - max_x * resolution + origin['x'], + max_x * resolution + origin['x'], min_y * resolution + origin['y'] ], [ - max_x * resolution + origin['x'], + max_x * resolution + origin['x'], max_y * resolution + origin['y'] ], [ - min_x * resolution + origin['x'], + min_x * resolution + origin['x'], max_y * resolution + origin['y'] ] ],\ @@ -197,9 +207,9 @@ def findUsefulBoundaries(ogm, origin, resolution): return { - 'min_x': min_x, - 'max_x': max_x, - 'min_y': min_y, + 'min_x': min_x, + 'max_x': max_x, + 'min_y': min_y, 'max_y': max_y } @@ -272,4 +282,3 @@ def printMarker(poses, m_type, action, frame, ns, color, scale): RvizHandler.markers_publisher.publish(markers) -