This repository has been archived by the owner on Sep 16, 2021. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 6
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
21 changed files
with
429 additions
and
583 deletions.
There are no files selected for viewing
This file was deleted.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,5 +1,6 @@ | ||
| <?xml version="1.0" ?> | ||
| <package format="2"> | ||
| <?xml version="1.0"?> | ||
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>planning_simulator_launcher</name> | ||
| <version>0.0.0</version> | ||
| <description>The planning_simulator_launcher package</description> | ||
|
|
@@ -9,8 +10,9 @@ | |
| <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer> | ||
|
|
||
| <license>Apache 2</license> | ||
| <exec_depend>scenario_runner</exec_depend> | ||
|
|
||
| <buildtool_depend>catkin</buildtool_depend> | ||
|
|
||
| <depend>rospy</depend> | ||
| <export> | ||
| <build_type>ament_python</build_type> | ||
| </export> | ||
| </package> | ||
File renamed without changes.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,153 @@ | ||
| import argparse | ||
| import json | ||
| from pathlib import Path | ||
| from typing import Optional, Union | ||
|
|
||
| import launch | ||
| from planning_simulator_launcher.launch_script import launch_description | ||
| from planning_simulator_launcher.scenario_parser import ScenarioParser | ||
|
|
||
|
|
||
| # If a base dir is given, resolves relative paths relative to the base dir. | ||
| # Otherwise, ensures that the path is absolute. | ||
| def absolute_path(path: Union[Path, str], base_dir: Optional[Path] = None): | ||
| path = Path(path) | ||
| if path.is_absolute(): | ||
| return path.resolve() | ||
| if base_dir: | ||
| return (base_dir / path).resolve() | ||
| raise ValueError(f"Relative path '{path}' is not allowed.") | ||
|
|
||
|
|
||
| class Database: | ||
| def __init__(self, database_path: Path, *, base_dir: Optional[Path] = None): | ||
| print('Loading Database') | ||
| print(f' Opening \x1b[36m{database_path}\x1b[0m => ', end='') | ||
| with database_path.open() as file: | ||
| database = json.load(file) | ||
| print('\x1b[32mSuccess\x1b[0m') | ||
|
|
||
| print(f' Validating => ', end='') | ||
| self.launch_path = absolute_path(database['launch_path'], base_dir) | ||
| # Check if it exists, because the error message if we try to access it | ||
| # later is not helpful. | ||
| if not self.launch_path.is_file(): | ||
| raise ValueError(f"launch_path '{self.launch_path}' is not a file") | ||
|
|
||
| self.log_output_base_dir = absolute_path(database['log_output_base_dir'], base_dir) | ||
|
|
||
| self.map = {name: absolute_path(path, base_dir) for name, path in database['map'].items()} | ||
| for path in self.map.values(): | ||
| if not path.is_file(): | ||
| raise ValueError(f"map '{path}' is not a file") | ||
|
|
||
| self.scenario = [absolute_path(path, base_dir) for path in database['scenario']] | ||
| for path in self.scenario: | ||
| if not path.is_file(): | ||
| raise ValueError(f"scenario '{path}' is not a file") | ||
| print('\x1b[32mSuccess\x1b[0m') | ||
|
|
||
|
|
||
| class Launcher: | ||
| def __init__(self, args): | ||
| # Only created for validation, doesn't need to be stored | ||
| database = Database(args.database) | ||
|
|
||
| # Only use database if command line arg is None | ||
| self.launch_path = args.launch_path or database.launch_path | ||
| self.log_output_base_dir = args.log_output_base_dir or database.log_output_base_dir | ||
| # Similar, but wrap it in a list | ||
| self.scenarios = [args.scenario] if args.scenario else database.scenario | ||
| self.map = database.map | ||
| self.timeout = args.timeout | ||
| self.log_output_type = args.log_output | ||
|
|
||
|
|
||
| # Look up the map name in the scenario in scenario_database.json | ||
| def map_path(self, parser: ScenarioParser) -> Path: | ||
| scenario_map_path = parser.getMapPath() | ||
| print(f' Loading map \x1b[36m{scenario_map_path}\x1b[0m as ', end='') | ||
|
|
||
| # If the key is not found, return the key itself | ||
| map_path = self.map.get( | ||
| parser.getMapPath(), | ||
| parser.getMapPath() | ||
| ) | ||
|
|
||
| if map_path == parser.getMapPath(): | ||
| print('is') | ||
| else: | ||
| print(f'\x1b[36m{map_path}\x1b[0m') | ||
|
|
||
| return absolute_path(map_path) | ||
|
|
||
|
|
||
| def run_all_scenarios(self): | ||
| # This allows running launch files from this script | ||
| ls = launch.LaunchService() | ||
|
|
||
| if not self.log_output_base_dir.is_dir(): | ||
| self.log_output_base_dir.mkdir(parents=True) | ||
|
|
||
| for scenario_path in self.scenarios: | ||
| print(f'\nRunning scenario \x1b[36m{scenario_path}\x1b[0m') | ||
| print(' Parameters') | ||
| parser = ScenarioParser(str(scenario_path), generate_mode=True) | ||
|
|
||
| n_total = len(parser.scenario_files_path) | ||
| for n, scenario_path in enumerate(parser.scenario_files_path, start=1): | ||
| scenario_path = Path(scenario_path) | ||
| print(f'\nLaunch {n} of {n_total}') | ||
| print(f' Scenario \x1b[36m{scenario_path}\x1b[0m') | ||
|
|
||
| ld = launch_description( | ||
| scenario_path=str(scenario_path), | ||
| scenario_id=parser.getScenarioId(), | ||
| log_dir=str(self.log_output_base_dir), | ||
| map_path=str(self.map_path(parser)), | ||
| uuid=scenario_path.stem, | ||
| launch_path=str(self.launch_path), | ||
| ) | ||
| ls.include_launch_description(ld) | ||
| ls.run() # TODO: Timeout | ||
| ls.shutdown() | ||
|
|
||
|
|
||
| def main(): | ||
| parser = argparse.ArgumentParser(description='launch simulator') | ||
| parser.add_argument( | ||
| '--timeout', type=int, default=180, | ||
| help='Specify simulation time limit in seconds. \ | ||
| The default is 180 seconds.') | ||
| parser.add_argument( | ||
| '--database', | ||
| type=Path, | ||
| required=True, | ||
| help='Specify path to your scenario_database.json.') | ||
| parser.add_argument( | ||
| '--launch-path', | ||
| type=absolute_path, | ||
| help='Overrides the path to your launch file from scenario_database.json') | ||
| parser.add_argument( | ||
| '--log-output', | ||
| choices=['screen', 'log', 'both', 'own_log', 'full'], | ||
| default='screen', | ||
| help='Specify the type of log output.') | ||
| parser.add_argument( | ||
| '--log-output-base-dir', | ||
| type=absolute_path, | ||
| help='Overrides the log directory output from scenario_database.json') | ||
| parser.add_argument( | ||
| '--scenario', | ||
| help='Override the path to the scenario to be executed from scenario_database.json') | ||
| parser.add_argument( | ||
| '--base-path', | ||
| help='Allow relative paths in scenario_database.json and command line arguments, which will be interpreted') | ||
| args = parser.parse_args() | ||
|
|
||
| launcher = Launcher(args) | ||
| launcher.run_all_scenarios() | ||
|
|
||
|
|
||
| if __name__ == "__main__": | ||
| main() |
Oops, something went wrong.