def skeletonizationCffi(self, ogm, origin, resolution, ogml): width = ogm.shape[0] height = ogm.shape[1] local = numpy.zeros(ogm.shape) for i in range(0, width): for j in range(0, height): if ogm[i][j] < 49: local[i][j] = 1 skeleton = Cffi.thinning(local, ogml) skeleton = Cffi.prune(skeleton, ogml, 10) viz = [] for i in range(0, width): for j in range(0, height): if skeleton[i][j] == 1: viz.append([ i * resolution + origin['x'], j * resolution + origin['y'] ]) RvizHandler.printMarker(\ viz,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_skeletonization_cffi", # Namespace [0.5, 0, 0, 0.5], # Color RGBA 0.05 # Scale ) return skeleton
def skeletonizationCffi(self, ogm, origin, resolution, ogml): # width = ogm.shape[0] # height = ogm.shape[1] local = numpy.zeros(ogm.shape) local[ogm < 49] = 1 # for i in range(0, width): # for j in range(0, height): # if ogm[i][j] < 49: # local[i][j] = 1 skeleton = Cffi.thinning(local, ogml) skeleton = Cffi.prune(skeleton, ogml, 10) # viz = [] # for i in range(0, width): # for j in range(0, height): # if skeleton[i][j] == 1: # viz.append([i * resolution + origin['x'],j * resolution + origin['y']]) viz = (numpy.array(numpy.where(skeleton == 1)).T * resolution + [origin['x'], origin['y']]).tolist() RvizHandler.printMarker(\ viz,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_skeletonization_cffi", # Namespace [0.5, 0, 0, 0.5], # Color RGBA 0.05 # Scale ) return skeleton
def checkTarget(self, event): # Check if we have a target or if the robot just wanders if not self.inner_target_exists or not self.move_with_target or self.next_subtarget == len( self.subtargets): return self.counter_to_next_sub -= 1 if self.counter_to_next_sub == 0: Print.art_print('\n~~~~ Time reset ~~~~', Print.RED) self.inner_target_exists = False self.target_exists = False self.force_random = True return # Get the robot pose in pixels [rx, ry] = [ self.robot_perception.robot_pose['x_px'] - self.robot_perception.origin['x'] / self.robot_perception.resolution, self.robot_perception.robot_pose['y_px'] - self.robot_perception.origin['y'] / self.robot_perception.resolution ] ######################### NOTE: QUESTION ############################## # What if a later subtarget or the end has been reached before the next subtarget? Alter the code # accordingly. Check if distance is less than 7 px (14 cm). Slice the list of subtargets and then reverse it. for idx, st in enumerate(self.subtargets[self.next_subtarget::][::-1]): # Right now, idx refers to the sliced & reversed array, fix it. idx = len(self.subtargets) - 1 - idx assert idx >= self.next_subtarget dist = math.hypot(rx - st[0], ry - st[1]) if dist < 7: self.next_subtarget = idx + 1 self.counter_to_next_sub = self.count_limit if self.next_subtarget == len(self.subtargets): self.target_exists = False break ######################################################################## # Publish the current target if self.next_subtarget == len(self.subtargets): return subtarget = [ self.subtargets[self.next_subtarget][0] * self.robot_perception.resolution + self.robot_perception.origin['x'], self.subtargets[self.next_subtarget][1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ] RvizHandler.printMarker( [subtarget], 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_next_subtarget", # Namespace [0, 0, 0.8, 0.8], # Color RGBA 0.2 # Scale )
def selectTarget(self, init_ogm, coverage, robot_pose, origin, resolution, force_random=False): target = [-1, -1] ######################### NOTE: QUESTION ############################## # Implement a smart way to select the next target. You have the # following tools: ogm_limits, Brushfire field, OGM skeleton, # topological nodes. # Find only the useful boundaries of OGM. Only there calculations # have meaning ogm_limits = OgmOperations.findUsefulBoundaries(init_ogm, origin, resolution) # Blur the OGM to erase discontinuities due to laser rays ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits) # Calculate Brushfire field tinit = time.time() brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE) # 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) # 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) # Visualization of topological nodes vis_nodes = [] for n in nodes: vis_nodes.append([ n[0] * resolution + origin['x'], n[1] * resolution + origin['y'] ]) RvizHandler.printMarker( vis_nodes, 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_topological_nodes", # Namespace [0.3, 0.4, 0.7, 0.5], # Color RGBA 0.1 # Scale ) # Random point if self.method == 'random' or force_random == True: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) ######################################################################## return target
def checkTarget(self, event): # Check if we have a target or if the robot just wanders if not self.inner_target_exists or not self.move_with_target or self.next_subtarget == len( self.subtargets): return # Check if timer has expired if self.timer_flag: if self.timerThread.expired: Print.art_print('\n~~~~ Time reset ~~~~', Print.RED) self.inner_target_exists = False self.target_exists = False return # Get the robot pose in pixels [rx, ry] = [ self.robot_perception.robot_pose['x_px'] - self.robot_perception.origin['x'] / self.robot_perception.resolution, self.robot_perception.robot_pose['y_px'] - self.robot_perception.origin['y'] / self.robot_perception.resolution ] theta_robot = self.robot_perception.robot_pose['th'] # Clear achieved targets self.subtargets = self.subtargets[self.next_subtarget:] self.next_subtarget = 0 # If distance between the robot pose and the next subtarget is < 7 pixel consider that Target is Reached dist = math.hypot(rx - self.subtargets[self.next_subtarget][0], ry - self.subtargets[self.next_subtarget][1]) if dist < 7: self.next_subtarget += 1 self.counter_to_next_sub = self.count_limit # Check if the final subtarget has been approached if self.next_subtarget == len(self.subtargets): self.target_exists = False # Publish the current target if self.next_subtarget >= len(self.subtargets): return subtarget = [ self.subtargets[self.next_subtarget][0] * self.robot_perception.resolution + self.robot_perception.origin['x'], self.subtargets[self.next_subtarget][1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ] RvizHandler.printMarker([subtarget], 1, 0, "map", "art_next_subtarget", [0, 0, 0.8, 0.8], 0.2)
def print_viz(skeleton, resolution, x, y): i, j = numpy.where(skeleton == 1) viz = zip(i * resolution + x, j * resolution + y) RvizHandler.printMarker( viz, 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_skeletonization", # Namespace [0.5, 0, 0, 0.5], # Color RGBA 0.05 # Scale )
def skeletonization(self, ogm, origin, resolution, ogml): width = ogm.shape[0] height = ogm.shape[1] useful_ogm = ogm[ogml['min_x']:ogml['max_x'], ogml['min_y']:ogml['max_y']] useful_width = useful_ogm.shape[0] useful_height = useful_ogm.shape[1] local = numpy.zeros(ogm.shape) useful_local = numpy.zeros(useful_ogm.shape) # for i in range(0, useful_width): # for j in range(0, useful_height): # if useful_ogm[i][j] < 49: # useful_local[i][j] = 1 useful_local[numpy.where(useful_ogm < 49)] = 1 #todo allagi skeleton = skeletonize(useful_local) skeleton = self.pruning(skeleton, 10) # padding skeleton_final = numpy.zeros(ogm.shape) skeleton_final[ogml['min_x']:ogml['max_x'], ogml['min_y']:ogml['max_y']] = skeleton viz = [] for i in range(0, width): for j in range(0, height): if skeleton_final[i][j] == 1: viz.append([ i * resolution + origin['x'], j * resolution + origin['y'] ]) RvizHandler.printMarker(\ viz,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_skeletonization", # Namespace [0.5, 0, 0, 0.5], # Color RGBA 0.05 # Scale ) #scipy.misc.imsave('/home/manos/Desktop/test.png', skeleton_final) return skeleton_final
def skeletonizationCffi(self, ogm, origin, resolution, ogml): width = ogm.shape[0] height = ogm.shape[1] # import time local = numpy.zeros(ogm.shape) # 10x faster local[numpy.where(ogm < 49)] = 1 # local2 = numpy.zeros(ogm.shape) # start = time.time() # for i in range(0, width): # for j in range(0, height): # if ogm[i][j] < 49: # local[i][j] = 1 # end = time.time() # print ("loop took:", end-start) # start = time.time() # local[numpy.where(ogm<49)] = 1 # end = time.time() # print ("numpy took:", end-start) skeleton = Cffi.thinning(local, ogml) skeleton = Cffi.prune(skeleton, ogml, 10) viz = [] for i in range(0, width): for j in range(0, height): if skeleton[i][j] == 1: viz.append([ i * resolution + origin['x'], j * resolution + origin['y'] ]) RvizHandler.printMarker(\ viz,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_skeletonization_cffi", # Namespace [0.5, 0, 0, 0.5], # Color RGBA 0.05 # Scale ) return skeleton
def checkTarget(self, event): # Check if we have a target or if the robot just wanders if self.inner_target_exists == False or self.move_with_target == False or\ self.next_subtarget == len(self.subtargets): return # Check if timer has expired if self.timerThread.expired == True: Print.art_print('\n~~~~ Time reset ~~~~',Print.RED) self.inner_target_exists = False self.target_exists = False return # Get the robot pose in pixels [rx, ry] = [\ self.robot_perception.robot_pose['x_px'] - \ self.robot_perception.origin['x'] / self.robot_perception.resolution,\ self.robot_perception.robot_pose['y_px'] - \ self.robot_perception.origin['y'] / self.robot_perception.resolution\ ] theta_robot = self.robot_perception.robot_pose['th'] # Clear achieved targets self.subtargets = self.subtargets[self.next_subtarget:] self.next_subtarget = 0 # Find the distance between the robot pose and all remaining subtargets dx = [rx - st[0] for st in self.subtargets] dy = [ry - st[1] for st in self.subtargets] dist = [math.hypot(v[0], v[1]) for v in zip(dx, dy)] # Try to optimize path only when there is more than one target available if len(self.subtargets) > 1: ogm = self.robot_perception.getMap() theta_goals = np.arctan2(-np.array(dy),-np.array(dx)) dtheta = theta_goals - theta_robot dtheta[dtheta > np.pi] -= 2*np.pi dtheta[dtheta < -np.pi] += 2*np.pi # Check if there are obstacles between robot and subtarget obstacles_subtarget = [] for st,i in zip(self.subtargets, range(len(self.subtargets))): # Find line in pixels between robot and goal line_pxls = list(bresenham(int(round(st[0])), int(round(st[1])),\ int(round(rx)), int(round(ry)))) # Find if there are any obstacles in line ogm_line = list(map(lambda pxl: ogm[pxl[0], pxl[1]], line_pxls)) N_occupied = len(list(filter(lambda x: x > 80, ogm_line))) obstacles_subtarget.append(N_occupied) # Check if one of next goals is aligned with the current robot theta # In this case plan immediately for this goal in order to improve motion behaviour for st, i in zip(self.subtargets, range(len(self.subtargets))): if np.fabs(dtheta[i]) < np.pi/10 and obstacles_subtarget[i] == 0: self.next_subtarget = i # Keep resetting timer, while moving towards target (without any obstacles inbetween) self.timerThread.reset() # Check if any target is in distance min_dist, min_idx = min(zip(dist, range(len(dist)))) if min_dist < 5: self.next_subtarget = min_idx + 1 # Reset timer if a goal has been reached self.timerThread.reset() # Check if the final subtarget has been approached if self.next_subtarget >= len(self.subtargets): self.target_exists = False # Publish the current target if self.next_subtarget >= len(self.subtargets): return subtarget = [\ self.subtargets[self.next_subtarget][0]\ * self.robot_perception.resolution + \ self.robot_perception.origin['x'], self.subtargets[self.next_subtarget][1]\ * self.robot_perception.resolution + \ self.robot_perception.origin['y']\ ] RvizHandler.printMarker(\ [subtarget],\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_next_subtarget", # Namespace [0, 0, 0.8, 0.8], # Color RGBA 0.2 # Scale )
def selectTarget(self): # Cancel previous goal timer self.timerThread.stop() # IMPORTANT: The robot must be stopped if you call this function until # it is over # Check if we have a map while self.robot_perception.have_map == False: Print.art_print("Navigation: No map yet", Print.RED) return print "\nClearing all markers" RvizHandler.printMarker(\ [[0, 0]],\ 1, # Type: Arrow 3, # Action: delete all "map", # Frame "null", # Namespace [0,0,0,0], # Color RGBA 0.1 # Scale ) print '\n\n----------------------------------------------------------' print "Navigation: Producing new target" # We are good to continue the exploration # Make this true in order not to call it again from the speeds assignment self.target_exists = True # Gets copies of the map and coverage local_ogm = self.robot_perception.getMap() local_ros_ogm = self.robot_perception.getRosMap() local_coverage = self.robot_perception.getCoverage() print "Got the map and Coverage" self.path_planning.setMap(local_ros_ogm) # Once the target has been found, find the path to it # Get the global robot pose g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_px']]) # Call the target selection function to select the next best goal # Choose target function self.path = [] force_random = False # Potential Target path_altered = False previous_target = None while len(self.path) == 0: start = time.time() target = self.target_selection.selectTarget(\ local_ogm,\ local_coverage,\ self.robot_perception.robot_pose, self.robot_perception.origin, self.robot_perception.resolution, force_random) # ipdb.set_trace() # Check if there was any path alteration if target[0] == True: previous_target = target[2] target = target[1] path_altered = True else: target = target[1] self.path = self.path_planning.createPath(\ g_robot_pose,\ target, self.robot_perception.resolution) print "Navigation: Path for target found with " + str(len(self.path)) +\ " points" if len(self.path) == 0: Print.art_print(\ "Path planning failed. Fallback to random target selection", \ Print.RED) force_random = True # Reverse the path to start from the robot self.path = self.path[::-1] # Break the path to subgoals every 2 pixels (1m = 20px) step = 1 n_subgoals = (int) (len(self.path)/step) self.subtargets = [] for i in range(0, n_subgoals): self.subtargets.append(self.path[i * step]) self.subtargets.append(self.path[-1]) self.next_subtarget = 0 print "The path produced " + str(len(self.subtargets)) + " subgoals" ######################### NOTE: QUESTION ############################## # The path is produced by an A* algorithm. This means that it is # optimal in length but 1) not smooth and 2) length optimality # may not be desired for coverage-based exploration ######################################################################## # Start timer thread self.timerThread.start() # Publish the path for visualization purposes ros_path = Path() ros_path.header.frame_id = "map" for p in self.path: ps = PoseStamped() ps.header.frame_id = "map" ps.pose.position.x = 0 ps.pose.position.y = 0 ######################### NOTE: QUESTION ############################## # Fill the ps.pose.position values to show the path in RViz # You must understand what self.robot_perception.resolution # and self.robot_perception.origin are. # ipdb.set_trace() ps.pose.position.x = p[0] * self.robot_perception.resolution \ + self.robot_perception.origin['x'] ps.pose.position.y = p[1] * self.robot_perception.resolution \ + self.robot_perception.origin['y'] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) RvizHandler.printMarker(\ subtargets_mark,\ 2, # Type: Sphere 0, # Action: Add "map", # Frame "art_subtargets", # Namespace [0, 0.8, 0.0, 0.8], # Color RGBA 0.2 # Scale ) # Print Old and Current Final Target if path_altered == True: subt = [ previous_target[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], previous_target[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] # ipdb.set_trace() RvizHandler.printMarker(\ [subt],\ 3, # Type: Sphere 0, # Action: Add "map", # Frame "art_subtargets", # Namespace [1.0, 0.0, 0.0, 0.8], # Color RGBA 0.2 # Scale ) # ipdb.set_trace() self.inner_target_exists = True
def selectTarget(self, init_ogm, coverage, robot_pose, origin, \ resolution, force_random = False): target = [-1, -1] ######################### NOTE: QUESTION ############################## # Implement a smart way to select the next target. You have the # following tools: ogm_limits, Brushfire field, OGM skeleton, # topological nodes. # Find only the useful boundaries of OGM. Only there calculations # have meaning ogm_limits = OgmOperations.findUsefulBoundaries( init_ogm, origin, resolution) # Blur the OGM to erase discontinuities due to laser rays ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits) # Calculate Brushfire field tinit = time.time() brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE) # 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) # 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) # Visualization of topological nodes vis_nodes = [] for n in nodes: vis_nodes.append([ n[0] * resolution + origin['x'], n[1] * resolution + origin['y'] ]) RvizHandler.printMarker(\ vis_nodes,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_topological_nodes", # Namespace [0.3, 0.4, 0.7, 0.5], # Color RGBA 0.1 # Scale ) # Check at autonomous_expl.yaml if target_selector = 'random' or 'smart' if self.method == 'random' or force_random == True: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) elif self.method == 'smart' and force_random == False: tinit = time.time() # Get the robot pose in pixels [rx, ry] = [\ robot_pose['x_px'] - \ origin['x'] / resolution,\ robot_pose['y_px'] - \ origin['y'] / resolution\ ] g_robot_pose = [rx, ry] # If nodes > 25 the calculation time-cost is very big # In order to avoid time-reset we perform sampling on # the nodes list and take a half-sized sample for i in range(0, len(nodes)): nodes[i].append(i) if (len(nodes) > 25): nodes = random.sample(nodes, int(len(nodes) / 2)) # Initialize weigths w_dist = [0] * len(nodes) w_rot = [robot_pose['th']] * len(nodes) w_cov = [0] * len(nodes) w_topo = [0] * len(nodes) # Calculate weights for every node in the topological graph for i in range(0, len(nodes)): # If path planning fails then give extreme values to weights path = self.path_planning.createPath(g_robot_pose, nodes[i], resolution) if not path: w_dist[i] = sys.maxsize w_rot[i] = sys.maxsize w_cov[i] = sys.maxsize w_topo[i] = sys.maxsize else: for j in range(1, len(path)): # Distance cost w_dist[i] += math.hypot(path[j][0] - path[j - 1][0], path[j][1] - path[j - 1][1]) # Rotational cost w_rot[i] += abs( math.atan2(path[j][0] - path[j - 1][0], path[j][1] - path[j - 1][1])) # Coverage cost w_cov[i] += coverage[int(path[j][0])][int( path[j][1])] / (len(path)) # Add the coverage cost of 0-th node of the path w_cov[i] += coverage[int(path[0][0])][int( path[0][1])] / (len(path)) # Topological cost # This metric depicts wether the target node # is placed in open space or near an obstacle # We want the metric to be reliable so we also check node's neighbour cells w_topo[i] += brush[nodes[i][0]][nodes[i][1]] w_topo[i] += brush[nodes[i][0] - 1][nodes[i][1]] w_topo[i] += brush[nodes[i][0] + 1][nodes[i][1]] w_topo[i] += brush[nodes[i][0]][nodes[i][-1]] w_topo[i] += brush[nodes[i][0]][nodes[i][+1]] w_topo[i] += brush[nodes[i][0] - 1][nodes[i][1] - 1] w_topo[i] += brush[nodes[i][0] + 1][nodes[i][1] + 1] w_topo[i] = w_topo[i] / 7 # Normalize weights between 0-1 for i in range(0, len(nodes)): w_dist[i] = 1 - (w_dist[i] - min(w_dist)) / (max(w_dist) - min(w_dist)) w_rot[i] = 1 - (w_rot[i] - min(w_rot)) / (max(w_rot) - min(w_rot)) w_cov[i] = 1 - (w_cov[i] - min(w_cov)) / (max(w_cov) - min(w_cov)) w_topo[i] = 1 - (w_topo[i] - min(w_topo)) / (max(w_topo) - min(w_topo)) # Set cost values # We set each cost's priority based on experimental results # from "Cost-Based Target Selection Techniques Towards Full Space # Exploration and Coverage for USAR Applications # in a Priori Unknown Environments" publication C1 = w_topo C2 = w_dist C3 = [1] * len(nodes) for i in range(0, len(nodes)): C3[i] -= w_cov[i] C4 = w_rot # Priority Weight C_PW = [0] * len(nodes) # Smoothing Factor C_SF = [0] * len(nodes) # Target's Final Priority C_FP = [0] * len(nodes) for i in range(0, len(nodes)): C_PW[i] = 2**3 * (1 - C1[i]) / .5 + 2**2 * ( 1 - C2[i]) / .5 + 2**1 * (1 - C3[i]) / .5 + 2**0 * ( 1 - C4[i]) / .5 C_SF[i] = (2**3 * (1 - C1[i]) + 2**2 * (1 - C2[i]) + 2**1 * (1 - C3[i]) + 2**0 * (1 - C4[i])) / (2**4 - 1) C_FP[i] = C_PW[i] * C_SF[i] # Select the node with the smallest C_FP value val, idx = min((val, idx) for (idx, val) in enumerate(C_FP)) Print.art_print( "Select smart target time: " + str(time.time() - tinit), Print.BLUE) Print.art_print("Selected Target - Node: " + str(nodes[idx][2]), Print.BLUE) target = nodes[idx] else: Print.art_print( "[ERROR] target_selector at autonomous_expl.yaml must be either 'random' or 'smart'", Print.RED) ######################################################################## return target
def selectTarget(self, init_ogm, coverage, robot_pose, origin, \ resolution, force_random = False ): target = [-1, -1] ######################### NOTE: QUESTION ############################## # Implement a smart way to select the next target. You have the # following tools: ogm_limits, Brushfire field, OGM skeleton, # topological nodes. # Find only the useful boundaries of OGM. Only there calculations # have meaning ogm_limits = OgmOperations.findUsefulBoundaries( init_ogm, origin, resolution) # Blur the OGM to erase discontinuities due to laser rays ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits) # Calculate Brushfire field tinit = time.time() brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE) # 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) # Find topological graph tinit = time.time() nodes = self.topo.topologicalNodes(ogm, skeleton, coverage, origin, \ resolution, brush, ogm_limits) for i in range(0, len(nodes)): print " node " + str(nodes[i]) Print.art_print("Topo nodes time: " + str(time.time() - tinit), Print.ORANGE) # Visualization of topological nodes vis_nodes = [] for n in nodes: vis_nodes.append([ n[0] * resolution + origin['x'], n[1] * resolution + origin['y'] ]) RvizHandler.printMarker(\ vis_nodes,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_topological_nodes", # Namespace [0.3, 0.4, 0.7, 0.5], # Color RGBA 0.1 # Scale ) if force_random == True: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) force_random = False print " force random: CANNOT CREATE PATH TO SELECTED POINT" return target else: # get robot's position #g_robot_pose = [robot_pose['x_px'] - int(origin['x'] / resolution),\ # robot_pose['y_px'] - int(origin['y'] / resolution)] [rx , ry] = [robot_pose['x_px'] - int(origin['x'] / resolution),\ robot_pose['y_px'] - int(origin['y'] / resolution)] # find all the x and y distances between robot and goals dis_x = [rx - target[0] for target in nodes] dis_y = [ry - target[1] for target in nodes] # calculate the euclidean distance dist = [math.hypot(dis[0], dis[1]) for dis in zip(dis_x, dis_y)] # calculate the manhattan distance between the robot and all nodes #dist = [scipy.spatial.distance.cityblock(nodes[i], g_robot_pose) for i in range(0,len(nodes)) ] # target is the closest node min_dist, min_idx = min(zip(dist, range(len(dist)))) goal = nodes[min_idx] target = goal print "TARGET " + str(target) + " TARGET IDX " + str(min_idx) ######################################################################## return target
def selectTarget(self, init_ogm, ros_ogm, coverage, robot_pose, origin, \ resolution, force_random = False): target = [-1, -1] ######################### NOTE: QUESTION ############################## # Implement a smart way to select the next target. You have the # following tools: ogm_limits, Brushfire field, OGM skeleton, # topological nodes. # Find only the useful boundaries of OGM. Only there calculations # have meaning ogm_limits = OgmOperations.findUsefulBoundaries( init_ogm, origin, resolution) # Blur the OGM to erase discontinuities due to laser rays ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits) # Calculate Brushfire field tinit = time.time() brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE) # 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) # 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) # Visualization of topological nodes vis_nodes = [] for n in nodes: vis_nodes.append([ n[0] * resolution + origin['x'], n[1] * resolution + origin['y'] ]) RvizHandler.printMarker(\ vis_nodes,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_topological_nodes", # Namespace [0.3, 0.4, 0.7, 0.5], # Color RGBA 0.1 # Scale ) # Random point if self.method == 'random' or force_random == True: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) # Custom point elif self.method == 'cost_based': self.path_planning.setMap(ros_ogm) g_robot_pose = [robot_pose['x_px'] - int(origin['x'] / resolution),\ robot_pose['y_px'] - int(origin['y'] / resolution)] # Remove all covered nodes from the candidate list nodes = np.array(nodes) uncovered_idxs = np.array( [coverage[n[0], n[1]] == 0 for n in nodes]) goals = nodes[uncovered_idxs] # Initialize costs w_dist = np.full(len(goals), np.inf) w_turn = np.full(len(goals), np.inf) w_topo = np.full(len(goals), np.inf) w_cove = np.full(len(goals), np.inf) for idx, node in zip(range(len(goals)), goals): subgoals = np.array( self.path_planning.createPath(g_robot_pose, node, resolution)) # If path is empty then skip to the next iteration if subgoals.size == 0: continue # subgoals should contain the robot pose, so we don't need to diff it explicitly subgoal_vectors = np.diff(subgoals, axis=0) # Distance cost dists = [math.hypot(v[0], v[1]) for v in subgoal_vectors] w_dist[idx] = np.sum(dists) # Turning cost w_turn[idx] = 0 theta = robot_pose['th'] for v in subgoal_vectors[:-1]: st_x, st_y = v theta2 = math.atan2(st_y - g_robot_pose[1], st_x - g_robot_pose[0]) w_turn[idx] += abs(theta2 - theta) theta = theta2 # Coverage cost w_cove[idx] = sum(coverage[x][y] for x, y in subgoal_vectors) # Topology cost w_topo[idx] = brush[node[0], node[1]] # Normalize weights w_dist = (w_dist - min(w_dist)) / (max(w_dist) - min(w_dist)) w_turn = (w_turn - min(w_turn)) / (max(w_turn) - min(w_turn)) w_cove = (w_cove - min(w_cove)) / (max(w_cove) - min(w_cove)) w_topo = (w_topo - min(w_topo)) / (max(w_topo) - min(w_topo)) # Cost weights c_topo = 4 c_dist = 3 c_cove = 2 c_turn = 1 # Calculate combination cost (final) cost = c_dist * w_dist + c_turn * w_turn + c_cove * w_cove + c_topo * w_topo min_dist, min_idx = min(zip(cost, range(len(cost)))) target = nodes[min_idx] # Check if next target exists and if it exists, check if is close to previous if target is None: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) else: target_dist = math.hypot(target[0] - self.previous_target[0], target[1] - self.previous_target[1]) if target_dist <= 5: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) ######################################################################## self.previous_target = target return target
def selectTarget(self, ogm, coverage, robot_pose, origin, resolution, force_random=False): ######################### NOTE: QUESTION ############################## # Implement a smart way to select the next target. You have the following tools: ogm_limits, Brushfire field, # OGM skeleton, topological nodes. # Find only the useful boundaries of OGM. Only there calculations have meaning. ogm_limits = OgmOperations.findUsefulBoundaries(ogm, origin, resolution) # Blur the OGM to erase discontinuities due to laser rays ogm = OgmOperations.blurUnoccupiedOgm(ogm, ogm_limits) # Calculate Brushfire field tinit = time.time() brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE) # Calculate skeletonization tinit = time.time() skeleton = topology.skeletonizationCffi(ogm, origin, resolution, ogm_limits) Print.art_print("Skeletonization time: " + str(time.time() - tinit), Print.ORANGE) # Find topological graph tinit = time.time() nodes = topology.topologicalNodes(ogm, skeleton, coverage, brush) Print.art_print("Topo nodes time: " + str(time.time() - tinit), Print.ORANGE) # Visualization of topological nodes vis_nodes = [[n[0] * resolution + origin['x'], n[1] * resolution + origin['y']] for n in nodes] RvizHandler.printMarker( vis_nodes, 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_topological_nodes", # Namespace [0.3, 0.4, 0.7, 0.5], # Color RGBA 0.1 # Scale ) try: tinit = time.time() except: tinit = None if self.method == 'random' or force_random: target = self.selectRandomTarget(ogm, coverage, brush) elif self.method_is_cost_based(): g_robot_pose = self.robot_perception.getGlobalCoordinates([robot_pose['x_px'], robot_pose['y_px']]) self.path_planning.setMap(self.robot_perception.getRosMap()) class MapContainer: def __init__(self): self.skeleton = skeleton self.coverage = coverage self.ogm = ogm self.brush = brush self.nodes = [node for node in nodes if TargetSelection.is_good(brush, ogm, coverage, node)] self.robot_px = [robot_pose['x_px'] - origin['x'] / resolution, robot_pose['y_px'] - origin['y'] / resolution] self.theta = robot_pose['th'] @staticmethod def create_path(path_target): return self.path_planning.createPath(g_robot_pose, path_target, resolution) target = self.select_by_cost(MapContainer()) else: assert False, "Invalid target_selector method." if tinit is not None: Print.art_print("Target method {} time: {}".format(self.method, time.time() - tinit), Print.ORANGE) return target
def checkTarget(self, event): # Check if we have a target or if the robot just wanders if self.inner_target_exists == False or self.move_with_target == False or\ self.next_subtarget == len(self.subtargets): return self.counter_to_next_sub -= 1 if self.counter_to_next_sub == 0: Print.art_print('\n~~~~ Time reset ~~~~', Print.RED) self.inner_target_exists = False self.target_exists = False return # Get the robot pose in pixels [rx, ry] = [\ self.robot_perception.robot_pose['x_px'] - \ self.robot_perception.origin['x'] / self.robot_perception.resolution,\ self.robot_perception.robot_pose['y_px'] - \ self.robot_perception.origin['y'] / self.robot_perception.resolution\ ] # Find the distance between the robot pose and the next subtarget dist = math.hypot(\ rx - self.subtargets[self.next_subtarget][0], \ ry - self.subtargets[self.next_subtarget][1]) ######################### NOTE: QUESTION ############################## # What if a later subtarget or the end has been reached before the # next subtarget? Alter the code accordingly. # If the robot is in the starting point it immediately sets the next subtarget if self.next_subtarget == 0: self.next_subtarget += 1 self.counter_to_next_sub = self.count_limit else: # Check if there is a later subtarget, closer than the next one # If one is found, make it the next subtarget and update the time for i in range(self.next_subtarget + 1, len(self.subtargets) - 1): # Find the distance between the robot pose and the later subtarget dist_from_later = math.hypot(\ rx - self.subtargets[i][0], \ ry - self.subtargets[i][1]) if dist_from_later < 15: self.next_subtarget = i self.counter_to_next_sub = self.count_limit + 100 dist = dist_from_later break # If distance from subtarget is less than 5, go to the next one if dist < 5: self.next_subtarget += 1 self.counter_to_next_sub = self.count_limit # Check if the final subtarget has been approached if self.next_subtarget == len(self.subtargets): self.target_exists = False ######################################################################## # Publish the current target if self.next_subtarget == len(self.subtargets): return subtarget = [\ self.subtargets[self.next_subtarget][0]\ * self.robot_perception.resolution + \ self.robot_perception.origin['x'], self.subtargets[self.next_subtarget][1]\ * self.robot_perception.resolution + \ self.robot_perception.origin['y']\ ] RvizHandler.printMarker(\ [subtarget],\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_next_subtarget", # Namespace [0, 0, 0.8, 0.8], # Color RGBA 0.2 # Scale )
def selectTarget(self, init_ogm, coverage, robot_pose, origin, \ resolution, force_random = False): target = [-1, -1] ######################### NOTE: QUESTION ############################## # Implement a smart way to select the next target. You have the # following tools: ogm_limits, Brushfire field, OGM skeleton, # topological nodes. # Find only the useful boundaries of OGM. Only there calculations # have meaning ogm_limits = OgmOperations.findUsefulBoundaries(init_ogm, origin, resolution) # Blur the OGM to erase discontinuities due to laser rays ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits) # Calculate Brushfire field tinit = time.time() brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE) # 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) # 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) # Visualization of topological nodes vis_nodes = [] for n in nodes: vis_nodes.append([ n[0] * resolution + origin['x'], n[1] * resolution + origin['y'] ]) RvizHandler.printMarker(\ vis_nodes,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_topological_nodes", # Namespace [0.3, 0.4, 0.7, 0.5], # Color RGBA 0.1 # Scale ) # Random point if self.method == 'random' or force_random == True: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) ######################################################################## # Challenge 6. Smart point selection demands autonomous_expl.yaml->target_selector: 'smart' # Smart point selection if self.method == 'smart' and force_random == False: nextTarget = self.selectSmartTarget(coverage, brush, robot_pose, resolution, origin, nodes) # Check if selectSmartTarget found a target if nextTarget is not None: # Check if the next target is the same as the previous dist = math.hypot( nextTarget[0] - self.previous_target[0], nextTarget[1] - self.previous_target[1]) if dist > 5: target = nextTarget else: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) else: # No target found. Choose a random target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) self.previous_target = target return target
def checkTarget(self, event): """ Check if target/sub-target reached. :param event: :return: """ if self.previous_next_subtarget is None or self.previous_next_subtarget < self.next_subtarget: Print.art_print("next_subtarget = %d" % self.next_subtarget, Print.ORANGE) self.previous_next_subtarget = self.next_subtarget # Check if we have a target or if the robot just wanders if self.inner_target_exists == False \ or self.move_with_target == False \ or self.next_subtarget == len(self.subtargets): return self.counter_to_next_sub -= 1 if self.counter_to_next_sub == 0: Print.art_print('\n~~~~ Time reset ~~~~', Print.RED) self.inner_target_exists = False self.target_exists = False return # Get the robot pose in: pixel units & global (map's) coordinate system rp_l_px = [ self.robot_perception.robot_pose['x_px'], self.robot_perception.robot_pose['y_px'] ] # rp_g_px = self.robot_perception.toGlobalCoordinates(rp_l_px) [rx, ry] = [ self.robot_perception.robot_pose['x_px'] - self.robot_perception.origin['x'] / self.robot_perception.resolution, self.robot_perception.robot_pose['y_px'] - self.robot_perception.origin['y'] / self.robot_perception.resolution ] rp_g_px = [rx, ry] # Find the distance between the robot pose and the next subtarget v_g_px = map(operator.sub, rp_g_px, self.subtargets[self.next_subtarget]) dist = math.hypot(v_g_px[0], v_g_px[1]) ######################### NOTE: QUESTION ############################## # What if a later subtarget or the end has been reached before the # next subtarget? Alter the code accordingly. # Check if distance is less than 7 px (14 cm) # if dist < 5: # self.next_subtarget += 1 # self.counter_to_next_sub = self.count_limit # # Check if the final subtarget has been approached # if self.next_subtarget == len(self.subtargets): # self.target_exists = False # Instead of checking only next sub-target check all remaining sub-targets (starting from end up to next # sub-target) and decide if robot is closer to another sub-target at later point in path for st_i in range( len(self.subtargets) - 1, self.next_subtarget - 1, -1): # @v_st_g_px: Difference of Global coordinates in Pixels between robot's position # and i-th" sub-target's position v_st_g_px = map(operator.sub, rp_g_px, self.subtargets[st_i]) if math.hypot(v_st_g_px[0], v_st_g_px[1]) < 5: # reached @st_i, set next sub-target in path as robot's next sub-target self.next_subtarget = st_i + 1 self.counter_to_next_sub = self.count_limit if self.next_subtarget == len(self.subtargets): self.target_exists = False ######################################################################## # Publish the current target if self.next_subtarget == len(self.subtargets): return subtarget = [ self.subtargets[self.next_subtarget][0] * self.robot_perception.resolution + self.robot_perception.origin['x'], self.subtargets[self.next_subtarget][1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ] RvizHandler.printMarker( [subtarget], 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_next_subtarget", # Namespace [0, 0, 0.8, 0.8], # Color RGBA 0.2 # Scale )
def selectTarget(self): # IMPORTANT: The robot must be stopped if you call this function until # it is over # Check if we have a map while not self.robot_perception.have_map: Print.art_print("Navigation: No map yet", Print.RED) return print "\nClearing all markers" RvizHandler.printMarker( [[0, 0]], 1, # Type: Arrow 3, # Action: delete all "map", # Frame "null", # Namespace [0, 0, 0, 0], # Color RGBA 0.1 # Scale ) print '\n\n----------------------------------------------------------' print "Navigation: Producing new target" # We are good to continue the exploration # Make this true in order not to call it again from the speeds assignment self.target_exists = True # Gets copies of the map and coverage local_ogm = self.robot_perception.getMap() local_ros_ogm = self.robot_perception.getRosMap() local_coverage = self.robot_perception.getCoverage() print "Got the map and Coverage" self.path_planning.setMap(local_ros_ogm) # Once the target has been found, find the path to it # Get the global robot pose # Get robot pose in global (map's) coordinates rp_l_px = [ self.robot_perception.robot_pose['x_px'], self.robot_perception.robot_pose['y_px'] ] rp_g_px = self.robot_perception.toGlobalCoordinates( rp_l_px, with_resolution=True) g_robot_pose = rp_g_px # Call the target selection function to select the next best goal # Choose target function self.path = [] force_random = False while len(self.path) == 0: start = time.time() target = self.target_selection.selectTarget( local_ogm, local_coverage, self.robot_perception.robot_pose, self.robot_perception.origin, self.robot_perception.resolution, force_random) self.path = self.path_planning.createPath( g_robot_pose, target, self.robot_perception.resolution) print "Navigation: Path for target found with " + str(len(self.path)) + \ " points" if len(self.path) == 0: Print.art_print( "Path planning failed. Fallback to random target selection", Print.RED) force_random = True # Reverse the path to start from the robot self.path = self.path[::-1] # Break the path to sub-goals every 2 pixels (1m = 20px) step = 1 n_subgoals = int(len(self.path) / step) self.subtargets = [] for i in range(0, n_subgoals): self.subtargets.append(self.path[i * step]) self.subtargets.append(self.path[-1]) self.next_subtarget = 0 print "The path produced " + str(len(self.subtargets)) + " subgoals" ######################### NOTE: QUESTION ############################## # The path is produced by an A* algorithm. This means that it is # optimal in length but 1) not smooth and 2) length optimality # may not be desired for coverage-based exploration ######################################################################## self.counter_to_next_sub = self.count_limit # Publish the path for visualization purposes ros_path = Path() ros_path.header.frame_id = "map" for p in self.path: ps = PoseStamped() ps.header.frame_id = "map" ######################### NOTE: QUESTION ############################## # Fill the ps.pose.position values to show the path in RViz # You must understand what self.robot_perception.resolution # and self.robot_perception.origin are. # Convert p from pixel-units to meter p_m = self.robot_perception.toMeterUnits(p, False) # Convert p_m from global (map's) coordinates to relative (image's) coordinates (using resolution = 1) p_m_r = self.robot_perception.toRelativeCoordinates(p_m, False) # Fill in $ps object ps.pose.position.x = p_m_r[0] ps.pose.position.y = p_m_r[1] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: st_m = self.robot_perception.toMeterUnits(s, False) st_m_r = self.robot_perception.toRelativeCoordinates(st_m, False) subtargets_mark.append(st_m_r) RvizHandler.printMarker( subtargets_mark, 2, # Type: Sphere 0, # Action: Add "map", # Frame "art_subtargets", # Namespace [0, 0.8, 0.0, 0.8], # Color RGBA 0.2 # Scale ) self.inner_target_exists = True
def selectTarget(self, ogm, coverage, robot_pose, origin, \ resolution, force_random = False): # Random point if self.method == 'random' or force_random == True: init_ogm = ogm # Find only the useful boundaries of OGM. Only there calculations # have meaning ogm_limits = OgmOperations.findUsefulBoundaries( init_ogm, origin, resolution) # Blur the OGM to erase discontinuities due to laser rays ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits) # Calculate Brushfire field tinit = time.time() brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE) # 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) # 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) # Visualization of topological nodes vis_nodes = [] for n in nodes: vis_nodes.append([ n[0] * resolution + origin['x'], n[1] * resolution + origin['y'] ]) RvizHandler.printMarker(\ vis_nodes,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_topological_nodes", # Namespace [0.3, 0.4, 0.7, 0.5], # Color RGBA 0.1 # Scale ) target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) return [False, target] tinit = time.time() g_robot_pose = [robot_pose['x_px'] - int(origin['x'] / resolution), \ robot_pose['y_px'] - int(origin['y'] / resolution)] # Calculate coverage frontier with sobel filters tinit = time.time() cov_dx = scipy.ndimage.sobel(coverage, 0) cov_dy = scipy.ndimage.sobel(coverage, 1) cov_frontier = np.hypot(cov_dx, cov_dy) cov_frontier *= 100 / np.max(cov_frontier) cov_frontier = 100 * (cov_frontier > 80) Print.art_print("Sobel filters time: " + str(time.time() - tinit), Print.BLUE) # Remove the edges that correspond to obstacles instead of frontiers (in a 5x5 radius) kern = 5 tinit = time.time() i_rng = np.matlib.repmat( np.arange(-(kern / 2), kern / 2 + 1).reshape(kern, 1), 1, kern) j_rng = np.matlib.repmat(np.arange(-(kern / 2), kern / 2 + 1), kern, 1) for i in range((kern / 2), cov_frontier.shape[0] - (kern / 2)): for j in range((kern / 2), cov_frontier.shape[1] - (kern / 2)): if cov_frontier[i, j] == 100: if np.any(ogm[i + i_rng, j + j_rng] > 99): cov_frontier[i, j] = 0 Print.art_print("Frontier trimming time: " + str(time.time() - tinit), Print.BLUE) # Save coverage frontier as image (for debugging purposes) # scipy.misc.imsave('test.png', np.rot90(cov_frontier)) # Frontier detection/grouping tinit = time.time() labeled_frontiers, num_frontiers = scipy.ndimage.label( cov_frontier, np.ones((3, 3))) Print.art_print("Frontier grouping time: " + str(time.time() - tinit), Print.BLUE) goals = np.full((num_frontiers, 2), -1) w_dist = np.full(len(goals), -1) w_turn = np.full(len(goals), -1) w_size = np.full(len(goals), -1) w_obst = np.full(len(goals), -1) # Calculate the centroid and its cost, for each frontier for i in range(1, num_frontiers + 1): points = np.where(labeled_frontiers == i) # Discard small groupings (we chose 20 as a treshold arbitrarily) group_length = points[0].size if group_length < 20: labeled_frontiers[points] = 0 continue sum_x = np.sum(points[0]) sum_y = np.sum(points[1]) centroid = np.array([sum_x / group_length, sum_y / group_length]).reshape(2, 1) # Find the point on the frontier nearest (2-norm) to the centroid, and use it as goal nearest_idx = np.linalg.norm(np.array(points) - centroid, axis=0).argmin() print ogm[int(points[0][nearest_idx]), int(points[1][nearest_idx])] goals[i - 1, :] = np.array( [points[0][nearest_idx], points[1][nearest_idx]]) # Save centroids for later visualisation (for debugging purposes) labeled_frontiers[int(goals[i - 1, 0]) + i_rng, int(goals[i - 1, 1]) + j_rng] = i # Calculate size of obstacles between robot and goal line_pxls = list(bresenham(int(goals[i-1,0]), int(goals[i-1,1]),\ g_robot_pose[0], g_robot_pose[1])) ogm_line = list(map(lambda pxl: ogm[pxl[0], pxl[1]], line_pxls)) N_occupied = len(list(filter(lambda x: x > 25, ogm_line))) N_line = len(line_pxls) w_obst[i - 1] = float(N_occupied) / N_line # print('Occupied = '+str(N_occupied)) # print('Full Line = '+str(N_line)) # ipdb.set_trace() # Manhattan distance w_dist[i - 1] = scipy.spatial.distance.cityblock( goals[i - 1, :], g_robot_pose) # Missalignment theta = np.arctan2(goals[i - 1, 1] - g_robot_pose[1], goals[i - 1, 0] - g_robot_pose[0]) w_turn[i - 1] = (theta - robot_pose['th']) if w_turn[i - 1] > np.pi: w_turn[i - 1] -= 2 * np.pi elif w_turn[i - 1] < -np.pi: w_turn[i - 1] += 2 * np.pi # We don't care about the missalignment direction so we abs() it w_turn[i - 1] = np.abs(w_turn[i - 1]) # Frontier size w_size[i - 1] = group_length # Save frontier groupings as an image (for debugging purposes) cmap = plt.cm.jet norm = plt.Normalize(vmin=labeled_frontiers.min(), vmax=labeled_frontiers.max()) image = cmap(norm(labeled_frontiers)) plt.imsave('frontiers.png', np.rot90(image)) # Remove invalid goals and weights valids = w_dist != -1 goals = goals[valids] w_dist = w_dist[valids] w_turn = w_turn[valids] w_size = w_size[valids] w_obst = w_obst[valids] # Normalize weights w_dist = (w_dist - min(w_dist)) / (max(w_dist) - min(w_dist)) w_turn = (w_turn - min(w_turn)) / (max(w_turn) - min(w_turn)) w_size = (w_size - min(w_size)) / (max(w_size) - min(w_size)) # Cancel turn weight for frontiers that have obstacles w_turn[np.where(w_obst != 0)] = 1 # Goal cost function c_dist = 3 c_turn = 2 c_size = 1 c_obst = 4 costs = c_dist * w_dist + c_turn * w_turn + c_size * w_size + c_obst * w_obst min_idx = costs.argmin() Print.art_print("Target selection time: " + str(time.time() - tinit), Print.ORANGE) print costs print goals ## Safety Distance from obstacles # Goal Coordinates grid_size = 20 [found_obstacle, closest_obstacle, min_dist] = self.detectObstacles(grid_size, ogm, goals[min_idx]) if found_obstacle == False: return [False, goals[min_idx]] # Calculate new goal: dist_from_obstacles = 10 normal_vector = goals[min_idx] - closest_obstacle normal_vector = normal_vector / np.hypot(normal_vector[0], normal_vector[1]) new_goal = closest_obstacle + dist_from_obstacles * normal_vector # Check new goal for nearby obstacles [found_obstacle, closest_obstacle, min_dist_new] = \ self.detectObstacles(grid_size, ogm, new_goal.round()) # Return if min_dist < 7: # return the target with max min_dist if min_dist_new > min_dist: return [True, new_goal.round(), goals[min_idx]] else: return [False, goals[min_idx]] else: return [False, goals[min_idx]]
def selectTarget(self): # IMPORTANT: The robot must be stopped if you call this function until # it is over # Cancel previous goal timer self.timerThread.stop() # Check if we have a map while not self.robot_perception.have_map: Print.art_print("Navigation: No map yet", Print.RED) return print "Clearing all markers !\n\n" RvizHandler.printMarker([[0, 0]], 1, 3, "map", "null", [0, 0, 0, 0], 0.1) # Visualize Map Container map_container_mark = [] for s in self.map_size: map_container_mark.append([ s[0] * self.robot_perception.resolution + self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ]) RvizHandler.printMarker(map_container_mark, 1, 0, "map", "art_map_container", [1.0, 0.0, 0.0, 1.0], 0.3) print '----------------------------------------------------------' print 'Navigation: Producing new target' # We are good to continue the exploration # Make this true in order not to call it again from the speeds assignment self.target_exists = True ######################################################################## # Get OGM Map and Coverage start_ = time.time() # Gets copies of the map and coverage start = time.time() local_ogm = self.robot_perception.getMap() if self.debug: print str('Robot Perception: Got the map in ') + str( time.time() - start) + str(' seconds.') start = time.time() local_ros_ogm = self.robot_perception.getRosMap() if self.debug: print str('Robot Perception: Got the ros map in ') + str( time.time() - start) + str(' seconds.') start = time.time() local_coverage = self.robot_perception.getCoverage() if self.debug: print str('Robot Perception: Got the coverage in ') + str( time.time() - start) + str(' seconds.') print str('Navigation: Got the map and Coverage in ') + str( time.time() - start_) + str(' seconds.') ######################################################################## # Part 1 - OGM Enhancement no_points = False ogm_limits_before = [] if self.turtlebot_save_progress_images: ####################### Save OGM and Coverage ########################## scipy.misc.imsave( '/home/vvv/pictures_slam/ogm.png', cv2.bitwise_not( exposure.rescale_intensity(np.array(local_ogm, dtype=np.uint8), in_range=(0, 100), out_range=(0, 255)))) scipy.misc.imsave('/home/vvv/pictures_slam/coverage.png', local_coverage) start_ = time.time() print str(self.robot_perception.percentage_explored) if self.limit_exploration_flag and self.robot_perception.percentage_explored < 1.0 and not self.first_run_flag: ############################################################################# # Subpart 1 - Find the Useful boundaries of OGM ############################################################################# start = time.time() (points, ogm_limits_before) = self.ogm_limit_calculation( local_ogm, self.robot_perception.resolution, self.robot_perception.origin, 20, 20, 12, self.map_size) if points is None or len(points) == 0: print str('No Limits Points Calculated ') no_points = True else: print str('Number of Limit Points Calculated is ') + str(len(points)) + str(' in ')\ + str(time.time() - start) + str(' seconds.') no_points = False if self.debug: print str('\n') + str(points) + str('\n') ############################################################################# ######################################################################### # Subpart 2 - Send Drone to Limits and Match the to OGM Map ######################################################################### startt = time.time() if not no_points: index = 0 p = points[index, :] while len(points): points = np.delete( points, [index], axis=0) # Delete Point thats Going to be Explored # Send Drone to OGM Limits start = time.time() while not self.sent_drone_to_limit(p[0], p[1]): pass if self.debug: Print.art_print( str('Navigation: Get Image From Drone took ') + str(time.time() - start) + str(' seconds.'), Print.ORANGE) # Image Matching start = time.time() if self.match_with_limit_pose: if self.match_with_drone_pose: [x_p, y_p] = [ int((self.drone_map_pose[0] - self.robot_perception.origin['x']) / self.robot_perception.resolution), int((self.drone_map_pose[1] - self.robot_perception.origin['y']) / self.robot_perception.resolution) ] local_ogm = ImageMatching.ogm_match_with_known_image_pose( ogm=local_ogm, new_data=self.drone_image, coordinates=[x_p, y_p, self.drone_yaw], map_boundries=[ self.map_size[0][0], self.map_size[3][0], self.map_size[0][1], self.map_size[3][1] ], debug=self.debug) else: local_ogm = ImageMatching.ogm_match_with_known_image_pose( ogm=local_ogm, new_data=self.drone_image, coordinates=[ int(p[2]), int(p[3]), self.drone_yaw ], map_boundries=[ int(self.map_size[0][0]), int(self.map_size[3][0]), int(self.map_size[0][1]), int(self.map_size[3][1]) ], debug=self.debug) else: local_ogm = ImageMatching.template_matching( ogm=local_ogm, drone_image=self.drone_image, lim_x=int(p[2]), lim_y=int(p[3]), drone_yaw=self.drone_yaw, window=200, s_threshold=0.8, debug=self.debug) if self.debug: Print.art_print( str('Navigation: OGM Matching Function took ') + str(time.time() - start) + str(' seconds.\n'), Print.ORANGE) # Calculate Point for Next Loop! if len(points) > 0: index = self.closest_limit_point(p[:2], points[:, :2]) p = points[index, :] ''' print('\x1b[35;1m' + str('Navigation: Taking Image and Matching took ') + str(time.time() - startt) + str(' seconds.') + '\x1b[0m') ''' ######################################################################## ######################################################################## # Subpart 3 - -Copy Enhanced Data to ROS OGM ######################################################################## if not no_points: start = time.time() local_ros_ogm.data = cp_data_to_ros_ogm( np.array(local_ros_ogm.data), local_ogm, local_ros_ogm.info.width, local_ros_ogm.info.height) if self.debug: print('\x1b[35;1m' + str('Navigation: Copying OGM Data took ') + str(time.time() - start) + str(' seconds.') + '\x1b[0m') ######################################################################## print('\x1b[38;1m' + str('Navigation: OGM Enhancement took ') + str(time.time() - start_) + str(' seconds.') + '\x1b[0m') if self.turtlebot_save_progress_images and self.limit_exploration_flag and not no_points: ########################## Save Enhance OGM ############################ scipy.misc.imsave( '/home/vvv/pictures_slam/ogm_enhanced.png', cv2.bitwise_not( exposure.rescale_intensity(np.array(local_ogm, dtype=np.uint8), in_range=(0, 100), out_range=(0, 255)))) ######################################################################## # Part 2 - Set Map, Get Robot Position and Choose Target start = time.time() self.target_selection.path_planning.setMap(local_ros_ogm) print str('Navigation: Set Map in ') + str(time.time() - start) + str(' seconds.') # Once the target has been found, find the path to it # Get the global robot pose start = time.time() g_robot_pose = self.robot_perception.getGlobalCoordinates([ self.robot_perception.robot_pose['x_px'], self.robot_perception.robot_pose['y_px'] ]) print str('Navigation: Got Robot Pose in ') + str( time.time() - start) + str(' seconds.') # Call the target selection function to select the next best goal self.path = [] force_random = False while len(self.path) == 0: # Get Target and Path to It! start = time.time() self.path = self.target_selection.selectTarget( local_ogm, local_coverage, self.robot_perception.robot_pose, self.robot_perception.origin, self.robot_perception.resolution, g_robot_pose, ogm_limits_before, force_random) #self.target_selection.path_planning.createPath(g_robot_pose, target, self.robot_perception.resolution) if len(self.path) == 0: Print.art_print( 'Navigation: Path planning failed. Fallback to random target selection', Print.RED) print str('\n') force_random = True else: print str('Navigation: Target Selected and Path to Target found with ') + str(len(self.path)) \ + str(' points in ') + str(time.time() - start) + str(' seconds.') ######################################################################## # Part 3 - Create Subgoals and Print Path to RViz start = time.time() # Reverse the path to start from the robot self.path = self.path[::-1] # Break the path to subgoals every 2 pixels (1m = 20px) step = 1 n_subgoals = int(len(self.path) / step) self.subtargets = [] for i in range(0, n_subgoals): self.subtargets.append(self.path[i * step]) self.subtargets.append(self.path[-1]) self.next_subtarget = 0 if self.debug: print str('Navigation: The path produced ') + str(len(self.subtargets)) + str(' subgoals in ') \ + str(time.time() - start) + str('seconds.') # Start timer thread self.timerThread.start() # Publish the path for visualization purposes ros_path = Path() ros_path.header.frame_id = self.map_frame for p in self.path: ps = PoseStamped() ps.header.frame_id = self.map_frame ps.pose.position.x = 0 ps.pose.position.y = 0 ps.pose.position.x = p[ 0] * self.robot_perception.resolution + self.robot_perception.origin[ 'x'] ps.pose.position.y = p[ 1] * self.robot_perception.resolution + self.robot_perception.origin[ 'y'] ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subtargets_mark.append([ s[0] * self.robot_perception.resolution + self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ]) RvizHandler.printMarker(subtargets_mark, 2, 0, 'map', 'art_subtargets', [0, 0.8, 0.0, 0.8], 0.2) # Update NUmber of Targets self.robot_perception.num_of_explored_targets += 1 self.inner_target_exists = True # Reset First Run Flag if self.first_run_flag: self.first_run_flag = False
def checkTarget(self, event): # Check if we have a target or if the robot just wanders if self.inner_target_exists == False or self.move_with_target == False or\ self.next_subtarget == len(self.subtargets): return self.counter_to_next_sub -= 1 if self.counter_to_next_sub == 0: Print.art_print('\n~~~~ Time reset ~~~~',Print.RED) self.inner_target_exists = False self.target_exists = False if self.Reset == False: self.TimeOut = True else: self.Reset = False return # Get the robot pose in pixels [rx, ry] = [\ self.robot_perception.robot_pose['x_px'] - \ self.robot_perception.origin['x'] / self.robot_perception.resolution,\ self.robot_perception.robot_pose['y_px'] - \ self.robot_perception.origin['y'] / self.robot_perception.resolution\ ] # remove visited sub targets self.subtargets = self.subtargets[self.next_subtarget:] # set the next sub target as the first one that should be visited self.next_subtarget = 0 dis_x = [rx - target[0] for target in self.subtargets] dis_y = [ry - target[1] for target in self.subtargets] #for i in range(0,len(dis_x)): # print " x dis " + str(dis_x[i]) # print " y dis " + str(dis_y[i]) # target distances dist_t = [math.hypot(dis[0],dis[1]) for dis in zip(dis_x,dis_y) ] # find the closest target min_target = 0 min_dis = dist_t[0] for i in range(0,len(dist_t)): if min_dis > dist_t[i]: min_dis = dist_t[i] min_target = i ######################### NOTE: QUESTION ############################## # What if a later subtarget or the end has been reached before the # next subtarget? Alter the code accordingly. # print " min target " + str(min_target) + " min dis " + str(min_dis) # Check if distance is less than 7 px (14 cm) if min_dis < 5: self.next_subtarget = min_target + 1 self.counter_to_next_sub = self.count_limit # print "REACHED" # print " next target " + str(self.next_subtarget) + " len " +str(len(self.subtargets)) # Check if the final subtarget has been approached if self.next_subtarget == len(self.subtargets): self.target_exists = False ######################################################################## # Publish the current target if self.next_subtarget == len(self.subtargets): return subtarget = [\ self.subtargets[self.next_subtarget][0]\ * self.robot_perception.resolution + \ self.robot_perception.origin['x'], self.subtargets[self.next_subtarget][1]\ * self.robot_perception.resolution + \ self.robot_perception.origin['y']\ ] RvizHandler.printMarker(\ [subtarget],\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_next_subtarget", # Namespace [0, 0, 0.8, 0.8], # Color RGBA 0.2 # Scale )
def selectTarget(self): # IMPORTANT: The robot must be stopped if you call this function until # it is over # Check if we have a map while self.robot_perception.have_map == False: Print.art_print("Navigation: No map yet", Print.RED) return print "\nClearing all markers" RvizHandler.printMarker(\ [[0, 0]],\ 1, # Type: Arrow 3, # Action: delete all "map", # Frame "null", # Namespace [0,0,0,0], # Color RGBA 0.1 # Scale ) print '\n\n----------------------------------------------------------' print "Navigation: Producing new target" # We are good to continue the exploration # Make this true in order not to call it again from the speeds assignment self.target_exists = True # Gets copies of the map and coverage local_ogm = self.robot_perception.getMap() local_ros_ogm = self.robot_perception.getRosMap() local_coverage = self.robot_perception.getCoverage() print "Got the map and Coverage" self.path_planning.setMap(local_ros_ogm) # Once the target has been found, find the path to it # Get the global robot pose g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_px']]) # Call the target selection function to select the next best goal # Choose target function print "TimeOut Flag " +str(self.TimeOut) self.path = [] force_random = False while len(self.path) == 0: start = time.time() target = self.target_selection.selectTarget(\ local_ogm,\ local_coverage,\ self.robot_perception.robot_pose, self.robot_perception.origin, self.robot_perception.resolution, force_random) self.path = self.path_planning.createPath(\ g_robot_pose,\ target, self.robot_perception.resolution) print "Navigation: Path for target found with " + str(len(self.path)) +\ " points" if len(self.path) == 0: Print.art_print(\ "Path planning failed. Fallback to random target selection", \ Print.RED) force_random = True # Reverse the path to start from the robot self.path = self.path[::-1] tolerance = 0.000001 weight_data = 0.5 weight_smooth = 0.1 new = deepcopy(self.path) dims = 2 change = tolerance # Path smoothing using gradient descent if len(self.path) > 3: print "PATH SMOOTHING" while change >= tolerance: change = 0.0 for i in range(1, len(new) - 1): for j in range(dims): x_i = self.path[i][j] y_i, y_prev, y_next = new[i][j], new[i - 1][j], new[i + 1][j] y_i_saved = y_i y_i += weight_data * (x_i - y_i) + weight_smooth * (y_next + y_prev - (2 * y_i)) new[i][j] = y_i change += abs(y_i - y_i_saved) self.path = new # Break the path to subgoals every 2 pixels (1m = 20px) step = 1 n_subgoals = (int) (len(self.path)/step) self.subtargets = [] for i in range(0, n_subgoals): self.subtargets.append(self.path[i * step]) self.subtargets.append(self.path[-1]) self.next_subtarget = 0 print "The path produced " + str(len(self.subtargets)) + " subgoals" ######################### NOTE: QUESTION ############################## # The path is produced by an A* algorithm. This means that it is # optimal in length but 1) not smooth and 2) length optimality # may not be desired for coverage-based exploration ######################################################################## self.counter_to_next_sub = self.count_limit # Publish the path for visualization purposes ros_path = Path() ros_path.header.frame_id = "map" for p in self.path: ps = PoseStamped() ps.header.frame_id = "map" ######################### NOTE: QUESTION ############################## # Fill the ps.pose.position values to show the path in RViz # You must understand what self.robot_perception.resolution # and self.robot_perception.origin are. #p[0] * resolution => gives the path's point x-coordinate expressed in the map's coordinate system (meters) # As the path's(p) first point-set is the pixels that are occupied by the robot, multyplying it by resolution (meter/pixel) # gives the values of that point expressed in meters. # Adding the robot's initial position we have a path starting from the robot's position expressed in the robot's frame. ps.pose.position.x = p[0]*0.05 + self.robot_perception.origin['x'] ps.pose.position.y = p[1]*0.05 + self.robot_perception.origin['y'] # print " x val " + str(ps.pose.position.x) # print " y val " + str(ps.pose.position.y ) # print " origin x " + str(self.robot_perception.origin['x']) # print " origin y " + str(self.robot_perception.origin['y']) # print " p0 " +str(p[0]) # print " p1 " +str(p[1]) ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) RvizHandler.printMarker(\ subtargets_mark,\ 2, # Type: Sphere 0, # Action: Add "map", # Frame "art_subtargets", # Namespace [0, 0.8, 0.0, 0.8], # Color RGBA 0.2 # Scale ) self.inner_target_exists = True
def selectTarget(self): # IMPORTANT: The robot must be stopped if you call this function until # it is over # Check if we have a map while self.robot_perception.have_map == False: Print.art_print("Navigation: No map yet", Print.RED) return print "\nClearing all markers" RvizHandler.printMarker(\ [[0, 0]],\ 1, # Type: Arrow 3, # Action: delete all "map", # Frame "null", # Namespace [0,0,0,0], # Color RGBA 0.1 # Scale ) print '\n\n----------------------------------------------------------' print "Navigation: Producing new target" # We are good to continue the exploration # Make this true in order not to call it again from the speeds assignment self.target_exists = True # Gets copies of the map and coverage local_ogm = self.robot_perception.getMap() local_ros_ogm = self.robot_perception.getRosMap() local_coverage = self.robot_perception.getCoverage() print "Got the map and Coverage" self.path_planning.setMap(local_ros_ogm) # Once the target has been found, find the path to it # Get the global robot pose g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_px']]) # Call the target selection function to select the next best goal # Choose target function self.path = [] force_random = False while len(self.path) == 0: start = time.time() target = self.target_selection.selectTarget(\ local_ogm,\ local_coverage,\ self.robot_perception.robot_pose, self.robot_perception.origin, self.robot_perception.resolution, force_random) self.path = self.path_planning.createPath(\ g_robot_pose,\ target, self.robot_perception.resolution) print "Navigation: Path for target found with " + str(len(self.path)) +\ " points" if len(self.path) == 0: Print.art_print(\ "Path planning failed. Fallback to random target selection", \ Print.RED) force_random = True # Reverse the path to start from the robot self.path = self.path[::-1] ######################### NOTE: QUESTION ############################## # The path is produced by an A* algorithm. This means that it is # optimal in length but 1) not smooth and 2) length optimality # may not be desired for coverage-based exploration weight_data = 0.5 # How much weight to update the data (a) weight_smooth = 0.1 # How much weight to smooth the coordinates (b) min_change = 0.0001 # Minimum change per iteration to keep iterating new_path = np.copy(np.array(self.path)) path_length = len(self.path[0]) change = min_change while change >= min_change: change = 0.0 for i in range(1, len(new_path) - 1): for j in range(path_length): # Initialize x, y x_i = self.path[i][j] y_i = new_path[i][j] y_prev = new_path[i - 1][j] y_next = new_path[i + 1][j] y_i_saved = y_i # Minimize the distance between coordinates of the original # path (y) and the smoothed path (x). Also minimize the # difference between the coordinates of the smoothed path # at time step i, and neighboring time steps. In order to do # all the minimizations, we use Gradient Ascent. y_i += weight_data * (x_i - y_i) + weight_smooth * ( y_next + y_prev - (2 * y_i)) new_path[i][j] = y_i change += abs(y_i - y_i_saved) self.path = new_path ######################################################################## # Break the path to subgoals every 2 pixels (1m = 20px) step = 1 n_subgoals = (int)(len(self.path) / step) self.subtargets = [] for i in range(0, n_subgoals): self.subtargets.append(self.path[i * step]) self.subtargets.append(self.path[-1]) self.next_subtarget = 0 print "The path produced " + str(len(self.subtargets)) + " subgoals" self.counter_to_next_sub = self.count_limit # Publish the path for visualization purposes ros_path = Path() ros_path.header.frame_id = "map" for p in self.path: ps = PoseStamped() ps.header.frame_id = "map" ps.pose.position.x = 0 ps.pose.position.y = 0 ######################### NOTE: QUESTION ############################## # Fill the ps.pose.position values to show the path in RViz # You must understand what self.robot_perception.resolution # and self.robot_perception.origin are. # Multiply subgoal's coordinates with resolution and # add robot.origin in order to translate between the (0,0) of the robot # pose and (0,0) of the map ps.pose.position.x = p[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'] ps.pose.position.y = p[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) RvizHandler.printMarker(\ subtargets_mark,\ 2, # Type: Sphere 0, # Action: Add "map", # Frame "art_subtargets", # Namespace [0, 0.8, 0.0, 0.8], # Color RGBA 0.2 # Scale ) self.inner_target_exists = True
def selectTarget(self): # IMPORTANT: The robot must be stopped if you call this function until # it is over # Check if we have a map while self.robot_perception.have_map == False: Print.art_print("Navigation: No map yet", Print.RED) return print "\nClearing all markers" RvizHandler.printMarker(\ [[0, 0]],\ 1, # Type: Arrow 3, # Action: delete all "map", # Frame "null", # Namespace [0,0,0,0], # Color RGBA 0.1 # Scale ) print '\n\n----------------------------------------------------------' print "Navigation: Producing new target" # We are good to continue the exploration # Make this true in order not to call it again from the speeds assignment self.target_exists = True # Gets copies of the map and coverage local_ogm = self.robot_perception.getMap() local_ros_ogm = self.robot_perception.getRosMap() local_coverage = self.robot_perception.getCoverage() print "Got the map and Coverage" self.path_planning.setMap(local_ros_ogm) # Once the target has been found, find the path to it # Get the global robot pose g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_px']]) # Call the target selection function to select the next best goal # Choose target function self.path = [] force_random = False while len(self.path) == 0: start = time.time() target = self.target_selection.selectTarget(\ local_ogm,\ local_coverage,\ self.robot_perception.robot_pose, self.robot_perception.origin, self.robot_perception.resolution, force_random) self.path = self.path_planning.createPath(\ g_robot_pose,\ target, self.robot_perception.resolution) print "Navigation: Path for target found with " + str(len(self.path)) +\ " points" if len(self.path) == 0: Print.art_print(\ "Path planning failed. Fallback to random target selection", \ Print.RED) force_random = True # 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) self.subtargets = [] for i in range(0, n_subgoals): self.subtargets.append(self.path[i * step]) self.subtargets.append(self.path[-1]) self.next_subtarget = 0 print "The path produced " + str(len(self.subtargets)) + " subgoals" ######################### NOTE: QUESTION ############################## # The path is produced by an A* algorithm. This means that it is # optimal in length but 1) not smooth and 2) length optimality # may not be desired for coverage-based exploration ######################################################################## self.counter_to_next_sub = self.count_limit # Publish the path for visualization purposes ros_path = Path() ros_path.header.frame_id = "map" for p in self.path: ps = PoseStamped() ps.header.frame_id = "map" ps.pose.position.x = 0 ps.pose.position.y = 0 ######################### NOTE: QUESTION ############################## # Fill the ps.pose.position values to show the path in RViz # You must understand what self.robot_perception.resolution # and self.robot_perception.origin are. ps.pose.position.x = p[0] * self.robot_perception.resolution + self.robot_perception.origin['x'] ps.pose.position.y = p[1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) RvizHandler.printMarker(\ subtargets_mark,\ 2, # Type: Sphere 0, # Action: Add "map", # Frame "art_subtargets", # Namespace [0, 0.8, 0.0, 0.8], # Color RGBA 0.2 # Scale ) self.inner_target_exists = True
def targetSelection(self, initOgm, coverage, origin, resolution, robotPose, force_random): rospy.loginfo("-----------------------------------------") rospy.loginfo( "[Target Select Node] Robot_Pose[x, y, th] = [%f, %f, %f]", robotPose['x'], robotPose['y'], robotPose['th']) rospy.loginfo("[Target Select Node] OGM_Origin = [%i, %i]", origin['x'], origin['y']) rospy.loginfo("[Target Select Node] OGM_Size = [%u, %u]", initOgm.shape[0], initOgm.shape[1]) # # willow stuff ogm_limits = {} # ogm_limits['min_x'] = 150 # used to be 200 # ogm_limits['max_x'] = 800 # used to be 800 # ogm_limits['min_y'] = 200 # ogm_limits['max_y'] = 800 # # corridor # ogm_limits = {} # ogm_limits['min_x'] = 100 # used to be 200 # ogm_limits['max_x'] = 500 # used to be 800 # ogm_limits['min_y'] = 100 # ogm_limits['max_y'] = 500 # Big Map # ogm_limits = {} ogm_limits['min_x'] = 200 # used to be 200 # ogm_limits['max_x'] = 800 # used to be 800 ogm_limits['max_x'] = 850 ogm_limits['min_y'] = 300 ogm_limits['max_y'] = 1080 # ogm_limits['max_y'] = 1100 # Big Map Modified # ogm_limits = {} # ogm_limits['min_x'] = 450 # used to be 200 # ogm_limits['max_x'] = 650 # used to be 800 # ogm_limits['min_y'] = 500 # ogm_limits['max_y'] = 700 # publisher marker_pub = rospy.Publisher("/vis_nodes", MarkerArray, queue_size=1) # Find only the useful boundaries of OGM. Only there calculations have meaning # ogm_limits = OgmOperations.findUsefulBoundaries(initOgm, origin, resolution) print ogm_limits # Blur the OGM to erase discontinuities due to laser rays #ogm = OgmOperations.blurUnoccupiedOgm(initOgm, ogm_limits) ogm = initOgm # find brushfire field brush2 = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) # Calculate skeletonization skeleton = self.topo.skeletonizationCffi(ogm, origin, resolution, ogm_limits) # Find Topological Graph tinit = time.time() nodes = self.topo.topologicalNodes(ogm, skeleton, coverage, origin, \ resolution, brush2, ogm_limits) # print took to calculate.... rospy.loginfo("Calculation time: %s", str(time.time() - tinit)) if len(nodes) == 0 and force_random: brush = self.brush.coverageLimitsBrushfire(initOgm, coverage, robotPose, origin, resolution) throw = set() throw = self.filterGoal(brush, initOgm, resolution, origin) brush.difference_update(throw) goal = random.sample(brush, 1) rospy.loginfo("nodes are zero and random node chosen!!!!") th_rg = math.atan2(goal[0][1] - robotPose['y'], \ goal[0][0] - robotPose['x']) self.target = [goal[0][0], goal[0][1], th_rg] return self.target if len(nodes) == 0: brush = self.brush.coverageLimitsBrushfire(initOgm, coverage, robotPose, origin, resolution) throw = set() throw = self.filterGoal(brush, initOgm, resolution, origin) brush.difference_update(throw) distance_map = dict() distance_map = self.calcDist(robotPose, brush) self.target = min(distance_map, key=distance_map.get) th_rg = math.atan2(self.target[1] - robotPose['y'], \ self.target[0] - robotPose['x']) self.target = list(self.target) self.target.append(th_rg) return self.target # pick Random node!! if force_random: ind = random.randrange(0, len(nodes)) rospy.loginfo('index is: %d', ind) rospy.loginfo('Random raw node is: [%u, %u]', nodes[ind][0], nodes[ind][1]) tempX = nodes[ind][0] * resolution + origin['x'] tempY = nodes[ind][1] * resolution + origin['y'] th_rg = math.atan2(tempY - robotPose['y'], \ tempX - robotPose['x']) self.target = [tempX, tempY, th_rg] rospy.loginfo("[Main Node] Random target found at [%f, %f]", self.target[0], self.target[1]) rospy.loginfo("-----------------------------------------") return self.target if len(nodes) > 0: rospy.loginfo("[Main Node] Nodes ready! Elapsed time = %fsec", time.time() - tinit) rospy.loginfo("[Main Node] # of nodes = %u", len(nodes)) # Remove previous targets from the list of nodes rospy.loginfo("[Main Node] Prev. target = [%u, %u]", self.previousTarget[0], \ self.previousTarget[1]) if len(nodes) > 1: nodes = [i for i in nodes if i != self.previousTarget] vis_nodes = [] for n in nodes: vis_nodes.append([ n[0] * resolution + origin['x'], n[1] * resolution + origin['y'] ]) RvizHandler.printMarker(\ vis_nodes,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_topological_nodes", # Namespace [0.3, 0.4, 0.7, 0.5], # Color RGBA 0.1 # Scale ) self.publish_markers(marker_pub, vis_nodes) # Check distance From Other goal # for node in nodes: # node_x = node[0] * resolution + origin['x'] # node_y = node[1] * resolution + origin['y'] # dist = math.hypot(node_x - other_goal['x'], node_y - other_goal['y']) # if dist < 5 and len(nodes) > 2: # nodes.remove(node) # Calculate topological cost # rayLength = 800 # in pixels # obstThres = 49 # wTopo = [] # dRad = [] # for i in range(8): # dRad.append(rayLength) # for k in range(0, len(nodes)): # # Determine whether the ray length passes the OGM limits # if nodes[k][0] + rayLength > ogm.shape[0]: # self.xLimitUp = ogm.shape[0] - 1 # else: # self.xLimitUp = nodes[k][0] + rayLength # if nodes[k][0] - rayLength < 0: # self.xLimitDown = 0 # else: # self.xLimitDown = nodes[k][0] - rayLength # if nodes[k][1] + rayLength > ogm.shape[1]: # self.yLimitUp = ogm.shape[1] - 1 # else: # self.yLimitUp = nodes[k][1] + rayLength # if nodes[k][1] - rayLength < 0: # self.yLimitDown = 0 # else: # self.yLimitDown = nodes[k][1] - rayLength # #### Here We Do the Ray Casting #### # # Find the distance between the node and obstacles # for i in range(nodes[k][0], self.xLimitUp): # if ogm[i][nodes[k][1]] > obstThres: # dRad[0] = i - nodes[k][0] # break # for i in range(self.xLimitDown, nodes[k][0]): # if ogm[i][nodes[k][1]] > obstThres: # dRad[1] = nodes[k][0] - i # break # for i in range(nodes[k][1], self.yLimitUp): # if ogm[nodes[k][0]][i] > obstThres: # dRad[2] = i - nodes[k][1] # break # for i in range(self.yLimitDown, nodes[k][1]): # if ogm[nodes[k][0]][i] > obstThres: # dRad[3] = nodes[k][1] - i # break # for i in range(nodes[k][0], self.xLimitUp): # for j in range(nodes[k][1], self.yLimitUp): # if ogm[i][j] > obstThres: # crosscut = \ # math.sqrt((i - nodes[k][0])**2 + (j - nodes[k][1])**2) # dRad[4] = crosscut # break # else: # break # if ogm[i][j] > obstThres: # break # for i in range(self.xLimitDown, nodes[k][0]): # for j in range(self.yLimitDown, nodes[k][1]): # if ogm[i][j] > obstThres: # crosscut = \ # math.sqrt((nodes[k][0] - i)**2 + (nodes[k][1] - j)**2) # dRad[5] = crosscut # break # else: # break # if ogm[i][j] > obstThres: # break # for i in range(nodes[k][0], self.xLimitUp): # for j in range(self.yLimitDown, nodes[k][1]): # if ogm[i][j] > obstThres: # crosscut = \ # math.sqrt((i - nodes[k][0])**2 + (nodes[k][1] - j)**2) # dRad[6] = crosscut # break # else: # break # if ogm[i][j] > obstThres: # break # for i in range(self.xLimitDown, nodes[k][0]): # for j in range(nodes[k][1], self.yLimitUp): # if ogm[i][j] > obstThres: # crosscut = \ # math.sqrt((nodes[k][0] - i)**2 + (j - nodes[k][1])**2) # dRad[7] = crosscut # break # else: # break # if ogm[i][j] > obstThres: # break # # wTopo.append(sum(dRad) / 8) # for i in range(len(nodes)): # rospy.logwarn("Topo Cost is: %f ",wTopo[i]) # Calculate distance cost wDist = [] nodesX = [] nodesY = [] for i in range(len(nodes)): nodesX.append(nodes[i][0]) nodesY.append(nodes[i][1]) for i in range(len(nodes)): #dist = math.sqrt((nodes[i][0] * resolution + origin['x'] - robotPose['x'])**2 + \ # (nodes[i][1] * resolution + origin['y'] - robotPose['y'])**2) dist = math.sqrt((nodes[i][0] + origin['x_px'] - robotPose['x_px'])**2 + \ (nodes[i][1] + origin['y_px'] - robotPose['y_px'])**2) # Manhattan Dist # dist = abs(nodes[i][0] + origin['x_px'] - robotPose['x_px']) + \ # abs(nodes[i][1] + origin['y_px'] - robotPose['y_px']) # numpy.var is covariance # tempX = ((robotPose['x'] - nodesX[i] * resolution + origin['x'])**2) / (2 * numpy.var(nodesX)) # tempY = ((robotPose['y'] - nodesY[i] * resolution + origin['y'])**2) / (2 * numpy.var(nodesY)) # tempX = ((robotPose['x_px'] - nodesX[i] + origin['x_px'])**2) / (2 * numpy.var(nodesX)) # tempY = ((robotPose['y_px'] - nodesY[i] + origin['y_px'])**2) / (2 * numpy.var(nodesY)) # try: # temp = 1 - math.exp(tempX + tempY) + 0.001 # \epsilon << 1 # except OverflowError: # print 'OverflowError!!!' # temp = 10**30 # gaussCoeff = 1 / temp gaussCoeff = 1 wDist.append(dist * gaussCoeff) #for i in range(len(nodes)): # rospy.logwarn("Distance Cost is: %f ",wDist[i]) #return self.target # Calculate coverage cost # dSamp = 1 / resolution # wCove = [] # for k in range(len(nodes)): # athr = 0 # for i in range(-1, 1): # indexX = int(nodes[k][0] + i * dSamp) # if indexX >= 0: # for j in range(-1, 1): # indexY = int(nodes[k][1] + j * dSamp) # if indexY >= 0: # athr += coverage[indexX][indexY] # wCove.append(athr) # for i in range(len(nodes)): # rospy.logwarn("Cove Cost is: %f ",wCove[i]) # Calculate rotational cost # wRot = [] # for i in range(len(nodes)): # dTh = math.atan2(nodes[i][1] - robotPose['y_px'], \ # nodes[i][0] - robotPose['x_px']) - robotPose['th'] # wRot.append(dTh) # for i in range(len(nodes)): # rospy.logwarn("Rot Cost is: %f ",wRot[i]) # Normalize costs # wTopoNorm = [] wDistNorm = [] # wCoveNorm = [] # wRotNorm = [] for i in range(len(nodes)): # if max(wTopo) - min(wTopo) == 0: # normTopo = 0 # else: # normTopo = 1 - (wTopo[i] - min(wTopo)) / (max(wTopo) - min(wTopo)) if max(wDist) - min(wDist) == 0: normDist = 0 else: normDist = 1 - (wDist[i] - min(wDist)) / (max(wDist) - min(wDist)) # if max(wCove) - min(wCove) == 0: # normCove = 0 # else: # normCove = 1 - (wCove[i] - min(wCove)) / (max(wCove) - min(wCove)) # if max(wRot) - min(wRot) == 0: # normRot = 0 # else: # normRot = 1 - (wRot[i] - min(wRot)) / (max(wRot) - min(wRot)) # wTopoNorm.append(normTopo) wDistNorm.append(normDist) # wCoveNorm.append(normCove) # wRotNorm.append(normRot) # rospy.logwarn("Printing TopoNorm....\n") # print wTopoNorm # rospy.logwarn("Printing DistNorm....\n") # print wDistNorm # rospy.logwarn("Printing CoveNorm....\n") # print wCoveNorm # rospy.logwarn("Printing RotNorm....\n") # print wRotNorm # Calculate Priority Weight priorWeight = [] for i in range(len(nodes)): #pre = round((wDistNorm[i] / 0.5), 0) pre = wDistNorm[i] / 0.5 # pre = 1 * round((wTopoNorm[i] / 0.5), 0) + \ # 8 * round((wDistNorm[i] / 0.5), 0) + \ # 4 * round((wCoveNorm[i] / 0.5), 0) + \ # 2 * round((wRotNorm[i] / 0.5), 0) #pre = 1 * round((wDistNorm[i] / 0.5), 0) + \ # 2 * round((wTopoNorm[i] / 0.5), 0) # + round((wRotNorm[i] / 0.5), 0) priorWeight.append(pre) # Calculate smoothing factor smoothFactor = [] for i in range(len(nodes)): coeff = 1 - wDistNorm[i] # coeff = (1 * (1 - wTopoNorm[i]) + 8 * (1 - wDistNorm[i]) + \ # 4 * (1 - wCoveNorm[i]) + 2 * (1 - wRotNorm[i])) / (2**4 - 1) #coeff = ((1 - wDistNorm[i]) + 2 * (1 - wTopoNorm[i])) / (2**2 - 1) smoothFactor.append(coeff) # Calculate costs self.costs = [] for i in range(len(nodes)): self.costs.append(smoothFactor[i] * priorWeight[i]) # print 'len nodes is:' # print len(nodes) goals_and_costs = zip(nodes, self.costs) #print goals_and_costs goals_and_costs.sort(key=lambda t: t[1], reverse=False) #sorted(goals_and_costs, key=operator.itemgetter(1)) #print goals_and_costs # ind = random.randrange(0,min(6, len(nodes))) rospy.loginfo("[Main Node] Raw node = [%u, %u]", goals_and_costs[0][0][0], goals_and_costs[0][0][1]) tempX = goals_and_costs[0][0][0] * resolution + origin['x'] tempY = goals_and_costs[0][0][1] * resolution + origin['y'] th_rg = math.atan2(tempY - robotPose['y'], \ tempX - robotPose['x']) self.target = [tempX, tempY, th_rg] rospy.loginfo("[Main Node] Eligible node found at [%f, %f]", self.target[0], self.target[1]) rospy.loginfo("[Main Node] Node Index: %u", i) rospy.loginfo("[Main Node] Node Cost = %f", goals_and_costs[0][1]) rospy.loginfo("-----------------------------------------") self.previousTarget = [ goals_and_costs[0][0][0], goals_and_costs[0][0][1] ] return self.target
def skeletonizationCffi(self, ogm, origin, resolution, ogml): width = ogm.shape[0] height = ogm.shape[1] local = numpy.zeros(ogm.shape) start = time.time() ######################################################################## ####################### Original Code ################################## # for i in range(0, width): # for j in range(0, height): # if ogm[i][j] < 49: # local[i][j] = 1 ############################ Added code ################################# step = 50 first = [0, 0] last = [0, 0] # Find the area in which there is propability of coverage for i in range(0, width, step): for j in range(0, height, step): if ogm[i][j] < 49: if first == [0, 0]: first = [i, j] last = [i, j] # Search only in the area found for i in range(first[0] - step, last[0] + step): for j in range(first[1] - step, last[1] + step): if ogm[i][j] < 49: local[i][j] = 1 ######################################################################### end = time.time() print end - start skeleton = Cffi.thinning(local, ogml) skeleton = Cffi.prune(skeleton, ogml, 10) viz = [] for i in range(0, width): for j in range(0, height): if skeleton[i][j] == 1: viz.append([ i * resolution + origin['x'], j * resolution + origin['y'] ]) RvizHandler.printMarker(\ viz,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_skeletonization_cffi", # Namespace [0.5, 0, 0, 0.5], # Color RGBA 0.05 # Scale ) return skeleton
def selectTarget(self, init_ogm, coverage, robot_pose, origin, \ resolution, force_random = False): target = [-1, -1] ######################### NOTE: QUESTION ############################## # Implement a smart way to select the next target. You have the # following tools: ogm_limits, Brushfire field, OGM skeleton, # topological nodes. # Find only the useful boundaries of OGM. Only there calculations # have meaning ogm_limits = OgmOperations.findUsefulBoundaries( init_ogm, origin, resolution) #print(ogm_limits) # Blur the OGM to erase discontinuities due to laser rays ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits) # Calculate Brushfire field tinit = time.time() brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE) # 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) # 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) # Visualization of topological nodes vis_nodes = [] for n in nodes: vis_nodes.append([ n[0] * resolution + origin['x'], n[1] * resolution + origin['y'] ]) RvizHandler.printMarker(\ vis_nodes,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_topological_nodes", # Namespace [0.3, 0.4, 0.7, 0.5], # Color RGBA 0.1 # Scale ) # Random point #if statement that makes the target selection random in case the remaining nodes are two if len(nodes) <= 2: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) return target pose_global_x = int(robot_pose['x_px'] - origin['x'] / resolution) pose_global_y = int(robot_pose['y_px'] - origin['y'] / resolution) #the folowing variables receive the values read by the sonar sensor sonar_left = self.sonar.sonar_left_range sonar_right = self.sonar.sonar_right_range sonar_front = self.sonar.sonar_front_range #a total sum is calculated for the front,right and left sonar sensors sonar_sum = sonar_left + sonar_right + sonar_front numrows = ogm.shape[1] numcols = ogm.shape[0] #if statement used in case the robot is in a tight spot and determines the target in the direction there is maximum space if sonar_sum < 1.2: target = self.sonar_avoidance(pose_global_x, pose_global_y, ogm, numcols, numrows) return target #in case of a time out or a failure in path planning if self.method == 'random' or force_random == True: target = [ self.node2_index_x, self.node2_index_y ] #sets the current target as the previously calculated second best-scored target if self.timeout_happened == 1: target = self.sonar_avoidance(pose_global_x, pose_global_y, ogm, numcols, numrows) self.timeout_happened = 0 return target self.timeout_happened = 1 return target ######################################################################## sum_min = 10000000 dist_min = 10000000 node_index_x = 0 #x value of the node with the lowest total cost node_index_y = 0 #y value of the node with the lowest total cost topo_cost = [] #list of topological costs for each node dist_cost = [] #list of distance costs for each node cov_cost = [] #list of coverage costs for each node #using the brushfire array in order to calculate topology cost for each node for n in nodes: sum_temp = 0 index = n[1] + 1 while brush[n[0], index] != 0: sum_temp += 1 index += 1 if index == numrows - 1: #numrows break index = n[1] - 1 while brush[n[0], index] != 0: sum_temp += 1 index -= 1 if index == 0: break index = n[0] + 1 while brush[index, n[1]] != 0: sum_temp += 1 index += 1 if index == numcols - 1: #numcols break index = n[0] - 1 while brush[index, n[1]] != 0: sum_temp += 1 index -= 1 if index == 0: break topo_cost.append(sum_temp / 4) #using the coverage array in order to calculate coverage cost for each node numrows = len(coverage) numcols = len(coverage[0]) for n in nodes: total_sum = 0 sum_temp = 0 index = n[1] + 1 if index >= numrows or n[0] >= numcols: total_sum = 5000 cov_cost.append(total_sum) continue while coverage[n[0], index] != 100: sum_temp += 1 index += 1 if index >= numcols - 1: #numrows-1: break total_sum += sum_temp * coverage[n[0], index] / 100 index = n[1] - 1 sum_temp = 0 while coverage[n[0], index] != 100: sum_temp += 1 index -= 1 if index == 0: break total_sum += sum_temp * coverage[n[0], index] / 100 index = n[0] + 1 sum_temp = 0 if index >= numcols or n[1] >= numrows: total_sum = 5000 cov_cost.append(total_sum) continue while coverage[index, n[1]] != 100: sum_temp += 1 index += 1 if index >= numrows - 1: #numcols-1 break total_sum += sum_temp * coverage[index, n[1]] / 100 index = n[0] - 1 sum_temp = 0 while coverage[index, n[1]] != 100: sum_temp += 1 index -= 1 if index == 0: break total_sum += sum_temp * coverage[index, n[1]] / 100 if total_sum == 0: total_sum = 5000 cov_cost.append(total_sum) pose_global_x = int(robot_pose['x_px'] - origin['x'] / resolution) pose_global_y = int(robot_pose['y_px'] - origin['y'] / resolution) #eucledean distance between the robot pose and each node dist = math.sqrt( math.pow(pose_global_x - n[0], 2) + math.pow(pose_global_y - n[1], 2)) dist_cost.append(dist) maxi = 0 for i in range(0, len(cov_cost)): if cov_cost[i] != 5000 and cov_cost[i] > maxi: maxi = cov_cost[i] for i in range(0, len(cov_cost)): if cov_cost[i] == 5000: cov_cost[i] = maxi * 1.2 #lists to store the normalized costs for each node topo_cost_norm = [] dist_cost_norm = [] cov_cost_norm = [] final_cost = [] min_final_cost = 1000000 for i in range(0, len(dist_cost)): topo_cost_norm.append((np.float(topo_cost[i] - min(topo_cost)) / np.float(max(topo_cost) - min(topo_cost)))) dist_cost_norm.append((dist_cost[i] - min(dist_cost)) / (max(dist_cost) - min(dist_cost))) if np.float(max(cov_cost) - min(cov_cost)) != 0: cov_cost_norm.append((np.float(cov_cost[i] - min(cov_cost)) / np.float(max(cov_cost) - min(cov_cost)))) if max(cov_cost) != min(cov_cost): final_cost.append( 4 * topo_cost_norm[i] + 2 * dist_cost_norm[i] + 4 * cov_cost_norm[i] ) #optimal factor values in order to determine the best node to approach else: final_cost.append( 6 * topo_cost_norm[i] + 4 * dist_cost_norm[i] ) # exception if statement for the case coverage array cannot be used yet if (final_cost[i] < min_final_cost): min_final_cost = final_cost[i] self.node2_index_x = node_index_x #storing the second best node to approach in case of path planning failure or time out self.node2_index_y = node_index_y # node_index_x = nodes[i][0] node_index_y = nodes[i][1] target = [node_index_x, node_index_y] #in case current target is the same with the previous , sonar avoidance function is used to modify the robot pose if target == self.previous_target: target = self.sonar_avoidance(pose_global_x, pose_global_y, ogm, numcols, numrows) self.previous_target = target return target
def checkTarget(self, event): # Check if we have a target or if the robot just wanders if self.inner_target_exists == False or self.move_with_target == False or\ self.next_subtarget == len(self.subtargets): return self.counter_to_next_sub -= 1 if self.counter_to_next_sub == 0: Print.art_print('\n~~~~ Time reset ~~~~', Print.RED) self.inner_target_exists = False self.target_exists = False return # Get the robot pose in pixels [rx, ry] = [\ self.robot_perception.robot_pose['x_px'] - \ self.robot_perception.origin['x'] / self.robot_perception.resolution,\ self.robot_perception.robot_pose['y_px'] - \ self.robot_perception.origin['y'] / self.robot_perception.resolution\ ] for i in range(self.next_subtarget, len(self.subtargets)): # Find the distance between the robot pose and the next subtarget dist = math.hypot(\ rx - self.subtargets[i][0], \ ry - self.subtargets[i][1]) ######################### NOTE: QUESTION ############################## # What if a later subtarget or the end has been reached before the # next subtarget? Alter the code accordingly. # Check if distance is less than 7 px (14 cm) if dist < 7: self.next_subtarget = 1 + i self.counter_to_next_sub = self.count_limit # Check if the final subtarget has been approached if self.next_subtarget == len(self.subtargets): self.target_exists = False ######################################################################## # Publish the current target if self.next_subtarget == len(self.subtargets): return subtarget = [\ self.subtargets[self.next_subtarget][0]\ * self.robot_perception.resolution + \ self.robot_perception.origin['x'], self.subtargets[self.next_subtarget][1]\ * self.robot_perception.resolution + \ self.robot_perception.origin['y']\ ] RvizHandler.printMarker(\ [subtarget],\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_next_subtarget", # Namespace [0, 0, 0.8, 0.8], # Color RGBA 0.2 # Scale )
def checkTarget(self, event): # Check if we have a target or if the robot just wanders if self.inner_target_exists == False or self.move_with_target == False or \ self.next_subtarget == len(self.subtargets): return self.counter_to_next_sub -= 1 if self.counter_to_next_sub == 0: Print.art_print('\n~~~~ Time reset ~~~~', Print.RED) self.inner_target_exists = False self.target_exists = False return # Get the robot pose in pixels [rx, ry] = [ self.robot_perception.robot_pose['x_px'] - self.robot_perception.origin['x'] / self.robot_perception.resolution, self.robot_perception.robot_pose['y_px'] - self.robot_perception.origin['y'] / self.robot_perception.resolution ] # # Find the distance between the robot pose and the next subtarget # dist = math.hypot( # rx - self.subtargets[self.next_subtarget][0], # ry - self.subtargets[self.next_subtarget][1]) ######################### NOTE: QUESTION ############################## # What if a later subtarget or the end has been reached before the # next subtarget? Alter the code accordingly. # Check if distance is less than 7 px (14 cm) # if dist < 5: # self.next_subtarget += 1 # self.counter_to_next_sub = self.count_limit # # Check if the final subtarget has been approached # if self.next_subtarget == len(self.subtargets): # self.target_exists = False # find the distance between robot and ALL subtargets dists = (math.hypot(rx - self.subtargets[subtarget][0], ry - self.subtargets[subtarget][1]) for subtarget, _ in enumerate(self.subtargets)) # check the distance of each subtarget from the robot # go towards the nearest and change index of next_subtarget # accordingly for idx, dist in enumerate(dists): if dist < 5: self.next_subtarget = idx + 1 self.counter_to_next_sub = self.count_limit # Check if the final subtarget has been approached if self.next_subtarget == len(self.subtargets): self.target_exists = False ######################################################################## # Publish the current target if self.next_subtarget == len(self.subtargets): return subtarget = [ self.subtargets[self.next_subtarget][0] * self.robot_perception.resolution + self.robot_perception.origin['x'], self.subtargets[self.next_subtarget][1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ] RvizHandler.printMarker( [subtarget], 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_next_subtarget", # Namespace [0, 0, 0.8, 0.8], # Color RGBA 0.2 # Scale )
def checkTarget(self, event): # Check if we have a target or if the robot just wanders if self.inner_target_exists == False or self.move_with_target == False or\ self.next_subtarget == len(self.subtargets): return self.counter_to_next_sub -= 1 if self.counter_to_next_sub == 0: Print.art_print('\n~~~~ Time reset ~~~~',Print.RED) self.inner_target_exists = False self.target_exists = False return # Get the robot pose in pixels [rx, ry] = [\ self.robot_perception.robot_pose['x_px'] - \ self.robot_perception.origin['x'] / self.robot_perception.resolution,\ self.robot_perception.robot_pose['y_px'] - \ self.robot_perception.origin['y'] / self.robot_perception.resolution\ ] ######################### NOTE: QUESTION ############################## # What if a later subtarget or the end has been reached before the # next subtarget? Alter the code accordingly. # Challenge 5 # Instead of checking the distance of the next target, # check the distance of the remaining targets, therefore you may reach to a next subject # bypassing current. for i in range(self.next_subtarget, len(self.subtargets)): # Find the distance between the robot pose and the next subtarget dist = math.hypot( rx - self.subtargets[i][0], ry - self.subtargets[i][1]) # check distance with the i_th target if i != (len(self.subtargets)-1): if dist < 10: #if distance found to be small from a terget set as next the target i + 1 self.next_subtarget = i + 1 self.counter_to_next_sub = self.count_limit else: if dist < 5: #if distance found to be small from a terget set as next the target i + 1 self.next_subtarget = i + 1 self.counter_to_next_sub = self.count_limit # Check if the final subtarget has been approached if self.next_subtarget == len(self.subtargets): self.target_exists = False ######################################################################## # Publish the current target if self.next_subtarget == len(self.subtargets): return subtarget = [\ self.subtargets[self.next_subtarget][0]\ * self.robot_perception.resolution + \ self.robot_perception.origin['x'], self.subtargets[self.next_subtarget][1]\ * self.robot_perception.resolution + \ self.robot_perception.origin['y']\ ] RvizHandler.printMarker(\ [subtarget],\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_next_subtarget", # Namespace [0, 0, 0.8, 0.8], # Color RGBA 0.2 # Scale )