diff --git a/osgar/lib/config.py b/osgar/lib/config.py index 72fe74de1..585dfbcba 100644 --- a/osgar/lib/config.py +++ b/osgar/lib/config.py @@ -2,7 +2,9 @@ Osgar Config Class """ import json +import re import sys + from importlib import import_module from osgar.drivers import all_drivers @@ -64,4 +66,27 @@ def merge_dict(dict1, dict2): ret[key] = dict2[key] return ret + +def _replace(input, data): + m = re.match("{(.*)}", input) + if m is not None and m.group(1) in data: + return data[m.group(1)] + return input + + +def config_expand(cfg, data): + if isinstance(cfg, dict): + ret = {} + for k,v in cfg.items(): + ret[_replace(k, data)] = config_expand(v, data) + return ret + if isinstance(cfg, list): + ret = [] + for v in cfg: + ret.append(config_expand(v, data)) + return ret + if isinstance(cfg, str): + return _replace(cfg, data) + return cfg + # vim: expandtab sw=4 ts=4 diff --git a/osgar/lib/test_config.py b/osgar/lib/test_config.py index 18846ee40..109b9093a 100644 --- a/osgar/lib/test_config.py +++ b/osgar/lib/test_config.py @@ -1,6 +1,6 @@ import os import unittest -from osgar.lib.config import config_load, merge_dict, MergeConflictError, get_class_by_name +from osgar.lib.config import config_load, merge_dict, MergeConflictError, get_class_by_name, config_expand from osgar.drivers.logsocket import LogTCPStaticIP as LogTCP def test_data(filename, test_dir='test_data'): @@ -62,5 +62,21 @@ def test_application(self): conf = config_load(filename, application='osgar.lib.test_config:MyTestRobot') self.assertTrue(conf['robot']['modules']['app']['driver'].endswith('lib.test_config:MyTestRobot')) + def test_expand(self): + a = config_expand('{application}', dict(application="subt")) + self.assertEqual(a, 'subt') + a = config_expand('before{application}after', dict(application="subt")) + self.assertEqual(a, 'before{application}after') + a = config_expand(['{application}'], dict(application="subt")) + self.assertEqual(a, ['subt']) + + a = config_expand({'{application}': []}, dict(application="subt")) + self.assertEqual(a, {'subt': []}) + a = config_expand({'{application}': ['{item}']}, dict(application="subt", item='first')) + self.assertEqual(a, {'subt': ['first']}) + + a = config_expand('{speed-something}', dict(speed=1)) + self.assertEqual(a, '{speed-something}') + # vim: expandtab sw=4 ts=4 diff --git a/osgar/record.py b/osgar/record.py index af257153e..53c337d12 100644 --- a/osgar/record.py +++ b/osgar/record.py @@ -10,7 +10,7 @@ import logging from osgar.logger import LogWriter -from osgar.lib.config import config_load, get_class_by_name +from osgar.lib.config import config_load, get_class_by_name, config_expand from osgar.bus import Bus g_logger = logging.getLogger(__name__) @@ -62,7 +62,9 @@ def request_stop(self, signum=None, frame=None): # pylint: disable=unused-argume self.stop_requested.set() -def record(config, log_prefix, log_filename=None, duration_sec=None): +def record(config, log_prefix, log_filename=None, duration_sec=None, args={}): + if len(args) > 0: + config['robot']['modules'] = config_expand(config['robot']['modules'], args) with LogWriter(prefix=log_prefix, filename=log_filename, note=str(sys.argv)) as log: log.write(0, bytes(str(config), 'ascii')) g_logger.info(log.filename) @@ -81,16 +83,38 @@ def main(): format='%(asctime)s %(name)-12s %(levelname)-8s %(message)s', datefmt='%Y-%m-%d %H:%M', ) - parser = argparse.ArgumentParser(description='Record run on real HW with given configuration') - parser.add_argument('config', nargs='+', help='configuration file') + parser = argparse.ArgumentParser(description='Record run given configuration', add_help=False) + config_arg = parser.add_argument('config', nargs='*', help='configuration file') + parser.add_argument("-h", "--help", help="show this help message and exit", action="store_true") parser.add_argument('--note', help='add description') parser.add_argument('--duration', help='recording duration (sec), default infinite', type=float) - parser.add_argument('--application', help='import string to application', default=None) - args = parser.parse_args() + parser.add_argument('--log', help='record log filename') + args, unknown = parser.parse_known_args() + + if len(args.config) > 0: + prefix = os.path.basename(args.config[0]).split('.')[0] + '-' + cfg = config_load(*args.config) + types = { + "float": float, + "int": int + } + for k, v in cfg['robot'].get('arguments', {}).items(): + if "type" in v: + v["type"] = types[v["type"]] + parser.add_argument("--" + k, **v) + + config_arg.nargs = "+" + if args.help: + parser.print_help() + parser.exit() + + if len(args.config) == 0: + parser.error("the following arguments are required: config") + parser.exit() - prefix = os.path.basename(args.config[0]).split('.')[0] + '-' - cfg = config_load(*args.config, application=args.application) - record(cfg, log_prefix=prefix, duration_sec=args.duration) + args = parser.parse_args() + cfg_args = { k: getattr(args, k.replace('-','_')) for k in cfg['robot'].get('arguments', {})} + record(cfg, log_prefix=prefix, log_filename=args.log, duration_sec=args.duration, args=cfg_args) if __name__ == "__main__": main() diff --git a/subt/__main__.py b/subt/__main__.py deleted file mode 100644 index c43000f4b..000000000 --- a/subt/__main__.py +++ /dev/null @@ -1,3 +0,0 @@ -from .main import main -main() - diff --git a/subt/docker/robotika/run_solution.bash b/subt/docker/robotika/run_solution.bash index a9d23d311..cd394e314 100755 --- a/subt/docker/robotika/run_solution.bash +++ b/subt/docker/robotika/run_solution.bash @@ -22,7 +22,7 @@ trap "kill -s SIGINT 0; wait" EXIT echo "Waiting for robot name" while [ -z "$ROBOT_NAME" ]; do - ROBOT_NAME=$(rosparam get /robot_names) + ROBOT_NAME=$(rosparam get /robot_names 2>/dev/null) sleep 0.5 done echo "Robot name is '$ROBOT_NAME'" @@ -45,7 +45,7 @@ roslaunch $LAUNCH_FILE --wait robot_name:=$ROBOT_NAME & echo "Starting osgar" export OSGAR_LOGS=/osgar-ws/logs -/osgar-ws/env/bin/python3 -m subt run /osgar-ws/src/osgar/subt/$CONFIG_FILE --side auto --walldist 0.8 --timeout 100 --speed 1.0 --note "run_solution.bash" +/osgar-ws/env/bin/python3 -m osgar.record /osgar-ws/src/osgar/subt/$CONFIG_FILE --side auto --walldist 0.8 --timeout 100 --speed 1.0 --note "run_solution.bash" echo "Sleep and finish" sleep 30 diff --git a/subt/main.py b/subt/main.py index 1aba80ff3..f0dd6cf09 100644 --- a/subt/main.py +++ b/subt/main.py @@ -93,7 +93,7 @@ def __init__(self, config, bus): self.start_pose = None self.traveled_dist = 0.0 self.time = None - self.max_speed = config['max_speed'] + self.max_speed = config.get('speed') or config['max_speed'] self.max_angular_speed = math.radians(60) self.walldist = config['walldist'] self.timeout = timedelta(seconds=config['timeout']) @@ -129,7 +129,7 @@ def __init__(self, config, bus): self.emergency_stop = None self.monitors = [] # for Emergency Stop Exception - self.use_right_wall = config['right_wall'] + self.use_right_wall = 'auto' if config.get('side') == 'auto' else config.get('side') == 'right' self.use_center = False # navigate into center area (controlled by name ending by 'C') self.is_virtual = config.get('virtual_world', False) # workaround to handle tunnel differences @@ -896,71 +896,4 @@ def join(self, timeout=None): self.thread.join(timeout) -def main(): - import argparse - from osgar.lib.config import config_load - from osgar.record import record - - parser = argparse.ArgumentParser(description='SubT Challenge') - subparsers = parser.add_subparsers(help='sub-command help', dest='command') - subparsers.required = True - parser_run = subparsers.add_parser('run', help='run on real HW') - parser_run.add_argument('config', nargs='+', help='configuration file') - parser_run.add_argument('--note', help='add description') - parser_run.add_argument('--walldist', help='distance for wall following (default: %(default)sm)', default=1.0, type=float) - parser_run.add_argument('--side', help='which side to follow', choices=['left', 'right', 'auto'], required=True) - parser_run.add_argument('--speed', help='maximum speed (default: from config)', type=float) - parser_run.add_argument('--timeout', help='seconds of exploring before going home (default: %(default)s)', - type=int, default=10*60) - parser_run.add_argument('--log', nargs='?', help='record log filename') - parser_run.add_argument('--init-offset', help='inital 3D offset accepted as a string of comma separated values (meters)') - parser_run.add_argument('--init-path', help='inital path to be followed from (0, 0). 2D coordinates are separated by ;') - parser_run.add_argument('--start-paused', dest='start_paused', action='store_true', - help='start robota Paused and wait for LoRa Contine command') - - parser_replay = subparsers.add_parser('replay', help='replay from logfile') - parser_replay.add_argument('logfile', help='recorded log file') - parser_replay.add_argument('--force', '-F', dest='force', action='store_true', help='force replay even for failing output asserts') - parser_replay.add_argument('--config', nargs='+', help='force alternative configuration file') - args = parser.parse_args() - - if args.command == 'replay': - from osgar.replay import replay - args.module = 'app' - app = replay(args, application=SubTChallenge) - app.play() - - elif args.command == 'run': - # To reduce latency spikes as described in https://morepypy.blogspot.com/2019/01/pypy-for-low-latency-systems.html. - # Increased latency leads to uncontrolled behavior and robot either missing turns or hitting walls. - # Disabled garbage collection needs to be paired with gc.collect() at place(s) that are not time sensitive. - gc.disable() - - cfg = config_load(*args.config, application=SubTChallenge) - - # apply overrides from command line - cfg['robot']['modules']['app']['init']['walldist'] = args.walldist - if args.side == 'auto': - cfg['robot']['modules']['app']['init']['right_wall'] = 'auto' - else: - cfg['robot']['modules']['app']['init']['right_wall'] = args.side == 'right' - cfg['robot']['modules']['app']['init']['timeout'] = args.timeout - if args.init_offset is not None: - x, y, z = [float(x) for x in args.init_offset.split(',')] - cfg['robot']['modules']['app']['init']['init_offset'] = [int(x*1000), int(y*1000), int(z*1000)] - if args.init_path is not None: - cfg['robot']['modules']['app']['init']['init_path'] = args.init_path - - if args.speed is not None: - cfg['robot']['modules']['app']['init']['max_speed'] = args.speed - - cfg['robot']['modules']['app']['init']['start_paused'] = args.start_paused - - prefix = os.path.basename(args.config[0]).split('.')[0] + '-' - record(cfg, prefix, args.log) - - -if __name__ == "__main__": - main() - # vim: expandtab sw=4 ts=4 diff --git a/subt/test_subt.py b/subt/test_subt.py index 6f7743a81..9ab2d091d 100644 --- a/subt/test_subt.py +++ b/subt/test_subt.py @@ -35,6 +35,19 @@ def test_maybe_remember_artifact(self): # 2nd report should be ignored self.assertEqual(game.maybe_remember_artifact(artf_data, artf_xyz), False) + def test_init(self): + bus = MagicMock() + config = {'virtual_world': True, 'max_speed': 1.0, 'walldist': 0.8, 'timeout': 600, 'symmetric': False, 'right_wall': 'auto'} + game = SubTChallenge(config, bus) + + config = {'virtual_world': True, 'speed': 1.0, 'walldist': 0.8, 'timeout': 600, 'symmetric': False, 'right_wall': 'auto'} + game = SubTChallenge(config, bus) + + # either speed or max_speed is required + with self.assertRaises(KeyError): + config = {'virtual_world': True, 'walldist': 0.8, 'timeout': 600, 'symmetric': False, 'right_wall': 'auto' } + game = SubTChallenge(config, bus) + def test_go_to_entrance(self): config = {'virtual_world': True, 'max_speed': 1.0, 'walldist': 0.8, 'timeout': 600, 'symmetric': False, 'right_wall': 'auto'} bus = Bus(simulation.SimLogger()) diff --git a/subt/zmq-subt-x2.json b/subt/zmq-subt-x2.json index 87ef70c8c..735b1fd44 100644 --- a/subt/zmq-subt-x2.json +++ b/subt/zmq-subt-x2.json @@ -1,14 +1,50 @@ { "version": 2, "robot": { + "arguments": { + "walldist": { + "help": "distance for wall following (default: %(default)sm)", + "default": 1.0, + "type": "float" + }, + "side": { + "help": "which side to follow", + "choices": ["left", "right", "auto"], + "required": true + }, + "speed": { + "help": "maximum speed (%(default)sm)", + "default": 1.0, + "type": "float" + }, + "timeout": { + "help": "seconds of exploring before going home (default: %(default)ss)", + "default": 600, + "type": "int" + }, + "init-offset": { + "help": "inital 3D offset accepted as a string of comma separated values (meters)" + }, + "init-path": { + "help": "inital path to be followed from (0, 0). 2D coordinates are separated by ;" + }, + "start-paused": { + "dest": "start_paused", + "action": "store_true", + "help": "start robot Paused and wait for LoRa Contine command" + } + }, "modules": { "app": { - "driver": "application", + "driver": "subt.main:SubTChallenge", "in": ["emergency_stop", "pose2d", "scan", "rot", "artf", "sim_time_sec", "acc", "origin"], "out": ["desired_speed", "pose2d", "artf_xyz", "pose3d", "stdout", "request_origin"], "init": { - "max_speed": 0.5, + "walldist": "{walldist}", + "side": "{side}", + "speed": "{speed}", + "timeout": "{timeout}", "symmetric": false, "virtual_bumper_sec": 60, "virtual_bumper_radius": 10.0,