def path_planning_controller(config):
"""
Factory for a PID controller which uses path planning in order to determine what waypoints must be visited.
Inputs:
- f: dictionary containing all necessary configuration variables, namely a list of goals, and 6 proportionality
constants (ap, ai, ad, lp, li, ld) which will be used to define the PID controller proper (a stands for angular,
l for linear), as well as the algorithm to be used, the start and goal points, the heuristic to be used, and
configuration variables linked to the specific algorithm. These configuration variables must be a list of waypoints
if the variant of the algorithm uses a navigation mesh, or a number representing the number of divisions to perform
on each axis of the map if it's grid-based.
Outputs:
- A fully configured and ready to use Sequential_PID_Controller object.
"""
# Loads the controller config (only 1 expected) and overwrite the parameters with the ones in the scenario (it allows default values to be set at controller)
f = {**config['controllers'][0], **config}
if f['start'] == f['goal']:
raise ValueError('Start and goal are the same spot.')
controller = Sequential_PID_Controller(config)
if 'mesh' in f['algorithm']:
controller.goal = pp.run_path_planning_mesh(f['waypoints'],
f['algorithm'], f['start'],
f['goal'], f['heuristic'])
else:
controller.goal = pp.run_path_planning(f['grid_size'], f['algorithm'],
f['start'], f['goal'],
f['heuristic'])
return controller
def path_planning_controller():
"""
Factory for a PID controller which uses path planning in order to determine what waypoints must be visited.
Inputs:
- f: dictionary containing all necessary configuration variables, namely a list of goals, and 6 proportionality
constants (ap, ai, ad, lp, li, ld) which will be used to define the PID controller proper (a stands for angular,
l for linear), as well as the algorithm to be used, the start and goal points, the heuristic to be used, and
configuration variables linked to the specific algorithm. These configuration variables must be a list of waypoints
if the variant of the algorithm uses a navigation mesh, or a number representing the number of divisions to perform
on each axis of the map if it's grid-based.
Outputs:
- A fully configured and ready to use Sequential_PID_Controller object.
"""
with open('../conf/controller-pathplanning.json', 'r') as fp:
f = json.load(fp)
if f['start'] == f['goal']:
raise ValueError('Start and goal are the same spot.')
controller = Sequential_PID_Controller()
if 'mesh' in f['algorithm']:
controller.goal = pp.run_path_planning_mesh(
f['waypoints'], f['algorithm'], f['start'], f['goal'],
f['heuristic'])
else:
controller.goal = pp.run_path_planning(f['grid_size'],
f['algorithm'], f['start'],
f['goal'], f['heuristic'])
return controller
def __calculate_path(self, dst):
step = 40
shape = u.npdata.shape
step_x = shape[0] / step
step_y = shape[1] / step
self.goal = pp.run_path_planning(step,
start=(int(self.robot.x / step_x),
int(self.robot.y / step_y)),
finish=(int(dst[0]), int(dst[1])),
show_grid=True)
def calculate_path(src, dst):
step = 40
shape = pp.npdata.shape
step_x = shape[0]/step
step_y = shape[1]/step
# Add algo='algo name' and heur='heur name' to the parameters for run_path_planning
# in order to modify the behavior of the path planning part of the execution.
return pp.run_path_planning(step,
start=(src[0], src[1]),
finish=(dst[0], dst[1]), algo='A*', heur='naive',
show_grid=True)
def run_path_planning(scenario_file, navmesh_file, grid_size, algorithm,
heuristic, start, finish):
pp.load_image(scenario_file)
if navmesh_file:
with open(navmesh_file) as navmesh:
path = pp.run_path_planning_mesh(navmesh['mesh'],
algo=algorithm,
heur=heuristic,
start=(
int(start.replace('(', '').\
replace(')', '').\
replace(' ', '').split(',')[0]),
int(start.replace('(', '').\
replace(')', '').\
replace(' ', '').split(',')[1])),
finish=(
int(finish.replace('(', '').\
replace(')', '').\
replace(' ', '').split(',')[0]),
int(finish.replace('(', '').\
replace(')', '').\
replace(' ', '').split(',')[1])))
else:
if not grid_size:
print("Please, define a grid size.")
exit()
path = pp.run_path_planning(int(grid_size), algo=algorithm,
heur=heuristic, start=(
int(start.replace('(', '').\
replace(')', '').\
replace(' ', '').split(',')[0]),
int(start.replace('(', '').\
replace(')', '').\
replace(' ', '').split(',')[1])),
finish=(
int(finish.replace('(', '').\
replace(')', '').\
replace(' ', '').split(',')[0]),
int(finish.replace('(', '').\
replace(')', '').\
replace(' ', '').split(',')[1])))
pp.output_image(scenario_file, path)