def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning()
def __init__(self): self.xLimitUp = 0 self.xLimitDown = 0 self.yLimitUp = 0 self.yLimitDown = 0 self.brush = Brushfires() self.topo = Topology() self.target = [-1, -1] self.previousTarget = [-1, -1] self.costs = []
def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.path = [] self.prev_target = [0, 0] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() self.robot_perception = RobotPerception() # can i use that?
def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() # Initialize previous target self.previous_target = [-1, -1]
def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method if self.method_is_cost_based(): from robot_perception import RobotPerception self.robot_perception = RobotPerception() self.cost_based_properties = rospy.get_param("cost_based_properties") numpy.set_printoptions(precision=3, threshold=numpy.nan, suppress=True) self.brush = Brushfires() self.path_planning = PathPlanning()
def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.previous_target = [] self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() self.previous_target.append(50) self.previous_target.append(50) self.node2_index_x = 0 self.node2_index_y = 0 self.sonar = SonarDataAggregator() self.timeout_happened = 0
def __init__(self, selection_method): self.initial_time = time() self.method = selection_method self.initialize_gains = False self.brush = Brushfires() self.topology = Topology() self.path_planning = PathPlanning() self.droneConnector = DroneCommunication() # Parameters from YAML File self.debug = True #rospy.get_param('debug') self.map_discovery_purpose = rospy.get_param('map_discovery_purpose') self.color_evaluation_flag = rospy.get_param('color_rating') self.drone_color_evaluation_topic = rospy.get_param( 'drone_pub_color_rating') self.evaluate_potential_targets_srv_name = rospy.get_param( 'rate_potential_targets_srv') # Explore Gains self.g_color = 0.0 self.g_brush = 0.0 self.g_corner = 0.0 self.g_distance = 0.0 self.set_gain() if self.color_evaluation_flag: # Color Evaluation Service self.color_evaluation_service = rospy.ServiceProxy( self.evaluate_potential_targets_srv_name, EvaluateTargets) # Subscribe to Color Evaluation Topic to Get Results from Color Evaluation self.drone_color_evaluation_sub = rospy.Subscriber( self.drone_color_evaluation_topic, ColorEvaluationArray, self.color_evaluation_cb) # Parameters self.targets_color_evaluated = False # Set True Once Color Evaluation of Targets Completed self.color_evaluation = [] # Holds the Color Evaluation of Targets self.corner_evaluation = [ ] # Holds the Number of Corners Near Each Target
class TargetSelection: # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() 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 selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits): # The next target in pixels tinit = time.time() next_target = [0, 0] found = False while not found: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \ brushogm[x_rand][y_rand] > 5: next_target = [x_rand, y_rand] found = True Print.art_print("Select random target time: " + str(time.time() - tinit), \ Print.ORANGE) return next_target def lineOccupation(self, ogm_line): occupied = 0 potentially_occupied = 0 found_obstacle = False for grid in ogm_line: if grid > 90: occupied_cells += 1 + potentially_occupied elif grid < 55 and grid > 45 and found_obstacle: potentially_occupied += 1 elif grid < 20: potentially_occupied = 0 found_obstacle = False return occupied def detectObstacles(self, grid_size, ogm, point): # Goal Coordinates x_goal = point[0] y_goal = point[1] obstacles_found = False min_dist = 0 closest_obstacle = None # Obstacles in a grid_size x grid_size neighborhood ogm_segment = ogm[x_goal-grid_size: x_goal+grid_size,\ y_goal-grid_size: y_goal+grid_size] obs_idxs = np.where(ogm_segment > 80) # If there are no obstacles terminate normally if obs_idxs[0].size == 0: obstacles_found = False return [False, closest_obstacle, min_dist] # Find the closest obstacle obs_idxs = np.array([obs_idxs[0], obs_idxs[1]]).transpose() distances = np.hypot(obs_idxs[0:, 0] - grid_size, obs_idxs[0:, 1] - grid_size) closest_idx = distances.argmin() min_dist = distances.min() closest_obstacle = obs_idxs[closest_idx] - np.array( [grid_size, grid_size]) + point return [True, closest_obstacle, min_dist]
class TargetSelection: # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.previous_target = [] self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() self.previous_target.append(50) self.previous_target.append(50) self.node2_index_x = 0 self.node2_index_y = 0 self.sonar = SonarDataAggregator() self.timeout_happened = 0 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 selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits): # The next target in pixels tinit = time.time() next_target = [0, 0] found = False while not found: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \ brushogm[x_rand][y_rand] > 5: next_target = [x_rand, y_rand] found = True Print.art_print("Select random target time: " + str(time.time() - tinit), \ Print.ORANGE) return next_target #sonar_avoidance is a function that calculates the direction the robot should head in order to escape a tight spot def sonar_avoidance(self, pose_global_x, pose_global_y, ogm, numcols, numrows): tally = [ 0, 0, 0, 0 ] #a sum is calculated for north,east,south and west direction index = pose_global_y + 1 while ogm[pose_global_x, index] != 100: tally[0] = tally[0] + 1 index += 1 if index == numrows - 1: #numrows break index = pose_global_y - 1 while ogm[pose_global_x, index] != 100: tally[1] = tally[1] + 1 index -= 1 if index == 0: break index = pose_global_x + 1 while ogm[index, pose_global_y] != 100: tally[2] = tally[2] + 1 index += 1 if index == numcols - 1: #numcols break index = pose_global_x - 1 while ogm[index, pose_global_y] != 100: tally[3] = tally[3] + 1 index -= 1 if index == 0: break index = tally.index(max(tally)) if index == 0: target = [pose_global_x, pose_global_y + (int)(tally[0] / 2)] return target elif index == 1: target = [pose_global_x, pose_global_y - (int)(tally[1] / 2)] return target elif index == 2: target = [pose_global_x + (int)(tally[2] / 2), pose_global_y] return target elif index == 3: target = [pose_global_x - (int)(tally[3] / 2), pose_global_y] return target
class TargetSelection: # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() 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 ) # SMART TARGET SELECTION USING: # 1. Brush-fire field # 2. OGM Skeleton # 3. Topological Nodes # 4. Coverage field # 5. OGM Limits # Next subtarget is selected based on a # weighted-calculated score for each node. The score # is calculated using normalized values of the brush # field and the number of branches. The weight values # are defined experimentaly through the tuning method. temp_score = 0 max_score = 0 best_node = nodes[0] # the max-min boundaries are set arbitarily BRUSH_MAX = 17 BRUSH_MIN = 1 BRUSH_WEIGHT = 2.5 BRANCH_MAX = 10 BRANCH_MIN = 0 BRANCH_WEIGHT = 2.5 DISTANCE_MIN = 0 DISTANCE_MAX = 40 DISTANCE_WEIGHT = 0.5 for n in nodes: # Use brushfire to increase temp_score temp_score = (brush[n[0]][n[1]] - BRUSH_MIN) / (BRUSH_MAX - BRUSH_MIN) * BRUSH_WEIGHT # Use OGM Skeleton to find potential # branches following the target branches = 0 for i in range(-1, 2): for j in range(-1, 2): if (i != 0 or j != 0): branches += skeleton[n[0] + i][n[1] + j] # Use OGM-Skeleton to increase temp_score (select a goal with more future options) temp_score += (branches - BRANCH_MIN) / (BRANCH_MAX - BRUSH_MIN) * BRANCH_WEIGHT # Use OGM-Limits to decrease temp_score # the goal closest to OGM limits is best exploration option distance = math.sqrt((ogm_limits['max_x'] - n[0])**2 + (ogm_limits['max_y'] - n[1])**2) temp_score -= (distance - DISTANCE_MIN) / ( DISTANCE_MAX - DISTANCE_MIN) * DISTANCE_WEIGHT # If temp_score is higher than current max # score, then max score is updated and current node # becomes next goal - target if temp_score > max_score: max_score = temp_score best_node = n final_x = best_node[0] final_y = best_node[1] target = [final_x, final_y] # Random point # if self.method == 'random' or force_random == True: # target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) ######################################################################## return target def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits): # The next target in pixels tinit = time.time() next_target = [0, 0] found = False while not found: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \ brushogm[x_rand][y_rand] > 5: next_target = [x_rand, y_rand] found = True Print.art_print("Select random target time: " + str(time.time() - tinit), \ Print.ORANGE) return next_target
class TargetSelection(object): # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method if self.method_is_cost_based(): from robot_perception import RobotPerception self.robot_perception = RobotPerception() self.cost_based_properties = rospy.get_param("cost_based_properties") numpy.set_printoptions(precision=3, threshold=numpy.nan, suppress=True) self.brush = Brushfires() self.path_planning = PathPlanning() def method_is_cost_based(self): return self.method in ['cost_based', 'cost_based_fallback'] 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 @staticmethod def selectRandomTarget(ogm, coverage, brushogm): # The next target in pixels while True: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and brushogm[x_rand][y_rand] > 5: return x_rand, y_rand @staticmethod def is_good(brush, ogm, coverage, target=None): """ :rtype: numpy.ndarray """ if target is not None: x, y = target return ogm[x][y] < 50 and coverage[x][y] < 50 and brush[x][y] > 5 else: return numpy.logical_and(numpy.logical_and(ogm < 50, coverage < 50), brush > 5) def select_by_cost(self, map_info): nodes, paths, topo_costs = zip(*self.choose_best_nodes(map_info)) if nodes[0] is None: # choose_best_node's yield when no path is found will make nodes = (None,) return -1, -1 elif len(nodes) == 1: return nodes[0] best_path_idx = self.weight_costs( topo_costs, [self.distance_cost(path, map_info.robot_px) for path in paths], # Distance [self.coverage_cost(path, map_info.coverage) for path in paths], # Coverage [self.rotation_cost(path, map_info.robot_px, map_info.theta) for path in paths] # Rotational ).argmax() assert paths[best_path_idx] target = nodes[best_path_idx] Print.art_print("Best: {}".format(best_path_idx), Print.BLUE) return target def choose_best_nodes(self, map_info): # Since path planning takes a lot of time for many nodes we should reduce the possible result to the nodes # with the best distance and topological costs. if self.method == 'cost_based_fallback': yield self.closer_node(map_info), None, None return nodes = list(self.cluster_nodes(map_info.nodes, map_info.robot_px, self.cost_based_properties['node_clusters'])) topo_costs = [self._topological_cost(node, map_info.ogm) for node in nodes] best_nodes_idx = self.weight_costs( topo_costs, [euclidean(node, map_info.robot_px) for node in nodes] ).argsort()[::-1] # We need descending order since we now want to maximize. count = 0 for idx in best_nodes_idx: node = nodes[idx] path = map_info.create_path(node) if path: count += 1 yield node, path, topo_costs[idx] if count == self.cost_based_properties['max_paths']: break if count == 0: Print.art_print("Failed to create any path. Falling back to closer unoccupied.", Print.RED) self.method = 'cost_based_fallback' yield self.closer_node(map_info), None, None @staticmethod def cluster_nodes(nodes_original, robot_px, n_clusters): Print.art_print("Trying to cluster:" + str(nodes_original), Print.BLUE) whitened = whiten(nodes_original) _, cluster_idx = kmeans2(whitened, n_clusters) Print.art_print("Ended with clusters:" + str(cluster_idx), Print.BLUE) keyfun = lambda x: cluster_idx[x[0]] nodes = sorted(enumerate(nodes_original), key=keyfun) for _, group in itertools.groupby(nodes, key=keyfun): # For each cluster pick the farthest one. max_idx = max(group, key=lambda x: euclidean(robot_px, x[1]))[0] yield nodes_original[max_idx] def weight_costs(self, *cost_vectors, **kwargs): costs = self.normalize_costs(numpy.array(tuple(vector for vector in cost_vectors))) if 'weights' in kwargs: weights = kwargs['weights'] else: weights = 2 ** numpy.arange(costs.shape[0] - 1, -1, -1) return numpy.average(costs, axis=0, weights=weights) @staticmethod def closer_node(map_info): robot_px = numpy.array(map_info.robot_px) nodes = numpy.array(numpy.where(TargetSelection.is_good( numpy.array(map_info.brush), numpy.array(map_info.ogm), numpy.array(map_info.coverage), target=None ))).transpose() closer_idx = numpy.linalg.norm(nodes - robot_px, axis=1).argmin() return nodes[closer_idx] def _topological_cost(self, node, ogm): threshold = self.cost_based_properties['topo_threshold'] return self.topological_cost(node, ogm, threshold) @staticmethod def topological_cost(node, ogm, threshold): result = 0 for dir_x, dir_y in [(0, 1), (1, 1), (1, 0), (1, -1), (0, -1), (-1, -1), (-1, 0), (-1, 1)]: cost = 0 idx = 0 x, y = node while cost < threshold: idx += 1 x_c = x + idx * dir_x y_c = y + idx * dir_y cost = (x - x_c) ** 2 + (y - y_c) ** 2 if ogm[x_c][y_c] > 50: break elif ogm[x_c][y_c] in [50, -1]: Print.art_print("{},{} is unknown!".format(x_c, y_c), Print.RED) cost = threshold break result += min(threshold, cost) return result @staticmethod def distance_cost(path, robot_px): weighted_distances = (TargetSelection.distance(node1, node2) * TargetSelection.distance_coeff(node1, robot_px) for node1, node2 in zip(path[:-1], path[1:])) return sum(weighted_distances) @staticmethod def distance_coeff(node, robot_px, s=30, epsilon=0.0001): x_n, y_n = node x_g, y_g = robot_px coeff = 1 - math.exp(-((x_n - x_g) ** 2 / (2 * s ** 2) + (y_n - y_g) ** 2 / (2 * s ** 2))) + epsilon return 1 / coeff @staticmethod def distance(node_a, node_b): x_1, y_1 = node_a x_2, y_2 = node_b return math.sqrt((x_1 - x_2) ** 2 + (y_1 - y_2) ** 2) @staticmethod def rotation_cost(path, robot_px, theta): rotation = 0 rx, ry = robot_px theta_old = theta for node in path: st_x, st_y = node theta_new = math.atan2(st_y - ry, st_x - rx) rotation += abs(theta_new - theta_old) theta_old = theta_new return rotation @staticmethod def coverage_cost(path, coverage): coverage_sum = sum(coverage[x][y] for x, y in path) return coverage_sum @staticmethod def normalize_costs(costs): """ :rtype: numpy.ndarray """ Print.art_print("Costs before normalization:\n" + str(costs), Print.BLUE) assert (costs >= 0).all() return 1 - (costs.transpose() / numpy.abs(costs).max(axis=1)).transpose()
class TargetSelect: def __init__(self): self.xLimitUp = 0 self.xLimitDown = 0 self.yLimitUp = 0 self.yLimitDown = 0 self.brush = Brushfires() self.topo = Topology() self.target1 = [-1, -1] self.target2 = [-1, -1] self.previousTarget = [-1, -1] self.costs = [] def targetSelection(self, initOgm, coverage, origin, resolution, robotPose1, robotPose2): rospy.loginfo("-----------------------------------------") rospy.loginfo("[Target Select Node] Robot_Pose1[x, y, th] = [%f, %f, %f]", \ robotPose1['x'], robotPose1['y'], robotPose1['th']) rospy.loginfo("[Target Select Node] Robot_Pose2[x, y, th] = [%f, %f, %f]", \ robotPose2['x'], robotPose2['y'], robotPose2['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]) # Blur the OGM to erase discontinuities due to laser rays #ogm = OgmOperations.blurUnoccupiedOgm(initOgm, ogmLimits) ogm = initOgm rospy.loginfo("Calculating brushfire of first robot...") brush1 = self.brush.coverageLimitsBrushfire(initOgm, coverage, robotPose1, origin, resolution) rospy.loginfo("Calculating brushfire of second robot...") brush2 = self.brush.coverageLimitsBrushfire(initOgm, coverage, robotPose2, origin, resolution) print 'size of brush1:' print len(brush1) print 'size of brush2:' print len(brush2) min_dist = 10**24 store_goal1 = () throw1 = set() store_goal2 = () throw2 = set() throw1 = self.filterGoal(brush1, ogm, resolution, origin) throw2 = self.filterGoal(brush2, ogm, resolution, origin) print 'size of throw1:' print len(throw1) print 'size of throw2:' print len(throw2) brush1.difference_update(throw1) brush2.difference_update(throw2) print 'size of brush1 after update:' print len(brush1) print 'size of brush2 after update:' print len(brush2) ## Sample randomly the sets ???? # brush1 = random.sample(brush1, int(len(brush1)/5)) # brush2 = random.sample(brush2, int(len(brush2)/5)) # print 'size of brush 1 after sampling... ' # print len(brush1) # print 'size of brush 2 after sampling... ' # print len(brush2) # topo_gain1 = dict() # topo_gain1 = self.topoGain(brush1, resolution, origin, ogm) # # topo_gain2 = dict() # topo_gain2 = self.topoGain(brush2, resolution, origin, ogm) ## Sort Topo Gain goal ## #sorted_topo_gain = sorted(topo_gain.items(), key=operator.itemgetter(1), reverse = True) #rospy.loginfo('the length of sorted_topo_gain is %d !!', len(sorted_topo_gain)) distance_map1 = dict() distance_map1 = self.calcDist(robotPose1, brush1) rospy.loginfo('the length of distance map1 is %d !!', len(distance_map1)) distance_map2 = dict() distance_map2 = self.calcDist(robotPose2, brush2) rospy.loginfo('the length of distance map2 is %d !!', len(distance_map2)) self.target1 = min(distance_map1, key=distance_map1.get) self.target2 = min(distance_map2, key=distance_map2.get) #self.target1 = self.findGoal(brush1, distance_map1, topo_gain1) #self.target2 = self.findGoal(brush2, distance_map2, topo_gain2) return self.target1, self.target2 def filterGoal(self, brush2, ogm, resolution, origin): throw = set() for goal in brush2: goal = list(goal) for i in range(-3, 4): if int(goal[0] / resolution - origin['x'] / resolution) + i >= len(ogm): break if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) ] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) ] == -1: goal = tuple(goal) throw.add(goal) break for goal in brush2: goal = list(goal) for j in range(-3, 4): if int(goal[1] / resolution - origin['y'] / resolution) + j >= len(ogm[0]): break if ogm[int(goal[0]/resolution - origin['x']/resolution)]\ [int(goal[1]/resolution - origin['y']/resolution) + j] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) ] == -1: goal = tuple(goal) throw.add(goal) break for goal in brush2: goal = list(goal) for i in range(-3, 4): if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \ int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]): break if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) + i] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) + i] == -1: goal = tuple(goal) throw.add(goal) break # for goal in brush2: # goal = list(goal) # for i in range(-3,4): # if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \ # int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]): # break # if ogm[int(goal[0]/resolution - origin['x']/resolution) - i]\ # [int(goal[1]/resolution - origin['y']/resolution) - i] > 49 \ # or ogm[int(goal[0]/resolution - origin['x']/resolution) - i]\ # [int(goal[1]/resolution - origin['y']/resolution) - i] == -1: # goal = tuple(goal) # throw.add(goal) # break for goal in brush2: goal = list(goal) for i in range(-3, 4): if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \ int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]): break if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) - i] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) - i] == -1: goal = tuple(goal) throw.add(goal) break # for goal in brush2: # goal = list(goal) # for i in range(-3,4): # if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \ # int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]): # break # if ogm[int(goal[0]/resolution - origin['x']/resolution) - i]\ # [int(goal[1]/resolution - origin['y']/resolution) + i] > 49 \ # or ogm[int(goal[0]/resolution - origin['x']/resolution) - i]\ # [int(goal[1]/resolution - origin['y']/resolution) + i] == -1: # goal = tuple(goal) # throw.add(goal) # break return throw def topoGain(self, brush, resolution, origin, ogm): topo_gain = dict() half_side = rospy.get_param('radius') for goal in brush: xx = goal[0] / resolution yy = goal[1] / resolution rays_len = numpy.full([8], rospy.get_param('radius')) line = list( bresenham(int(xx), int(yy), int(xx), int(yy + half_side / resolution))) for idx, coord in enumerate(line): if ogm[coord[0] - origin['x_px']][coord[1] - origin['y_px']] > 80: rays_len[0] = len(line[0:idx]) * resolution break line = list( bresenham(int(xx), int(yy), int(xx), int(yy - half_side / resolution))) for idx, coord in enumerate(line): if ogm[coord[0] - origin['x_px']][coord[1] - origin['y_px']] > 80: rays_len[1] = len(line[0:idx]) * resolution break line = list( bresenham(int(xx), int(yy), int(xx + half_side / resolution), int(yy))) for idx, coord in enumerate(line): if ogm[coord[0] - origin['x_px']][coord[1] - origin['y_px']] > 80: rays_len[2] = len(line[0:idx]) * resolution break line = list( bresenham(int(xx), int(yy), int(xx - half_side / resolution), int(yy))) for idx, coord in enumerate(line): if ogm[coord[0] - origin['x_px']][coord[1] - origin['y_px']] > 80: rays_len[3] = len(line[0:idx]) * resolution break line = list( bresenham(int(xx), int(yy), int(xx + half_side / resolution), int(yy + half_side / resolution))) for idx, coord in enumerate(line): if ogm[coord[0] - origin['x_px']][coord[1] - origin['y_px']] > 80: rays_len[4] = len(line[0:idx]) * resolution break line = list( bresenham(int(xx), int(yy), int(xx + half_side / resolution), int(yy - half_side / resolution))) for idx, coord in enumerate(line): if ogm[coord[0] - origin['x_px']][coord[1] - origin['y_px']] > 80: rays_len[5] = len(line[0:idx]) * resolution break line = list( bresenham(int(xx), int(yy), int(xx - half_side / resolution), int(yy + half_side / resolution))) for idx, coord in enumerate(line): if ogm[coord[0] - origin['x_px']][coord[1] - origin['y_px']] > 80: rays_len[6] = len(line[0:idx]) * resolution break line = list( bresenham(int(xx), int(yy), int(xx - half_side / resolution), int(yy - half_side / resolution))) for idx, coord in enumerate(line): if ogm[coord[0] - origin['x_px']][coord[1] - origin['y_px']] > 80: rays_len[7] = len(line[0:idx]) * resolution break #topo_gain[goal] = sum(rays_len)/len(rays_len) topo_gain[goal] = sum(rays_len) #/len(rays_len) #rospy.loginfo('The topo gain for goal = [%f,%f] is %f', xx, yy, topo_gain[goal]) return topo_gain def calcDist(self, robotPose, brush): distance_map = dict() for goal in brush: dist = math.hypot(goal[0] - robotPose['x'], goal[1] - robotPose['y']) distance_map[goal] = dist return distance_map def findGoal(self, brush, distance_map, topo_gain): ###################################################################################### ##################### Here I calculate the gain of my Goals ########################## ###################################################################################### normTopo = dict() normDist = dict() for goal in brush: if max(topo_gain.values()) - min(topo_gain.values()) == 0: normTopo[(0, 0)] = 0 else: # 1 - ... normTopo[goal] = (topo_gain[goal] - min(topo_gain.values())) \ / (max(topo_gain.values()) - min(topo_gain.values())) if max(distance_map.values()) - min(distance_map.values()) == 0: normDist[(0, 0)] = 0 else: normDist[goal] = 1 - (distance_map[goal] - min(distance_map.values())) \ / (max(distance_map.values()) - min(distance_map.values())) # Calculate Priority Weight priorWeight = dict() for goal in brush: pre = 2 * round((normTopo[goal] / 0.5), 0) + \ 1 * round((normDist[goal] / 0.5), 0) priorWeight[goal] = pre # Calculate smoothing factor smoothFactor = dict() for goal in brush: coeff = (2 * (normTopo[goal]) + 1 * (1 - normDist[goal])) / (2**2 - 1) # coeff = (4 * (1 - wDistNorm[i]) + 2 * (1 - wCoveNorm[i]) + \ # (1 - wRotNorm[i])) / (2**3 - 1) smoothFactor[goal] = coeff # Calculate costs goalGains = dict() for goal in brush: goalGains[goal] = priorWeight[goal] * smoothFactor[goal] # Choose goal with max gain store_goal1 = set() for goal in brush: if goalGains[goal] == max(goalGains.values()): store_goal = goal rospy.loginfo("[Main Node] Goal1 at = [%u, %u]!!!", goal[0], goal[1]) rospy.loginfo("The Gain1 is = %f!!", goalGains[goal]) else: pass #rospy.logwarn("[Main Node] Did not find any goals :( ...") #self.target = self.selectRandomTarget(ogm, coverage, brush2, \ # origin, ogm_limits, resolution) # # goal = list(goal) goal = list(store_goal) print 'the ogm value is' #print ogm[int(goal1[0] - origin['x_px'])][int(goal1[1] - origin['y_px'])] print goal return goal
class TargetSelect: def __init__(self): self.xLimitUp = 0 self.xLimitDown = 0 self.yLimitUp = 0 self.yLimitDown = 0 self.brush = Brushfires() self.topo = Topology() self.target = [-1, -1] self.previousTarget = [-1, -1] self.costs = [] 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 rotateRobot(self): velocityMsg = Twist() angularSpeed = 0.3 relativeAngle = 2 * math.pi currentAngle = 0 rospy.loginfo("Roatating robot...") velocityMsg.linear.x = 0 velocityMsg.linear.y = 0 velocityMsg.linear.z = 0 velocityMsg.angular.x = 0 velocityMsg.angular.y = 0 velocityMsg.angular.z = angularSpeed t0 = rospy.Time.now().to_sec() rospy.logwarn(rospy.get_caller_id() + ": Rotate Robot! Please wait...") while currentAngle < relativeAngle: self.velocityPub.publish(velocityMsg) t1 = rospy.Time.now().to_sec() currentAngle = angularSpeed * (t1 - t0) velocityMsg.angular.z = 0 self.velocityPub.publish(velocityMsg) rospy.logwarn(rospy.get_caller_id() + ": Robot Rotation OVER!") return def publish_markers(self, marker_pub, vis_nodes): markers = MarkerArray() c = 0 for n in vis_nodes: c += 1 msg = Marker() msg.header.frame_id = "map" msg.ns = "lines" msg.action = msg.ADD msg.type = msg.CUBE msg.id = c #msg.scale.x = 1.0 #msg.scale.y = 1.0 #msg.scale.z = 1.0 # I guess I have to take into consideration resolution too msg.pose.position.x = n[0] msg.pose.position.y = n[1] msg.pose.position.z = 0 msg.pose.orientation.x = 0.0 msg.pose.orientation.y = 0.0 msg.pose.orientation.z = 0.05 msg.pose.orientation.w = 0.05 msg.scale.x = 0.2 msg.scale.y = 0.2 msg.scale.z = 0.2 msg.color.a = 1.0 # Don't forget to set the alpha! msg.color.r = 0.0 msg.color.g = 1.0 msg.color.b = 0.0 # print 'I publish msg now!!!' markers.markers.append(msg) marker_pub.publish(markers) # return def selectRandomTarget(self, ogm, brush, origin, ogmLimits, resolution): rospy.logwarn("[Main Node] Random Target Selection!") target = [-1, -1] found = False while not found: x_rand = random.randint(0, int(ogm.shape[0] - 1)) y_rand = random.randint(0, int(ogm.shape[1] - 1)) if ogm[x_rand][y_rand] <= 49 and brush[x_rand][ y_rand] != -1: # and \#coverage[x_rand][y_rand] != 100: tempX = x_rand * resolution + int(origin['x']) tempY = y_rand * resolution + int(origin['y']) target = [tempX, tempY] found = True rospy.loginfo("[Main Node] Random node selected at [%f, %f]", target[0], target[1]) rospy.loginfo("-----------------------------------------") return self.target def calcDist(self, robotPose, brush): distance_map = dict() for goal in brush: dist = math.hypot(goal[0] - robotPose['x'], goal[1] - robotPose['y']) distance_map[goal] = dist return distance_map def filterGoal(self, brush2, ogm, resolution, origin): throw = set() for goal in brush2: goal = list(goal) for i in range(-9, 10): if int(goal[0] / resolution - origin['x'] / resolution) + i >= len(ogm): break if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution)] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution)] == -1: goal = tuple(goal) throw.add(goal) break for goal in brush2: goal = list(goal) for j in range(-9, 10): if int(goal[1] / resolution - origin['y'] / resolution) + j >= len(ogm[0]): break if ogm[int(goal[0]/resolution - origin['x']/resolution)]\ [int(goal[1]/resolution - origin['y']/resolution) + j] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution)] == -1: goal = tuple(goal) throw.add(goal) break for goal in brush2: goal = list(goal) for i in range(-9, 10): if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \ int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]): break if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) + i] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) + i] == -1: goal = tuple(goal) throw.add(goal) break for goal in brush2: goal = list(goal) for i in range(-9, 10): if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \ int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]): break if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) - i] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) - i] == -1: goal = tuple(goal) throw.add(goal) break return throw
class TargetSelection: # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() def selectTarget(self, init_ogm, coverage, robot_pose, origin, \ resolution, force_random, tries): 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. # CHALLENGE 6 # 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 ) # Robot Position x = robot_pose['x_px'] + abs(origin['x'] / resolution) y = robot_pose['y_px'] + abs(origin['y'] / resolution) # Random point if self.method == 'random' and force_random == True: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) # CHALLENGE 6 # ADDED elif (tries == 0): target = self.MAXTargetSel(ogm, coverage, brush, ogm_limits, nodes, x, y) elif (tries == 1): target = self.MINTargetSel(ogm, coverage, brush, ogm_limits, nodes, x, y) else: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) # target = self.myTargetSelection(ogm, coverage, brush, ogm_limits, nodes, x, y) ######################################################################## return target def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits): # The next target in pixels tinit = time.time() next_target = [0, 0] found = False while not found: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \ brushogm[x_rand][y_rand] > 5: next_target = [x_rand, y_rand] found = True Print.art_print("Select random target time: " + str(time.time() - tinit), \ Print.ORANGE) return next_target def MAXTargetSel(self, ogm, coverage, brushogm, ogm_limits, nodes, x, y): tinit = time.time() next_target = [0, 0] a = [] b = [] d = [] print(x) print(y) for n in nodes: #The max distance topo node from robot temp = (math.pow(x - n[0], 2) + math.pow(y - n[1], 2)) a.append(temp) b.append(n[0]) d.append(n[1]) c = a.index(max(a)) next_target = [b[c], d[c]] Print.art_print("Select MAX target time: " + str(time.time() - tinit), \ Print.ORANGE) print(next_target) return next_target def MINTargetSel(self, ogm, coverage, brushogm, ogm_limits, nodes, x, y): tinit = time.time() next_target = [0, 0] a = [] b = [] d = [] print(x) print(y) for n in nodes: #The min distance topo node from robot temp = -(math.pow(x - n[0], 2) + math.pow(y - n[1], 2)) a.append(temp) b.append(n[0]) d.append(n[1]) c = a.index(max(a)) next_target = [b[c], d[c]] Print.art_print("Select MIN target time: " + str(time.time() - tinit), \ Print.ORANGE) print(next_target) return next_target
class TargetSelection: # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() 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 selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits): # The next target in pixels tinit = time.time() next_target = [0, 0] found = False while not found: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \ brushogm[x_rand][y_rand] > 5: next_target = [x_rand, y_rand] found = True Print.art_print("Select random target time: " + str(time.time() - tinit), \ Print.ORANGE) return next_target
class TargetSelection: # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() 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) # Furhest node-point if self.method == 'furthest': target = self.selectFurthestTarget(ogm, coverage, brush, ogm_limits, nodes) return target def selectFurthestTarget(self, ogm, coverage, brushogm, ogm_limits, nodes): # The next target in pixels tinit = time.time() next_target = [0, 0] for i in range(len(nodes) - 1, 0, -1): x = nodes[i][0] y = nodes[i][1] if ogm[x][y] < 50 and coverage[x][y] < 50 and \ brushogm[x][y] > 10: next_target = [x, y] Print.art_print("Select furthest target time: " + str(time.time() - tinit), \ Print.ORANGE) return next_target return self.selectRandomTarget(ogm, coverage, brushogm, ogm_limits) ######################################################################## def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits): # The next target in pixels tinit = time.time() next_target = [0, 0] found = False while not found: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \ brushogm[x_rand][y_rand] > 5: next_target = [x_rand, y_rand] found = True Print.art_print("Select random target time: " + str(time.time() - tinit), \ Print.ORANGE) return next_target
class TargetSelection: # Constructor def __init__(self, selection_method): self.initial_time = time() self.method = selection_method self.initialize_gains = False self.brush = Brushfires() self.topology = Topology() self.path_planning = PathPlanning() self.droneConnector = DroneCommunication() # Parameters from YAML File self.debug = True #rospy.get_param('debug') self.map_discovery_purpose = rospy.get_param('map_discovery_purpose') self.color_evaluation_flag = rospy.get_param('color_rating') self.drone_color_evaluation_topic = rospy.get_param( 'drone_pub_color_rating') self.evaluate_potential_targets_srv_name = rospy.get_param( 'rate_potential_targets_srv') # Explore Gains self.g_color = 0.0 self.g_brush = 0.0 self.g_corner = 0.0 self.g_distance = 0.0 self.set_gain() if self.color_evaluation_flag: # Color Evaluation Service self.color_evaluation_service = rospy.ServiceProxy( self.evaluate_potential_targets_srv_name, EvaluateTargets) # Subscribe to Color Evaluation Topic to Get Results from Color Evaluation self.drone_color_evaluation_sub = rospy.Subscriber( self.drone_color_evaluation_topic, ColorEvaluationArray, self.color_evaluation_cb) # Parameters self.targets_color_evaluated = False # Set True Once Color Evaluation of Targets Completed self.color_evaluation = [] # Holds the Color Evaluation of Targets self.corner_evaluation = [ ] # Holds the Number of Corners Near Each Target # Target Selection Function def selectTarget(self, init_ogm, coverage, robot_pose, origin, resolution, g_robot_pose, previous_limits=[], force_random=False): # Initialize Target target = [-1, -1] if self.running_time() > 15: print('\x1b[37;1m' + str('15 Minutes Constraint Passed!!!') + '\x1b[0m') # Find only the useful boundaries of OGM start = time() ogm_limits = OgmOperations.findUsefulBoundaries(init_ogm, origin, resolution, print_result=True, step=20) if self.debug: print('\x1b[34;1m' + str('Target Selection: OGM Boundaries ') + str(ogm_limits) + str(' in ') + str(time() - start) + str(' seconds.') + '\x1b[0m') # Blur the OGM to erase discontinuities due to laser rays start = time() ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits) if self.debug: print('\x1b[34;1m' + str('Target Selection: OGM Blurred in ') + str(time() - start) + str(' seconds.') + '\x1b[0m') # Calculate Brushfire field start = time() brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) if self.debug: print('\x1b[34;1m' + str('Target Selection: Brush in ') + str(time() - start) + str(' seconds.') + '\x1b[0m') # Calculate Robot Position [rx, ry] = [ robot_pose['x_px'] - origin['x'] / resolution, robot_pose['y_px'] - origin['y'] / resolution ] # Calculate Skeletonization start = time() skeleton = self.topology.skeletonizationCffi(ogm, origin, resolution, ogm_limits) if self.debug: print('\x1b[34;1m' + str('Target Selection: Skeletonization in ') + str(time() - start) + str(' seconds.') + '\x1b[0m') # Find Topological Graph start = time() potential_targets = self.topology.topologicalNodes( ogm, skeleton, coverage, brush, final_num_of_nodes=25, erase_distance=100, step=15) if self.debug: print('\x1b[34;1m' + str('Target Selection: Topological Graph in ') + str(time() - start) + str(' seconds.') + '\x1b[0m') print('\x1b[34;1m' + str("The Potential Targets to be Checked are ") + str(len(potential_targets)) + '\x1b[0m') if len(potential_targets) == 0: print('\x1b[32;1m' + str('\n------------------------------------------') + str("Finished Space Exploration!!! ") + str('------------------------------------------\n') + '\x1b[0m') sleep(10000) # Visualization of Topological Graph vis__potential_targets = [] for n in potential_targets: vis__potential_targets.append([ n[0] * resolution + origin['x'], n[1] * resolution + origin['y'] ]) RvizHandler.printMarker(vis__potential_targets, 1, 0, "map", "art_topological_nodes", [0.3, 0.4, 0.7, 0.5], 0.1) # Check if we have given values to Gains if not self.initialize_gains: self.set_gain() # Random Point Selection if Needed if self.method == 'random' or force_random: # Get Distance from Potential Targets distance = np.zeros((len(potential_targets), 1), dtype=np.float32) for idx, target in enumerate(potential_targets): distance[idx] = hypot(rx - target[0], ry - target[1]) distance *= 255.0 / distance.max() path = self.selectRandomTarget(ogm, coverage, brush, ogm_limits, potential_targets, distance, resolution, g_robot_pose) if path is not None: return path else: return [] # Sent Potential Targets for Color Evaluation (if Flag is Enable) if self.color_evaluation_flag: start_color = time() while not self.sent_potential_targets_for_color_evaluation( potential_targets, resolution, origin): pass # Initialize Arrays for Target Selection id = np.array(range(0, len(potential_targets))).reshape(-1, 1) brushfire = np.zeros((len(potential_targets), 1), dtype=np.float32) distance = np.zeros((len(potential_targets), 1), dtype=np.float32) color = np.zeros((len(potential_targets), 1), dtype=np.float32) corners = np.zeros((len(potential_targets), 1), dtype=np.float32) score = np.zeros((len(potential_targets), 1), dtype=np.float32) # Calculate Distance and Brush Evaluation start = time() for idx, target in enumerate(potential_targets): distance[idx] = hypot(rx - target[0], ry - target[1]) brushfire[idx] = brush[target[0], target[1]] if self.debug: print('\x1b[35;1m' + str('Distance and Brush Evaluation Calculated in ') + str(time() - start) + str(' seconds.') + '\x1b[0m') # Wait for Color Evaluation to be Completed if self.color_evaluation_flag: while not self.targets_color_evaluated: pass color = np.array(self.color_evaluation).reshape(-1, 1) corners = np.array(self.corner_evaluation, dtype=np.float64).reshape(-1, 1) # Reset Flag for Next Run self.targets_color_evaluated = False if self.debug: print('\x1b[35;1m' + str('Color Evaluation Calculated in ') + str(time() - start_color) + str(' seconds.') + '\x1b[0m') # Normalize Evaluation Arrays to [0, 255] distance *= 255.0 / distance.max() brushfire *= 255.0 / brushfire.max() if self.color_evaluation_flag: # color max is 640*320 = 204800 color *= 255.0 / color.max() color = 255.0 - color corners *= 255.0 / corners.max() # Calculate Score to Choose Best Target # Final Array = [ Id. | [X] | [Y] | Color | Brush | Dist. | Num. of Corners | Score ] # 0 1 2 3 4 5 6 7 # Max is: 255 + 255 - 0 - 0 = +510 # Min is: 0 + 0 - 255 - 255 = -510 evaluation = np.concatenate((id, potential_targets, color, brushfire, distance, corners, score), axis=1) for e in evaluation: # Choose Gains According to Type of Exploration (Default is Exploration) if self.map_discovery_purpose == 'coverage': e[7] = self.g_color * e[3] + self.g_brush * e[ 4] - self.g_distance * e[5] - self.g_corner * e[6] elif self.map_discovery_purpose == 'combined': # Gains Change over Time self.set_gain() e[7] = self.g_color * e[3] + self.g_brush * e[ 4] - self.g_distance * e[5] - self.g_corner * e[6] else: e[7] = self.g_color * e[3] + self.g_brush * e[ 4] - self.g_distance * e[5] - self.g_corner * e[6] # Normalize Score to [0, 255] and Sort According to Best Score (Increasingly) evaluation[:, 7] = self.rescale( evaluation[:, 7], (-self.g_distance * 255.0 - self.g_corner * 255.0), (self.g_color * 255.0 + self.g_brush * 255.0), 0.0, 255.0) evaluation = evaluation[evaluation[:, 7].argsort()] # Best Target is in the Bottom of Evaluation Table Now target = [ evaluation[[len(potential_targets) - 1], [1]], evaluation[[len(potential_targets) - 1], [2]] ] # Print The Score of the Target Selected if len(previous_limits) != 0: if not previous_limits['min_x'] < target[0] < previous_limits['max_x'] and not\ previous_limits['min_y'] < target[1] < previous_limits['max_y']: print('\x1b[38;1m' + str('Selected Target was inside Explored Area.') + '\x1b[0m') print('\x1b[35;1m' + str('Selected Target was ') + str(int(evaluation.item( (len(potential_targets) - 1), 0))) + str(' with score of ') + str(evaluation.item( (len(potential_targets) - 1), 7)) + str('.') + '\x1b[0m') return self.path_planning.createPath(g_robot_pose, target, resolution) # Send SIGNAL to Drone to Color Evaluate Potential Targets Function def sent_potential_targets_for_color_evaluation(self, targets, resolution, origin): # Calculate Position of Targets in Map targets = np.asarray(targets, dtype=np.float64) for target in targets: target[0] = target[0] * resolution + origin['x'] target[1] = target[1] * resolution + origin['y'] # Create Color Evaluation Request Message color_evaluation_request_msg = EvaluateTargetsRequest() color_evaluation_request_msg.pos_x = np.array(targets[:, 0]) color_evaluation_request_msg.pos_y = np.array(targets[:, 1]) # Call Service rospy.wait_for_service(self.evaluate_potential_targets_srv_name) try: response = self.color_evaluation_service( color_evaluation_request_msg) except rospy.ServiceException, e: print "Service call failed: %s" % e return response
class TargetSelection: # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() # Initialize previous target self.previous_target = [-1, -1] 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 selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits): # The next target in pixels tinit = time.time() next_target = [0, 0] found = False while not found: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \ brushogm[x_rand][y_rand] > 5: next_target = [x_rand, y_rand] found = True Print.art_print("Select random target time: " + str(time.time() - tinit), \ Print.ORANGE) return next_target
class TargetSelection: # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() 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 selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits): # The next target in pixels tinit = time.time() next_target = [0, 0] found = False while not found: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \ brushogm[x_rand][y_rand] > 5: next_target = [x_rand, y_rand] print " RANDOM TARGET " + str(next_target) found = True Print.art_print("Select random target time: " + str(time.time() - tinit), \ Print.ORANGE) return next_target
class TargetSelect: def __init__(self): self.xLimitUp = 0 self.xLimitDown = 0 self.yLimitUp = 0 self.yLimitDown = 0 self.brush = Brushfires() self.topo = Topology() self.target = [-1, -1] self.previousTarget = [-1, -1] self.costs = [] def targetSelection(self, initOgm, costmap, origin, resolution, robotPose): 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]) ogmLimits = {} ogmLimits['min_x'] = -1 ogmLimits['max_x'] = -1 ogmLimits['min_y'] = -1 ogmLimits['max_y'] = -1 # Find only the useful boundaries of OGM. Only there calculations have meaning ogmLimits = OgmOperations.findUsefulBoundaries(initOgm, origin, resolution) print(ogmLimits) while ogmLimits['min_x'] < 0 or ogmLimits['max_x'] < 0 or \ ogmLimits['min_y'] < 0 or ogmLimits['max_y'] < 0: rospy.logwarn("[Main Node] Negative OGM limits. Retrying...") ogmLimits = OgmOperations.findUsefulBoundaries( initOgm, origin, resolution) ogmLimits['min_x'] = abs(int(ogmLimits['min_x'])) ogmLimits['min_y'] = abs(int(ogmLimits['min_y'])) ogmLimits['max_x'] = abs(int(ogmLimits['max_x'])) ogmLimits['max_y'] = abs(int(ogmLimits['max_y'])) rospy.loginfo("[Target Select] OGM_Limits[x] = [%i, %i]", \ ogmLimits['min_x'], ogmLimits['max_x']) rospy.loginfo("[Target Select] OGM_Limits[y] = [%i, %i]", \ ogmLimits['min_y'], ogmLimits['max_y']) # Blur the OGM to erase discontinuities due to laser rays #ogm = OgmOperations.blurUnoccupiedOgm(initOgm, ogmLimits) ogm = initOgm # for i in range(len(ogm)): # for j in range(len(ogm)): # if ogm[i][j] == 100: # rospy.loginfo('i,j = [%d, %d]', i, j) # # Calculate Brushfire field #itime = time.time() #brush = self.brush.obstaclesBrushfireCffi(ogm, ogmLimits) #rospy.loginfo("[Target Select] Brush ready! Elapsed time = %fsec", time.time() - itime) #obst = self.brush.coverageLimitsBrushfire2(initOgm,ogm,robotPose,origin, resolution ) rospy.loginfo("Calculating brush2....") # brush = self.brush.obstaclesBrushfireCffi(ogm,ogmLimits) brush2 = self.brush.coverageLimitsBrushfire2(ogm, ogm, robotPose, origin, resolution) #goals = self.brush.closestUncoveredBrushfire(ogm, ogm, brush, robotPose, origin, resolution ) #robotPosePx = [] #robotPosePx[0] = robotPose['x']/resolution #robotPosePy[1] = robotPose['y']/resolution print 'size of brush2 :' print len(brush2) min_dist = 10**24 store_goal = () # rospy.loginfo("finding the difference between the two sets...") # brush2.difference(visited) #max_dist = random.randrange(1,10) #rospy.loginfo("max_dist for this it is: %d ", max_dist) throw = set() for goal in brush2: goal = list(goal) for i in range(-10, 11): if int(goal[0] / resolution - origin['x'] / resolution) + i >= len(ogm): break if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) ] == 100: goal = tuple(goal) throw.add(goal) break for goal in brush2: goal = list(goal) for j in range(-10, 11): if int(goal[1] / resolution - origin['y'] / resolution) + j >= len(ogm[0]): break if ogm[int(goal[0]/resolution - origin['x']/resolution)]\ [int(goal[1]/resolution - origin['y']/resolution) + j] == 100: goal = tuple(goal) throw.add(goal) break print 'size of throw :' print len(throw) brush2.difference_update(throw) print 'size of brush2 after update :' print len(brush2) distance_map = dict() for goal in brush2: dist = math.hypot(goal[0] - robotPose['x'], goal[1] - robotPose['y']) distance_map[goal] = dist sorted_dist_map = sorted(distance_map.items(), key=operator.itemgetter(1)) sorted_goal_list = list() for key, value in sorted(distance_map.iteritems(), key=lambda (k, v): (v, k)): sorted_goal_list.append(key) pass #print "%s: %s" % (key, value) # for key in distance_map: # if distance_map[key] > random.randrange(1,5): # goal = key # break # sorted_goal_list_sampled = sorted_goal_list[0:len(sorted_goal_list):10] #print sorted_goal_list_top_10 # stored_goal = list() # dist = 1000 # for goal in distance_map: # if distance_map[goal] < dist: # dist = distance_map[goal] # stored_goal = goal # # rospy.loginfo('The stored goal is = [%f,%f]!!' ,stored_goal[0], stored_goal[1]) # rospy.loginfo('The distance is %f!!', distance_map[stored_goal]) # rospy.loginfo('The gain is %f!!', topo_gain[stored_goal]) # #rand_target = random.choice(distance_map.keys()) # #goal = rand_target ind = random.randrange(0, min(4, len(sorted_goal_list))) print 'ind is' print ind goal = sorted_goal_list[ind] print 'the dist is' print distance_map[goal] goal = list(goal) goal[0] = goal[0] + random.uniform(-0.5, 0.5) goal[1] = goal[1] + random.uniform(-0.5, 0.5) print goal self.target = goal #for goal in brush2: # print sorted_distance_map[goal] return self.target rospy.loginfo("goal AFTER unifrom is: goal = [%f,%f]", store_goal[0], store_goal[1]) def selectRandomTarget(self, ogm, brush, origin, ogmLimits, resolution): rospy.logwarn("[Main Node] Random Target Selection!") target = [-1, -1] found = False while not found: x_rand = random.randint(0, int(ogm.shape[0] - 1)) y_rand = random.randint(0, int(ogm.shape[1] - 1)) if ogm[x_rand][y_rand] <= 49 and brush[x_rand][ y_rand] > 3: # and \#coverage[x_rand][y_rand] != 100: tempX = x_rand * resolution + int(origin['x']) tempY = y_rand * resolution + int(origin['y']) target = [tempX, tempY] found = True rospy.loginfo("[Main Node] Random node selected at [%f, %f]", target[0], target[1]) rospy.loginfo("-----------------------------------------") return self.target def rotateRobot(self): velocityMsg = Twist() angularSpeed = 0.3 relativeAngle = 2 * math.pi currentAngle = 0 rospy.loginfo("Roatating robot...") velocityMsg.linear.x = 0 velocityMsg.linear.y = 0 velocityMsg.linear.z = 0 velocityMsg.angular.x = 0 velocityMsg.angular.y = 0 velocityMsg.angular.z = angularSpeed t0 = rospy.Time.now().to_sec() rospy.logwarn(rospy.get_caller_id() + ": Rotate Robot! Please wait...") while currentAngle < relativeAngle: self.velocityPub.publish(velocityMsg) t1 = rospy.Time.now().to_sec() currentAngle = angularSpeed * (t1 - t0) velocityMsg.angular.z = 0 self.velocityPub.publish(velocityMsg) rospy.logwarn(rospy.get_caller_id() + ": Robot Rotation OVER!")
class TargetSelection: # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.previous_target = [-1, -1] self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() 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 ############################################################################# # Challenge 6. select Smart Target Function # this function follows the methodology presented # on lecture 9. def selectSmartTarget(self, coverage, brush, robot_pose, resolution, origin, nodes): tinit = time.time() # Get the robot pose in pixels [rx, ry] = [int(round(robot_pose['x_px'] - origin['x'] / resolution)), int(round(robot_pose['y_px'] - origin['y'] / resolution))] # Initialize weights matrix weights = [] # Do procedure described in presentation 9 # for each node. for i, node in enumerate(nodes): # Calculate the path path = np.flipud(self.path_planning.createPath([rx, ry], node, resolution)) # Check if it found a path if path.shape[0] > 2: # Vectors of the path vectors = path[1:, :] - path[:-1, :] # Calculate paths weighted distance vectorsMean = vectors.mean(axis=0) vectorsVar = vectors.var(axis=0) dists = np.sqrt(np.einsum('ij,ij->i', vectors, vectors)) weightCoeff = 1 / (1 - np.exp(-np.sum((vectors - vectorsMean)**2 / (2 * vectorsVar), axis=1)) + 1e-4) weightDists = np.sum(weightCoeff + dists) # Topological weight weightTopo = brush[node[0], node[1]] # Cosine of the angles c = np.sum(vectors[1:, :] * vectors[:-1, :], axis=1) / np.linalg.norm(vectors[1:, :], axis=1) / np.linalg.norm(vectors[:-1, :], axis=1) # Sum of all angles weightTurn = np.sum(abs(np.arccos(np.clip(c, -1, 1)))) # Calculate the coverage weight pathIndex = np.rint(path).astype(int) weightCove = 1 - np.sum(coverage[pathIndex[:, 0], pathIndex[:, 1]]) / (path.shape[0] * 255) weights.append([i, weightDists, weightTopo, weightTurn, weightCove]) if len(weights) > 0: weight = np.array(weights) # Normalize the weights at [0,1] weight[:,1:] = 1 - ((weight[:,1:] - np.min(weight[:,1:], axis=0)) / (np.max(weight[:,1:], axis=0) - np.min(weight[:,1:], axis=0))) # Calculatete the final weights finalWeights = 8 * weight[:, 2] + 4 * weight[:, 1] + 2 * weight[:, 4] + weight[:, 3] # Find the best path index = int(weight[max(xrange(len(finalWeights)), key=finalWeights.__getitem__)][0]) target = nodes[index] Print.art_print("Smart target selection time: " + str(time.time() - tinit), Print.ORANGE) return target else: Print.art_print("Smart target selection failed!!! Time: " + str(time.time() - tinit), Print.ORANGE) return None ############################################################################ def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits): # The next target in pixels tinit = time.time() next_target = [0, 0] found = False while not found: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \ brushogm[x_rand][y_rand] > 5: next_target = [x_rand, y_rand] found = True Print.art_print("Select random target time: " + str(time.time() - tinit), \ Print.ORANGE) return next_target
class TargetSelect: def __init__(self): self.xLimitUp = 0 self.xLimitDown = 0 self.yLimitUp = 0 self.yLimitDown = 0 self.brush = Brushfires() self.topo = Topology() self.target = [-1, -1] self.previousTarget = [-1, -1] self.costs = [] def targetSelection(self, initOgm, coverage, origin, resolution, robotPose, flag, other_goal, 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 params # ogm_limits = {} # ogm_limits['min_x'] = 350 # used to be 200 # ogm_limits['max_x'] = 800 # used to be 800 # ogm_limits['min_y'] = 200 # ogm_limits['max_y'] = 800 # # 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'] = 710 # publisher marker_pub = rospy.Publisher("/robot1/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 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 < 1 and len(nodes) > 2: nodes.remove(node) # 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 # Calculate distance cost wDist = [] nodesX = [] nodesY = [] for i in range(0, len(nodes)): nodesX.append(nodes[i][0]) nodesY.append(nodes[i][1]) for i in range(0, len(nodes)): dist = math.sqrt((nodes[i][0] * resolution + origin['x_px'] - robotPose['x_px'])**2 + \ (nodes[i][1] * resolution + origin['y_px'] - robotPose['y_px'])**2) # for i in range(len(nodes)): # rospy.logwarn("Distance Cost is: %f ",wDist[i]) gaussCoeff = 1 wDist.append(dist * gaussCoeff) #return self.target # Normalize costs # wTopoNorm = [] wDistNorm = [] # wCoveNorm = [] # wRotNorm = [] for i in range(0, len(nodes)): # if max(wTopo) - min(wTopo) == 0: # normTopo = 0 # else: # normTopo = 1 - (wTopo[i] - min(wTopo)) / (max(wTopo) - min(wTopo)) # if wDist[i] == max(wDist): # nodes.remove(nodes[i]) # continue 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) # Calculate Priority Weight priorWeight = [] for i in range(0, len(nodes)): pre = wDistNorm[i] / 0.5 #pre = 1 priorWeight.append(pre) # Calculate smoothing factor smoothFactor = [] for i in range(0, len(nodes)): coeff = 1 - wDistNorm[i] smoothFactor.append(coeff) # Calculate costs self.costs = [] for i in range(0, 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 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 rotateRobot(self): velocityMsg = Twist() angularSpeed = 0.3 relativeAngle = 2*math.pi currentAngle = 0 rospy.loginfo("Roatating robot...") velocityMsg.linear.x = 0 velocityMsg.linear.y = 0 velocityMsg.linear.z = 0 velocityMsg.angular.x = 0 velocityMsg.angular.y = 0 velocityMsg.angular.z = angularSpeed t0 = rospy.Time.now().to_sec() rospy.logwarn(rospy.get_caller_id() + ": Rotate Robot! Please wait...") while currentAngle < relativeAngle: self.velocityPub.publish(velocityMsg) t1 = rospy.Time.now().to_sec() currentAngle = angularSpeed * (t1 - t0) velocityMsg.angular.z = 0 self.velocityPub.publish(velocityMsg) rospy.logwarn(rospy.get_caller_id() + ": Robot Rotation OVER!") return def publish_markers(self, marker_pub, vis_nodes): markers = MarkerArray() c = 0 for n in vis_nodes: c += 1 msg = Marker() msg.header.frame_id = "map" msg.ns = "lines" msg.action = msg.ADD msg.type = msg.CUBE msg.id = c #msg.scale.x = 1.0 #msg.scale.y = 1.0 #msg.scale.z = 1.0 # I guess I have to take into consideration resolution too msg.pose.position.x = n[0] msg.pose.position.y = n[1] msg.pose.position.z = 0 msg.pose.orientation.x = 0.0 msg.pose.orientation.y = 0.0 msg.pose.orientation.z = 0.05 msg.pose.orientation.w = 0.05 msg.scale.x = 0.2 msg.scale.y = 0.2 msg.scale.z = 0.2 msg.color.a = 1.0 # Don't forget to set the alpha! msg.color.r = 0.0 msg.color.g = 1.0 msg.color.b = 0.0 # print 'I publish msg now!!!' markers.markers.append(msg) marker_pub.publish(markers) # return def calcDist(self, robotPose, brush): distance_map = dict() for goal in brush: dist = math.hypot(goal[0] - robotPose['x'], goal[1] - robotPose['y']) distance_map[goal] = dist return distance_map def filterGoal(self, brush2, ogm, resolution, origin): throw = set() for goal in brush2: goal = list(goal) for i in range(-9,10): if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm): break if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution)] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution)] == -1: goal = tuple(goal) throw.add(goal) break for goal in brush2: goal = list(goal) for j in range(-9,10): if int(goal[1]/resolution - origin['y']/resolution) + j >= len(ogm[0]): break if ogm[int(goal[0]/resolution - origin['x']/resolution)]\ [int(goal[1]/resolution - origin['y']/resolution) + j] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution)] == -1: goal = tuple(goal) throw.add(goal) break for goal in brush2: goal = list(goal) for i in range(-9,10): if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \ int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]): break if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) + i] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) + i] == -1: goal = tuple(goal) throw.add(goal) break for goal in brush2: goal = list(goal) for i in range(-9, 10): if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \ int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]): break if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) - i] > 49 \ or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\ [int(goal[1]/resolution - origin['y']/resolution) - i] == -1: goal = tuple(goal) throw.add(goal) break return throw
class TargetSelection: # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() 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) # print(ogm) # Calculate Brushfire field tinit = time.time() brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits) Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE) # 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) #closest point elif self.method == 'closest_node': target = self.selectClosestTarget(robot_pose, vis_nodes, nodes) elif self.method == 'cost_based': target = self.selectCostBasedTarget(robot_pose, vis_nodes, brush) return target def selectCostBasedTarget(self, robot_pose, vis_nodes, brush): dist_cost = [] topol_cost = [] turn_cost = [] dist_cost_norm = [] topol_cost_norm = [] turn_cost_norm = [] cost = 0 index_final = 0 [rx, ry] = [robot_pose['x'], robot_pose['y']] for n in vis_nodes: #distance cost dist_cost.append(pow(pow(n[1] - ry, 2) + pow(n[0] - rx, 2), 0.5)) #distance from node #topological cost for i in range(-1, 1): for j in range(-1, 1): if i == 0 and j == 0: continue cost += brush[int(n[0]) + i][int(n[1]) + j] topol_cost.append((1 / 8) * cost) #rotational cost angle = round( math.degrees( math.atan2(n[1] - ry + 0.00001, n[0] - rx + 0.00001))) turn_cost.append(angle % 360) # normalisation for cost in dist_cost: normalised_cost = 1 - ((cost - min(dist_cost)) / (max(dist_cost) / min(dist_cost))) dist_cost_norm.append(normalised_cost) for cost in topol_cost: normalised_cost = 1 - ((cost - min(topol_cost)) / (max(topol_cost) - min(topol_cost))) topol_cost_norm.append(normalised_cost) for cost in turn_cost: normalised_cost = 1 - ((cost - min(turn_cost)) / (max(turn_cost) - min(turn_cost))) turn_cost_norm.append(normalised_cost) final_cost = [] for i in range(len(topol_cost_norm)): cost = 4 * dist_cost_norm[i] + 2 * turn_cost_norm[ i] + topol_cost_norm[i] final_cost.append(cost) #find target with the index_final = final_cost.index(max(final_cost)) target = vis_nodes[index_final] return target def selectClosestTarget(self, robot_pose, vis_nodes, nodes): [rx, ry] = [robot_pose['x'], robot_pose['y']] dist = [] for n in vis_nodes: dist.append(pow(pow(n[1] - ry, 2) + pow(n[0] - rx, 2), 0.5)) #distance from node index_min = dist.index(min(dist)) next_target = [nodes[index_min][0], nodes[index_min][1]] return next_target ######################################################################## def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits): # The next target in pixels tinit = time.time() next_target = [0, 0] found = False while not found: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \ brushogm[x_rand][y_rand] > 5: next_target = [x_rand, y_rand] found = True Print.art_print("Select random target time: " + str(time.time() - tinit), \ Print.ORANGE) return next_target
class TargetSelection: # Constructor def __init__(self, selection_method): self.goals_position = [] self.goals_value = [] self.path = [] self.prev_target = [0, 0] self.omega = 0.0 self.radius = 0 self.method = selection_method self.brush = Brushfires() self.topo = Topology() self.path_planning = PathPlanning() self.robot_perception = RobotPerception() # can i use that? 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) print len(nodes) # 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) # CTN if self.method == 'CTN': target = self.selectNearestTopologyNode(robot_pose=robot_pose, resolution=resolution, nodes=nodes) if target is None: target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits) # target = self.selectNearestTopologyNode(robot_pose=robot_pose, resolution=resolution, nodes=nodes, ogm=ogm, # coverage=coverage, brushogm=brush) ######################################################################## return target def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits): # The next target in pixels tinit = time.time() next_target = [0, 0] found = False while not found: x_rand = random.randint(0, ogm.shape[0] - 1) y_rand = random.randint(0, ogm.shape[1] - 1) if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \ brushogm[x_rand][y_rand] > 5: next_target = [x_rand, y_rand] found = True Print.art_print( "Select random target time: " + str(time.time() - tinit), Print.ORANGE) return next_target # way too slow, maybe i didn't quite understood weigthed path find? def selectNearestTopologyNode(self, robot_pose, resolution, nodes): # The next target in pixels tinit = time.time() next_target = [0, 0] w_dist = 10000 x_g, y_g = 500, 500 sigma = 100 g_robot_pose = self.robot_perception.getGlobalCoordinates( [robot_pose['x_px'], robot_pose['y_px']]) # can I use that? for node in nodes: # print resolution self.path = self.path_planning.createPath( g_robot_pose, node, resolution) # can I use that? if len(self.path) == 0: break x_n, y_n = node[0], node[1] exp = ((x_n - x_g)**2 + (y_n - y_g)**2) / (2 * (sigma**2)) scale_factor = 1 / (1 - math.exp(-exp) + 0.01) coords = zip( *self.path) # dist is [[x1 x2 x3..],[y1 y2 y3 ..]] #transpose coord_shift = np.empty_like(coords) coord_shift[0] = coords[0][-1:] + coords[0][:-1] # (xpi+1) coord_shift[1] = coords[1][-1:] + coords[1][:-1] # (ypi+1) coords = np.asarray(coords) dist = [ (a - b)**2 for a, b in zip(coords[:, 1:], coord_shift[:, 1:]) ] # (xpi - xpi+1)^2 , (ypi-ypi+1)^2 dist = sum(np.sqrt(dist[0] + dist[1])) w = dist * (1 / scale_factor) if w < w_dist and len(self.path) != 0: if self.prev_target == node: break w_dist = w next_target = node self.prev_target = next_target # dont select the same target it we failed already Print.art_print( "Select nearest node target time: " + str(time.time() - tinit), Print.ORANGE) return next_target