def __init__(self):
rospy.init_node('maze_pro_solver', 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
self.map_loader = MapLoader(
start, target) # do not crop if target outside of maze
self.map_matrix = self.map_loader.loadMap()
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
# Calculate path
self.path_finder = PathFinder(self.map_matrix)
raw_path = self.path_finder.calculate_path()
self.path = [
Cell(r - self.path_finder.start.row,
c - self.path_finder.start.column) for r, c in raw_path
] # move rows to correct starting position
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=5)
# Setup subscribers
odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
def compute_all_moves(start_state, end_state):
"""
Computes all possible ways to rearrange the cars from
start_state to end_state.
:param start_state: order of cars in the start of the rearrangement
:param end_state: order of cars after rearrangement
:yields: a list of move steps. Each move is represented as a tuple
with two indeces,
the 1st index is the number of lot from which we move the car,
the 2nd index is the number of lot to which we move the car
"""
check_input_validity(start_state, end_state)
parking_size = len(start_state)
constraints = {i: tuple(range(parking_size)) for i in range(parking_size)}
path_finder = PathFinder(tuple(start_state),
tuple(end_state),
constraints=constraints)
paths = path_finder.find_all_paths()
for path in paths:
decoded_path = path_finder.decode_path(path)
moves_sequence = list()
for i in range(1, len(decoded_path)):
moves_sequence.append(
compute_move(decoded_path[i - 1], decoded_path[i]))
yield moves_sequence
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 _compute_graph(self, counterstrategy, z_array, countertrace):
"""
Computes a summarizing graph.
This method is able to calculate all states and all state transitions
which are feasible in the interactive game implemented in
L{InteractiveGame}. All states and state transitions are visualized
as a graph. The graph is created in DOT format. The DOT-program can
then be used to generate pictures of the graph by typing:
- dot -Tpdf filename.dot -o filename.pdf
@param counterstrategy: A counterstrategy, i.e., a strategy for the
environment to find inputs so that the system is forced to
violate the specification.
@type counterstrategy: L{BDD}
@param z_array: z_array[a] contains the intermediate results of the
fixpoint computation in the variable Z of the computation of
the winning region for the environment. 'a' counts the
iterations of the fixpoint computation in Z. It is used to
figure out how often the value of jx might still change in the
future of the play. (If the play is in z_array[a], jx might
change at most a-1 times.)
@type z_array: list<L{BDD}>
@param countertrace: A sequence of inputs so that the system is forced
to violate its specification.
@type countertrace: L{InfiniteTraceOfBdds}
"""
print "--------------- Summarizing Graph ---------------"
print "Computing a graph that summarizes all possible plays ..."
starttime = time.clock()
# 1000 means: abort if graph contains > 1000 states.
path_finder = PathFinder(self.__utils, counterstrategy, z_array, \
countertrace, 1000)
success = path_finder.write_graphs("./spec_debug_results/graph")
# Printing some statistics:
if not success:
PLog.log("Graph contains more than 1000 states -> aborted\n")
PLog.log(" Computing the graphs took ")
PLog.log("%.2f seconds\n" % (time.clock() - starttime))
return
PLog.log("Nr of states in graph: %d\n" % len(path_finder.get_graph()))
print "Graphs were written to 'spec_debug_results/graph.dot' and "
print " 'spec_debug_results/graph_with_signals.dot'"
print " You can produce pictures of the graphs by typing:"
print " 'dot -Tpdf -o ./graph.pdf ./graph.dot'"
print " for example."
print " Detailed information to the graphs was written to"
print " 'spec_debug_results/graph.info."
PLog.log(" Computing the graphs took ")
PLog.log("%.2f seconds\n" % (time.clock() - starttime))
def __init__(self, debug = False):
# configure chromedriver options
options = Options()
if not debug:
options.headless = True
options.add_argument('--log-level=3')
options.add_argument('--window-size=1920x1080')
# find driver path
path_finder = PathFinder()
driver_path = path_finder.find_driver_path()
# open chromedriver
self.driver = webdriver.Chrome(executable_path = driver_path, chrome_options = options)
self.driver.maximize_window()
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()
def test_multiple_paths(self):
map = 'tests/3x3_two_paths.xml'
self.assertEqual(
PathFinder.get_shortest_paths(map)['paths'], [{
'points': [{
'col': 'A',
'row': 1
}, {
'col': 'A',
'row': 2
}, {
'col': 'A',
'row': 3
}, {
'col': 'B',
'row': 3
}]
}, {
'points': [{
'col': 'A',
'row': 1
}, {
'col': 'A',
'row': 2
}, {
'col': 'B',
'row': 2
}, {
'col': 'B',
'row': 3
}]
}])
def compute_moves_with_constraints(start_state, end_state, constraints):
"""
Computes moves sequence under given constraints.
:param start_state: order of cars in the start of the rearrangement
:param end_state: order of cars after rearrangement
:param constraints: map from the parking lot to a tuple of the allowed cars
:yields: move steps. Each move is represented as a tuple with two indices,
the 1st index is the number of lot from which we move the car,
the 2nd index is the number of lot to which we move the car
"""
check_input_validity(start_state, end_state)
path_finder = PathFinder(tuple(start_state), tuple(end_state), constraints)
paths = path_finder.find_all_paths()
path = path_finder.decode_path(next(paths))
for i in range(1, len(path)):
yield compute_move(path[i - 1], path[i])
def test_no_paths(self):
map = 'tests/3x3_no_paths.xml'
self.assertEqual(
PathFinder.get_shortest_paths(map)['paths'],
[{
'points': ['No paths found']
}],
)
def set_return_route(self, vehicle_id, step):
# Get current vehicle and remove it from list
vehicle = self.vehicles[vehicle_id]
# Reset vehicle state (new id, stats, switch destination) and add it to vehicle list
vehicle.tl_controller.reset()
vehicle.switch_destination()
route = PathFinder.get_route(vehicle)
vehicle.set_route(route)
def __init__(self, json_data, traci_conn, rtc_conn, vehicle_mode_id=None):
self.vehicles = {}
"""Parse options and init simulation objects"""
self.routes = {r['id']: r for r in json_data['routes']}
if vehicle_mode_id is not None:
vehicle_modes = [
vm for vm in json_data['vehicle_modes']
if vm['id'] == vehicle_mode_id
][0]
for json_vehicle in json_data['vehicles']:
if 'repeat' in json_vehicle:
delay = json_vehicle['start_delay']
for i in range(0, json_vehicle['repeat']):
vehicle = InterventionVehicle(
traci_conn, rtc_conn,
self.routes[json_vehicle['route']], delay,
vehicle_modes['preemption_mode'] if vehicle_mode_id
is not None else json_vehicle['preemption_mode'],
vehicle_modes['reset_mode'] if vehicle_mode_id
is not None else json_vehicle['reset_mode'],
json_vehicle['path_finder_mode'],
json_vehicle['path_finder_algorithm'])
""" Pre-processing - calculate vehicles path """
route = PathFinder.get_route(vehicle)
vehicle.set_route(route)
self.vehicles[vehicle.id] = vehicle
delay += json_vehicle['repeat_period']
else:
vehicle = InterventionVehicle(
traci_conn, rtc_conn, self.routes[json_vehicle['route']],
json_vehicle['start_delay'],
vehicle_modes['preemption_mode'] if vehicle_mode_id
is not None else json_vehicle['preemption_mode'],
vehicle_modes['reset_mode'] if vehicle_mode_id is not None
else json_vehicle['reset_mode'],
json_vehicle['path_finder_mode'],
json_vehicle['path_finder_algorithm'])
""" Pre-processing - calculate vehicles path """
route = PathFinder.get_route(vehicle)
vehicle.set_route(route)
self.vehicles[vehicle.id] = vehicle
class Challenge1TestCase(unittest.TestCase):
VIZ = False
def setUp(self):
self.path_finder = PathFinder()
def tearDown(self):
self.path_finder = None
def _run_test(self, grid, queries):
for query in queries:
path = self.path_finder.get_path(grid, query[0], query[1])
if Challenge1TestCase.VIZ:
PathVisualizer.viz_path(grid, query, path)
self.assertTrue(PathValidator.is_valid_path(grid, query, path),
"Invalid path %s for query %s." % (path, query))
def test_no_obstacles_straight_line(self):
grid = np.zeros((20, 20)).astype(np.bool)
queries = [
[Waypoint(5, 5, 0), Waypoint(5, 8, 0)],
[Waypoint(16, 5, 1), Waypoint(8, 5, 1)],
[Waypoint(5, 15, 3), Waypoint(16, 15, 3)],
]
self._run_test(grid, queries)
def test_no_obstacles_with_turns(self):
grid = np.zeros((20, 20)).astype(np.bool)
queries = [
[Waypoint(5, 7, 0), Waypoint(15, 8, 3)],
[Waypoint(16, 5, 2), Waypoint(8, 5, 1)],
[Waypoint(15, 15, 1), Waypoint(16, 15, 3)],
]
self._run_test(grid, queries)
def test_with_one_obstacle(self):
grid = np.zeros((20, 20)).astype(np.bool)
grid[10:14, 10:14] = True
queries = [
[Waypoint(5, 7, 0), Waypoint(15, 11, 3)],
[Waypoint(16, 5, 2), Waypoint(8, 5, 1)],
[Waypoint(15, 15, 1), Waypoint(16, 15, 3)],
]
self._run_test(grid, queries)
def test_with_multiple_obstacles(self):
grid = np.zeros((20, 20)).astype(np.bool)
grid[3:4, 0:15] = True
grid[13:14, 5:20] = True
queries = [[Waypoint(0, 0, 0), Waypoint(19, 19, 3)]]
self._run_test(grid, queries)
def __init__(self):
self.platform_ = platform.platform()
self.pid = os.getpid()
fpid = open('./pid', 'w')
fpid.write(str(self.pid))
fpid.close()
self.is_stepcounter_on = False
self.curr_start_node = -1
self.curr_end_node = -1
self.transit = False
self.userinput = ''
self.transition = None
self.instruction = ""
self.start = time.time()
self.platform_pi = ["Linux-4.4.13-v6+-armv6l-with-debian-8.0",
"Linux-4.4.13+-armv6l-with-debian-8.0",
"Linux-4.4.30+-armv6l-with-debian-8.0"]
# Init submodules
if self.platform_ in self.platform_pi:
plot = False
else:
plot = False
self.PathFinder = PathFinder()
self.StepDetector = StepDetector(plot=plot)
self.Localization = Localization(x=0, y=0, north=self.PathFinder.get_angle_of_north(), plot=plot)
# self.LPF = LocalPathFinder(mode='demo')
# Init environment and user-defined variables
try:
self.ENV = json.loads(open(os.path.join(os.path.dirname(__file__), 'env.json')).read())
StepDetector.SAMPLES_PER_PACKET = self.ENV["STEP_SAMPLES_PER_PACKET"]
StepDetector.SAMPLES_PER_WINDOW = self.ENV["STEP_SAMPLES_PER_WINDOW"]
StepDetector.INTERRUPTS_PER_WINDOW = StepDetector.SAMPLES_PER_WINDOW / StepDetector.SAMPLES_PER_PACKET
self.StepDetector.THRES = self.ENV["STEP_THRES"]
self.Localization.stride = self.ENV["STRIDE_LENGTH"]
App.DATA_PIPE = self.ENV['DATA_PIPE']
App.EVENT_PIPE = self.ENV['EVENT_PIPE']
except Exception as e:
print("[MAIN] Environment file not found, using defaults instead")
def test_states_not_in_constraints(self):
# test input states don't satisfy given constraints
start_state = [0, 1, 2, 3]
end_state = [0, 2, 1, 3]
constraints = {
0: (1, 3),
1: (0, 1, 2, 3),
2: (0, 1, 2, 3),
3: (0, 1, 2, 3)
}
self.assertRaises(
ValueError,
lambda: PathFinder(start_state, end_state, constraints))
def generate_path():
"""
The request to generate a path.
:return: A dictionary with (maximum) two fields:
- 'result': will be 'ok' if a path was found, else an error message.
- 'path': a list of start_city, parks, and end_city.
- each element will have 'name' and 'next_distance' fields.
for the end_city, the next_distance field represents the total distance driven.
= parks will also have:
- 'rating' => the avg. Google Rating.
- 'num_reviews' => the total # of google reviews.
- 'state' => the state the park is in.
- 'photos' => a list of remote urls to the park's photos.
"""
starting_city = request.args.get(
"start_city") # A string representing the starting city.
max_distance = float(request.args.get(
"max_distance")) # The maximum driving distance (in kilometers).
p = PathFinder()
p.generate_path(starting_city=starting_city, max_distance=max_distance)
return p.return_path()
def main():
try:
map = sys.argv[1]
except IndexError:
print("First argument must be the path to the .xml file!")
else:
data = PathFinder.get_shortest_paths(map)
try:
result_file = sys.argv[2]
with open(result_file, 'w+') as res:
res.write(str(data))
print('Path to the results file:', os.path.abspath(result_file))
except IndexError:
pass
print(data)
def test_one_path(self):
map = 'tests/3x3_one_path.xml'
self.assertEqual(
PathFinder.get_shortest_paths(map)['paths'], [{
'points': [{
'row': 1,
'col': 'A'
}, {
'row': 2,
'col': 'A'
}, {
'row': 2,
'col': 'B'
}, {
'row': 2,
'col': 'C'
}]
}])
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 main():
"""
Applies bi-directional search in two phases to find the shortest paths
between every term-pair in the dataset: the first phase finds the nodes
along the shortest paths, while the second reconstructs the paths themselves.
"""
# Get the arguments
args = docopt("""Find the shortest paths between every term-pair in the dataset.
Usage:
search.py <dataset_path> <resource_matrix_path>
<resource_entities_path> <resource_properties_path>
<resource_l2r_path> <max_path_length>
<allow_reversed_edges> <find_relevant_nodes>
<relevant_nodes_file> <paths_out_file>
<dataset_path> = the dataset file
<resource_matrix_path> = the resource adjacency matrix file (.mm/.npz)
<resource_entities_path> = the entity str-id map file
<resource_properties_path> = the property str-id map file
<resource_l2r_path> = the edges file
<max_path_length> = the maximum path length
<allow_reversed_edges> = whether reversed edges are allowed in this resource
<find_relevant_nodes> = whether to find the relevant nodes (or use the results file)
<relevant_nodes_file> = relevant nodes file (input / output)
<paths_out_file> = the paths file (output)
""")
dataset_file = args['<dataset_path>']
resource_mat_file = args['<resource_matrix_path>']
entity_map_file = args['<resource_entities_path>']
property_map_file = args['<resource_properties_path>']
edges_file = args['<resource_l2r_path>']
max_length = int(args['<max_path_length>'])
allow_reversed_edges = args['<allow_reversed_edges>'][0].upper() == 'T'
do_find_relevant_nodes = args['<find_relevant_nodes>'][0].upper() == 'T'
relevant_nodes_file = args['<relevant_nodes_file>']
paths_file = args['<paths_out_file>']
initialize_logger()
# Find relevant nodes
if do_find_relevant_nodes:
# Load the resource
resource = KnowledgeResource(resource_mat_file, entity_map_file,
property_map_file, edges_file, allow_reversed_edges)
adjacency_matrix = resource.adjacency_matrix
term_to_id = resource.term_to_id
# Load the dataset
dataset = load_data_labels(dataset_file, adjacency_matrix)
node_finder = RelevantNodesFinder(adjacency_matrix)
relevant_nodes = find_relevant_nodes(dataset, max_length, relevant_nodes_file, term_to_id, node_finder)
else:
# Load the resource partially according to the relevant nodes
relevant_nodes = load_relevant_nodes(relevant_nodes_file)
resource = KnowledgeResource(resource_mat_file, entity_map_file,
property_map_file, edges_file, allow_reversed_edges, get_all_nodes(relevant_nodes))
adjacency_matrix = resource.adjacency_matrix
term_to_id = resource.term_to_id
# Load the dataset
dataset = load_data_labels(dataset_file, adjacency_matrix)
path_finder = PathFinder(resource)
paths_output = open(paths_file, 'w')
pair_num = 0
# For each term-pair, find relevant nodes and then find paths
for (x, y) in dataset.keys():
pair_num = pair_num + 1
x_id = -1
if x in term_to_id:
x_id = term_to_id[x]
y_id = -1
if y in term_to_id:
y_id = term_to_id[y]
# Limit the search space using the relevant nodes and find paths
nodes = relevant_nodes[(x_id, y_id)]
l2r_edges, r2l_edges = resource.get_nodes_edges(nodes)
paths = path_finder.find_shortest_paths(x_id, y_id, max_length, l2r_edges, r2l_edges)
paths_output.write('pair number ' + str(pair_num) + ': ' + x + '->' + y + '\n')
for path in paths:
paths_output.write(nice_print_path(path, resource.id_to_prop) + '\n')
paths_output.close()
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()
class ProSolver:
matrix = None
def __init__(self):
rospy.init_node('maze_pro_solver', 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
self.map_loader = MapLoader(
start, target) # do not crop if target outside of maze
self.map_matrix = self.map_loader.loadMap()
ProSolver.matrix = self.map_matrix
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
# Calculate path
self.path_finder = PathFinder(self.map_matrix)
raw_path = self.path_finder.calculate_path()
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)
# Setup subscribers
#odom_sub = 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()
while not rospy.is_shutdown():
# Calculate command
ang_speed = 0.4
inc_x = self.goal.x - self.pose.x
inc_y = self.goal.y - self.pose.y
angle_to_goal = atan2(inc_y, inc_x)
real_angle = self.pose.theta
# Normalize angle_diff
if angle_to_goal < 0:
angle_to_goal += 2 * pi
real_angle += 2 * pi
if real_angle < 0:
real_angle += 2 * pi
angle_to_goal += 2 * pi
angle_diff = angle_to_goal - real_angle
if (inc_x**2 + inc_y**2)**0.5 < 0.05:
speed.linear.x = 0.0
speed.angular.z = 0.0
self.next_pose()
elif abs(angle_diff) > 0.2: # increase tolerance?
speed.linear.x = 0.0
if angle_diff < 0:
speed.angular.z = -ang_speed
else:
speed.angular.z = +ang_speed
else:
speed.linear.x = 0.08
speed.angular.z = 0.0
self.cmd_vel_pub.publish(speed)
rate.sleep()
def __init__(self): # type: () -> None
self._path_finder = PathFinder()
def infer(dataset_file, max_length, output_dir, resource_path_list,
whitelist_path):
"""
Receives a list of knowledge resources (each represented by a directory containing all
the resource files), a dataset of unlabeled term-pairs, and the pre-trained whitelist.
Returns the prediction for every term pair.
:param dataset_file - The path of the dataset file with the term-pairs
:param max_length - Maximum path length
:param output_dir - Output directory for the predictions file
:param resource_path_list - A list of resources directories
:param whitelist_path - The path of the pre-trained whitelist
"""
# Load the dataset
dataset = load_dataset(dataset_file)
# Load the whitelist
whitelist = load_whitelist(whitelist_path)
# Load all the resources
resources = []
node_finders = []
path_finders = []
for dir in resource_path_list:
resource = PartialKnowledgeResource(dir + 'Matrix.mm.tmp.npz',
dir + 'Entities.txt',
dir + 'Properties.txt',
dir + '-l2r.txt', whitelist)
resources.append(resource)
adjacency_matrix = resource.adjacency_matrix
node_finders.append(RelevantNodesFinder(adjacency_matrix))
path_finders.append(PathFinder(resource))
with codecs.open(output_dir + '/predictions.txt', 'w', 'utf-8') as f_out:
# For each term-pair, find relevant nodes and then find paths
for (x, y) in dataset:
x = x.decode('utf-8')
y = y.decode('utf-8')
label = False
f_out.write('\t'.join([x, y]))
# Search paths in each resource, using only the allowed paths
for i, resource in enumerate(resources):
x_id, y_id, relevant_nodes = find_relevant_nodes(
x, y, max_length, resource.term_to_id, node_finders[i])
# Limit the search space using the relevant nodes and find paths
l2r_edges, r2l_edges = resource.get_nodes_edges(relevant_nodes)
paths = path_finders[i].find_shortest_paths(
x_id, y_id, max_length, l2r_edges, r2l_edges)
for path in paths:
str_path = nice_print_path(path, resource.id_to_prop)
# Make sure that all the properties are in the whitelist
if whitelist.issuperset(set(str_path.split(' '))):
label = True
f_out.write('\t' + '\t'.join(['True', str_path]) +
'\n')
break
if label:
break
if not label:
f_out.write('\tFalse\n')
def bot_routine(user_id: int, game_id: int, token: str) -> None:
"""mod: this function is main function of the module."""
current_map = Map()
path_finder = PathFinder()
start_position = None
next_move = None
turbo_flag = False
current_state_response = None
game_status_flag = True
def send_current_state_request() -> None:
"""mod: this function sends REST API request."""
global api_gateway_urls, get_current_state_method
nonlocal user_id, game_id, token, current_state_response
current_state_response = requests.get(
api_gateway_urls + get_current_state_method,
params={'GameId': game_id})
current_state_response.raise_for_status()
def send_post_move_request() -> None:
global api_gateway_urls, post_move_method
"""mod: this function sends REST API request."""
nonlocal start_position, next_move, turbo_flag, user_id, game_id, token
direction = None
if next_move.row_index > start_position.row_index:
direction = 'UP'
elif next_move.row_index < start_position.row_index:
direction = 'DOWN'
elif next_move.column_index > start_position.column_index:
direction = 'RIGHT'
elif next_move.column_index < start_position.column_index:
direction = 'LEFT'
response = requests.post(
api_gateway_urls + post_move_method,
json={'Direction': direction, 'UserID': user_id, 'TurboFlag': turbo_flag})
response.raise_for_status()
def send_unregister_user_request() -> None:
"""mod: this function sends REST API request."""
global api_gateway_urls, unregister_user_method
nonlocal token
response = requests.delete(api_gateway_urls + unregister_user_method,
params={"token": token})
response.raise_for_status()
def parse_current_state_response() -> None:
"""mod: this function parses REST API response."""
nonlocal current_state_response, current_map, path_finder, start_position, game_status_flag, user_id
game_state = json.loads(current_state_response.form.get('data'))
time_to_update = 0
current_map.map_scheme['height'] = game_state['map']['height']
current_map.map_scheme['width'] = game_state['map']['width']
current_map.obstacles_positions = game_state['map']['obstacles']
current_map.bots_positions = game_state['map']['power-ups']
current_map.traces_positions = game_state['map']['tracers']
path_finder.tron_map = current_map
players_positions = list()
for player in game_state['players']:
if player['id'] == user_id:
start_position = player['headPosition']
path_finder.start_position = start_position
path_finder.turbos_number = player['turboAmount']
time_to_update = player["timeToUpdate"]
else:
players_positions.append(player['headPosition'])
current_map.players_positions = players_positions
if not time_to_update:
game_status_flag = False
elif time_to_update < 1:
path_finder.algorithm = 'Random'
elif time_to_update < 2:
path_finder.algorithm = 'Survival'
else:
path_finder.algorithm = 'A*'
while True:
try:
send_current_state_request()
parse_current_state_response()
if not game_status_flag:
send_unregister_user_request()
return
try:
next_move, turbo_flag = path_finder.get_direction()
send_post_move_request()
except NoSolution:
continue
except requests.exceptions.HTTPError as http_err:
print(f'HTTP error occurred: {http_err}')
return
from path_finder import PathFinder
from structures import Coords
from flask import Flask, jsonify, request
#from json import jsonify
app = Flask(__name__)
pf = PathFinder()
@app.route('/', methods=['POST'])
def execute_smartshopping():
content = request.get_json(silent=True)
print(content)
activities = []
for activity in content['activities']:
activities.append(activity)
home_coords = Coords(latitude=content['latitude'], longitude=content['longitude'])
best_path = pf.find_best_path(desired_activities=activities, my_coords=home_coords)
print(best_path)
return jsonify(best_path)
class Challenge1TestCase(unittest.TestCase):
VIZ = False
def setUp(self):
self.path_finder = PathFinder()
def tearDown(self):
self.path_finder = None
def _run_test(self, grid, queries):
for query in queries:
path = self.path_finder.get_path(grid, query[0], query[1])
if Challenge1TestCase.VIZ:
PathVisualizer.viz_path(grid, query, path)
self.assertTrue(PathValidator.is_valid_path(grid, query, path),
"Invalid path %s for query %s." % (path, query))
def _run_test_with_no_solution(self, grid, queries):
for query in queries:
path = self.path_finder.get_path(grid, query[0], query[1])
if Challenge1TestCase.VIZ:
PathVisualizer.viz_path(grid, query, path)
self.assertTrue(len(path) == 2, "No path should be return from this one")
def test_no_obstacles_straight_line(self):
grid = np.zeros((20, 20)).astype(np.bool)
queries = [
[Waypoint(5, 5, 0), Waypoint(5, 8, 0)],
[Waypoint(16, 5, 1), Waypoint(8, 5, 1)],
[Waypoint(5, 15, 3), Waypoint(16, 15, 3)],
]
self._run_test(grid, queries)
def test_no_obstacles_with_turns(self):
grid = np.zeros((20, 20)).astype(np.bool)
queries = [
[Waypoint(5, 7, 0), Waypoint(15, 8, 3)],
[Waypoint(16, 5, 2), Waypoint(8, 5, 1)],
[Waypoint(15, 15, 1), Waypoint(16, 15, 3)],
]
self._run_test(grid, queries)
def test_with_one_obstacle(self):
grid = np.zeros((20, 20)).astype(np.bool)
grid[10:14, 10:14] = True
queries = [
[Waypoint(5, 7, 0), Waypoint(15, 11, 3)],
[Waypoint(16, 5, 2), Waypoint(8, 5, 1)],
[Waypoint(15, 15, 1), Waypoint(16, 15, 3)],
]
self._run_test(grid, queries)
def test_with_random_obstacle(self):
grid = np.zeros((20, 20)).astype(np.bool)
for variable in range(1,4):
rand_a = random.randint(1, 10)
rand_b = random.randint(0, 5)
rand_c = random.randint(5, 10)
rand_d = random.randint(0, 5)
grid[rand_a:rand_a + rand_b, rand_c:rand_c + rand_d] = True
queries = [
[Waypoint(0, 0, 0), Waypoint(19, 19, 3)],
]
self._run_test(grid, queries)
def test_with_multiple_obstacles(self):
grid = np.zeros((20, 20)).astype(np.bool)
grid[3:4, 0:15] = True
grid[13:14, 5:20] = True
queries = [
[Waypoint(0, 0, 0), Waypoint(19, 19, 3)]
]
self._run_test(grid, queries)
def test_with_complex_obstacles(self):
grid = np.zeros((20, 20)).astype(np.bool)
grid[3:4, 5:20] = True
grid[7:8, 0:15] = True
grid[13:14, 5:20] = True
grid[11:14, 4:5] = True
grid[14:17, 7:8] = True
grid[15:20, 12:13] = True
queries = [
[Waypoint(0, 0, 0), Waypoint(19, 19, 3)]
]
self._run_test(grid, queries)
def test_with_no_solution(self):
grid = np.zeros((20, 20)).astype(np.bool)
grid[15:16, 0:10] = True
grid[15:20, 10:11] = True
queries = [
[Waypoint(0, 0, 0), Waypoint(19, 2, 3)]
]
self._run_test_with_no_solution(grid, queries)
def test_start_or_end_out_of_matrix(self):
map = 'tests/3x3_start_out_of_matrix.xml'
self.assertEqual(PathFinder.get_shortest_paths(map),
"Start/End node not in matrix.")
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()
def test_invalid_xml_content(self):
map = 'tests/3x3_invalid_xml_tag.xml'
self.assertEqual(PathFinder.get_shortest_paths(map),
"Invalid content of .xml file.")
def setUp(self):
self.path_finder = PathFinder()
WINDOW_SIZE = [255, 255]
goal_reached = False
if __name__ == '__main__':
done = False
file = input('Enter file name : ')
maze = MazeMaker('./input/' + file)
grid = maze.grid
root = maze.start_pos
goal = maze.end_pos
visited = maze.visited
path = PathFinder(grid,root,goal,visited)
final_path = path.path
print(final_path)
node_stack = path.node_stack
# for i in range(len(node_stack)):
# print(node_stack[i])
print(len(node_stack))
pygame.init()
screen = pygame.display.set_mode(WINDOW_SIZE)
pygame.display.set_caption("Array Backed Grid")
clock = pygame.time.Clock()
i = 0
while not done:
for event in pygame.event.get():
if event.type == pygame.QUIT:
new_obstacle.set_uid(uid)
env.add_obstacle(new_obstacle)
# update position of existing tag
else:
new_obstacle = deepcopy(obstacle)
new_obstacle = transform_shape(new_obstacle, rotation,
translation)
env.update_obstacle(new_obstacle)
# create boundaries for obstacles
env.create_boundaries()
print("Done creating boundaries")
# create a planner with current env, agent, and goal location and solve for a path
planner = PathFinder(env, env.agent, goal_.points[0])
planner.solve()
print("Done solving")
profiler = Profiler()
profiler.add_path(planner.export_path())
smooth_path = profiler.get_profile()
i = 0
# The following parameters may need to be tuned
step_size = 0.01
step_size_rot = 0.005
threshold = 0.05
rot_threshold = 0.01