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])
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])
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]]
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!")
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()
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()
#!/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)