Esempio n. 1
0
 def test_map_with_40_nodes_start_5_end_34(self):
     map_40 = Map(Map40.intersections, Map40.roads)
     start_node_id = 5
     dest_node_id = 34
     astar = AStar(map_40)
     self.assertEqual(astar.search(start_node_id, dest_node_id),
                      [5, 16, 37, 12, 34])
Esempio n. 2
0
 def test_map_with_40_nodes_start_8_end_24(self):
     map_40 = Map(Map40.intersections, Map40.roads)
     start_node_id = 8
     dest_node_id = 24
     astar = AStar(map_40, start_node_id, dest_node_id)
     self.assertEqual(astar.search(start_node_id, dest_node_id),
                      [8, 14, 16, 37, 12, 17, 10, 24])
Esempio n. 3
0
    def __init__(self, auto=False):
        """
        """
        if auto:
            meshSize = (50, 50)
            weights = np.linspace(0, 20, 41)
            heurs = [{
                'comp': 'vel',
                'metric': 'octile',
                'alpha': 1.
            }, {
                'comp': 'stock',
                'metric': 'octile',
                'alpha': 1.
            }]

            points = [(1, 1), (1, 48), (20, 20), (20, 28), (28, 28), (28, 48),
                      (48, 1), (48, 48)]
            self.points = points
            searches = list(it.permutations(points, 2))
            template = pd.DataFrame(columns=weights, index=searches)
            self.results = {
                'counts': {i['comp']: template.copy()
                           for i in heurs},
                'costs': {i['comp']: template.copy()
                          for i in heurs}
            }

            for comp in heurs:
                for weight in weights:
                    star = AStar(meshSize=meshSize,
                                 heur=comp,
                                 heur_weight=weight)
                    for search in searches:
                        star.search(search)
                        self.results['counts'][
                            comp['comp']][weight].loc[search] = star.iterCount
                        self.results['costs'][comp['comp']][weight].loc[
                            search] = star.pathCost[search[1]]
Esempio n. 4
0
def run():
    # Initialize a 10x10 stage. No other stage sizes are
    # coded yet, but these constructor arguments exist in
    # case we want to later on
    board = Board()

    # Initialize the A-Star algorithm class with our
    # current graph and reference to the board because
    # I'm lazy. It shouldn't have a dependency on a board,
    # but it's been 8 hours already and I'm tired.
    astar = AStar(board.graph, board)
    # Render the board to show where we stand
    board.render()
    inp = get_input()
    while inp[0].lower() not in "qe":
        if inp[0].lower() in "h":
            # Players is babies and wants helps!
            with open(os.path.join(resource_root, "maze", "help.txt"), "rb") as help_file:
                print(help_file.read())
            board.render()
            inp = raw_input("Next move? (enter h or help for help)").strip()
            if inp == "":
                inp = " "
            continue
        # Try to move the player. Run, Forrest! Run!
        if inp[0].lower() == "u":
            (x, y) = board.player
            board.move_entity("player", x, y - 1)
        elif inp[0].lower() == "d":
            (x, y) = board.player
            board.move_entity("player", x, y + 1)
        elif inp[0].lower() == "l":
            (x, y) = board.player
            board.move_entity("player", x - 1, y)
        elif inp[0].lower() == "r":
            (x, y) = board.player
            board.move_entity("player", x + 1, y)

        # The enemy is a greedy bastard and moves every chance he can
        path = astar.get_path(astar.search(board.enemy, board.player)[0], board.enemy, board.player)
        board.move_entity("enemy", *path[1])
        board.render(path=path[2:-1])
        if board.enemy == board.player:
            print("You lost!")
            exit()
        if board.player == board.goal:
            print("You escaped death! Huzzah!")
            exit()
        inp = get_input()
    print("Scaredy-cat!")
Esempio n. 5
0
class Cop:

    __ROBBERS = ['robber_0', 'robber_1', 'robber_2']

    def __init__(self, name, speed=0.6):
        rospy.init_node(name)
        self.name = name
        self.speed = speed
        self.my_pose = GroundtruthPose(name)
        self.occupancy_grid = OccupancyGrid(0.2, 0.5)
        # assign cop to unique robber
        self.target = 'robber_' + name[-1]
        self.target_pose = GroundtruthPose(self.target)
        self.rate_limit = rospy.Rate(100)  # 200ms
        self.path_finder = AStar('euclidean',
                                 self.occupancy_grid,
                                 resolution=0.2)
        self.velocity_publisher = rospy.Publisher('/{}/cmd_vel'.format(name),
                                                  Twist,
                                                  queue_size=5)
        self.catch_publisher = rospy.Publisher('/caught', String, queue_size=1)
        rospy.Subscriber('/caught', String, self.catch_listener)

        # file for pose history
        self.pose_history = []
        self.pose_history_fn = '/tmp/gazebo_{}.txt'.format(name)
        with open(self.pose_history_fn, 'w'):
            pass

        # wait for ground truth poses node to init
        while not self.my_pose.ready and not self.target_pose.ready:
            self.rate_limit.sleep()
            continue

    @property
    def position(self):
        return (self.my_pose.pose[X], self.my_pose.pose[Y])

    @property
    def target_position(self):
        return (self.target_pose.pose[X], self.target_pose.pose[Y])

    def braitenberg(self, f, fl, fr, l, r):
        f, fl, fr, l, r = np.tanh([f, fl, fr, l, r])
        u = min(self.speed, 0.2 * (abs(fl) + abs(fr) + abs(f)))
        w = 0.2 * fl + 0.1 * l + 0.2 * f - 0.2 * fr - 0.1 * r
        return u, w

    def log_pose(self):
        self.pose_history.append(self.my_pose.pose)
        if len(self.pose_history) % 50:
            with open(self.pose_history_fn, 'a') as fp:
                fp.write('\n'.join(','.join(format(v, '.5f') for v in p)
                                   for p in self.pose_history) + '\n')
            self.pose_history = []

    def line_of_sight(self, a, b):
        ax, ay = a[0] / self.occupancy_grid.res, a[1] / self.occupancy_grid.res
        bx, by = b[0] / self.occupancy_grid.res, b[1] / self.occupancy_grid.res
        line_x, line_y = np.arange(ax, bx, 1), np.arange(ay, by, 1)
        return all(self.occupancy_grid.is_free(p) for p in zip(line_x, line_y))

    def get_path_to_target(self):
        return self.path_finder.search(self.position, self.target_position)[0]

    def check_caught(self):
        if np.linalg.norm(
                np.array(self.position) -
                np.array(self.target_position)) < 0.3:
            self.__ROBBERS.remove(self.target)
            print(self.target, 'was caught after', rospy.get_time(),
                  'seconds by', self.name)
            catch_msg = String()
            catch_msg.data = self.target
            self.catch_publisher.publish(catch_msg)
            if self.__ROBBERS:
                self.target = random.choice(self.__ROBBERS)
                self.target_pose = GroundtruthPose(self.target)

        if self.target not in self.__ROBBERS:
            if self.__ROBBERS:
                self.target = random.choice(self.__ROBBERS)
                self.target_pose = GroundtruthPose(self.target)

    def catch_listener(self, msg):
        if msg.data in self.__ROBBERS:
            self.__ROBBERS.remove(msg.data)

    def check_finished(self):
        if not self.__ROBBERS:
            print('Finished!')
            vel_msg = Twist()
            vel_msg.linear.x = 0.0
            vel_msg.angular.z = 0.0
            self.velocity_publisher.publish(vel_msg)
            return True

    def control_loop(self):
        path = self.get_path_to_target()
        s = rospy.get_time()

        while not rospy.is_shutdown():

            self.check_caught()
            if self.check_finished():
                break

            if (rospy.get_time() - s) > 5:  # get new path every 5s
                path = self.get_path_to_target()
                s = rospy.get_time()

            v = get_velocity_to_follow_path(np.array(self.position), path,
                                            self.speed)
            u, w = feedback_linearize(self.my_pose.pose, v)
            vel_msg = Twist()
            vel_msg.linear.x = u
            vel_msg.angular.z = w

            self.velocity_publisher.publish(vel_msg)
            self.log_pose()
            self.rate_limit.sleep()
Esempio n. 6
0
class Robber:
    def __init__(self, name, speed=0.8):
        rospy.init_node(name)
        self.name = name
        self.speed = speed
        self.caught = False
        self.my_pose = GroundtruthPose(name)
        self.cop_poses = [
            GroundtruthPose(c) for c in ['cop_0', 'cop_1', 'cop_2']
        ]
        self.rate_limit = rospy.Rate(100)  # 200ms
        self.res = 0.2
        self.occupancy_grid = OccupancyGrid(self.res, 0.5)
        self.path_finder = AStar('euclidean',
                                 self.occupancy_grid,
                                 resolution=self.res)
        self.map = list(self.path_finder.occupancy_grid.map)
        self.velocity_publisher = rospy.Publisher('/{}/cmd_vel'.format(name),
                                                  Twist,
                                                  queue_size=5)
        rospy.Subscriber('/caught', String, self.catch_listener)

        # file for pose history
        self.pose_history = []
        self.pose_history_fn = '/tmp/gazebo_{}.txt'.format(name)
        with open(self.pose_history_fn, 'w'):
            pass

        # wait for ground truth poses node to init
        while not self.my_pose.ready or not all(c.ready
                                                for c in self.cop_poses):
            self.rate_limit.sleep()
            continue

        self.target_position, self.path = self.get_random_target_position_and_path(
        )

    @property
    def position(self):
        return (self.my_pose.pose[X], self.my_pose.pose[Y])

    def braitenberg(self, f, fl, fr, l, r):
        f, fl, fr, l, r = np.tanh([f, fl, fr, l, r])
        u = min(self.speed, 0.2 * (abs(fl) + abs(fr) + abs(f)))
        w = 0.2 * fl + 0.1 * l + 0.2 * f - 0.2 * fr - 0.1 * r
        return u, w

    def log_pose(self):
        self.pose_history.append(self.my_pose.pose)
        if len(self.pose_history) % 50:
            with open(self.pose_history_fn, 'a') as fp:
                fp.write('\n'.join(','.join(format(v, '.5f') for v in p)
                                   for p in self.pose_history) + '\n')
            self.pose_history = []

    def line_of_sight(self, a, b):
        ax, ay = a[0] / self.occupancy_grid.res, a[1] / self.occupancy_grid.res
        bx, by = b[0] / self.occupancy_grid.res, b[1] / self.occupancy_grid.res
        line_x, line_y = np.arange(ax, bx, 1), np.arange(ay, by, 1)
        return all(self.occupancy_grid.is_free(p) for p in zip(line_x, line_y))

    def get_random_target_position_and_path(self):
        choices = [random.choice(self.map) for _ in range(5)]
        choices = [(c[0] * self.res, c[1] * self.res) for c in choices]
        cops = [
            c.pose[:2] for c in self.cop_poses
            if self.line_of_sight(self.my_pose.pose, c.pose)
        ]
        ranked = sorted(choices,
                        key=lambda x: sum(dist(c, x) for c in cops) / len(cops)
                        if cops else 0)
        goal = ranked[-1]
        path = self.path_finder.search(self.position, goal, cops)[0]
        return goal, path

    def get_maximin_random_target_position(self):
        choices = {random.choice(self.map): np.inf for _ in range(5)}
        cops = [
            c.pose[:2] for c in self.cop_poses
            if self.line_of_sight(self.my_pose.pose, c.pose)
        ]
        for goal_point in choices.keys():
            minimax = np.inf
            for cop_location in cops:
                cop_location_res = (cop_location[0] * self.res,
                                    cop_location[1] * self.res)
                path = self.path_finder.search(cop_location_res, goal_point)[0]
                for path_point in path:
                    d = dist(path_point, cop_location)
                    if d < minimax:
                        minimax = d
            choices[goal_point] = minimax
        best = max(choices, key=choices.get)
        return best

    def check_goal(self):
        return dist(self.position, self.target_position) < 0.2

    def get_path_to_target(self):
        return self.path_finder.search(self.position, self.target_position)[0]

    def catch_listener(self, msg):
        if msg.data == self.name:
            self.caught = True

    def control_loop(self):
        s = rospy.get_time()

        while not rospy.is_shutdown():

            if self.caught:
                vel_msg = Twist()
                vel_msg.linear.x = 0
                vel_msg.angular.z = 0
                self.velocity_publisher.publish(vel_msg)
                break

            if self.check_goal() or rospy.get_time() - s > 10:
                self.target_position, self.path = self.get_random_target_position_and_path(
                )
                s = rospy.get_time()

            v = get_velocity_to_follow_path(np.array(self.position), self.path,
                                            self.speed)
            u, w = feedback_linearize(self.my_pose.pose, v)
            vel_msg = Twist()
            vel_msg.linear.x = u
            vel_msg.angular.z = w

            self.velocity_publisher.publish(vel_msg)
            self.log_pose()
            self.rate_limit.sleep()
class GlobalPlanner:
    matrix = None

    def __init__(self, precision):
        rospy.init_node("global_planner", anonymous=True)

        # Try to load parameters from launch file, otherwise set to None
        try:
            positions = rospy.get_param("~position")
            startX, startY = positions["startX"], positions["startY"]
            targetX, targetY = positions["targetX"], positions["targetY"]
            start = (startX, startY)
            target = (targetX, targetY)
        except:
            start, target = None, None

        # Load maze matrix
        # do not crop if target outside of maze
        self.map_loader = MapLoader(start, target)
        self.map_matrix = self.map_loader.loadMap()
        GlobalPlanner.matrix = self.map_matrix
        Cell.resolution = self.map_loader.occupancy_grid.info.resolution

        self.precision = precision

        # Calculate path
        t = time.time()
        self.path_finder = AStar(self.map_matrix)
        raw_path = self.path_finder.search()
        print(time.time() - t)
        Cell.start = self.path_finder.start
        self.path = [Cell(r, c) for r, c in raw_path]
        self.goal = self.path[0].pose()
        self.path_index = 0
        self.pose = Pose(0, 0, 0)

        # Setup publishers
        self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
        # self.pid_pub = rospy.Publisher("/pid_err", Float64, queue_size=10)

        # Setup subscribers
        _ = rospy.Subscriber("/odom", Odometry, self.odom_callback)
        # odom_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.odom_callback)

    def odom_callback(self, msg):
        self.pose.x = msg.pose.pose.position.x
        self.pose.y = msg.pose.pose.position.y

        rot_q = msg.pose.pose.orientation
        (_, _, self.pose.theta) = euler_from_quaternion(
            [rot_q.x, rot_q.y, rot_q.z, rot_q.w])

    def next_pose(self):
        try:
            self.path_index += 1
            self.goal = self.path[self.path_index].pose()
            rospy.loginfo("Moving to next pose: [%s, %s]", self.goal.x,
                          self.goal.y)
        except IndexError:
            rospy.logwarn("REACHED END OF PATH!")

    def run(self):
        rate = rospy.Rate(10)  # 10hz
        speed = Twist()
        goto = GotoController()
        goto.set_max_linear_acceleration(.05)
        goto.set_max_angular_acceleration(.2)
        goto.set_forward_movement_only(True)
        while not rospy.is_shutdown():

            speed_pose = goto.get_velocity(self.pose, self.goal, 0)
            # self.pid_pub.publish(goto.desiredAngVel)
            speed.linear.x = speed_pose.xVel
            speed.angular.z = speed_pose.thetaVel
            self.cmd_vel_pub.publish(speed)
            if goto.get_goal_distance(self.pose,
                                      self.goal) <= .05:  # 5 cm precision
                self.next_pose()
            rate.sleep()
Esempio n. 8
0
#!/usr/bin/env python
# -*- coding: utf-8 -*- 

# Loading classes from other files
from graph import Graph
from astar import AStar

# Loading txt file with maze
maze_file = open('maze.txt', 'r+')
maze = maze_file.readlines()
maze_file.close()

# Parsing maze to matrix of nodes
graph = Graph(maze)

# Generating AStar object
astar = AStar(graph.start, graph.stop)

# Searching for path
path = astar.search()

# Printing path
for element in graph.to_s(path):
        print ''.join(element)