diff --git a/missions/example_template/mission.yaml b/missions/example_template/mission.yaml index 33da786..c6dbc2d 100644 --- a/missions/example_template/mission.yaml +++ b/missions/example_template/mission.yaml @@ -13,7 +13,8 @@ mission_file: altitude_origin: 0.0 entities: - - template: entity.template.xml + - type: uav + template: entity.template.xml config: id: 1 x: -10 @@ -21,7 +22,8 @@ entities: z: 20 heading: 270 - - template: entity.template.xml + - type: uav + template: entity.template.xml config: id: 2 x: 10 @@ -29,7 +31,8 @@ entities: z: 10 heading: 0 - - template: entity.template.xml + - type: uav + template: entity.template.xml config: id: 5 x: -10 diff --git a/src/scrimmage_ros/EntityLaunch.py b/src/scrimmage_ros/EntityLaunch.py index 63e8712..ee8fc9d 100644 --- a/src/scrimmage_ros/EntityLaunch.py +++ b/src/scrimmage_ros/EntityLaunch.py @@ -1,51 +1,92 @@ #!/usr/bin/env python import os +import yaml from jinja2 import Template import scrimmage_ros.MultiProcessLogger as MPL import scrimmage_ros.SimFilesGenerator as SFG import scrimmage_ros.utils as sru -def get_name_command(process, entity_id): - name_t = Template(process['name']) +def get_name_command(process_info, entity_id): + name_t = Template(process_info['name']) name = name_t.render(id=entity_id) - cmd_t = Template(process['command']) + cmd_t = Template(process_info['command']) cmd = cmd_t.render(id=entity_id) return name, cmd +def create_process(process_info, entity_id, output_dir, env, console, terminal): + # Allow user to specify a post delay for a process + try: + post_delay = process_info['post_delay'] + except: + post_delay = 0 + + # Allow the user to append the environment with additional variables + try: + environment = process_info['environment'] + except: + environment = dict() + env.update(environment) + + # Allow the user to change whether to write to the console or not + try: + console = process_info['console'] + except: + # Don't override console type + pass + + # Allow the user to change the terminal type + try: + terminal = MPL.Terminal(process_info['terminal']) + except: + # Use the default terminal + pass + + # Perform template substitution based on entity_id {{ id }} + name, command = get_name_command(process_info, entity_id) + + return { + 'command': command, + 'env': env, + 'console': console, + 'terminal': terminal, + 'file': output_dir + '/%s.log' % name, + 'post_delay': post_delay + } + class EntityLaunch(): - def __init__(self, mission_yaml_filename, output_dir, ros_package_name, - launch_filename, launch_args='team_id:=1 entity_id:={{ id }}', - entity_ids=None, terminal=MPL.Terminal.gnome, - processes_per_entity=[], clean_up_processes_per_entity=[], - clean_up_processes=[]): + def __init__(self, mission_yaml_filename, processess_yaml_file, + output_dir, entity_ids=None, entity_type=None): + self.entity_ids = entity_ids + # The list of processes to pass to MultiProcessLogger self.processes = [] self.clean_up_processes = [] - self._mpl = MPL.MultiProcessLogger() + # Get the full path to the processes yaml file (ros find substitution) + processes_yaml_file_path = sru.ros_find_path(processess_yaml_file) - env = os.environ.copy() + # Parse the processes file + with open(processes_yaml_file_path) as f: + self._processes_yaml = yaml.load(f, Loader=yaml.FullLoader) - env['HOME'] = sru.user_home() + self.parse_defaults() - # Ensures that python prints and logs are displayed to screen - env['PYTHONUNBUFFERED'] = '1' - - # Append the roscore process - self.processes.append( - { 'command': "roscore", - 'env': env, - 'console': True, - 'file': output_dir + '/roscore.log', - 'post_delay': 1.5 - } - ) - - if entity_ids is None: + # Get processes that are run before entities + try: + processes_list = self._processes_yaml['processes'] + except: + processes_list = [] + + # Append the processes that are run before entities + for p in processes_list: + self.processes.append(create_process(p, 0, output_dir, self.env, True, self.terminal)) + + # If the entity_ids is none, use scrimmage to run a simulation + if self.entity_ids is None: # Generate the scrimmage mission file and scrimmage file sfg = SFG.SimFilesGenerator(mission_yaml_filename, output_dir) @@ -55,61 +96,90 @@ def __init__(self, mission_yaml_filename, output_dir, ros_package_name, # Append the scrimmage process self.processes.append( { 'command': "scrimmage %s" % sfg.mission_file_path, - 'env': env, + 'env': self.env, 'console': True, - 'terminal': terminal, + 'terminal': self.terminal, 'file': output_dir + '/scrimmage.log', } ) + # Create a mapping of entity type to process infos + entity_type_to_processes = dict() + for entity_type_processes in self._processes_yaml['entity_processes']: + entity_type = entity_type_processes['type'] + entity_type_to_processes[entity_type] = entity_type_processes + # Append the processes for each entity's roslaunch for entity_id in self.entity_ids: - args_t = Template(launch_args) - args = args_t.render(id=entity_id) + # Get the entity type: + entity_type = sfg.entity_id_to_type(entity_id) - self.processes.append( - { 'command': sru.ros_launch(ros_package_name, launch_filename, args), - 'env': env, - 'console': True, - 'terminal': terminal, - 'file': output_dir + '/entity%d/entity.log' % entity_id - } - ) + # Get the entity processes for this type + try: + entity_processes = entity_type_to_processes[entity_type]['processes'] + except: + entity_processes = [] - # The user can pass in additional processes to run per entity - for process in processes_per_entity: - name, cmd = get_name_command(process, entity_id) - - self.processes.append( - { 'command': cmd, - 'env': env, - 'console': True, - 'terminal': terminal, - 'file': output_dir + '/entity%d/%s.log' % (entity_id, name) - } - ) - - # Append the cleanup processes that are associated with each entity - for process in clean_up_processes_per_entity: - name, cmd = get_name_command(process, entity_id) - self.clean_up_processes.append( - { - 'command': cmd, - 'env': env, - } - ) + # Append the entity processes to the processes list + for p in entity_processes: + self.processes.append(create_process(p, entity_id, output_dir, self.env, True, self.terminal)) + + # Get the clean up processes for this type + try: + entity_clean_up_processes = entity_type_to_processes[entity_type]['clean_up'] + except: + entity_clean_up_processes = [] + + # Append the clean up processes for this entity to the clean up list + for p in entity_clean_up_processes: + self.clean_up_processes.append(create_process(p, entity_id, output_dir, self.env, True, self.terminal)) # Append the cleanup processes that apply to all entities - for process in clean_up_processes: - name, cmd = get_name_command(process, 0) - self.clean_up_processes.append( - { - 'command': cmd, - 'env': env, - } - ) + try: + clean_up_process_infos = self._processes_yaml['clean_up'] + except: + clean_up_process_infos = [] + + try: + for p in clean_up_process_infos: + self.clean_up_processes.append(create_process(p, 0, output_dir, self.env, True, self.terminal)) + except: + pass + + def parse_defaults(self): + # Parse the defaults block + try: + self.terminal = MPL.Terminal(self._processes_yaml['defaults']['terminal']) + except: + self.terminal=MPL.Terminal.gnome + + # Get environment variables to append to current environment + try: + environment = self._processes_yaml['defaults']['environment'] + except: + environment = dict() + + # Environment setup + self.env = os.environ.copy() + self.env['HOME'] = sru.user_home() + # Ensures that python prints and logs are displayed to screen + self.env['PYTHONUNBUFFERED'] = '1' + + # Append user-defined environment variables + self.env.update(environment) + + def print_processes(self): + print('---------- Processes ------------') + for p in self.processes: + print(p['command']) + + print('---------- Clean up ------------') + for p in self.clean_up_processes: + print(p['command']) def run(self): + mpl = MPL.MultiProcessLogger() + # Run the processes. Blocking. - self._mpl.run(self.processes, self.clean_up_processes) + mpl.run(self.processes, self.clean_up_processes) diff --git a/src/scrimmage_ros/MultiProcessLogger.py b/src/scrimmage_ros/MultiProcessLogger.py index e6c80b8..c355dbc 100644 --- a/src/scrimmage_ros/MultiProcessLogger.py +++ b/src/scrimmage_ros/MultiProcessLogger.py @@ -148,5 +148,4 @@ def run(self, process_info, clean_up_processes=[]): if len(clean_up_processes) > 0: print('Running cleanup processes...') for process in clean_up_processes: - print(process['command']) call(process['command'].split()) diff --git a/src/scrimmage_ros/SimFilesGenerator.py b/src/scrimmage_ros/SimFilesGenerator.py index ee43542..eed9a73 100644 --- a/src/scrimmage_ros/SimFilesGenerator.py +++ b/src/scrimmage_ros/SimFilesGenerator.py @@ -53,10 +53,11 @@ def __init__(self, mission_yaml_file, output_dir): with open(self.mission_file_path, 'w') as f: f.write(self._mission_gen.mission) - # self.launch_file_path = os.path.join(sim_dir, 'sim.launch') - # with open(self.launch_file_path, 'w') as f: - # config = {'mission_file' : self.mission_file_path} - # f.write(scrimmage_ros.utils.generate_sim_roslaunch(config)) - def entity_ids(self): return self._mission_gen.entity_ids() + + def entity_id_to_type(self, id): + return self._mission_gen.entity_id_to_type(id) + + def options(self): + return self._mission_gen.options diff --git a/src/scrimmage_ros/test/templates/nav_2d_mission.yaml b/src/scrimmage_ros/test/templates/nav_2d_mission.yaml index 333afb1..f1c0d4b 100644 --- a/src/scrimmage_ros/test/templates/nav_2d_mission.yaml +++ b/src/scrimmage_ros/test/templates/nav_2d_mission.yaml @@ -15,7 +15,8 @@ mission_file: # Configuration for each entity that will be injected into the mission file entities: - - template: entity.template.xml + - type: car + template: entity.template.xml config: id: 1 team_id: 1 @@ -25,7 +26,8 @@ entities: z: 0.42 heading: 0 - - template: entity.template.xml + - type: car + template: entity.template.xml config: id: 2 team_id: 2 diff --git a/src/scrimmage_ros/test/templates/nav_2d_processes.yaml b/src/scrimmage_ros/test/templates/nav_2d_processes.yaml new file mode 100644 index 0000000..8c2ad91 --- /dev/null +++ b/src/scrimmage_ros/test/templates/nav_2d_processes.yaml @@ -0,0 +1,33 @@ +version: "1.0" + +# Specify default values +defaults: + terminal: gnome + environment: + some_variable: some_value + +# Processes that are run before entities are launched +processes: + - name: roscore + command: roscore + terminal: none + post_delay: 1.5 + + - name: map_server + command: stdbuf -oL roslaunch scrimmage_ros map_server.launch + terminal: none + +# Processes that are run for each entity type +entity_processes: + - type: car + processes: + - name: 'entity{{ id }}' + command: 'stdbuf -oL roslaunch scrimmage_ros entity_nav_2d.launch team_id:=1 entity_id:={{ id }}' + + # Clean up commands run for each entity + clean_up: + - name: hello + command: 'echo "Goodbye, Entity {{ id }}' + +# Overall clean up commands (run once) +clean_up: diff --git a/src/scrimmage_ros/test/test_MultiProcessLogger.py b/src/scrimmage_ros/test/test_MultiProcessLogger.py index f329e6a..61e7f63 100644 --- a/src/scrimmage_ros/test/test_MultiProcessLogger.py +++ b/src/scrimmage_ros/test/test_MultiProcessLogger.py @@ -6,7 +6,6 @@ import scrimmage_ros.utils as sru import scrimmage_ros.MultiProcessLogger as MPL -import scrimmage_ros.SimFilesGenerator as SFG # The list of processes to pass to MultiProcessLogger processes = [] @@ -14,33 +13,28 @@ mpl = MPL.MultiProcessLogger() env = os.environ.copy() +env['PYTHONUNBUFFERED'] = '1' tmp_dir = tempfile.mkdtemp() -# Generate the scrimmage mission file and simulation roslaunch file -sfg = SFG.SimFilesGenerator('$(find scrimmage_ros)/missions/example_template/mission.yaml', tmp_dir) - -# Get the entity IDs from the mission.yaml file -entity_ids = sfg.entity_ids() - -# Create the process that launches scrimmage and sets simulation ROS params. processes.append( - { 'command': sru.ros_launch_file(sfg.launch_file_path), + { 'command': 'roscore', 'env': env, 'console': True, - 'file': tmp_dir + '/roslaunch.log', + 'terminal': mpl.terminal.none, + 'file': tmp_dir + '/roscore.log', 'post_delay': 1.5 } ) -for entity_id in range(1, 3): - args = 'team_id:=1 entity_id:=%d' % entity_id +for id in range(1, 5): + cmd = 'echo "Hello, Entity %d"' % id processes.append( - { 'command': sru.ros_launch('scrimmage_ros', 'test_entity.launch', args), + { 'command': cmd, 'env': env, 'console': True, - 'terminal': mpl.terminal.gnome, - 'file': tmp_dir + '/entity%d/entity.log' % entity_id + 'terminal': mpl.terminal.none, + 'file': tmp_dir + '/entity%d/entity.log' % id } ) diff --git a/src/scrimmage_ros/test/test_entity_launch.py b/src/scrimmage_ros/test/test_entity_launch.py index cd3ecbb..3cf2e87 100644 --- a/src/scrimmage_ros/test/test_entity_launch.py +++ b/src/scrimmage_ros/test/test_entity_launch.py @@ -14,40 +14,24 @@ def main(): description='Runs the autonomy in HIL or SIL.') parser.add_argument('-s', '--sim_mission_yaml_file', default='$(find scrimmage_ros)/src/scrimmage_ros/test/templates/nav_2d_mission.yaml') - parser.add_argument('-p', '--ros_package_name', type=str, - default='scrimmage_ros', - help='ROS package that contains launch file') - parser.add_argument('-f', '--launch_file', type=str, - default='entity_nav_2d.launch', - help='Launch file in ROS package') - parser.add_argument('-t', '--terminal', type=Terminal, - choices=list(Terminal), - default=Terminal.gnome) + parser.add_argument('-p', '--processes_yaml_file', + default='$(find scrimmage_ros)/src/scrimmage_ros/test/templates/nav_2d_processes.yaml') + parser.add_argument('-d', '--debug', action='store_true') # Parse the arguments args = parser.parse_args() - # ROS launch args. {{ id }} is substituted for entity ID - launch_args = 'team_id:=1 entity_id:={{ id }}' - # Specify a directory to hold logs and generated files logs_path = os.path.join(sru.user_home(), '.ros/scrimmage') run_dir = sru.make_get_run_dir(logs_path) - entity_launch = EntityLaunch(args.sim_mission_yaml_file, run_dir, - args.ros_package_name, args.launch_file, - launch_args, None, args.terminal) - - # Append a custom ros launch file to the processes list to launch a common - # map server for both agents. The map server exists outside of a ROS - # namespace - entity_launch.processes.append( - { 'command': sru.ros_launch('scrimmage_ros', 'map_server.launch', ''), - 'env': os.environ.copy(), - 'console': False, - 'terminal': Terminal.gnome - } - ) + entity_launch = EntityLaunch(args.sim_mission_yaml_file, + args.processes_yaml_file, + run_dir, None, None) + + if args.debug: + entity_launch.print_processes() + return # Run the processes. Blocking. entity_launch.run() diff --git a/src/scrimmage_ros/test/test_sim_file_generation.py b/src/scrimmage_ros/test/test_sim_file_generation.py index 692d47a..d6c6699 100644 --- a/src/scrimmage_ros/test/test_sim_file_generation.py +++ b/src/scrimmage_ros/test/test_sim_file_generation.py @@ -14,9 +14,3 @@ print("==================================================================") with open(sfg.mission_file_path, 'r') as file: print(file.read()) - - print("==================================================================") - print("ROS Launch File: %s" % sfg.launch_file_path) - print("==================================================================") - with open(sfg.launch_file_path, 'r') as file: - print(file.read()) diff --git a/src/scrimmage_ros/utils.py b/src/scrimmage_ros/utils.py index 026a7ff..b222956 100644 --- a/src/scrimmage_ros/utils.py +++ b/src/scrimmage_ros/utils.py @@ -60,7 +60,7 @@ def gnome_terminal_title(title): return "echo -ne \\\"\\033]0;" + title + "\\007\\\"" def gnome_terminal_cmd(title, cmd, log_file): - return "gnome-terminal --disable-factory -x bash -c '" + cmd \ + return "gnome-terminal --disable-factory -- bash -c '" + cmd \ + " 2>&1 | tee " + log_file + "; exec bash'" def user_home():