diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 3c2e403..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -project(planning_simulator_launcher) - -find_package(catkin REQUIRED COMPONENTS rospy) - -catkin_package(CATKIN_DEPENDS rospy) - -# ==== Install Python Scripts ================================================== - -configure_file( - ${CMAKE_CURRENT_SOURCE_DIR}/scripts/${PROJECT_NAME}/scenario_loader.py.cmake - ${CMAKE_CURRENT_SOURCE_DIR}/scripts/${PROJECT_NAME}/scenario_loader.py) - -file( - GLOB ${PROJECT_NAME}_SCRIPTS - ${CMAKE_CURRENT_SOURCE_DIR}/scripts/${PROJECT_NAME}/*.py) - -catkin_install_python( - PROGRAMS ${${PROJECT_NAME}_SCRIPTS} - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -# ==== Generate Default scenario_database.json ================================= - -file( - GLOB ${PROJECT_NAME}_DEFAULT_SCENARIOS_GLOB - ${CMAKE_CURRENT_SOURCE_DIR}/scenario/debug/*.yaml) - -function(convert reference delimiter) - list(GET ARGV 2 RESULT) - math(EXPR LENGTH "${ARGC}-1") - foreach(INDEX RANGE 3 ${LENGTH}) - list(GET ARGV ${INDEX} EACH) - set(RESULT "${RESULT}\",\n \"${EACH}") - endforeach() - set(${reference} "${RESULT}" PARENT_SCOPE) -endfunction() - -convert(${PROJECT_NAME}_DEFAULT_SCENARIOS ${${PROJECT_NAME}_DEFAULT_SCENARIOS_GLOB}) - -configure_file( - ${CMAKE_CURRENT_SOURCE_DIR}/scenario_database_template.json - ${CMAKE_CURRENT_SOURCE_DIR}/scenario_database_autoconf.json) - -# install( -# FILES ${CMAKE_CURRENT_SOURCE_DIR}/scenario_database.json -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -# ==== Miscallenous ============================================================ - -install( - DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -# install( -# DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/rviz -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) diff --git a/README.md b/README.md index 5b82b81..04a5075 100644 --- a/README.md +++ b/README.md @@ -28,7 +28,7 @@ See [https://github.com/tier4/AutowareArchitectureProposal#how-to-setup](https:/ ## 2. Usage - (2020/07/01 last modified) + (2020/11/23 last modified) ### 2.1 Get Sample Scenario @@ -66,22 +66,12 @@ Note that the Map path must be rewritten when sharing a scenario with multiple u #### (3) Execute -##### (a) Terminal A - -``` shell -source /opt/ros/melodic/setup.bash -source /path/to/AutowareArchitectureProposal/install/setup.bash - -rosrun planning_simulator_launcher scenario_roslaunch_server.py -``` - -#### (b) Terminal B ``` shell -source /opt/ros/melodic/setup.bash +source /opt/ros/foxy/setup.bash source /path/to/AutowareArchitectureProposal/install/setup.bash -rosrun planning_simulator_launcher launch.py --timeout=180 && rosrun planning_simulator_launcher show_result.py ./scenario/generated ./scenario/log +ros2 run planning_simulator_launcher launch.py --timeout=180 && ros2 run planning_simulator_launcher show_result.py ./scenario/generated ./scenario/log ``` Command-line arguments of script `python show_result.py` is depends on the path you written in `scenario_database.json`. 1st argument is path to your directory your scenarios placed on + `/generated`. diff --git a/launch.py b/launch.py index 7fa8fd8..6cf04ef 100644 --- a/launch.py +++ b/launch.py @@ -2,9 +2,9 @@ print('\x1B[1;31mThis script has been deprecated.\x1B[0m') print('\x1B[1;31mPlease use the following command.\x1B[0m') -print('\x1B[1;32m> rosrun planning_simulator_launcher launch.py\x1B[0m') +print('\x1B[1;32m> ros2 run planning_simulator_launcher launch.py\x1B[0m') import os import sys -os.system('rosrun planning_simulator_launcher launch.py ' + ' '.join(sys.argv[1:])) +os.system('ros2 run planning_simulator_launcher launch.py ' + ' '.join(sys.argv[1:])) diff --git a/launch/default.launch b/launch/default.launch index c8065f2..d6f73fe 100644 --- a/launch/default.launch +++ b/launch/default.launch @@ -5,10 +5,7 @@ - - - - - - + + + diff --git a/launch/scenario_launcher.launch b/launch/scenario_launcher.launch index b445f34..0d02612 100644 --- a/launch/scenario_launcher.launch +++ b/launch/scenario_launcher.launch @@ -3,72 +3,34 @@ - + - - - - - - + - - + - - - + + + - - + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + diff --git a/package.xml b/package.xml index fa615db..9f9e176 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,6 @@ - - + + + planning_simulator_launcher 0.0.0 The planning_simulator_launcher package @@ -9,8 +10,9 @@ Tatsuya Yamasaki Apache 2 + scenario_runner - catkin - - rospy + + ament_python + diff --git a/scripts/planning_simulator_launcher/__init__.py b/planning_simulator_launcher/__init__.py similarity index 100% rename from scripts/planning_simulator_launcher/__init__.py rename to planning_simulator_launcher/__init__.py diff --git a/planning_simulator_launcher/launch.py b/planning_simulator_launcher/launch.py new file mode 100755 index 0000000..0f9704f --- /dev/null +++ b/planning_simulator_launcher/launch.py @@ -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() diff --git a/planning_simulator_launcher/launch_script.py b/planning_simulator_launcher/launch_script.py new file mode 100644 index 0000000..af50dfa --- /dev/null +++ b/planning_simulator_launcher/launch_script.py @@ -0,0 +1,91 @@ +from pathlib import Path + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch_xml import Parser + +from launch import LaunchContext, LaunchDescription +from launch.actions import (ExecuteProcess, IncludeLaunchDescription, + RegisterEventHandler, Shutdown) +from launch.event import Event +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import FrontendLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def launch_description(*, scenario_path, scenario_id, log_dir, map_path, uuid, launch_path): + vehicle_info_param_path = FindPackageShare('lexus_description').find('lexus_description') + '/config/vehicle_info.yaml' + scenario_runner = Node( + package='scenario_runner', + executable='scenario_runner_node', + parameters=[ + {'scenario_id': scenario_id}, + {'scenario_path': scenario_path}, + {'log_output_path': f'{log_dir}/{scenario_id}.json'}, + {'json_dump_path': f'{log_dir}/result-of-{uuid}.json'}, + # base link of front camera + {'camera_frame_id': 'camera5/camera_optical_link'}, + {'initial_engage_state': True}, + Path(vehicle_info_param_path) + ], + + remappings=[ + ("input/pointcloud", "/sensing/lidar/no_ground/pointcloud"), + ("input/vectormap", "/map/vector_map"), + ("input/route", "/planning/mission_planning/route"), + ("input/autoware_state", "/autoware/state"), + ("input/vehicle_twist", "/vehicle/status/twist"), + ("input/signal_command", "/vehicle/status/turn_signal"), + ("output/start_point", "/initialpose"), + ("output/initial_velocity", "/initialtwist"), + ("output/goal_point", "/planning/mission_planning/goal"), + ("output/check_point", "/planning/mission_planning/checkpoint"), + ("output/autoware_engage", "/autoware/engage"), + ("output/simulator_engage", "/vehicle/engage"), + ("output/npc_simulator_engage", "/simulation/npc_simulator/engage"), # noqa: E501 + ("output/limit_velocity", "/planning/scenario_planning/max_velocity"), # noqa: E501 + ("output/object_info", "/simulation/npc_simulator/object_info"), + ("output/traffic_detection_result", "/perception/traffic_light_recognition/traffic_light_states"), # noqa: E501 + ("output/lane_change_permission", "/planning/scenario_planning/lane_driving/lane_change_approval"), # noqa: E501 + # rosparam: add simulator noise or not + ("rosparam/add_simulator_noise", "/simple_planning_simulator/add_measurement_noise"), # noqa: E501 + # rosparam: std of simulator pos noise + ("rosparam/simulator_pos_noise", "/simple_planning_simulator/pos_noise_stddev"), # noqa: E501 + # rosparam: max velocity + ("rosparam/max_velocity", "/planning/scenario_planning/motion_velocity_optimizer/max_velocity"), # noqa: E501 + ] + ) + + # When the scenario_runner exits, all nodes should be shut down. + def on_exit(event: Event, context: LaunchContext): + return Shutdown(reason="Simulation complete") + + shutdown_handler = RegisterEventHandler( + OnProcessExit( + target_action=scenario_runner, + on_exit=on_exit + ) + ) + + scenario_api = IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + launch_file_path=launch_path, parser=Parser()), # noqa: E501 + launch_arguments=[ + ('map_path', map_path), + ('log_dir', log_dir), + ('scenario_id', scenario_id), + ('scenario_path', scenario_path), + ] + ) + + # rosbag_record = ExecuteProcess( + # cmd=['ros2', 'bag', 'record', '-a'], + # output='screen' + # ) + + return LaunchDescription([ + scenario_runner, + shutdown_handler, + # scenario_api, + # rosbag_record, + ]) diff --git a/scripts/planning_simulator_launcher/scenario_generator.py b/planning_simulator_launcher/scenario_generator.py similarity index 72% rename from scripts/planning_simulator_launcher/scenario_generator.py rename to planning_simulator_launcher/scenario_generator.py index 6dfa843..85304d5 100644 --- a/scripts/planning_simulator_launcher/scenario_generator.py +++ b/planning_simulator_launcher/scenario_generator.py @@ -1,14 +1,10 @@ -#!/usr/bin/env python - -from copy import deepcopy -import datetime import itertools import json -import math import os import re -import shutil import uuid +from copy import deepcopy + import yaml @@ -36,39 +32,38 @@ def __init__(self, params, path): with open(path, "r+") as file: self.yaml_str = file.read() - self.__yaml_generate_dir = self.generated_directory + "/" + scenario_name + "/" + self._yaml_generate_dir = self.generated_directory + "/" + \ + scenario_name + "/" - if os.path.exists(self.__yaml_generate_dir): + if os.path.exists(self._yaml_generate_dir): + rel_dir = os.path.relpath(self._yaml_generate_dir) print('') - print('Cleanup target directory ' + os.path.relpath(self.__yaml_generate_dir)) + print('Cleanup target directory ' + rel_dir) - for each in os.listdir(self.__yaml_generate_dir): + for each in os.listdir(self._yaml_generate_dir): print(' Removing ' + each) - os.remove(os.path.join(self.__yaml_generate_dir, each)) + os.remove(os.path.join(self._yaml_generate_dir, each)) else: - os.makedirs(self.__yaml_generate_dir) + os.makedirs(self._yaml_generate_dir) print('') print("Generating scenario " + scenario_name) - self.__generate() + self._generate() def __del__(self): self.generate_log.close() def getScenarioFilesPath(self): - return self.__yaml_files_path + return self._yaml_files_path - def __generate(self): - self.__yaml_files_path = [] + def _generate(self): + self._yaml_files_path = [] self.num_scenarios = 0 print(' Parameters') for variable, configuration in self.params.items(): - print(' '), - print(variable), - print('='), - print(configuration.values) + print(f' {variable} = {configuration.values}'), generate_logs = [] @@ -86,21 +81,19 @@ def __generate(self): for bindings in itertools.product(*list_of_list_of_bindings): scenario_id = str(uuid.uuid4()) - yaml_path = self.__yaml_generate_dir + scenario_id + ".yaml" - self.__yaml_files_path.append(yaml_path) + yaml_path = self._yaml_generate_dir + scenario_id + ".yaml" + self._yaml_files_path.append(yaml_path) print('') print(' ' + os.path.relpath(yaml_path)) result = deepcopy(self.yaml_str) for variable, value in bindings: - print(' '), - print(variable), - print('=>'), - print(value) + print(f' {variable} = {value}'), result = result.replace(variable, str(value)) - # NOTE: Expand macro after variable assignment (to use variable in expression) + # NOTE: Expand macro after variable assignment + # (to use variable in expression) result = self.macroexpand(result) with open(yaml_path, 'w') as file: @@ -122,8 +115,8 @@ def __generate(self): else: scenario_id = str(uuid.uuid4()) - yaml_path = self.__yaml_generate_dir + scenario_id + ".yaml" - self.__yaml_files_path.append(yaml_path) + yaml_path = self._yaml_generate_dir + scenario_id + ".yaml" + self._yaml_files_path.append(yaml_path) print(' ' + os.path.relpath(yaml_path)) result = deepcopy(self.yaml_str) @@ -143,7 +136,7 @@ def __generate(self): json.dump(generate_logs, self.generate_log) - def __getDefaultValueKeys(self, used_key): + def _getDefaultValueKeys(self, used_key): variables = [] for variable in self.params.keys(): @@ -156,8 +149,8 @@ def __getDefaultValueKeys(self, used_key): return variables - def __update_default_params(self, name, yaml_string): - for variable in self.__getDefaultValueKeys(name): + def _update_default_params(self, name, yaml_string): + for variable in self._getDefaultValueKeys(name): print(' '), print(variable), print('=>'), @@ -169,8 +162,10 @@ def __update_default_params(self, name, yaml_string): return yaml_string def evaluate(self, match): - pi = math.pi return match.group(1) + str(eval(match.group(2))) + match.group(3) def macroexpand(self, source): - return re.sub(r"^(.*)\${{\s+((?:pi|[\d\s+\-\*/%\(\).])*)\s+}}(.*)$", self.evaluate, source, flags=re.M) + return re.sub( + r"^(.*)\${{\s+((?:pi|[\d\s+\-\*/%\(\).])*)\s+}}(.*)$", + self.evaluate, source, flags=re.M + ) diff --git a/scripts/planning_simulator_launcher/scenario_parameter.py b/planning_simulator_launcher/scenario_parameter.py similarity index 98% rename from scripts/planning_simulator_launcher/scenario_parameter.py rename to planning_simulator_launcher/scenario_parameter.py index 46a1b60..4f1c119 100644 --- a/scripts/planning_simulator_launcher/scenario_parameter.py +++ b/planning_simulator_launcher/scenario_parameter.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python - import numpy as np diff --git a/scripts/planning_simulator_launcher/scenario_parser.py b/planning_simulator_launcher/scenario_parser.py similarity index 59% rename from scripts/planning_simulator_launcher/scenario_parser.py rename to planning_simulator_launcher/scenario_parser.py index 4ff67c2..b06a04e 100644 --- a/scripts/planning_simulator_launcher/scenario_parser.py +++ b/planning_simulator_launcher/scenario_parser.py @@ -1,10 +1,10 @@ -#!/usr/bin/env python - -from scenario_generator import ScenarioGenerator -from scenario_parameter import ScenarioParameter import os + import yaml +from planning_simulator_launcher.scenario_generator import ScenarioGenerator +from planning_simulator_launcher.scenario_parameter import ScenarioParameter + class ScenarioParser: @@ -12,15 +12,13 @@ def __init__(self, yaml_path, generate_mode=False): self.yaml_path = yaml_path with open(yaml_path, "r+") as f: - self.yaml = yaml.load(f) + self.yaml = yaml.load(f, Loader=yaml.FullLoader) if not self.parseScenarioParameter(): print(' No parameter specified.') if generate_mode: - generator = ScenarioGenerator( - self.scenario_params, - self.yaml_path) + generator = ScenarioGenerator(self.scenario_params, self.yaml_path) print('') print(str(generator.num_scenarios) + " Scenarios Generated") @@ -39,18 +37,13 @@ def parseScenarioParameter(self): self.scenario_params = {} try: for param in self.yaml["ParameterDeclaration"]: - scenario_parameter = ScenarioParameter(param) - if scenario_parameter.is_valid: - self.scenario_params[scenario_parameter.name] = scenario_parameter + sp = ScenarioParameter(param) + if sp.is_valid: + self.scenario_params[sp.name] = sp else: - print("Failed to load parameter: " + scenario_parameter.name) + print("Failed to load parameter: " + sp.name) return False - except Exception as exception: - # print(exception) + except Exception: return False return True - - -if __name__ == "__main__": - pass diff --git a/scripts/planning_simulator_launcher/show_result.py b/planning_simulator_launcher/show_result.py similarity index 61% rename from scripts/planning_simulator_launcher/show_result.py rename to planning_simulator_launcher/show_result.py index c8caef4..45a1d4d 100755 --- a/scripts/planning_simulator_launcher/show_result.py +++ b/planning_simulator_launcher/show_result.py @@ -1,16 +1,18 @@ -#!/usr/bin/env python - import argparse import glob import json + import termcolor + class Viewer: - def __init__(self,scenario_generated_path,log_path,verbose,quiet,output): + def __init__(self, scenario_generated_path, + log_path, verbose, quiet, output): self.quiet = quiet self.verbose = verbose self.log_path = log_path - generate_log_f = open(scenario_generated_path+'/generate_log.json', 'r') + log_file_path = scenario_generated_path + '/generate_log.json' + generate_log_f = open(log_file_path, 'r') self.generate_log = json.load(generate_log_f) generate_log_f.close() self.raw_data = {} @@ -23,51 +25,56 @@ def __init__(self,scenario_generated_path,log_path,verbose,quiet,output): self.raw_data[scenario_id] = json_load if self.getGenerateLog(scenario_id) is not None: self.output_data.append(self.getResultDict(scenario_id)) - if self.quiet == False: + if self.quiet is False: self.outputResult(output) self.printResult() + def printResult(self): if self.verbose: self.printVerboseResult() else: self.printSimpleResult() self.printSummary() + def printVerboseResult(self): i = 1 for data in self.output_data: message = "[" + str(i) + "/" + str(len(self.output_data)) + "] " if data["passed"]: - message = message + "OK \n" + message += "OK \n" else: - message = message + "NG \n" - message = message + "id = " + data["id"] + "\n" - message = message + "parent = " + data["parent"] + "\n" - message = message + "rosbg = " + data["rosbag"] + "\n" - message = message + "json = " + data["json_log"] + "\n" + message += "NG \n" + message += "id = " + data["id"] + "\n" + message += "parent = " + data["parent"] + "\n" + message += "rosbg = " + data["rosbag"] + "\n" + message += "json = " + data["json_log"] + "\n" metadata = data["metadata"] - message = message + "start_datetime = " + metadata["start_datetime"] + "\n" - message = message + "end_datetime = " + metadata["end_datetime"] + "\n" - message = message + "duration = " + metadata["duration"] + "\n" - message = message + "params\n" + message += "start_datetime = " + metadata["start_datetime"] + "\n" + message += "end_datetime = " + metadata["end_datetime"] + "\n" + message += "duration = " + metadata["duration"] + "\n" + message += "params\n" if len(data["params"]) == 0: - message = message + " no scenario parameters" + message += " no scenario parameters" param_index = 1 for param in data["params"]: - message = message + " (" + str(param_index) + "/" + str(len(data["params"])) + ") " + param + " : " + str(data["params"][param]) + message += " (" + str(param_index) + "/" + \ + str(len(data["params"])) + ") " + param + \ + " : " + str(data["params"][param]) param_index = param_index + 1 - message = message + "\n" + message += "\n" if data["passed"]: message = termcolor.colored(message, 'green') else: message = termcolor.colored(message, 'red') print(message) i = i + 1 + def printSimpleResult(self): i = 1 for data in self.output_data: message = "[" + str(i) + "/" + str(len(self.output_data)) + "] " if data["passed"]: - message = message + "OK " + message = message + "OK " else: message = message + "NG " message = message + "id = " + data["id"] @@ -77,23 +84,28 @@ def printSimpleResult(self): message = termcolor.colored(message, 'red') print(message) i = i + 1 + def printSummary(self): print("[summary]") i = 0 for data in self.output_data: if data["passed"]: i = i + 1 - print("passed " + str(i) + "/" + str(len(self.output_data)) + " test cases.") - def outputResult(self,output): - if output != None: + print("passed " + str(i) + "/" + + str(len(self.output_data)) + " test cases.") + + def outputResult(self, output): + if output is not None: with open(output, 'w') as f: json.dump(self.output_data, f, indent=4) - def getGenerateLog(self,id): + + def getGenerateLog(self, id): for log in self.generate_log: if log["id"] == id: return log return None - def getResultDict(self,id): + + def getResultDict(self, id): ret = {} if self.getResult(id): ret["passed"] = True @@ -102,7 +114,7 @@ def getResultDict(self,id): gen_log = self.getGenerateLog(id) ret["parent"] = gen_log["parent_path"] ret["id"] = id - ret["json_log"] = self.log_path + id + ".json" + ret["json_log"] = self.log_path + id + ".json" ret["rosbag"] = self.log_path + id + ".bag" metadata = self.raw_data[id]["metadata"] ret["metadata"] = metadata @@ -113,7 +125,8 @@ def getResultDict(self,id): else: ret["params"] = {} return ret - def getResult(self,id): + + def getResult(self, id): logs = self.raw_data[id]["log"] scenario_succeded = False for log in logs: @@ -126,12 +139,38 @@ def getResult(self,id): return scenario_succeded -if __name__ == "__main__": - parser = argparse.ArgumentParser(description='Show result') - parser.add_argument('scenario_generated_path', type=str, help='path of the generated scenario') - parser.add_argument('log_path', type=str,help='path of the log') - parser.add_argument('--verbose', help='show detail or not', action="store_true") - parser.add_argument('--quiet', help='does not show output', action="store_true") - parser.add_argument('--output',type=str,help='output path of the result(if not set, only output result to the screen)') +def main(): + parser = argparse.ArgumentParser( + description='Show result') + parser.add_argument( + 'scenario_generated_path', + type=str, + help='path of the generated scenario') + parser.add_argument( + 'log_path', + type=str, + help='path of the log') + parser.add_argument( + '--verbose', + help='show detail or not', + action="store_true") + parser.add_argument( + '--quiet', + help='does not show output', + action="store_true") + parser.add_argument( + '--output', + type=str, + help='output path of the result (if not set, only output result to \ + the screen)') args = parser.parse_args() - viwer = Viewer(args.scenario_generated_path,args.log_path,args.verbose,args.quiet,args.output) + viwer = Viewer( + args.scenario_generated_path, + args.log_path, + args.verbose, + args.quiet, + args.output + ) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/COLCON_IGNORE b/resource/planning_simulator_launcher similarity index 100% rename from COLCON_IGNORE rename to resource/planning_simulator_launcher diff --git a/scenario_roslaunch_server.py b/scenario_roslaunch_server.py index 9879be9..762526a 100644 --- a/scenario_roslaunch_server.py +++ b/scenario_roslaunch_server.py @@ -2,9 +2,9 @@ print('\x1B[1;31mThis script has been deprecated.\x1B[0m') print('\x1B[1;31mPlease use the following command.\x1B[0m') -print('\x1B[1;32m> rosrun planning_simulator_launcher scenario_roslaunch_server.py\x1B[0m') +print('\x1B[1;32m> ros2 run planning_simulator_launcher scenario_roslaunch_server.py\x1B[0m') import os import sys -os.system('rosrun planning_simulator_launcher scenario_roslaunch_server.py ' + ' '.join(sys.argv[1:])) +os.system('ros2 run planning_simulator_launcher scenario_roslaunch_server.py ' + ' '.join(sys.argv[1:])) diff --git a/scripts/planning_simulator_launcher/launch.py b/scripts/planning_simulator_launcher/launch.py deleted file mode 100755 index d4aed7f..0000000 --- a/scripts/planning_simulator_launcher/launch.py +++ /dev/null @@ -1,207 +0,0 @@ -#!/usr/bin/env python - -from scenario_loader import ScenarioLoader -from scenario_parser import ScenarioParser -import argparse -import json -import os -import roslaunch -import sys -import time -import xmlrpclib - - -class Launcher: - - def __init__(self, args, loader): - self.timeout = args.timeout - self.log_output_type = args.log_output - self.server = None - self.client = xmlrpclib.ServerProxy('http://0.0.0.0:10000') - self.loader = loader - self.args = args - - print('Loading Databse') - - self.json_path = args.database - sys.stdout.write(' Opening \x1b[36m') - sys.stdout.write(self.json_path) - sys.stdout.write('\x1b[0m => ') - - with open(self.json_path) as file: - print('\x1b[32mSuccess\x1b[0m') - self.database = json.load(file) - - def map_path(self): - sys.stdout.write(' load map \x1b[36m') - sys.stdout.write(self.parser.getMapPath()) - sys.stdout.write('\x1b[0m as ') - - map_path = self.database['map'].get( - self.parser.getMapPath(), - self.parser.getMapPath()); - - if map_path == self.parser.getMapPath(): - print('is') - else: - sys.stdout.write('\x1b[36m') - sys.stdout.write(map_path) - print('\x1b[0m') - - return self.loader.absolute(map_path) - - def launch_path(self): - sys.stdout.write(' via \x1b[36m') - - path = None - - if self.args.launch: - path = os.path.abspath(self.args.launch) - else: - path = self.loader.absolute(self.database["launch_path"]) - - sys.stdout.write(path) - print('\x1b[0m') - return path - - def start(self, scenario_path): - if self.args.log_output_base_dir: - log_output_base_dir = os.path.abspath(self.args.log_output_base_dir) - else: - log_output_base_dir = self.loader.absolute(self.database["log_output_base_dir"]) - - if not os.path.exists(log_output_base_dir): - os.makedirs(log_output_base_dir) - - self.client.start( - self.launch_path(), - scenario_path, - self.parser.getScenarioId(), - self.map_path(), - log_output_base_dir, - self.log_output_type) - - def terminate(self): - self.client.terminate(1) - - def waitUntilSimulationFinished(self): - start = time.time() - - print(' The simulation is forced to terminate after'), - print(str(self.timeout)), - print('seconds.') - - while self.client.simulationRunnig() and (time.time() - start) < self.timeout: - time.sleep(1) - - if self.client.simulationRunnig(): - print(' The specified maximum simulation time (option --timeout) has been reached.') - print(' Terminate simulation.') - print(' The scenario is likely to be in a deadlock.') - self.terminate() - - def write_result(self, path, uuid): - result = {} - - result['code'] = self.client.status() - result['duration'] = self.client.current_duration() - result['mileage'] = self.client.current_mileage() - - if self.client.status() == 0: # boost::exit_success - result['message'] = 'exit_success' - print(' \x1b[1;32m=> Success\x1b[0m') - - elif self.client.status() == 201: # boost::exit_test_failure - result['message'] = 'exit_test_failure' - print(' \x1b[1;31m=> Failure\x1b[0m') - - elif self.client.status() == 1: # boost::exit_failure - result['message'] = 'exit_failure' - print(' \x1b[1;31m=> Aborted\x1b[0m') - - elif self.client.status() == 200: # boost::exit_exception_failure - result['message'] = 'exit_exception_failure' - print(' \x1b[1;31m=> Invalid\x1b[0m') - - else: - result['message'] = 'scenario_runner broken' - sys.stdout.write(' \x1b[1;33m=> Broken (') - sys.stdout.write(str(self.client.status())) - print(')\x1b[0m') - - if self.args.log_output_base_dir: - log_output_base_dir = os.path.abspath(self.args.log_output_base_dir) - else: - log_output_base_dir = self.loader.absolute(self.database["log_output_base_dir"]) - - if not os.path.exists(log_output_base_dir): - os.makedirs(log_output_base_dir) - - result_path = os.path.join(log_output_base_dir, 'result-of-' + uuid + '.json') - - with open(result_path, 'w') as file: - json.dump(result, file, indent=2, ensure_ascii=False, sort_keys=True, separators=(',', ': ')) - - def run_all_scenario(self): - if self.args.scenario: - scenarios = [ os.path.abspath(self.args.scenario) ] - else: - scenarios = self.database["scenario"] - - for scenario in scenarios: - scenario_path = self.loader.absolute(scenario) - - print('') - print('Running scenario \x1b[36m' + os.path.relpath(scenario_path) + '\x1b[0m') - - print(' Parameters') - self.parser = ScenarioParser(scenario_path, generate_mode=True) - - n = 0 - - for path in self.parser.scenario_files_path: - n = n + 1 - - print('') - print('Launch ' + str(n) + ' of ' + str(len(self.parser.scenario_files_path))) - - print(' scenario \x1b[36m' + path + '\x1b[0m') - - self.start(path) - - self.waitUntilSimulationFinished() - - self.write_result( - scenario_path, - os.path.splitext(os.path.basename(path))[0]) - - -if __name__ == "__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.') - - loader = ScenarioLoader() - - parser.add_argument('--database', - default=os.path.relpath(loader.default_scenario_database_pathname), - help='Specify relative path to your scenario_database.json.') - - parser.add_argument('--launch', - help='Specify relative path to your launch file.') - - parser.add_argument('--log-output', default='screen', - help='Specify the type of log output.') - - parser.add_argument('--log-output-base-dir', - help='Specify the directory log output to.') - - parser.add_argument('--scenario', - help='Specify the scenario you want to execute.') - - args = parser.parse_args() - - launcher = Launcher(args, loader) - launcher.run_all_scenario() diff --git a/scripts/planning_simulator_launcher/scenario_loader.py.cmake b/scripts/planning_simulator_launcher/scenario_loader.py.cmake deleted file mode 100644 index 689651c..0000000 --- a/scripts/planning_simulator_launcher/scenario_loader.py.cmake +++ /dev/null @@ -1,50 +0,0 @@ -#!/usr/bin/env python - -import os -import rospkg - - -class ScenarioLoader: - - def __init__(self): - print('Load Paths') - - self.package_name = '${PROJECT_NAME}' - print(' Package Name =\x1B[36m'), - print(self.package_name), - print('\x1B[0m') - - self.package_source_directory = '${CMAKE_CURRENT_SOURCE_DIR}' - print(' Package Source Directory =\x1B[36m'), - print(self.package_source_directory), - print('\x1B[0m') - - self.package_install_directory = rospkg.RosPack().get_path( - self.package_name) - print(' Package Install Directory =\x1B[36m'), - print(self.package_install_directory), - print('\x1B[0m') - - self.default_scenario_database_basename = 'scenario_database.json' - self.default_scenario_database_pathname = os.path.join( - self.package_source_directory, - self.default_scenario_database_basename) - print(' Default Scenario Database =\x1B[36m'), - print(self.default_scenario_database_pathname), - print('\x1B[0m') - - self.default_launch_basename = 'planning_simulator.launch' - self.default_launch_pathname = os.path.join( - self.package_source_directory, 'launch', - self.default_launch_basename) - print(' Default Launch File =\x1B[36m'), - print(self.default_launch_pathname), - print('\x1B[0m') - - print('') - - def absolute(self, path): - if os.path.isabs(path): - return path - else: - return os.path.join(self.package_source_directory, path) diff --git a/scripts/planning_simulator_launcher/scenario_roslaunch_server.py b/scripts/planning_simulator_launcher/scenario_roslaunch_server.py deleted file mode 100755 index af915d5..0000000 --- a/scripts/planning_simulator_launcher/scenario_roslaunch_server.py +++ /dev/null @@ -1,102 +0,0 @@ -#!/usr/bin/env python - -from os.path import join, dirname -from scenario_parser import ScenarioParser -import SimpleXMLRPCServer as xmlrpc_server -import json -import os -import roslaunch -import signal -import sys -import time - - -class ScenarioRoslaunchServer(): - - def __init__(self): - signal.signal(signal.SIGINT, self.signal_handler) - self.server = xmlrpc_server.SimpleXMLRPCServer( - ("localhost", 10000), allow_none=True, logRequests=False) - self.server.register_introspection_functions() - self.parent = None - self.simulation_runnig = False - - self.duration = 0 - self.exit_status = 42 - self.mileage = 0 - - def signal_handler(self,signal,frame): - sys.exit(0) - - def start(self,launch_path,scenario_path,scenario_id,map_path,log_output_dir, log_output_type): - parser = ScenarioParser(scenario_path,generate_mode=False) - arg_scenario_path = "scenario_path:=" + scenario_path - arg_map_path = "map_path:=" + map_path - arg_log_output_path = "log_output_dir:=" + log_output_dir - if os.path.exists(log_output_dir) == False: - os.makedirs(log_output_dir) - arg_scenario_id = "scenario_id:=" + parser.getScenarioId() - arg_log_output = "scenario_runner_output:=" + (log_output_type if log_output_type is not None else "screen") - cli_args = [launch_path, arg_map_path, arg_scenario_path, arg_log_output_path, arg_scenario_id, arg_log_output] - roslaunch_args = cli_args[1:] - roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)] - uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) - self.parent = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) - self.parent.start() - self.simulation_runnig = True - - def simulationRunnig(self): - return self.simulation_runnig - - def terminate(self, exit_status): - if self.parent != None: - self.exit_status = exit_status - self.simulation_runnig = False - self.parent.shutdown() - self.parent = None - - # if self.exit_status == 0: - # print(' \x1b[32mSimulation succeeded\x1b[0m') - # elif self.exit_status == 1: - # print(' \x1b[31mSimulation failed\x1b[0m') - # else: - # print(' \x1b[31mSimulation failed (unexpected)\x1b[0m') - - def status(self): - return self.exit_status - - def update_mileage(self, new_mileage): - # print('update mileage from '), - # print(self.mileage), - # print('to'), - # print(new_mileage) - self.mileage = new_mileage - - def current_mileage(self): - return self.mileage - - def update_duration(self, new_duration): - # print('update duration from '), - # print(self.duration), - # print('to'), - # print(new_duration) - self.duration = new_duration - - def current_duration(self): - return self.duration - - def run(self): - self.server.register_function(self.current_duration) - self.server.register_function(self.current_mileage) - self.server.register_function(self.simulationRunnig) - self.server.register_function(self.start) - self.server.register_function(self.status) - self.server.register_function(self.terminate) - self.server.register_function(self.update_duration) - self.server.register_function(self.update_mileage) - self.server.serve_forever() - - -if __name__ == "__main__": - launcher = ScenarioRoslaunchServer() - launcher.run() diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..6de1af2 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/planning_simulator_launcher +[install] +install-scripts=$base/lib/planning_simulator_launcher \ No newline at end of file diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..1aabcac --- /dev/null +++ b/setup.py @@ -0,0 +1,39 @@ +from glob import glob + +from setuptools import setup + +package_name = 'planning_simulator_launcher' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', glob('launch/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + author='Masaya Kataoka, Tomoya Kimura, Tatsuya Yamasaki', + author_email='masaya.kataoka@tier4.jp, tomoya.kimura@tier4.jp, httperror@404-notfound.jp', + maintainer='Masaya Kataoka, Tomoya Kimura, Tatsuya Yamasaki', + maintainer_email='masaya.kataoka@tier4.jp, tomoya.kimura@tier4.jp, httperror@404-notfound.jp', + keywords=['ROS'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: Apache Software License', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description='The planning_simulator_launcher package.', + license='Apache License, Version 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'launch_main = planning_simulator_launcher.launch:main', + 'show_result_main = planning_simulator_launcher.show_result', + ], + }, +) diff --git a/show_result.py b/show_result.py index f1e9bf0..b310f7a 100644 --- a/show_result.py +++ b/show_result.py @@ -2,8 +2,8 @@ print('\x1B[1;31mThis script has been deprecated.\x1B[0m') print('\x1B[1;31mPlease use the following command.\x1B[0m') -print('\x1B[1;32m> rosrun planning_simulator_launcher show_result.py\x1B[0m') +print('\x1B[1;32m> ros2 run planning_simulator_launcher show_result.py\x1B[0m') import os -os.system('rosrun planning_simulator_launcher show_result.py') +os.system('ros2 run planning_simulator_launcher show_result.py')