def checkNearbyObstacleCover(self, pose): xx = pose[0] yy = pose[1] th_deg = pose[2] return_obstacles = True indexes = [] for s in range(self.sensor_number): cover_length = int(self.sensor_range[s] / self.resolution) if self.sensor_shape[s] == 'circular': temp = Cffi.circularRayCastCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s], return_obstacles) indexes.extend(temp) else: rospy.loginfo("Error!Sensor's shape not found!") return indexes = list(set(indexes)) return indexes
def checkAndUpdateCover(self, brushfire, pose = None, threshold = 1.): if pose == None: self.readPose = True self.readRobotPose() while self.readPose: pass xx = int(self.robot_pose['x']) yy = int(self.robot_pose['y']) th = self.robot_pose['th'] th_deg = math.degrees(th) else: xx = pose[0] yy = pose[1] th_deg = pose[2] indexes = [] for s in range(self.sensor_number): cover_length = int(self.sensor_range[s] / self.resolution) if self.sensor_shape[s] == 'rectangular': temp = Cffi.rectangularBrushfireCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s]) indexes.extend(temp) elif self.sensor_shape[s] == 'circular': temp = Cffi.circularRayCastCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s]) indexes.extend(temp) else: rospy.loginfo("Error!Sensor's shape not found!") return covered_area = self.coverage[zip(*indexes)] brushfire_area = brushfire[zip(*indexes)] near_obstacles_len = len(np.where(brushfire_area == 2)[0]) if near_obstacles_len == 0: return False old_obstacles_len = len(np.where((brushfire_area == 2) & (covered_area > 80))[0]) perc = old_obstacles_len / near_obstacles_len if perc < threshold: for x,y in indexes: self.coverage[x, y] = 100 * self.sensor_reliability[0] i = int(x + self.ogm_width * y) self.coverage_ogm.data[i] = 100 updated = True else: updated = False return updated
class Map_To_Graph: def __init__(self): self.brushfire_cffi = Cffi() self.topology = Topology() self.routing = Routing() self.coverage = Coverage() # Origin is the translation between the (0,0) of the robot pose and the # (0,0) of the map self.origin = {} self.origin['x'] = 0 self.origin['y'] = 0 self.resolution = 0.05 self.step = 0 # Load map's translation if rospy.has_param('origin'): translation = rospy.get_param('origin') self.origin['x'] = translation[0] self.origin['y'] = translation[1] if rospy.has_param('resolution'): self.resolution = rospy.get_param('resolution') # Initilize sensor's specs self.sensor_names = [] self.sensor_direction = [] self.sensor_fov = [] self.sensor_range = [] self.sensor_shape = [] self.sensor_reliability = [] self.min_distance = 0 self.navigation_target = 0 # Read sensor's specs self.robot_radius = rospy.get_param('robot_radius') self.sensor_names = rospy.get_param('sensor_names') self.sensor_number = len(self.sensor_names) self.navigation_target = rospy.get_param('navigation_target') self.navigation_pattern = rospy.get_param('navigation_pattern') self.fixed_step = rospy.get_param('fixed_step') for name in self.sensor_names: self.sensor_direction.append( rospy.get_param(name + '/sensor_direction')) self.sensor_fov.append(rospy.get_param(name + '/fov')) self.sensor_range.append(rospy.get_param(name + '/range')) self.sensor_shape.append(rospy.get_param(name + '/shape')) self.sensor_reliability.append( rospy.get_param(name + '/reliability')) self.ogm_topic = '/map' self.ogm = 0 self.ogm_raw = 0 self.ogm_width = 0 self.ogm_height = 0 self.ogm_header = 0 # Flag to wait ogm subscriber to finish self.ogm_compute = False self.gvd = 0 self.brush = 0 self.nodes = [] self.door_nodes = [] self.rooms = [] self.room_doors = [] self.room_type = [] self.room_sequence = [] self.entering_doors = {} self.exiting_doors = {} self.wall_follow_nodes = [] self.wall_follow_sequence = [] self.zig_zag_nodes = [] self.zig_zag_sequence = [] self.simple_nodes = [] self.simple_sequence = [] # Load nodes from json file map_name = rospy.get_param('map_name') filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json' with open(filename, 'r') as read_file: self.data = json.load(read_file) self.nodes = self.data['nodes'] self.door_nodes = self.data['doors'] self.rooms = self.data['rooms'] self.room_doors = self.data['roomDoors'] self.room_type = self.data['roomType'] if 'room_sequence' in self.data: self.room_sequence = self.data['room_sequence'] if 'entering_doors' in self.data: self.entering_doors = { int(k): v for k, v in self.data['entering_doors'].items() } if 'exiting_doors' in self.data: self.exiting_doors = { int(k): v for k, v in self.data['exiting_doors'].items() } if 'wall_follow_nodes' in self.data: self.wall_follow_nodes = self.data['wall_follow_nodes'] if 'wall_follow_sequence' in self.data: self.wall_follow_sequence = self.data['wall_follow_sequence'] if 'zig_zag_nodes' in self.data: self.zig_zag_nodes = self.data['zig_zag_nodes'] if 'zig_zag_sequence' in self.data: self.zig_zag_sequence = self.data['zig_zag_sequence'] if 'simple_nodes' in self.data: self.simple_nodes = self.data['simple_nodes'] if 'simple_sequence' in self.data: self.simple_sequence = self.data['simple_sequence'] self.node_publisher = rospy.Publisher('/nodes', Marker, queue_size=100) self.candidate_door_node_pub = rospy.Publisher('/nodes/candidateDoors', Marker, queue_size=100) self.door_node_pub = rospy.Publisher('/nodes/doors', Marker, queue_size=100) self.room_node_pub = rospy.Publisher('/nodes/rooms', Marker, queue_size=100) def server_start(self): rospy.init_node('map_to_graph') rospy.loginfo('map_to_graph node initialized.') # Read ogm self.ogm_compute = True rospy.Subscriber(self.ogm_topic, OccupancyGrid, self.read_ogm) while self.ogm_compute: pass # Calculate brushfire field self.brush = self.brushfire_cffi.obstacleBrushfireCffi(self.ogm) # Calculate gvd from brushfire and ogm self.gvd = self.topology.gvd(self.brush) time.sleep(1) # self.print_markers(self.nodes, [1., 0., 0.], self.node_publisher) # self.print_markers(self.door_nodes, [0., 0., 1.], self.door_node_pub) # Find sequence of rooms if not already exists if self.room_sequence == []: self.find_room_sequence(True) if self.navigation_pattern == 'simple': self.find_all_wall_nodes() rospy.loginfo("Visualize node sequence") self.visualise_node_sequence(self.simple_sequence) self.simple_sequence, self.simple_nodes = self.a_priori_coverage( self.simple_sequence, False) # Compute length of sequence i = 0 for temp_nodes in self.simple_nodes: distances = cdist(np.array(temp_nodes), np.array(temp_nodes), 'euclidean') cost = self.routing.route_length(distances, range(len(temp_nodes))) string = 'Length of zig zag nodes at room ' + str( i) + ': ' + str(cost) print(string) i += 1 else: self.find_best_path_wall_nodes() self.visualise_node_sequence(self.wall_follow_sequence) self.wall_follow_sequence, self.wall_follow_nodes = self.a_priori_coverage( self.wall_follow_sequence) # Save wall nodes to json self.data['wall_follow_nodes'] = self.wall_follow_nodes self.data['wall_follow_sequence'] = self.wall_follow_sequence rospy.loginfo("Visualize node sequence") self.visualise_node_sequence(self.wall_follow_sequence) # Compute length of sequence i = 0 for temp_nodes in self.wall_follow_nodes: distances = cdist(np.array(temp_nodes), np.array(temp_nodes), 'euclidean') cost = self.routing.route_length(distances, range(len(temp_nodes))) string = 'Length of wall follow nodes at room ' + str( i) + ': ' + str(cost) print(string) i += 1 map_name = rospy.get_param('map_name') filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json' with open(filename, 'w') as outfile: data_to_json = json.dump(self.data, outfile) print("5 secs sleep with wall_follow_sequence") rospy.sleep(5) self.zig_zag_sequence, self.zig_zag_nodes = self.add_zig_zag_nodes( self.wall_follow_sequence) self.zig_zag_sequence, self.zig_zag_nodes = self.a_priori_coverage( self.zig_zag_sequence, False) # Save zig zag nodes to json self.data['zig_zag_nodes'] = self.zig_zag_nodes self.data['zig_zag_sequence'] = self.zig_zag_sequence rospy.loginfo("Visualize zig zag node sequence") self.visualise_node_sequence(self.zig_zag_sequence) # Compute length of sequence i = 0 for temp_nodes in self.zig_zag_nodes: distances = cdist(np.array(temp_nodes), np.array(temp_nodes), 'euclidean') cost = self.routing.route_length(distances, range(len(temp_nodes))) string = 'Length of zig zag nodes at room ' + str( i) + ': ' + str(cost) print(string) i += 1 map_name = rospy.get_param('map_name') filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json' with open(filename, 'w') as outfile: data_to_json = json.dump(self.data, outfile) return # Method to visualize sequence of nodes splited into rooms with different color def visualise_node_sequence(self, rooms): id = 0 for nodes in rooms: rgb = [0, 0, 0] i = 0 for node in nodes: rgb[0] = i / len(nodes) rgb[1] = min(2 * i, len(nodes)) / len(nodes) rgb[2] = min(3 * i, len(nodes)) / len(nodes) self.print_markers([node['position']], rgb, self.room_node_pub, id) rospy.sleep(0.01) i += 1 id += 1 print("Printed entire room") return # Method to visualize room nodes splited into segments with different color in each one def visualise_node_segments(self, room, id=0): color_num = 0 color = [1., 0., 0.] # threshold = 256/len(splited_node_route) for segment in room: self.print_markers(segment, np.array(color_num) * color, self.room_node_pub, id) id += 1 color_num += 30 / 256 if color_num > 1: color = (color[1:] + color[:1]) color_num -= 1 rospy.sleep(0.1) return id # Do uniform sampling across whole map and gather points near obstacles def uniform_sampling(self): # Uniform sampling on map nodes = [] # Sampling step is half the sensor's range min_range = int(min(self.sensor_range) / self.resolution) max_range = int(max(self.sensor_range) / self.resolution) step = int(1.5 * self.robot_radius / self.resolution) step_list = [] while step <= max_range: temp_nodes = [] step_list.append(step) indx = np.where((self.brush > step) & (self.brush <= 2 * step) & (self.brush <= max_range)) indx = zip(*indx) for x, y in indx: if not x % step and not y % step: temp_nodes.append((x, y)) step *= 2 nodes.append(temp_nodes) obstacles = np.zeros((self.ogm_width, self.ogm_height)) final_nodes = [] i = len(nodes) - 1 while i >= 0: temp_nodes = nodes[i] # self.print_markers(temp_nodes, [1., 0., 0.], self.node_publisher) # rospy.sleep(2) # Check if new nodes override previous ones and delete them for point in temp_nodes: x, y = point indexes = self.brushfire_cffi.circularRayCastCoverageCffi( point, self.ogm, max_range, 360, 0, 0, True) if not len(indexes): continue if i == len(nodes) - 1: obstacles[zip(*indexes)] = 1 final_nodes.append(point) else: temp = obstacles[zip(*indexes)] p = len(np.where(temp > 0)[0]) / len(indexes) if p < 0.9: final_nodes.append((x, y)) obstacles[zip(*indexes)] = 1 i -= 1 return final_nodes, step_list # Find sequence of rooms def find_room_sequence(self, save_result): rospy.loginfo("Finding room sequence...") self.room_sequence = [] # Create graph graph = Graph() for room in self.room_doors: if len(room) > 1: for i in range(len(room)): for j in range(i + 1, len(room)): node_id_1 = self.door_nodes.index(room[i]) node_id_2 = self.door_nodes.index(room[j]) # Find brushfire distance (closest to real dist) between doors dist = self.brushfire_cffi.pointToPointBrushfireCffi(tuple(room[i]), \ tuple(room[j]), self.ogm) if dist < 0: rospy.loginfo("Error computing door distance!") return graph.add_edge(node_id_1, node_id_2, dist) graph.add_edge(node_id_2, node_id_1, dist) # Calculate graph's distances between all nodes doors_length = len(self.door_nodes) if doors_length > 2: distances = np.zeros((doors_length, doors_length), dtype=np.float64) for i in range(doors_length): for j in range(i + 1, doors_length): _, _, _, dist = find_path(graph, i, j) distances[i][j] = dist distances[j][i] = distances[i][j] # Call hill climb algorithm max_iterations = 500 * doors_length # door_route, cost, iter = self.routing.hillclimb(distances, max_iterations) door_route, cost, iter = self.routing.random_restart_hillclimb( distances, max_iterations) print('Optimal RRHC room sequence', door_route, self.routing.route_length(distances, door_route)) # door_route, cost, iter = self.routing.anneal(distances, max_iterations, 1.0, 0.95) # print('Optimal ANNEAL room sequence', door_route, self.routing.route_length(distances, door_route)) #Sub-optimal / greedy sequence greedy_route = [] greedy_route.append(door_route[0]) for i in range(1, doors_length): #find min previous = greedy_route[i - 1] unvisited = [] for j in range(doors_length): if j not in greedy_route: unvisited.append(j) minimum = distances[previous, unvisited[0]] min_idx = unvisited[0] for idx in unvisited: if distances[previous][idx] < minimum: minimum = distances[previous][idx] min_idx = idx greedy_route.append(min_idx) greedy_cost = self.routing.route_length(distances, greedy_route) print('Greedy room sequence', greedy_route, greedy_cost) elif doors_length == 2: door_route = [0, 1] _, _, _, dist = find_path(graph, 0, 1) print('Only 2 doors with distance ', dist) elif self.door_nodes != []: door_route = [0] print('One door found!') else: door_route = [] self.room_sequence = [0] print('One room only!') # Correspond door route to room route for door in door_route: for i in range(len(self.room_doors)): if self.door_nodes[door] in self.room_doors[ i] and i not in self.room_sequence: self.room_sequence.append(i) enter = {} leave = {} if doors_length >= 2: # Compute enter & exit doors of each room according to found room_sequence room_i = 1 # Start from 2nd room room_idx = self.room_sequence[room_i] entered = False # Simulate door/room sequence on the graph for i in range(len(door_route)): # Find path from current to next door door_idx = door_route[i] next_door_idx = door_route[(i + 1) % len(door_route)] # print('Room', room_idx) path = find_path(graph, door_idx, next_door_idx) # print('Path',path.nodes) if entered and len( path.nodes ) > 1: # if already inside room, first node of path is already used ii = 1 else: ii = 0 # Transverse through path and check for corresponding rooms while ii < len(path.nodes): node_idx = path.nodes[ii] door = self.door_nodes[node_idx] if door in self.room_doors[room_idx]: # print('Door',node_idx, door,'in room', room_idx) if not entered: enter[room_idx] = door entered = not entered if len(self.room_doors[room_idx]) == 1: ii -= 1 # To revisit this door else: leave[room_idx] = door entered = not entered room_i += 1 room_idx = self.room_sequence[room_i % len( self.room_sequence)] # print('Room changed', room_idx) ii -= 1 # To revisit this door ii += 1 elif self.door_nodes != []: for i in range(len(self.room_sequence)): enter[i] = self.door_nodes[0] leave[i] = self.door_nodes[0] self.entering_doors = enter self.exiting_doors = leave # # Print results per room (blue - entering door, read - exiting door, green - room nodes) # for room_idx in self.room_sequence: # print('Room',room_idx, 'all doors',self.room_doors[room_idx]) # self.print_markers([enter[room_idx]], [0,0,1], self.node_publisher) # self.print_markers([leave[room_idx]], [1,0,0], self.door_node_pub) # self.print_markers(self.rooms[room_idx], [0,1,0], self.room_node_pub) # rospy.sleep(4) if save_result: # Save room sequence self.data['room_sequence'] = self.room_sequence self.data['entering_doors'] = self.entering_doors self.data['exiting_doors'] = self.exiting_doors map_name = rospy.get_param('map_name') filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json' with open(filename, 'w') as outfile: data_to_json = json.dump(self.data, outfile) return # Find the yaw that maximizes the covered walls of a given node def find_best_yaw(self, node, next_node, loop_threshold, obstacle_weight, rotation_weight): yaw_between_nodes = math.degrees( math.atan2(next_node[1] - node[1], next_node[0] - node[0])) yaw = [] steps = int(max(self.sensor_range) / self.resolution) # Find reachable obstacles obstacles = self.brushfire_cffi.inRangeObstacleBrushfireCffi( node, self.ogm, steps, True) if len(obstacles) == 0: return yaw # In case of many obstacles more poses are needed for full coverage found = 0 while found < loop_threshold * len(obstacles) and len(obstacles): best_evaluation = 0 best_yaw = 0 best_found = 0 best_covered_obstacles = [] for angle in range(-180, 180, 10): pose = [node[0], node[1], angle] all_covered_obstacles = self.coverage.checkNearbyObstacleCover( pose) covered_obstacles = [] for ob in all_covered_obstacles: if ob in obstacles: covered_obstacles.append(ob) if not len(covered_obstacles): continue # Absolute yaw of current and next node in [0,180] next_rotation = np.abs(angle - yaw_between_nodes) while next_rotation >= 360: next_rotation -= 360 if next_rotation > 180: next_rotation = 360 - next_rotation # Evaluate candidate angle # Use Motor Schema with covered area and rotation to next node as parameters evaluation = obstacle_weight * len(covered_obstacles) / len( obstacles) + rotation_weight * (180 - next_rotation) / 180 if evaluation > best_evaluation: best_evaluation = evaluation best_yaw = angle best_found = len(covered_obstacles) best_covered_obstacles = covered_obstacles found += best_found # Check if obstacle is coverable if best_evaluation: yaw.append(best_yaw) else: break # Speed up process bypassing deletions if all obstacles are covered if found >= loop_threshold * len(obstacles): break else: # Discard checked obstacles for ob in best_covered_obstacles: if ob in obstacles: obstacles.remove(ob) yaw = list(set(yaw)) return yaw # Find the yaw that maximizes the covered walls of a given node def find_yaw(self, node, next_node, obstacle_weight, rotation_weight): yaw_between_nodes = math.degrees( math.atan2(next_node[1] - node[1], next_node[0] - node[0])) yaw = [] steps = int(max(self.sensor_range) / self.resolution) node_brush = self.brush[node] steps = max(steps, node_brush) # Find reachable obstacles obstacles = self.brushfire_cffi.inRangeObstacleBrushfireCffi( node, self.ogm, steps, True) if len(obstacles) == 0: return yaw # In case of many obstacles more poses are needed for full coverage found = 0 best_evaluation = 0 best_yaw = 0 for angle in range(-180, 180, 10): pose = [node[0], node[1], angle] all_covered_obstacles = self.coverage.checkNearbyObstacleCover( pose) covered_obstacles = 0 for ob in all_covered_obstacles: if ob in obstacles: covered_obstacles += 1 if not covered_obstacles: continue # Absolute yaw of current and next node in [0,180] next_rotation = np.abs(angle - yaw_between_nodes) while next_rotation >= 360: next_rotation -= 360 if next_rotation > 180: next_rotation = 360 - next_rotation # Evaluate candidate angle # Use Motor Schema with covered area and rotation to next node as parameters evaluation = obstacle_weight * covered_obstacles / len( obstacles) + rotation_weight * (180 - next_rotation) / 180 if evaluation > best_evaluation: best_evaluation = evaluation best_yaw = angle # Check if obstacle is coverable if best_evaluation: yaw.append(best_yaw) return yaw def find_weights(self): if self.navigation_target == 'cover_first': angles = [] for s in range(self.sensor_number): theta = self.sensor_direction[s] while theta < 0: theta += 360 while theta >= 360: theta -= 360 if theta > 180: theta = 360 - theta if theta > 90: theta = 180 - theta angles.append((90 - theta) / 90) obstacle_weight = 1 + np.mean(angles) rotation_weight = 2 - np.mean(angles) elif self.navigation_target == 'fast_first': angles = [] for s in range(len(self.sensor_number)): theta = self.sensor_direction[s] fov = self.sensor_fov[s] / 2 while theta < 0: theta += 360 while theta >= 360: theta -= 360 if theta > 180: theta = 360 - theta if theta > 90: theta = 180 - theta theta = min(theta + fov, 90) angles.append((90 - theta) / 90) obstacle_weight = 1 + np.mean(angles) rotation_weight = 2 - np.mean(angles) else: obstacle_weight = 1 rotation_weight = 1 return obstacle_weight, rotation_weight # Add zig zag nodes inside already computed node sequence def add_zig_zag_nodes(self, wall_follow_sequence): zig_zag_nodes = [] zig_zag_sequence = [] filled_ogm = self.topology.doorClosure(self.door_nodes, self.ogm, self.brushfire_cffi) j = 0 for room in wall_follow_sequence: temp_nodes = [] temp_sequence = [] room_brush = self.brushfire_cffi.pointBrushfireCffi( self.rooms[j], filled_ogm) for i in range(len(room)): if not i: temp_nodes.append(room[i]['position']) temp_sequence.append(room[i]) continue node = room[i]['position'] previous_node = room[i - 1]['position'] x_final, y_final = 0, 0 if previous_node[0] == node[0] and np.abs( previous_node[1] - node[1]) and np.abs(previous_node[1] - node[1]) <= max( self.sensor_range) / self.resolution: dif = np.abs(previous_node[1] - node[1]) x1 = int(node[0] - dif / 2) x2 = int(node[0] + dif / 2) y = int(np.min([previous_node[1], node[1]]) + dif / 2) if self.brush[x1, y] >= self.brush[x2, y]: x_final = x1 y_final = y yaw = math.degrees( math.atan2(y - node[1], x1 - node[0])) else: x_final = x2 y_final = y yaw = math.degrees( math.atan2(y - node[1], x2 - node[0])) elif previous_node[1] == node[1] and np.abs( previous_node[0] - node[0]) and np.abs(previous_node[0] - node[0]) <= max( self.sensor_range) / self.resolution: dif = np.abs(previous_node[0] - node[0]) y1 = int(node[1] - dif / 2) y2 = int(node[1] + dif / 2) x = int(np.min([previous_node[0], node[0]]) + dif / 2) if self.brush[x, y1] >= self.brush[x, y2]: x_final = x y_final = y1 yaw = math.degrees( math.atan2(y1 - node[1], x - node[0])) else: x_final = x y_final = y2 yaw = math.degrees( math.atan2(y2 - node[1], x - node[0])) if room_brush[x_final][y_final] > 1: temp_nodes.append((x_final, y_final)) temp_sequence.append({ 'position': (x_final, y_final), 'yaw': yaw }) temp_nodes.append(room[i]['position']) temp_sequence.append(room[i]) zig_zag_nodes.append(temp_nodes) zig_zag_sequence.append(temp_sequence) j += 1 return zig_zag_sequence, zig_zag_nodes # Find nodes for wall following coverage def find_all_wall_nodes(self): rospy.loginfo("Finding simple strategy nodes...") self.simple_nodes = [] self.simple_sequence = [] obstacle_weight, rotation_weight = 1, 1 # Uniform sampling on map nodes = [] step = int(self.fixed_step / self.resolution) indx = np.where((self.brush > step) & (self.brush <= 2 * step)) indx = zip(*indx) for x, y in indx: if not x % step and not y % step: nodes.append((x, y)) self.print_markers(nodes, [1., 0., 0.], self.node_publisher) rospy.sleep(2) # Find rooms of nodes # Fill door holes with "wall" filled_ogm = self.topology.doorClosure(self.door_nodes, self.ogm, self.brushfire_cffi) id = 0 for i in range(len(self.rooms)): # Brushfire inside each room and gather brushed wall_follow_nodes found_nodes = [] brush = self.brushfire_cffi.pointBrushfireCffi( self.rooms[i], filled_ogm) for n in nodes: if brush[n] > 0: found_nodes.append(n) found_nodes.sort() # Optimize path finding print("Found {} nodes in room {}.".format(len(found_nodes), i)) # # DEBUG nodes_length = len(found_nodes) if nodes_length: distances = cdist(np.array(found_nodes), np.array(found_nodes), 'euclidean') print('Distances of nodes done.') # DEBUG: found_nodes.insert(0, self.entering_doors[i]) found_nodes.append(self.exiting_doors[i]) # Call RR hillclimb to optimize path max_iterations = 2000 * len(found_nodes) distances = cdist(np.array(found_nodes), np.array(found_nodes), 'euclidean') cost = self.routing.route_length(distances, range(len(found_nodes))) print('Route length after adding doors', cost) # node_route, cost, iter = self.routing.hillclimb(distances, max_iterations) fixed_edges = True node_route, cost, iter = self.routing.random_restart_hillclimb( distances, max_iterations, fixed_edges) print('Second HC route length', cost, 'made iters', iter, 'from', max_iterations) found_nodes_with_yaw = [] k = 1 # DEBUG: for n in range(len(found_nodes)): print('Closest obstacles process: {}/{}'.format( k, nodes_length)) # Find closest obstacle to get best yaw x, y = found_nodes[node_route[n]] x2, y2 = found_nodes[node_route[(n + 1) % len(node_route)]] yaw = self.find_yaw((x, y), (x2, y2), obstacle_weight, rotation_weight) if yaw == []: continue for point in yaw: temp_dict = {'position': (x, y), 'yaw': point} found_nodes_with_yaw.append(temp_dict) k += 1 self.simple_nodes.append(found_nodes) self.simple_sequence.append(found_nodes_with_yaw) # Save wall nodes to json self.data['simple_nodes'] = self.simple_nodes self.data['simple_sequence'] = self.simple_sequence map_name = rospy.get_param('map_name') filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json' with open(filename, 'w') as outfile: data_to_json = json.dump(self.data, outfile) return # Find nodes for wall following coverage with optimal path def find_best_path_wall_nodes(self): rospy.loginfo("Finding wall follow nodes...") self.wall_follow_nodes = [] self.wall_follow_sequence = [] loop_threshold = 0.6 obstacle_weight, rotation_weight = self.find_weights() # Uniform sampling on map nodes, self.step = self.uniform_sampling() # self.print_markers(nodes, [1., 0., 0.], self.node_publisher) rospy.sleep(1) # Find rooms of nodes # Fill door holes with "wall" filled_ogm = self.topology.doorClosure(self.door_nodes, self.ogm, self.brushfire_cffi) id = 0 for i in range(0, len(self.rooms)): # Brushfire inside each room and gather brushed wall_follow_nodes found_nodes = [] brush = self.brushfire_cffi.pointBrushfireCffi( self.rooms[i], filled_ogm) for n in nodes: if brush[n] > 0: found_nodes.append(n) found_nodes.sort() # Optimize path finding print("Found {} nodes in room {}.".format(len(found_nodes), i)) # # DEBUG if len(found_nodes) == 0: print("ERROR!") continue nodes_length = len(found_nodes) distances = cdist(np.array(found_nodes), np.array(found_nodes), 'euclidean') print('Distances of nodes done.') # DEBUG: cost = self.routing.route_length(distances, range(nodes_length)) print('Sorted route length', cost) # Call hill climb algorithm max_iterations = 150 * nodes_length node_route, cost, iter = self.routing.step_hillclimb( distances, max_iterations, self.step) print('Route of wall follow nodes found.') print('First step HC route length', cost) # Reorder according to step HC results temp_route = [] for n in node_route: temp_route.append(found_nodes[n]) # Add entering & exiting doors and rotate already found route # so that 2nd point is the closest one to the entering door first_route = [] enter = self.entering_doors[i] first_route.append(enter) closest_idx = cdist(np.array([enter]), np.array(temp_route)).argmin() for ii in range(len(temp_route)): first_route.append(temp_route[(ii + closest_idx) % nodes_length]) first_route.append(self.exiting_doors[i]) # Do another hillclimb to optimize path max_iterations = 2000 * len(first_route) distances = cdist(np.array(first_route), np.array(first_route), 'euclidean') cost = self.routing.route_length(distances, range(len(first_route))) print('Route length after adding doors and rotating sequence', cost) fixed_edges = True node_route, cost, iter = self.routing.random_restart_hillclimb( distances, max_iterations, fixed_edges) print('Second HC route length', cost, 'made iters', iter, 'from', max_iterations) final_route = [] for n in node_route: final_route.append(first_route[n]) found_nodes_with_yaw = [] k = 1 # DEBUG: for n in range(len(final_route)): # print('Closest obstacles process: {}/{}'.format(k, nodes_length)) # Find closest obstacle to get best yaw x, y = final_route[n] x2, y2 = final_route[(n + 1) % len(final_route)] # Find cover_first poses yaw = self.find_best_yaw((x, y), (x2, y2), loop_threshold, obstacle_weight, rotation_weight) if yaw != []: for point in yaw: temp_dict = {'position': (x, y), 'yaw': point} found_nodes_with_yaw.append(temp_dict) k += 1 self.wall_follow_nodes.append(found_nodes) self.wall_follow_sequence.append(found_nodes_with_yaw) # Save wall nodes to json self.data['wall_follow_best_yaw_weigths'] = { 'loop_threshold': loop_threshold, 'obstacle_weight': obstacle_weight, 'rotation_weight': rotation_weight } map_name = rospy.get_param('map_name') filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json' with open(filename, 'w') as outfile: data_to_json = json.dump(self.data, outfile) return # Do an a priori coverage with found order of nodes to eliminate the not needed def a_priori_coverage(self, nodes, eliminate=True): self.coverage.initCoverage() new_sequence = [] new_nodes = [] for room in nodes: # i = 0 new_room = [] new_room_nodes = [] for i in range(len(room)): node = room[i] x, y = node['position'] yaw = node['yaw'] if not i or i == len(room) - 1 or not eliminate: self.coverage.updateCover([x, y, yaw], publish=False, track_specs=False) updated = True else: updated = self.coverage.checkAndUpdateCover( self.brush, [x, y, yaw], 0.85) if updated: new_room.append(node) new_room_nodes.append(node['position']) self.coverage.coverage_pub.publish(self.coverage.coverage_ogm) new_sequence.append(new_room) new_nodes.append(new_room_nodes) rospy.loginfo( "A-priori coverage done at room {0}, room nodes size changed from {1} to {2}" .format(i, len(room), len(new_room))) i += 1 near_obstacles = np.where(self.brush == 2) near_obstacles_cover = self.coverage.coverage[near_obstacles] covered_obstacles = len(np.where(near_obstacles_cover >= 80)[0]) rospy.loginfo("Estimated coverage percentage {}".format( covered_obstacles / len(near_obstacles_cover))) return new_sequence, new_nodes def read_ogm(self, data): # OGM is a 2D array of size width x height # The values are from 0 to 100 # 0 is an unoccupied pixel # 100 is an occupied pixel # 50 or -1 is the unknown self.ogm_raw = np.array(data.data) self.ogm_width = data.info.width self.ogm_height = data.info.height self.ogm_header = data.header self.ogm = np.zeros((data.info.width, data.info.height), \ dtype = np.int) # Reshape ogm to a 2D array for x in range(0, data.info.width): for y in range(0, data.info.height): self.ogm[x][y] = data.data[x + data.info.width * y] # Initilize coverage OGM with same size width x height self.coverage.coverage = np.zeros((self.ogm_width, self.ogm_height)) self.coverage.coverage_ogm.info = data.info self.coverage.coverage_ogm.data = np.zeros(self.ogm_width * self.ogm_height) self.coverage.ogm = self.ogm self.coverage.ogm_raw = self.ogm_raw self.coverage.ogm_width = self.ogm_width self.coverage.ogm_height = self.ogm_height self.coverage.ogm_header = self.ogm_header self.ogm_compute = False return # Method to publish points to rviz def print_markers(self, nodes, color, publisher, id=0): points = [] for point in nodes: p = Point() p.x = point[0] * self.resolution + self.origin['x'] p.y = point[1] * self.resolution + self.origin['y'] p.z = 0 points.append(p) # Create Marker for nodes marker = Marker() marker.header.frame_id = "/map" marker.id = id marker.type = marker.POINTS marker.action = marker.ADD marker.points = points marker.pose.orientation.w = 1.0 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] # rospy.loginfo("Printing nodes!") publisher.publish(marker) return
def updateCover(self, pose = None, publish = True, track_specs = True): if pose == None: self.readPose = True self.readRobotPose() while self.readPose: pass xx = int(self.robot_pose['x']) yy = int(self.robot_pose['y']) th = self.robot_pose['th'] th_deg = math.degrees(th) else: xx = pose[0] yy = pose[1] th_deg = pose[2] # Check if robot moved move = ((xx - self.previous_robot_pose['x'])**2 + \ (yy - self.previous_robot_pose['y'])**2 + \ (th_deg - self.previous_robot_pose['th'])**2)**(1/2) # Update coverage only if robot moved if move > 1.0: for s in range(self.sensor_number): cover_length = int(self.sensor_range[s] / self.resolution) if self.sensor_shape[s] == 'rectangular': indexes = Cffi.rectangularBrushfireCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s]) for x,y in indexes: self.coverage[x, y] = 100 * self.sensor_reliability[s] i = int(x + self.ogm_width * y) self.coverage_ogm.data[i] = 100 if track_specs: self.coverage_number_ogm.data[i] += 1 self.coverage_number[x, y] += 1 elif self.sensor_shape[s] == 'circular': indexes = Cffi.circularRayCastCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s]) for x,y in indexes: self.coverage[x, y] = 100 * self.sensor_reliability[s] i = int(x + self.ogm_width * y) self.coverage_ogm.data[i] = 100 if track_specs: self.coverage_number[x, y] += 1 if self.coverage_number[x, y] < 100: self.coverage_number_ogm.data[i] += 1 yaw_between_nodes = math.degrees(math.atan2(yy-y, xx-x)) angle = yaw_between_nodes + th_deg + self.sensor_direction[s] # print('th', th_deg, 'dir', self.sensor_direction[s], 'yaw', yaw_between_nodes, 'angle', angle) if angle > 180: angle -= 360 if angle <= -180: angle += 360 for a in range(len(self.bins)): if angle >= self.bins[a][0] and angle < self.bins[a][1]: self.coverage_angles[x][y][a] += 1 ii = int(x + self.ogm_width * y + self.ogm_height * self.ogm_width * a) self.coverage_angles_ogm.data[ii] += 1 # print(self.bins[a], a, angle) break else: rospy.loginfo("Error!Sensor's shape not found!") return if publish: self.coverage_pub.publish(self.coverage_ogm) if track_specs: self.coverage_number_pub.publish(self.coverage_number_ogm) max_val = np.max(self.coverage_number) rospy.loginfo("Maximum number of coverage look ups of points is {}.".format(max_val)) self.coverage_angles_pub.publish(self.coverage_angles_ogm) rospy.loginfo("Update coverage ogm!") self.previous_robot_pose['x'] = xx self.previous_robot_pose['y'] = yy self.previous_robot_pose['th'] = th_deg return