demo_sim2d_ship_ice_navigation.py

import argparse
import pickle

from ship_ice_planner.experiments.generate_rand_exp import build_obs_dicts
from ship_ice_planner.sim2d import sim
from ship_ice_planner.utils.utils import DotDict


def demo(cfg_file: str,
         exp_config_file: str,
         ice_concentration: float,
         ice_field_idx: int,
         start: list = None,
         goal: list = None,
         show_anim=True,
         output_dir: str = None,
         debug=False,
         logging=False,
         log_level=10):
    # demo to run a single simulation, for consecutive simulations see ship_ice_planner/experiments/sim_exp.py
    print('2D simulation start')

    # load config
    cfg = DotDict.load_from_file(cfg_file)
    cfg.cfg_file = cfg_file

    # update parameters
    cfg.anim.show = show_anim
    cfg.output_dir = output_dir
    if output_dir and show_anim:
        cfg.anim.save = True  # cannot show and save anim at same time
        cfg.anim.show = False

    # load ice field data
    with open(exp_config_file, 'rb') as f:
        # can either load the experiment config generated by generate_rand_exp.py
        # or can simply load obstacle data encoded as a list of lists
        exp = pickle.load(f)
        if type(exp) is list:
            exp = {'obstacles': exp}
        else:
            exp = exp['exp'][ice_concentration][ice_field_idx]

    # if none then use the start and goal from the experiment config file
    if start is not None:
        exp['ship_state'] = start
    if goal is not None:
        exp['goal'] = goal

    sim(cfg=cfg,
        debug=debug,          # enable planner debugging mode
        logging=logging,      # enable planner logs
        log_level=log_level,  # log level for planner https://docs.python.org/3/library/logging.html#levels
        init_queue=exp        # first message sent to planner process
        )


if __name__ == '__main__':
    from ship_ice_planner import FULL_SCALE_SIM_EXP_CONFIG, FULL_SCALE_SIM_PARAM_CONFIG

    # setup arg parser
    parser = argparse.ArgumentParser(description='Ship ice navigation demo. '
                                                 'Runs a single simulation of a ship transiting an ice field. '
                                                 'Demo can be ran with no arguments to use default values.')
    parser.add_argument('exp_config_file', nargs='?', type=str, help='File path to experiment config pickle file '
                                                                     'generated by generate_rand_exp.py',
                        default=FULL_SCALE_SIM_EXP_CONFIG)
    parser.add_argument('planner_config_file', nargs='?', type=str, help='File path to planner and simulation '
                                                                         'parameter config yaml file (see configs/)',
                        default=FULL_SCALE_SIM_PARAM_CONFIG)
    parser.add_argument('-c', dest='ice_concentration', type=float,
                        help='Pick an ice concentration from {0.2, 0.3, 0.4, 0.5}', default=0.5)
    parser.add_argument('-i', dest='ice_field_idx', type=int,
                        help='Pick an ice field from {0, 1, ..., 99}', default=1)
    parser.add_argument('-s', '--start', nargs=3, metavar=('x', 'y', 'psi'), type=float,
                        help='initial ship position (x, y) in meters and heading (psi) in radians',
                        default=None)
    parser.add_argument('-g', '--goal', nargs=2, metavar=('x', 'y'), type=float,
                        help='goal position (x, y) in meters', default=None)
    parser.add_argument('--no_anim', action='store_true', help='Disable rendering (significantly speeds up sim!)')
    parser.add_argument('--output_dir', type=str, default=None, help='Directory path to store output data')
    parser.add_argument('-d', '--debug', action='store_true', help='Debug mode')
    parser.add_argument('-l', '--logging', action='store_true', help='Logging mode')
    parser.add_argument('-ll', '--log_level', type=int, default=10, help='Logging level')

    args = parser.parse_args()

    print('Launching ship ice navigation demo...\nCmd-line arguments:'
          '\n\tExperiment config file: %s'
          '\n\tPlanner config file: %s'
          '\n\tIce concentration: %s'
          '\n\tIce field index: %s'
          '\n\tStart: %s'
          '\n\tGoal: %s'
          '\n\tShow live animation: %s'
          '\n\tOutput directory: %s'
          '\n\tDebug: %s'
          '\n\tLogging: %s'
          '\n\tLog level: %s' % (args.exp_config_file, args.planner_config_file, args.ice_concentration,
                                 args.ice_field_idx, args.start, args.goal, not args.no_anim, args.output_dir,
                                 args.debug, args.logging, args.log_level))

    demo(cfg_file=args.planner_config_file,
         exp_config_file=args.exp_config_file,
         ice_concentration=args.ice_concentration,
         ice_field_idx=args.ice_field_idx,
         start=args.start,
         goal=args.goal,
         show_anim=not args.no_anim,
         output_dir=args.output_dir,
         debug=args.debug,
         logging=args.logging,
         log_level=args.log_level)