コード例 #1
0
ファイル: explorer_team.py プロジェクト: LinearLogic/cs69
    def execute_rendezvous(
        self, master_coords, drone_coords
    ):  # calculates rendezvous path and sets nav goal for robots at the provided poses
        if self.executing_rendezvous or not self.map_initialized:
            return False

        # init and run the path planner
        path_planner = PathPlanner(self.occupancy_grid, self.map_width,
                                   self.map_height, self.map_resolution,
                                   self.map_origin)
        path_planner.set_start(master_coords)
        path_planner.set_goal(drone_coords)
        rendezvous_path = path_planner.a_star()
        # abort if no path found
        if len(rendezvous_path) == 0:
            return False

        # parse the result and start nav to goal for both robots
        grid_goal = rendezvous_path[int(math.floor(len(rendezvous_path) / 2))]
        world_goal = path_planner.grid_to_world(grid_goal)
        print 'executing rendezvous at ' + str(world_goal)
        self.master_node.start_navigation(
            world_goal[0], world_goal[1], 0,
            (master_coords[0], master_coords[1], 0))
        self.drone_node.start_navigation(world_goal[0], world_goal[1], 0,
                                         (drone_coords[0], drone_coords[1], 0))
        self.executing_rendezvous = True
        return True
コード例 #2
0
    def calc_cspace(self, mapdata):
        """
        Calculates the C-Space, i.e., makes the obstacles in the map thicker.
        :return        [OccupancyGrid] The C-Space.
        """

        rospy.loginfo("Calculating C-Space")
        # Go through each cell in the occupancy grid
        # Inflate the obstacles where necessary
        padding = 2
        newmap = deepcopy(mapdata)
        # for each cell in mapdata
        #   if (cell is walkable) and (cell has an obstacle within padding)
        #     add padding in newmap at current cell

        # input (x,y,pad)
        # sx = min(max(0, x-pad), width)
        # sy = min(max(0, y-pad), height)
        # ex = max(min(width, x+pad), 0)
        # ey = max(min(height, y+pad), 0)
        # for each xx in range(sx,ex)
        #   for each yy in range(sy,ey)
        #     if cell(xx,yx) has obstacle
        #        return True
        #   return False

        new_data = []
        for item in mapdata.data:
            new_data.append(item)

        for h in range(mapdata.info.height - 1):
            for w in range(mapdata.info.width - 1):
                neighbors = ExpandMap.neighbors_of_8(mapdata, w, h)
                if len(neighbors) < 8:
                    new_data[ExpandMap.grid_to_index(mapdata, w, h)] = 100

        #Create a GridCells message and publish it
        expanded_cells = GridCells()
        expanded_cells.header.frame_id = "map"
        expanded_cells.cell_width = mapdata.info.resolution
        expanded_cells.cell_height = mapdata.info.resolution
        expanded_cells.cells = []

        for h in range(mapdata.info.height):
            for w in range(mapdata.info.width):
                index = ExpandMap.grid_to_index(mapdata, w, h)
                if new_data[index] == 100:
                    msg_Point = Point()
                    world_point = PathPlanner.grid_to_world(mapdata, w, h)
                    msg_Point.x = world_point.x
                    msg_Point.y = world_point.y
                    msg_Point.z = 0
                    expanded_cells.cells.append(msg_Point)

        self.expanded_cells.publish(expanded_cells)
        ## Return the C-space
        #print("END")
        #print(mapdata)
        newmap.data = new_data
        return newmap
コード例 #3
0
    def calc_cspace(self, mapdata, padding):
        """
        Calculates the C-Space, i.e., makes the obstacles in the map thicker.
        Publishes the list of cells that were added to the original map.
        :param mapdata [OccupancyGrid] The map data.
        :param padding [int]           The number of cells around the obstacles.
        :return        [[int8]]        The C-Space as a list of values from 0 to 100.
        """
        rospy.loginfo("Calculating C-Space")
        ## Goes through each cell in the occupancy grid
        ## Inflates the obstacles where necessary based on padding argument
        new_map = []
        total_range = mapdata.info.width * mapdata.info.height
        for p in range(0,padding):
            copy = list(mapdata.data)
            for i in range(0, total_range):
                if mapdata.data[i] == 100:
                    #coordinates are found via indexing through occupancy grid.
                    #Index to grid is used to convert the index to useable grid coordinates
                    coordinates = PathPlanner.index_to_grid(mapdata, i)
                    pos_x = coordinates[0]
                    pos_y = coordinates[1]
                    neighbors = PathPlanner.neighbors_of_8(mapdata, pos_x, pos_y)
                    #iterates through neighbors list and changes all available surrounding neighbors to obstacles
                    for n in neighbors:
                        index_value = PathPlanner.grid_to_index(mapdata, n[0], n[1])
                        copy[index_value] = 100
                        world_pos = PathPlanner.grid_to_world(mapdata, n[0], n[1])
                        cell_point = Point()
                        cell_point.x = world_pos[0]
                        cell_point.y = world_pos[1]
                        cell_point.z = 0
                        new_map.append(cell_point)
            mapdata.data = copy

        return mapdata.data
コード例 #4
0
    def find_frontier(self, msg):
        """
        Takes in map [OccupancyGrid] from service call
        return: [Path] list of PoseStamped frontier centroids
        """
        #cspace is calculated to find walkable area
        msg.map.data = self.calc_cspace(msg.map, 2)

        #variables are initialized
        walkable_cells = []
        cells_with_unknown_neighbors = []
        centroids = []
        distances = []
        frontiers = []
        frontier_locations = []
        sorted_distances = []
        total_range = msg.map.info.width * msg.map.info.height

        mapdata = msg.map

        #finds walkable cells in cspace
        for i in range(0, total_range):
            coordinate = PathPlanner.index_to_grid(mapdata, i)
            pos_x = coordinate[0]
            pos_y = coordinate[1]
            if PathPlanner.is_cell_walkable(mapdata, pos_x, pos_y) == 1:
                walkable_cells.append(coordinate)

        # finds all cells that are neighboring an unknown cell(value = -1)
        for c in walkable_cells:
            neighbors = PathPlanner.unknown_neighbors_of_8(mapdata, c[0], c[1])

            for n in neighbors:
                if c not in cells_with_unknown_neighbors:
                    cells_with_unknown_neighbors.append(c)

        # finds which cells in cells_with_unknown_neighbors neighbor each other
        # and generates a list of those that touch
        frontiers = Frontier.separate(cells_with_unknown_neighbors)

        for f in frontiers:
            if len(f) < 2:
                frontiers.remove(f)

        for f in frontiers:
            #finds centroids of all contiguous frontier groups
            centroid_value = Frontier.calc_centroid(f)
            final = centroid_value

            #finds the walkable cell that is closest to the centroid location
            for k in walkable_cells:
                neighbors = PathPlanner.neighbors_of_8(mapdata, k[0], k[1])
                if len(neighbors) == 8:
                    distToCentroid = PathPlanner.euclidean_distance(k[0], k[1], final[0], final[1])
                    distances.append((distToCentroid, k))
            #list of distances is sorted in descending order in terms of size
            sorted_distances = sorted(distances)

        for s in sorted_distances:
            final = s[1]
            world_coord = PathPlanner.grid_to_world(mapdata, final[0], final[1])

            frontier_location = PoseStamped()
            frontier_location.pose.position.x = world_coord[0]
            frontier_location.pose.position.y = world_coord[1]
            frontier_location.pose.position.z = 0.0
            frontier_location.pose.orientation = Quaternion()
            frontier_location.header.stamp = rospy.Time.now()
            frontier_locations.append(frontier_location)

        all_frontiers = Path()
        all_frontiers.header.frame_id = 'map'
        all_frontiers.poses = frontier_locations

        return all_frontiers