def main():
# Create AStar map
world = Map()
# Load map from bmp file
bmp_path = os.path.join(os.getcwd(), 'map.bmp')
world.load_map(bmp_path)
world.start = Cell((1, 20))
world.finish = Cell((85, 20))
# world.start = Cell(1, 1)
# world.finish = Cell(700, 350)
world.draw_map()
a_star_algorithm = PathFinder(world)
a_star_algorithm.find_path()
class GoalSelector:
def __init__(self): # type: () -> None
self._path_finder = PathFinder()
def select_goal(self, goals, grid, robot_state):
# type: (list, Grid, RobotState) -> Goal
""" Selects the next best goal to collect.
Args:
goals: The list of goals, which come into question to collect.
grid: The grid of the world.
robot_state: The robot state.
Returns: The selected goal.
"""
min_distance_reward = None
nearest_goal = None
for goal in list(goals):
# optimization: check if direct distance is short enough to possibly beat current nearest goal
if min_distance_reward is not None:
max_direct_distance = min_distance_reward * goal.reward
if goal.distance_sqrt(
robot_state.exact_position) > max_direct_distance:
continue
goal_grid_position = grid.nearby_free_grid_position(
(goal.x, goal.y), GOAL_RADIUS_SQRT)
if goal_grid_position is None:
continue
path = self._path_finder.find_path(grid.obstacles,
robot_state.proximal_position,
goal_grid_position)
if len(path) <= 1:
continue
distance_sqrt = _path_distance(path)
distance_reward = distance_sqrt * (1.0 / goal.reward)
if min_distance_reward is None or min_distance_reward > distance_reward:
min_distance_reward = distance_reward
nearest_goal = goal
return nearest_goal
def get_route(train_number, source, target, date):
finder = PathFinder(train_number, source, target, date)
found_route = finder.find_path()
visualiser.print_route(found_route)
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
self.penholder.set_color(0.0, 1.0, 0.0)
# generate spline points and line drawing order
splines, splines_color = PathFinder.get_spline_points(self.img.paths)
# in format [index, is_parallel, is_spline]
line_index_list = PathFinder.find_path(self.img.lines, splines,
splines_color)
prev_color = None
# loop to draw all lines and paths
for draw_info in line_index_list:
index = int(draw_info[0])
is_parallel = draw_info[1]
is_spline = draw_info[2]
line = self.img.lines[index]
curr_color = self.img.lines[index].color
if curr_color != prev_color:
prev_color = curr_color
self.penholder.set_color(*get_color(curr_color))
# ===== spline routine =====
if is_spline:
path_points = self.draw_path_coords(splines[index],
is_parallel)
self.penholder.set_color(*get_color(splines_color[0]))
# go to start of the curve and begin drawing
for i in range(0, 2):
# go to start of curve
goal_x, goal_y = path_points[0, 0], path_points[0, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
if i == 1:
goal_x, goal_y = path_points[9, 0], path_points[9, 1]
# turn to goal
# self.tracker.update()
self.filter.update()
curr_x = self.filter.x
curr_y = self.filter.y
goal_theta = math.atan2(goal_y - curr_y, goal_x - curr_x)
if i == 1:
goal_theta = math.atan2(goal_y - path_points[0, 1],
goal_x - path_points[0, 0])
self.penholder.go_to(0.0)
self.go_to_angle(goal_theta)
self.go_to_goal(goal_x, goal_y)
# start drawing after correctly oriented
# uses only odometry during spline drawing
self.penholder.go_to(self.penholder_depth)
prev_base_speed = self.base_speed
self.filter.updateFlag = False
self.base_speed = 25
print("Draw!")
last_drew_index = 0
# draw the rest of the curve. Draws every 10th point.
for i in range(10, len(path_points), 5):
goal_x, goal_y = path_points[i, 0], path_points[i, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
self.go_to_goal(goal_x, goal_y, useOdo=True)
print("\nlast drew index {}\n".format(last_drew_index))
if last_drew_index < len(path_points) - 1:
goal_x, goal_y = path_points[-1, 0], path_points[-1, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
self.go_to_goal(goal_x, goal_y, useOdo=False)
# stop drawing and restore parameter values
self.base_speed = prev_base_speed
self.filter.updateFlag = True
self.penholder.go_to(0.0)
# ===== line routine =====
else:
for i in range(0, 2):
goal_x, goal_y = self.draw_coords(line,
is_parallel=is_parallel,
at_start=True)
if i == 1:
goal_x, goal_y = self.draw_coords(
line, is_parallel=is_parallel, at_start=False)
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
# turn to goal
# self.tracker.update()
self.filter.update()
curr_x = self.filter.x
curr_y = self.filter.y
goal_theta = math.atan2(goal_y - curr_y, goal_x - curr_x)
self.penholder.go_to(0.0)
self.go_to_angle(goal_theta)
if i == 1:
# start drawing
self.penholder.go_to(self.penholder_depth)
print("Draw!")
self.go_to_goal(goal_x, goal_y)
# graph the final result
self.draw_graph()
self.create.stop()
from log_buddy import lb
from path_finder import PathFinder, PathFinderError
from route_printer import RoutePrinter
if __name__ == "__main__":
lb.set_log_level(lb.INFO)
source = "Van Kooten, S"
dest = "Rast, Mark"
exclude = []
pf = PathFinder(source, dest, exclude)
try:
pf.find_path()
except PathFinderError as e:
lb.e(e)
else:
print(RoutePrinter(pf))
lb.log_stats()
class RobotNavigation:
def __init__(self): # type: () -> None
self._robot_state = RobotState()
self._robot_control = RobotControl()
self._goal_pool = GoalPool()
self._grid = Grid()
self._goal_selector = GoalSelector()
self._path_finder = PathFinder()
self._marker_drawer = MarkerDrawer()
self._current_goal = None
def update(self): # type: () -> None
""" Updates the navigation of the robot. """
if not self._robot_state.received_all_data():
return
self._grid.update(self._robot_state)
self._marker_drawer.draw_obstacles(self._grid.obstacles, self._robot_state)
self._goal_pool.check_goals(self._robot_state)
new_goal = self._goal_selector.select_goal(self._goal_pool.get_uncollected_goals(), self._grid,
self._robot_state)
if new_goal is not self._current_goal and new_goal is not None:
rospy.loginfo("Target: (%s %s), reward=%s" % (new_goal.x, new_goal.y, new_goal.reward))
self._current_goal = new_goal
self._marker_drawer.draw_goals(self._current_goal, self._goal_pool.goals, self._robot_state)
# stop robot if no goal is selected
if self._current_goal is None:
self._robot_control.stop()
return
goal_grid_position = self._grid.nearby_free_grid_position((self._current_goal.x, self._current_goal.y),
GOAL_RADIUS_SQRT)
# set goal as unreachable if no grid position found
if goal_grid_position is None:
self._current_goal.unreachable = True
self._robot_control.stop()
return
# find the path to the goal
path = self._path_finder.find_path(self._grid.obstacles, self._robot_state.proximal_position,
goal_grid_position)
# check if path was found
if len(path) <= 1:
self._current_goal.unreachable = True
self._robot_control.stop()
return
self._marker_drawer.draw_path(path, self._robot_state)
# get furthest away target position which is still in sight
target_position = self._grid.first_in_sight(path[:0:-1], self._robot_state.proximal_position)
if target_position is None:
target_position = path[1]
# check if target position is in obstacle
if self._grid.obstacles.__contains__(target_position):
self._robot_control.stop()
return
self._robot_control.navigate(target_position, self._robot_state)
self._marker_drawer.draw_direction(target_position, self._robot_state)
