def skeletonizationCffi(ogm, origin, resolution, ogml): skeleton = (ogm < 49).astype("int32") skeleton = Cffi.thinning(skeleton, ogml) skeleton = Cffi.prune(skeleton, ogml, 10) print_viz(skeleton, resolution, origin['x'], origin['y']) return skeleton
def skeletonizationCffi(ogm, origin, resolution, ogml): local = numpy.zeros(ogm.shape) local = set_ogm(ogm, local, ogm.shape[0], ogm.shape[1]) #start = time() skeleton = Cffi.thinning(local, ogml) #print str('SkeletonizationCffi: Thinning in ') + str(time() - start) + str(' seconds.') #start = time() skeleton = Cffi.prune(skeleton, ogml, 10) #print str('SkeletonizationCffi: Prunning in ') + str(time() - start) + str(' seconds.') #start = time() ''' viz = set_skeleton_viz(skeleton, numpy.zeros((set_skeleton_viz_number(skeleton, ogm.shape[0], ogm.shape[1]), 2), dtype=numpy.float64), ogm.shape[0], ogm.shape[1], resolution, origin['x'], origin['y']) RvizHandler.printMarker(viz, 1, 0, "map", "art_skeletonization_cffi", [0.5, 0, 0, 0.5], 0.05) ''' ''' viz = [] for i in range(0, ogm.shape[0]): for j in range(0, ogm.shape[1]): if skeleton[i][j] == 1: viz.append([i * resolution + origin['x'],j * resolution + origin['y']]) print str('SkeletonizationCffi: Visualization in ') + str(time() - start) + str(' seconds.') RvizHandler.printMarker(viz, 1, 0, "map", "art_skeletonization_cffi", [0.5, 0, 0, 0.5], 0.05) ''' return skeleton
def skeletonizationCffi(self, ogm, origin, resolution, ogml): width = ogm.shape[0] height = ogm.shape[1] local = numpy.zeros(ogm.shape) for i in range(0, width): for j in range(0, height): if ogm[i][j] < 49: local[i][j] = 1 skeleton = Cffi.thinning(local, ogml) skeleton = Cffi.prune(skeleton, ogml, 10) viz = [] for i in range(0, width): for j in range(0, height): if skeleton[i][j] == 1: viz.append([ i * resolution + origin['x'], j * resolution + origin['y'] ]) RvizHandler.printMarker(\ viz,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_skeletonization_cffi", # Namespace [0.5, 0, 0, 0.5], # Color RGBA 0.05 # Scale ) return skeleton
def skeletonizationCffi(self, ogm, origin, resolution, ogml): # width = ogm.shape[0] # height = ogm.shape[1] local = numpy.zeros(ogm.shape) local[ogm < 49] = 1 # for i in range(0, width): # for j in range(0, height): # if ogm[i][j] < 49: # local[i][j] = 1 skeleton = Cffi.thinning(local, ogml) skeleton = Cffi.prune(skeleton, ogml, 10) # viz = [] # for i in range(0, width): # for j in range(0, height): # if skeleton[i][j] == 1: # viz.append([i * resolution + origin['x'],j * resolution + origin['y']]) viz = (numpy.array(numpy.where(skeleton == 1)).T * resolution + [origin['x'], origin['y']]).tolist() RvizHandler.printMarker(\ viz,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_skeletonization_cffi", # Namespace [0.5, 0, 0, 0.5], # Color RGBA 0.05 # Scale ) return skeleton
def skeletonizationCffi(self, ogm, origin, resolution, ogml): width = ogm.shape[0] height = ogm.shape[1] # import time local = numpy.zeros(ogm.shape) # 10x faster local[numpy.where(ogm < 49)] = 1 # local2 = numpy.zeros(ogm.shape) # start = time.time() # for i in range(0, width): # for j in range(0, height): # if ogm[i][j] < 49: # local[i][j] = 1 # end = time.time() # print ("loop took:", end-start) # start = time.time() # local[numpy.where(ogm<49)] = 1 # end = time.time() # print ("numpy took:", end-start) skeleton = Cffi.thinning(local, ogml) skeleton = Cffi.prune(skeleton, ogml, 10) viz = [] for i in range(0, width): for j in range(0, height): if skeleton[i][j] == 1: viz.append([ i * resolution + origin['x'], j * resolution + origin['y'] ]) RvizHandler.printMarker(\ viz,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_skeletonization_cffi", # Namespace [0.5, 0, 0, 0.5], # Color RGBA 0.05 # Scale ) return skeleton
def 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
def obstaclesBrushfireCffi(self, ogm, ogm_limits): brush = numpy.full(ogm.shape, -1, dtype='int32') #TODO: improve for i in range(ogm_limits['min_x'], ogm_limits['max_x'] - 1): for j in range(ogm_limits['min_y'], ogm_limits['max_y'] - 1): if ogm[i][j] > 49 or ogm[i][j] == -1: brush[i][j] = 0 brush = Cffi.brushfireFromObstacles(ogm, brush, ogm_limits) return brush
def obstaclesBrushfireCffi(self, ogm, ogm_limits): start = time() brush = numpy.full(ogm.shape, -1) brush = brush_set(ogm, brush, ogm_limits['min_x'], ogm_limits['max_x'], ogm_limits['min_y'], ogm_limits['max_y']) brush = Cffi.brushfireFromObstacles(ogm, brush, ogm_limits) if self.debug: print str('Brushfire (Cffi): Set and Run Brushfire in ') + str( time() - start) + str(' seconds.') return brush
def obstaclesBrushfireCffi(self, ogm, ogm_limits): brush = numpy.full(ogm.shape, -1) width = ogm.shape[0] height = ogm.shape[1] for i in range(ogm_limits['min_x'], ogm_limits['max_x'] - 1): for j in range(ogm_limits['min_y'], ogm_limits['max_y'] - 1): if ogm[i][j] > 49 or ogm[i][j] == -1: brush[i][j] = 0 brush = Cffi.brushfireFromObstacles(ogm, brush, ogm_limits) return brush
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 obstaclesBrushfireCffi(self, ogm, ogm_limits): brush = numpy.full(ogm.shape, -1) # width = ogm.shape[0] # height = ogm.shape[1] brush[ogm_limits['min_x']:ogm_limits['max_x'] - 1, ogm_limits['min_y']:ogm_limits['max_y'] - 1]\ [ (ogm[ogm_limits['min_x']:ogm_limits['max_x'] - 1, ogm_limits['min_y']:ogm_limits['max_y'] - 1] > 49) |\ (ogm[ogm_limits['min_x']:ogm_limits['max_x'] - 1, ogm_limits['min_y']:ogm_limits['max_y'] - 1] == -1) \ ] = 0 # for i in range(ogm_limits['min_x'], ogm_limits['max_x'] - 1): # for j in range(ogm_limits['min_y'], ogm_limits['max_y'] - 1): # if ogm[i][j] > 49 or ogm[i][j] == -1: # brush[i][j] = 0 brush = Cffi.brushfireFromObstacles(ogm, brush, ogm_limits) return brush
def __init__(self): self.brushfire_cffi = Cffi() # 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 # Initilize sensor's specs self.sensor_names = [] self.sensor_direction = [] self.sensor_fov = [] self.sensor_range = [] self.sensor_shape = [] self.sensor_reliability = [] # Read sensor's specs self.sensor_names = rospy.get_param('sensor_names') self.sensor_number = len(self.sensor_names) 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')) # Compute bins to keep with which angle a sensor covers each point max_fov = max(self.sensor_fov)/2 # compute for one half (symmetric with sensor_direction) self.bins = [] # self.number_of_bins = 2 * int(math.ceil(max_fov/5)) # 5 degrees in each bin # for i in range(self.number_of_bins): # down = - self.number_of_bins/2 * 5 + i*5 # up = down + 5 # self.bins.append((down,up)) # Compute bins of negative and positive only self.number_of_bins = 2 self.bins.append((-max_fov, 0)) self.bins.append((0, max_fov)) # Load map's translation translation = rospy.get_param('origin') self.origin['x'] = translation[0] self.origin['y'] = translation[1] self.resolution = rospy.get_param('resolution') # OGM related attributes self.ogm_topic = '/map' self.ogm = 0 self.ogm_raw = 0 self.ogm_width = 0 self.ogm_height = 0 self.ogm_header = 0 # Flags to wait subscribers to finish reading data self.ogm_compute = True self.readPose = False # Holds the coverage information. This has the same size as the ogm # If a cell has the value of 0 it is uncovered # In the opposite case the cell's value will be 100 self.coverage = [] self.coverage_number = [] self.coverage_angles = [] self.coverage_diverge = [] self.brush = 0 # coverage_ogm will be published in the coverage_topic self.coverage_ogm = OccupancyGrid() self.coverage_ogm.header.frame_id = "map" self.coverage_topic = '/map/coverage' self.coverage_diverge_ogm = OccupancyGrid() self.coverage_diverge_ogm.header.frame_id = "map" self.coverage_diverge_topic = '/map/coverage_angles/diverge' self.coverage_angles_ogm = OccupancyGrid() self.coverage_angles_ogm.header.frame_id = "map" self.coverage_angles_topic = '/map/coverage_angles' self.coverage_diverge_pub = rospy.Publisher(self.coverage_diverge_topic, \ OccupancyGrid, queue_size = 1)
class Navigation: def __init__(self): # self.routing = Routing() self.brushfire_cffi = Cffi() self.topology = Topology() # 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 # Load map's translation translation = rospy.get_param('origin') self.origin['x'] = translation[0] self.origin['y'] = translation[1] 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 = [] # Read sensor's specs self.sensor_names = rospy.get_param('sensor_names') self.sensor_number = len(self.sensor_names) 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')) # Flag to wait ogm subscriber to finish self.ogm_compute = False self.current_pose = Pose() # OGM related attributes self.ogm_topic = '/map' self.ogm = 0 self.ogm_raw = 0 self.ogm_width = 0 self.ogm_height = 0 self.ogm_header = 0 self.gvd = 0 self.brush = 0 # Load path pattern for navigation self.navigation_pattern = rospy.get_param('navigation_pattern') # Attributes read from json file self.nodes = [] self.door_nodes = [] self.rooms = [] self.room_doors = [] self.room_type = [] self.room_sequence = [] self.wall_follow_nodes = [] self.wall_follow_sequence = [] self.zig_zag_nodes = [] self.zig_zag_sequence = [] self.simple_nodes = [] self.simple_sequence = [] self.data = [] # 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'] self.room_sequence = self.data['room_sequence'] 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.door_node_pub = rospy.Publisher('/nodes/doors', Marker, queue_size=100) self.room_node_pub = rospy.Publisher('/nodes/rooms', Marker, queue_size=100) self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback) self.move_base_client = actionlib.SimpleActionClient( 'move_base', MoveBaseAction) # Cb to read robot's pose def odom_callback(self, msg): self.current_pose = msg.pose.pose return def server_start(self): rospy.init_node('navigation') rospy.loginfo('navigation node initialized.') # Read ogm self.ogm_compute = True rospy.Subscriber(self.ogm_topic, OccupancyGrid, self.read_ogm) while self.ogm_compute: pass # Calculate current x,y in map's frame current_x = int((self.current_pose.position.x - self.origin['x']) / self.resolution) current_y = int((self.current_pose.position.y - self.origin['y']) / self.resolution) # Calculate brushfire field self.brush = self.brushfire_cffi.obstacleBrushfireCffi(self.ogm) # Calculate gvd from brushfire and ogm self.gvd = self.topology.gvd(self.brush) # Find current room start = (int(current_x), int(current_y)) if self.gvd[start]: gvd_nodes = [start] else: gvd_nodes = self.brushfire_cffi.pointToGvdBrushfireCffi( start, self.ogm, self.gvd) if not len(gvd_nodes): rospy.loginfo("Error. No gvd in the neighborhood.") return neighbor_nodes = self.brushfire_cffi.gvdNeighborBrushfireCffi( gvd_nodes[0], self.nodes, self.gvd) current_room = -1 for p in neighbor_nodes: node = list(p) if node in self.door_nodes: continue for i in range(len(self.rooms)): if node in self.rooms[i]: current_room = i break if current_room != -1: break if current_room == -1: rospy.loginfo("Problem finding current room! Exiting...") return current_room_index = self.room_sequence.index(current_room) start_time = rospy.get_time() if self.navigation_pattern == 'simple': if self.simple_sequence == []: rospy.loginfo( "This navigation pattern has not been calculated for this map!" ) return for i in range(len(self.room_sequence)): # print nodes nodes = self.simple_nodes[current_room] self.print_markers(nodes) # find nodes of current room nodes = self.simple_sequence[current_room] # navigate to all nodes for node in nodes: result = self.goToGoal(node['position'], node['yaw']) rospy.sleep(0.1) current_room_index = (current_room_index + 1) % len( self.room_sequence) current_room = self.room_sequence[current_room_index] elif self.navigation_pattern == 'wall_follow': if self.wall_follow_sequence == []: rospy.loginfo( "This navigation pattern has not been calculated for this map!" ) return for i in range(len(self.room_sequence)): # print nodes nodes = self.wall_follow_nodes[current_room] self.print_markers(nodes) # find nodes of current room nodes = self.wall_follow_sequence[current_room] # navigate to all nodes for node in nodes: result = self.goToGoal(node['position'], node['yaw']) rospy.sleep(0.1) current_room_index = (current_room_index + 1) % len( self.room_sequence) current_room = self.room_sequence[current_room_index] elif self.navigation_pattern == 'zig_zag': if self.zig_zag_sequence == []: rospy.loginfo( "This navigation pattern has not been calculated for this map!" ) return for i in range(len(self.room_sequence)): # print nodes nodes = self.zig_zag_nodes[current_room] self.print_markers(nodes) # find nodes of current room nodes = self.zig_zag_sequence[current_room] # navigate to all nodes for node in nodes: result = self.goToGoal(node['position'], node['yaw']) rospy.sleep(0.1) current_room_index = (current_room_index + 1) % len( self.room_sequence) current_room = self.room_sequence[current_room_index] finish_time = rospy.get_time() print('Start time', start_time, 'Finish time', finish_time, 'Duration', finish_time - start_time) return # Gets target pose, sends it to move_base and waits for the result def goToGoal(self, target, yaw): self.move_base_client.wait_for_server() goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = target[ 0] * self.resolution + self.origin['x'] goal.target_pose.pose.position.y = target[ 1] * self.resolution + self.origin['y'] quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw) goal.target_pose.pose.orientation.x = quaternion[0] goal.target_pose.pose.orientation.y = quaternion[1] goal.target_pose.pose.orientation.z = quaternion[2] goal.target_pose.pose.orientation.w = quaternion[3] self.move_base_client.send_goal(goal) wait = self.move_base_client.wait_for_result() if not wait: rospy.logerr("Action server not available!") rospy.signal_shutdown("Action server not available!") else: return self.move_base_client.get_result() 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 rospy.loginfo("Navigation node reading ogm.") 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] rospy.loginfo("OGM read!") self.ogm_compute = False return def print_markers(self, nodes): rospy.loginfo("Start collecting markers") 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) rospy.loginfo("Markers ready!") # Create Marker for nodes marker = Marker() marker.header.frame_id = "/map" 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 = 1.0 rospy.loginfo("Printing nodes!") self.node_publisher.publish(marker) return
def skeletonizationCffi(self, ogm, origin, resolution, ogml): width = ogm.shape[0] height = ogm.shape[1] local = numpy.zeros(ogm.shape) start = time.time() ######################################################################## ####################### Original Code ################################## # for i in range(0, width): # for j in range(0, height): # if ogm[i][j] < 49: # local[i][j] = 1 ############################ Added code ################################# step = 50 first = [0, 0] last = [0, 0] # Find the area in which there is propability of coverage for i in range(0, width, step): for j in range(0, height, step): if ogm[i][j] < 49: if first == [0, 0]: first = [i, j] last = [i, j] # Search only in the area found for i in range(first[0] - step, last[0] + step): for j in range(first[1] - step, last[1] + step): if ogm[i][j] < 49: local[i][j] = 1 ######################################################################### end = time.time() print end - start skeleton = Cffi.thinning(local, ogml) skeleton = Cffi.prune(skeleton, ogml, 10) viz = [] for i in range(0, width): for j in range(0, height): if skeleton[i][j] == 1: viz.append([ i * resolution + origin['x'], j * resolution + origin['y'] ]) RvizHandler.printMarker(\ viz,\ 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_skeletonization_cffi", # Namespace [0.5, 0, 0, 0.5], # Color RGBA 0.05 # Scale ) return skeleton
class CoverageSubscriber: def __init__(self): self.brushfire_cffi = Cffi() # 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 # Initilize sensor's specs self.sensor_names = [] self.sensor_direction = [] self.sensor_fov = [] self.sensor_range = [] self.sensor_shape = [] self.sensor_reliability = [] # Read sensor's specs self.sensor_names = rospy.get_param('sensor_names') self.sensor_number = len(self.sensor_names) 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')) # Compute bins to keep with which angle a sensor covers each point max_fov = max(self.sensor_fov)/2 # compute for one half (symmetric with sensor_direction) self.bins = [] # self.number_of_bins = 2 * int(math.ceil(max_fov/5)) # 5 degrees in each bin # for i in range(self.number_of_bins): # down = - self.number_of_bins/2 * 5 + i*5 # up = down + 5 # self.bins.append((down,up)) # Compute bins of negative and positive only self.number_of_bins = 2 self.bins.append((-max_fov, 0)) self.bins.append((0, max_fov)) # Load map's translation translation = rospy.get_param('origin') self.origin['x'] = translation[0] self.origin['y'] = translation[1] self.resolution = rospy.get_param('resolution') # OGM related attributes self.ogm_topic = '/map' self.ogm = 0 self.ogm_raw = 0 self.ogm_width = 0 self.ogm_height = 0 self.ogm_header = 0 # Flags to wait subscribers to finish reading data self.ogm_compute = True self.readPose = False # Holds the coverage information. This has the same size as the ogm # If a cell has the value of 0 it is uncovered # In the opposite case the cell's value will be 100 self.coverage = [] self.coverage_number = [] self.coverage_angles = [] self.coverage_diverge = [] self.brush = 0 # coverage_ogm will be published in the coverage_topic self.coverage_ogm = OccupancyGrid() self.coverage_ogm.header.frame_id = "map" self.coverage_topic = '/map/coverage' self.coverage_diverge_ogm = OccupancyGrid() self.coverage_diverge_ogm.header.frame_id = "map" self.coverage_diverge_topic = '/map/coverage_angles/diverge' self.coverage_angles_ogm = OccupancyGrid() self.coverage_angles_ogm.header.frame_id = "map" self.coverage_angles_topic = '/map/coverage_angles' self.coverage_diverge_pub = rospy.Publisher(self.coverage_diverge_topic, \ OccupancyGrid, queue_size = 1) def server_start(self): rospy.init_node('coverage_subscriber') rospy.loginfo('Coverage Subscriber node initialized.') # Read ogm 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) # Read coverage angles ogm rospy.Subscriber(self.coverage_angles_topic, OccupancyGrid, self.cov_callback, queue_size = 1) rospy.spin() return def cov_callback(self, data): # Reshape ogm to a 3D array for a in range(self.number_of_bins): for x in range(0, self.ogm_width): for y in range(0, self.ogm_height): self.coverage_angles[x][y][a] = data.data[x + self.ogm_width * y + self.ogm_width * self.ogm_height * a] near_obstacles = np.where(self.brush == 2) near_obstacles_looked = [] for x in range(0, self.ogm_width): for y in range(0, self.ogm_height): if self.brush[x][y] == 2: left = self.coverage_angles[x][y][0] right = self.coverage_angles[x][y][1] if left + right: near_obstacles_looked.append((x,y)) self.coverage_diverge[x][y] = np.abs(left-right)/(left + right) self.coverage_diverge_ogm.data[x + self.ogm_width * y] = int(100 * self.coverage_diverge[x][y]) # Retrieve metrics values = self.coverage_diverge[zip(*near_obstacles_looked)] rospy.loginfo("Coverage Angle Divergence only of covered obstacles Mean: {}, Std: {}".format(np.mean(values), np.std(values))) self.coverage_diverge_pub.publish(self.coverage_diverge_ogm) # # for a in range(self.number_of_bins): # temp = self.coverage_angles[:,:,a].copy() # total_looks = sum(temp[near_obstacles]) # string = 'Bin: ' + `self.bins[a]` + ' total: ' + `total_looks` # rospy.loginfo(string) return 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 rospy.loginfo("Coverage Subscriber node reading ogm.") 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 = np.zeros((self.ogm_width, self.ogm_height)) self.coverage_ogm.info = data.info self.coverage_ogm.data = np.zeros(self.ogm_width * self.ogm_height) self.coverage_diverge = np.full((self.ogm_width, self.ogm_height), 100) self.coverage_diverge_ogm.info = data.info self.coverage_diverge_ogm.data = np.full(self.ogm_width * self.ogm_height, 100) self.coverage_angles = np.zeros((self.ogm_width, self.ogm_height, self.number_of_bins)) rospy.loginfo("OGM read!") self.ogm_compute = False return
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)
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 __init__(self): self.brushfire_cffi = Cffi() # 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 # Initilize sensor's specs self.sensor_names = [] self.sensor_direction = [] self.sensor_fov = [] self.sensor_range = [] self.sensor_shape = [] self.sensor_reliability = [] # Read sensor's specs self.sensor_names = rospy.get_param('sensor_names') self.sensor_number = len(self.sensor_names) 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')) # Load map's translation translation = rospy.get_param('origin') self.origin['x'] = translation[0] self.origin['y'] = translation[1] self.resolution = rospy.get_param('resolution') # OGM related attributes self.ogm_topic = '/map' self.ogm = 0 self.ogm_raw = 0 self.ogm_width = 0 self.ogm_height = 0 self.ogm_header = 0 # Flags to wait subscribers to finish reading data self.ogm_compute = True self.readPose = False # Holds the coverage information. This has the same size as the ogm # If a cell has the value of 0 it is uncovered # In the opposite case the cell's value will be 100 self.coverage = [] self.coverage_number = [] self.brush = 0 # coverage_ogm will be published in the coverage_topic self.coverage_ogm = OccupancyGrid() self.coverage_ogm.header.frame_id = "map" self.coverage_topic = '/map/coverage' self.coverage_number_ogm = OccupancyGrid() self.coverage_number_ogm.header.frame_id = "map" self.coverage_number_topic = '/map/coverage_number'
class CoverageNumberSubscriber: def __init__(self): self.brushfire_cffi = Cffi() # 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 # Initilize sensor's specs self.sensor_names = [] self.sensor_direction = [] self.sensor_fov = [] self.sensor_range = [] self.sensor_shape = [] self.sensor_reliability = [] # Read sensor's specs self.sensor_names = rospy.get_param('sensor_names') self.sensor_number = len(self.sensor_names) 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')) # Load map's translation translation = rospy.get_param('origin') self.origin['x'] = translation[0] self.origin['y'] = translation[1] self.resolution = rospy.get_param('resolution') # OGM related attributes self.ogm_topic = '/map' self.ogm = 0 self.ogm_raw = 0 self.ogm_width = 0 self.ogm_height = 0 self.ogm_header = 0 # Flags to wait subscribers to finish reading data self.ogm_compute = True self.readPose = False # Holds the coverage information. This has the same size as the ogm # If a cell has the value of 0 it is uncovered # In the opposite case the cell's value will be 100 self.coverage = [] self.coverage_number = [] self.brush = 0 # coverage_ogm will be published in the coverage_topic self.coverage_ogm = OccupancyGrid() self.coverage_ogm.header.frame_id = "map" self.coverage_topic = '/map/coverage' self.coverage_number_ogm = OccupancyGrid() self.coverage_number_ogm.header.frame_id = "map" self.coverage_number_topic = '/map/coverage_number' def server_start(self): rospy.init_node('coverage__number_subscriber') rospy.loginfo('Coverage Number Subscriber node initialized.') # Read ogm 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) # Read coverage angles ogm rospy.Subscriber(self.coverage_number_topic, OccupancyGrid, self.cov_callback, queue_size=1) rospy.spin() return def cov_callback(self, data): # Reshape ogm to a 2D array for x in range(0, self.ogm_width): for y in range(0, self.ogm_height): self.coverage_number[x][y] = data.data[x + self.ogm_width * y] near_obstacles = np.where(self.brush == 2) # Read coverage looks at obstacles number = self.coverage_number[near_obstacles] rospy.loginfo('Coverage Number Mean: {}, Std: {}'.format( np.mean(number), np.std(number))) # Count values and save to a dictionary counted = Counter(number) print(counted) # plt.close() # # # Covert to dataframe # df = pd.DataFrame(counted, index = counted.keys()) # df.drop(df.columns[1:], inplace = True) # # Plot results # df.plot(kind='bar') # plt.title('Number of each obstacle\'s coverage.') # print('Time of plot', rospy.get_time()) # plt.show() return 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 rospy.loginfo("Coverage Subscriber node reading ogm.") 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 = np.zeros((self.ogm_width, self.ogm_height)) self.coverage_ogm.info = data.info self.coverage_ogm.data = np.zeros(self.ogm_width * self.ogm_height) self.coverage_number = np.zeros((self.ogm_width, self.ogm_height)) rospy.loginfo("OGM read!") self.ogm_compute = False 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
def __init__(self): # self.routing = Routing() self.brushfire_cffi = Cffi() self.topology = Topology() # 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 # Load map's translation translation = rospy.get_param('origin') self.origin['x'] = translation[0] self.origin['y'] = translation[1] 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 = [] # Read sensor's specs self.sensor_names = rospy.get_param('sensor_names') self.sensor_number = len(self.sensor_names) 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')) # Flag to wait ogm subscriber to finish self.ogm_compute = False self.current_pose = Pose() # OGM related attributes self.ogm_topic = '/map' self.ogm = 0 self.ogm_raw = 0 self.ogm_width = 0 self.ogm_height = 0 self.ogm_header = 0 self.gvd = 0 self.brush = 0 # Load path pattern for navigation self.navigation_pattern = rospy.get_param('navigation_pattern') # Attributes read from json file self.nodes = [] self.door_nodes = [] self.rooms = [] self.room_doors = [] self.room_type = [] self.room_sequence = [] self.wall_follow_nodes = [] self.wall_follow_sequence = [] self.zig_zag_nodes = [] self.zig_zag_sequence = [] self.simple_nodes = [] self.simple_sequence = [] self.data = [] # 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'] self.room_sequence = self.data['room_sequence'] 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.door_node_pub = rospy.Publisher('/nodes/doors', Marker, queue_size=100) self.room_node_pub = rospy.Publisher('/nodes/rooms', Marker, queue_size=100) self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback) self.move_base_client = actionlib.SimpleActionClient( 'move_base', MoveBaseAction)
class Coverage: def __init__(self): self.brushfire_cffi = Cffi() # 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 # Initilize sensor's specs self.sensor_names = [] self.sensor_direction = [] self.sensor_fov = [] self.sensor_range = [] self.sensor_shape = [] self.sensor_reliability = [] # Read sensor's specs self.sensor_names = rospy.get_param('sensor_names') self.sensor_number = len(self.sensor_names) 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')) # Compute bins to keep with which angle a sensor covers each point max_fov = max(self.sensor_fov)/2 # compute for one half (symmetric with sensor_direction) # self.number_of_bins = 2 * int(math.ceil(max_fov/5)) # 5 degrees in each bin self.bins = [] # for i in range(self.number_of_bins): # down = - self.number_of_bins/2 * 5 + i*5 # up = down + 5 # self.bins.append((down,up)) # Compute bins of negative and positive only self.number_of_bins = 2 self.bins.append((-max_fov, 0)) self.bins.append((0, max_fov)) # 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') # OGM related attributes self.ogm_topic = '/map' self.ogm = 0 self.ogm_raw = 0 self.ogm_width = 0 self.ogm_height = 0 self.ogm_header = 0 # Flags to wait subscribers to finish reading data self.ogm_compute = True self.readPose = False # Holds the coverage information. This has the same size as the ogm # If a cell has the value of 0 it is uncovered # In the opposite case the cell's value will be 100 self.coverage = [] self.coverage_number = [] self.coverage_angles = [] self.brush = 0 # coverage_ogm will be published in the coverage_topic self.coverage_ogm = OccupancyGrid() self.coverage_ogm.header.frame_id = "map" self.coverage_topic = '/map/coverage' self.coverage_number_ogm = OccupancyGrid() self.coverage_number_ogm.header.frame_id = "map" self.coverage_number_topic = '/map/coverage_number' self.coverage_angles_ogm = OccupancyGrid() self.coverage_angles_ogm.header.frame_id = "map" self.coverage_angles_topic = '/map/coverage_angles' self.current_pose = Pose() # robot_pose is current_pose in map's frame (aka in pixels) self.robot_pose = {} self.robot_pose['x'] = 0 self.robot_pose['y'] = 0 self.robot_pose['th'] = 0 self.previous_robot_pose = {} self.previous_robot_pose['x'] = 0 self.previous_robot_pose['y'] = 0 self.previous_robot_pose['th'] = 0 self.coverage_pub = rospy.Publisher(self.coverage_topic, \ OccupancyGrid, queue_size = 10) self.coverage_number_pub = rospy.Publisher(self.coverage_number_topic, \ OccupancyGrid, queue_size = 10) self.coverage_angles_pub = rospy.Publisher(self.coverage_angles_topic, \ OccupancyGrid, queue_size = 10) def getCoverage(self): return np.copy(self.coverage) def getCoverageOgm(self): return self.coverage_ogm def initCoverage(self): self.coverage = np.zeros((self.ogm_width, self.ogm_height)) return def server_start(self): rospy.init_node('coverage') rospy.loginfo('Coverage node initialized.') # Read ogm 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) near_obstacles = np.where(self.brush == 2) iter = 0 while not rospy.is_shutdown(): self.updateCover(publish = False) if not iter % 500: self.coverage_pub.publish(self.coverage_ogm) self.coverage_number_pub.publish(self.coverage_number_ogm) self.coverage_angles_pub.publish(self.coverage_angles_ogm) rospy.loginfo("Update coverage ogm!") near_obstacles_cover = self.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))) iter = 0 iter += 1 rospy.sleep(0.1) return # Read current pose of robot from odometry def readRobotPose(self): rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.odom_callback, queue_size = 1) 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 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 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 # Cb to read robot's pose def odom_callback(self, msg): self.current_pose = msg.pose.pose # Calculate current x,y in map's frame (aka in pixels) self.robot_pose['x'] = int((self.current_pose.position.x - self.origin['x'])/self.resolution) self.robot_pose['y'] = int((self.current_pose.position.y - self.origin['y'])/self.resolution) # Getting the Euler angles q = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w) angles = tf.transformations.euler_from_quaternion(q) self.robot_pose['th'] = angles[2] # yaw self.readPose = False return 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 rospy.loginfo("Coverage node reading ogm.") 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 = np.zeros((self.ogm_width, self.ogm_height)) self.coverage_ogm.info = data.info self.coverage_ogm.data = np.zeros(self.ogm_width * self.ogm_height) # Initilize coverage_number OGM with same size width x height self.coverage_number = np.zeros((self.ogm_width, self.ogm_height)) self.coverage_number_ogm.info = data.info self.coverage_number_ogm.data = np.zeros(self.ogm_width * self.ogm_height) self.coverage_angles = np.zeros((self.ogm_width, self.ogm_height, self.number_of_bins)) self.coverage_angles_ogm.info.width = self.ogm_width self.coverage_angles_ogm.info.height = self.ogm_height * self.number_of_bins self.coverage_angles_ogm.data = np.zeros(self.ogm_width * self.ogm_height * self.number_of_bins) rospy.loginfo("OGM read!") self.ogm_compute = False return