def configure_visualiser(config: Configuration, args: argparse.Namespace) -> bool:
if args.visualiser:
config.load_simulator = True
config.simulator_graphics = True
if args.visualiser_flags is not None and not arg_valid("visualiser", args):
return False
return True
def augment_label_maps(self, atlases: List[str], feature_list: List[str], label_list: List[str],
single_feature_list: List[str], single_label_list: List[str]) -> None:
if not atlases:
return
self.__services.debug.write("""Starting Augmentation: [
atlases: {},
feature_list: {},
label_list: {},
single_feature_list: {},
single_label_list: {}
]
""".format(atlases, feature_list, label_list, single_feature_list, single_label_list), DebugLevel.BASIC)
label_atlas_name = "training_" + "_".join(atlases)
self.__services.debug.write("Loading maps", DebugLevel.BASIC)
maps: List[Map] = []
for name in atlases:
maps = maps + self.__services.resources.maps_dir.get_atlas(name).load_all()
self.__services.debug.write("Loading atlas", DebugLevel.BASIC)
t: List[Dict[str, any]] = self.__services.resources.training_data_dir.load(label_atlas_name)
progress_bar: Progress = self.__services.debug.progress_debug(len(t), DebugLevel.BASIC)
progress_bar.start()
for i in range(len(t)):
config = Configuration()
config.simulator_algorithm_type = AStar
config.simulator_testing_type = AStarTesting
config.simulator_initial_map = maps[i]
services: Services = Services(config)
simulator: Simulator = Simulator(services)
testing: AStarTesting = simulator.start()
if feature_list:
seq_features = MapProcessing.get_sequential_features(testing.map, feature_list)
for q in range(len(t[i]["features"])):
t[i]["features"][q].update(seq_features[q])
if label_list:
seq_labels = MapProcessing.get_sequential_labels(testing.map, label_list)
for q in range(len(t[i]["labels"])):
# print(q)
t[i]["labels"][q].update(seq_labels[q])
if single_feature_list:
t[i]["single_features"].update(MapProcessing.get_single_features(maps[i], single_feature_list))
if single_label_list:
t[i]["single_labels"].update(MapProcessing.get_single_labels(maps[i], single_label_list))
progress_bar.step()
self.__services.debug.write("Saving atlas augmentation: " + str(label_atlas_name), DebugLevel.BASIC)
self.__services.resources.training_data_dir.save(label_atlas_name, t)
self.__services.debug.write("Finished atlas augmentation: " + str(label_atlas_name) + "\n", DebugLevel.BASIC)
def __label_single_maps(self, atlas_name, feature_list: List[str], label_list: List[str],
single_feature_list: List[str], single_label_list: List[str], overwrite: bool) -> List[
Dict[str, any]]:
"""
Passed atlas name, feature list, label list, and returns res object with the map features labelled for training
"""
if not atlas_name:
return []
if not overwrite and self.__services.resources.training_data_dir.exists("training_" + atlas_name, ".pickle"):
self.__services.debug.write("Found in training data. Loading from training data", DebugLevel.BASIC)
return self.__services.resources.training_data_dir.load("training_" + atlas_name)
self.__services.debug.write("Loading maps", DebugLevel.BASIC)
maps: List[Map] = self.__services.resources.maps_dir.get_atlas(atlas_name).load_all()
res: List[Dict[str, any]] = []
progress_bar: Progress = self.__services.debug.progress_debug(len(maps), DebugLevel.BASIC)
progress_bar.start()
# process atlas
for m in maps:
config = Configuration()
config.simulator_algorithm_type = AStar
config.simulator_testing_type = AStarTesting
config.simulator_initial_map = m
services: Services = Services(config)
simulator: Simulator = Simulator(services)
testing: AStarTesting = simulator.start()
features: Dict[str, any] = {}
arg: str
for arg in ["map_obstacles_percentage",
"goal_found",
"distance_to_goal",
"original_distance_to_goal",
"trace",
"total_steps",
"total_distance",
"total_time",
"algorithm_type",
"fringe",
"search_space"
]:
features[arg] = testing.get_results()[arg]
features["features"] = MapProcessing.get_sequential_features(testing.map, feature_list)
features["labels"] = MapProcessing.get_sequential_labels(testing.map, label_list)
features["single_features"] = MapProcessing.get_single_features(m, single_feature_list)
features["single_labels"] = MapProcessing.get_single_labels(m, single_label_list)
res.append(features)
progress_bar.step()
return res
def get_new_runner(self,
mp: 'Map',
algorithm_type: Type['Algorithm'],
algorithm_parameters: Tuple[List, Dict] = None,
algorithm_testing: Optional[
Type['BasicTesting']] = None,
with_animations: bool = False) -> 'AlgorithmRunner':
if not algorithm_parameters:
algorithm_parameters = [], {}
from simulator.services.services import Services
config = Configuration()
config.simulator_initial_map = mp
config.simulator_algorithm_type = algorithm_type
config.simulator_testing_type = algorithm_testing
config.simulator_algorithm_parameters = algorithm_parameters
if with_animations and algorithm_testing:
config.simulator_graphics = True
config.simulator_key_frame_speed = self._services.settings.simulator_key_frame_speed
config.simulator_key_frame_skip = self._services.settings.simulator_key_frame_skip
s = Services(config)
if with_animations and algorithm_testing:
s.algorithm.__pass_root_key_frame = self.__pass_root_key_frame
return s.algorithm
def configure_common(config: Configuration, args: argparse.Namespace) -> bool:
# for generator & analyzer
config.num_dim = args.dims
if args.include_builtin_algorithms:
config.algorithms.update(AlgorithmManager.builtins)
if args.list_maps:
print("Available maps:")
for key in MapManager.builtins.keys():
print(f" {key}")
print("Can also specify a custom map,")
print(" (1) cached map stored in Maps")
print(" (2) external file that contains a global variable with type that inherits from Map")
sys.exit(0)
if args.maps:
maps = MapManager.load_all(args.maps)
if not all(maps):
invalid_maps = [args.maps[i] for i in range(len(maps)) if not maps[i]]
invalid_str = ",".join('"' + a + '"' for a in invalid_maps)
valid_str = ",".join('"' + a + '"' for a in MapManager.builtins.keys())
print(f"Invalid map(s) specified: {invalid_str}", file=sys.stderr)
print(f"Available maps: {valid_str}", file=sys.stderr)
print("Can also specify a custom map,", file=sys.stderr)
print(" (1) cached map stored in Maps", file=sys.stderr)
print(" (2) external file that contains a global variable with type that inherits from Map", file=sys.stderr)
return False
maps = list(flatten(maps, depth=1))
# name uniqueness
names = [a[0] for a in maps]
if len(set(names)) != len(names):
print("Name conflict detected in custom map list:", names, file=sys.stderr)
return False
maps = dict(maps)
if args.include_default_builtin_maps or args.include_all_builtin_maps:
maps.update(MapManager.builtins)
if args.include_all_builtin_maps:
maps.update(MapManager.cached_builtins)
config.maps = maps
elif args.include_all_builtin_maps:
config.maps.update(MapManager.cached_builtins)
return True
def configure_generator(config: Configuration, args: argparse.Namespace) -> bool:
if args.generator:
config.generator = True
if args.room_size:
if not arg_valid("generator", args):
return False
config.generator_min_room_size = args.room_size[0]
config.generator_max_room_size = args.room_size[1]
if args.fill_rate:
if not arg_valid("generator", args):
return False
config.generator_obstacle_fill_min = args.fill_rate[0]
config.generator_obstacle_fill_max = args.fill_rate[1]
if args.generatortype:
if not arg_valid("generator", args):
return False
config.generator_gen_type = args.generatortype
if args.num_maps:
if not arg_valid("generator", args):
return False
config.generator_nr_of_examples = args.num_maps
return True
def main():
c: Configuration = Configuration()
# set configuration params
c.simulator_write_debug_level = DebugLevel.HIGH
main_runner = MainRunner(c)
analyzer = Analyzer(main_runner.main_services)
analyzer.analyze_algorithms()
def configure_algorithms_flags(config: Configuration,
args: argparse.Namespace) -> bool:
if args.list_algorithms:
print("Available algorithms:")
for key in AlgorithmManager.builtins.keys():
print(f" {key}")
print(
"Or specify your own file that contains a class that inherits from Algorithm"
)
sys.exit(0)
d = vars(args)
if "algorithm" in d and d["algorithm"]:
in_algorithms = [d["algorithm"]]
elif "algorithms" in d:
in_algorithms = d["algorithms"]
else:
in_algorithms = []
if in_algorithms:
print(in_algorithms)
algorithms = AlgorithmManager.load_all(in_algorithms)
if not all(algorithms):
invalid_algorithms = [
in_algorithms[i] for i in range(len(algorithms))
if not algorithms[i]
]
invalid_str = ",".join('"' + a + '"' for a in invalid_algorithms)
valid_str = ",".join('"' + a + '"'
for a in AlgorithmManager.builtins.keys())
print(f"Invalid algorithm(s) specified: {invalid_str}",
file=sys.stderr)
print(f"Available algorithms: {valid_str}", file=sys.stderr)
print(
"Or specify your own file that contains a class that inherits from Algorithm",
file=sys.stderr)
return False
algorithms = list(flatten(algorithms, depth=1))
# name uniqueness
names = [a[0] for a in algorithms]
if len(set(names)) != len(names):
print("Name conflict detected in custom algorithm list:",
names,
file=sys.stderr)
return False
algorithms = dict(algorithms)
config.algorithms = algorithms
return True
def _setup_sim(self,
config: Optional[Configuration] = None,
goal: Optional[Point] = None) -> Simulator:
"""
Sets up the simulator (e.g. algorithm and map configuration).
"""
while self.grid is None or self.agent is None:
rospy.loginfo("Waiting for grid and agent to initialise...")
rospy.sleep(0.5)
if config is None:
config = Configuration()
# general
config.simulator_graphics = True
config.simulator_key_frame_speed = 0.16
config.simulator_key_frame_skip = 20
config.get_agent_position = lambda: self._world_to_grid(
Point(self.agent.pose.position.x, self.agent.pose.position.y))
config.visualiser_simulator_config = False # hide the simulator config window
# algorithm
if config.algorithm_name is None:
config.algorithm_name = "WPN-view"
config.simulator_algorithm_type, config.simulator_testing_type, config.simulator_algorithm_parameters = config.algorithms[
config.algorithm_name]
# map
goal = Goal(Point(0, 0) if goal is None else goal)
agent = Agent(self._world_to_grid(
Point(self.agent.pose.position.x, self.agent.pose.position.y)),
radius=self.INFLATE)
mp = RosMap(agent,
goal,
lambda: self.grid,
traversable_threshold=self.TRAVERSABLE_THRESHOLD,
unmapped_value=-1,
wp_publish=self._send_way_point,
update_requested=self._map_update_requested,
name="ROS Map")
config.maps = {mp.name: mp}
config.simulator_initial_map = list(config.maps.values())[0]
config.map_name = list(config.maps.keys())[0]
# create the simulator
s = Services(config)
s.algorithm.map.request_update()
sim = Simulator(s)
return sim
def main() -> bool:
parser = argparse.ArgumentParser(
prog="ros.py",
description="PathBench 2D ROS extension runner",
formatter_class=argparse.RawTextHelpFormatter)
configurers: List[Callable[[Configuration, argparse.Namespace], bool]] = []
configurers.append(
add_configuration_flags(parser,
visualiser_flags=True,
algorithms_flags=True,
multiple_algorithms_specifiable=False))
parser.add_argument("-g",
"--goal",
nargs=2,
type=int,
help="goal position \"x y\"")
args = parser.parse_args()
print("args:{}".format(args))
config = Configuration()
for c in configurers:
if not c(config, args):
return False
if args.algorithm:
config.algorithm_name = list(config.algorithms.keys())[0]
config.simulator_algorithm_type, config.simulator_testing_type, config.simulator_algorithm_parameters = config.algorithms[
config.algorithm_name]
goal = Point(*args.goal) if args.goal else None
ros = Ros()
ros.start(config, goal)
return True
def __start_simulator_callback(gui): #Setting the configuration file.
config = Configuration()
mp = gui.__maps[gui.__map_choice.get()]
algo = gui.__algorithms[gui.__algorithm_choice.get()]
ani = gui.__animations[gui.__animations_choice.get()]
debug = gui.__debug[gui.__debug_choice.get()]
config.load_simulator = True
config.simulator_graphics = True
config.simulator_initial_map, config.simulator_grid_display = mp
config.simulator_algorithm_type, config.simulator_testing_type, config.simulator_algorithm_parameters = algo
config.simulator_key_frame_speed, config.simulator_key_frame_skip = ani
config.simulator_write_debug_level = debug
MainRunner(config).run()
def main() -> bool:
parser = argparse.ArgumentParser(
prog="ros.py",
description="ROS 2D Occupancy Grid Publish-Subscriber Demo",
formatter_class=argparse.RawTextHelpFormatter)
configurers: List[Callable[[Configuration, argparse.Namespace], bool]] = []
configurers.append(
add_configuration_flags(parser,
visualiser_flags=True,
algorithms_flags=True,
multiple_algorithms_specifiable=False))
parser.add_argument("-f",
"--fake",
action="store_true",
help="fake the ROS publisher")
args = parser.parse_args()
print("args:{}".format(args))
config = Configuration()
for c in configurers:
if not c(config, args):
return False
if args.algorithm:
config.algorithm_name = list(config.algorithms.keys())[0]
config.simulator_algorithm_type, config.simulator_testing_type, config.simulator_algorithm_parameters = config.algorithms[
config.algorithm_name]
ros = Ros(fake=args.fake)
ros.start(config)
return True
def configure_and_run(args: argparse.Namespace, configurers: List[Callable[[Configuration, argparse.Namespace], bool]]):
config = Configuration()
for c in configurers:
if not c(config, args):
return False
if not configure_common(config, args) or \
not configure_generator(config, args) or \
not configure_analyzer(config, args) or \
not configure_trainer(config, args) or \
not configure_visualiser(config, args):
return False
mr = MainRunner(config)
mr.run()
return True
def _setup_sim(self, config: Optional[Configuration] = None) -> Simulator:
"""
Sets up the simulator (e.g. algorithm and map configuration).
"""
while self.grid is None:
rospy.loginfo("Waiting for grid to initialise...")
rospy.sleep(0.5)
if config is None:
config = Configuration()
# general
config.simulator_graphics = True
config.simulator_key_frame_speed = 0.16
config.simulator_key_frame_skip = 20
config.visualiser_simulator_config = False # hide the simulator config window
# algorithm
if config.algorithm_name is None:
config.algorithm_name = "A*"
config.simulator_algorithm_type, config.simulator_testing_type, config.simulator_algorithm_parameters = config.algorithms[
config.algorithm_name]
# map
mp = RosMap(Agent(Point(40, 40)),
Goal(Point(30, 20)),
lambda: self.grid,
traversable_threshold=0.4,
name="ROS Map")
config.maps = {mp.name: mp}
config.simulator_initial_map = list(config.maps.values())[0]
config.map_name = list(config.maps.keys())[0]
# create the simulator
s = Services(config)
s.algorithm.map.request_update()
sim = Simulator(s)
return sim
def __run_simulation(self,
grid: Map,
algorithm_type: Type[Algorithm],
testing_type: Type[BasicTesting],
algo_params: Tuple[list, dict],
agent_pos: Point = None) -> Dict[str, Any]:
config = Configuration()
config.simulator_initial_map = copy.deepcopy(grid)
config.simulator_algorithm_type = algorithm_type
config.simulator_testing_type = testing_type
config.simulator_algorithm_parameters = algo_params
if agent_pos:
config.simulator_initial_map.move_agent(agent_pos, True, False)
sim: Simulator = Simulator(Services(config))
return sim.start().get_results()
def configure_debug_flag(config: Configuration,
args: argparse.Namespace) -> bool:
config.simulator_write_debug_level = getattr(DebugLevel, args.debug)
return True
def configure_analyzer(config: Configuration, args: argparse.Namespace) -> bool:
if args.analyzer:
config.analyzer = True
return True
def configure_trainer(config: Configuration, args: argparse.Namespace) -> bool:
if args.trainer:
config.trainer = True
return True
from algorithms.basic_testing import BasicTesting
from algorithms.classic.testing.a_star_testing import AStarTesting
#from algorithms.classic.testing.combined_online_lstm_testing import CombinedOnlineLSTMTesting
from algorithms.classic.testing.dijkstra_testing import DijkstraTesting
from algorithms.classic.testing.wavefront_testing import WavefrontTesting
#from algorithms.classic.testing.way_point_navigation_testing import WayPointNavigationTesting
#Dictionaries of possible options
from main_gui import GUI
'''
This script is used on SSH servers to convert png House Expo images to PB compatible pickled maps.
'''
config = type(Configuration)
config = Configuration()
maps = {
"Uniform Random Fill": ("uniform_random_fill_10/0", True),
"Block": ("block_map_10/6", True),
"House": ("house_10/6", True),
"Long Wall": (Maps.grid_map_labyrinth2, True),
"Labyrinth": (Maps.grid_map_labyrinth, True),
"Small Obstacle":
(Maps.grid_map_one_obstacle.convert_to_dense_map(), True),
"SLAM Map 1": ("map10", False),
"SLAM Map 1 (compressed)": ("map11", True),
"SLAM Map 2": ("map14", False),
"SLAM Map 3": ("map12", False),
"House Expo Sample": ("_house_expo/house_expo_52", False)
}
def __init__(self):
from algorithms.configuration.configuration import Configuration
self.__services = Services(Configuration())
parser.add_argument('-m',
'--model',
choices=['OnlineLSTM', "CAE", "LSTMCAEModel"],
default='LSTM',
help="model to train")
parser.add_argument('-f',
'--full_train',
action='store_true',
help='set this to train from scratch')
args = parser.parse_args()
print("args:{}".format(args))
config = type(Configuration)
config = Configuration()
maps = MapManager.builtins
# LSTM Bagging is referred to as CombinedOnlineLSTM, it is used as a global kernel for LWP
algorithms = {
"A*": (AStar, AStarTesting, ([], {})),
"Global Way-point LSTM":
(WayPointNavigation, WayPointNavigationTesting, ([], {
"global_kernel": (CombinedOnlineLSTM, ([], {})),
"global_kernel_max_it":
100
})),
"LSTM Bagging": (CombinedOnlineLSTM, CombinedOnlineLSTMTesting, ([], {})),
"CAE Online LSTM": (OnlineLSTM, BasicTesting, ([], {