diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index dd73e59955..f049fcfb66 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -1239,6 +1239,41 @@ else: frame_infos, spawns=spawns) + +if cmd_opts.delay_start: + progress("Sleeping for %f seconds" % (cmd_opts.delay_start,)) + time.sleep(float(cmd_opts.delay_start)) + +tmp = None +if cmd_opts.frame in ['scrimmage-plane', 'scrimmage-copter']: + # import only here so as to avoid jinja dependency in whole script + from jinja2 import Environment, FileSystemLoader + from tempfile import mkstemp + entities = [] + config = {} + config['plane'] = cmd_opts.vehicle == 'ArduPlane' + config['terrain'] = 'mcmillan' + if location is not None: + config['lat'] = location[0] + config['lon'] = location[1] + config['alt'] = location[2] + config['entities'] = [] + for k in offsets: + (x, y, z, heading) = offsets[k] + config['entities'].append({'x': x, 'y': y, 'z': z, 'heading': heading, + 'to_ardupilot_port': 9003 + k * 10, + 'from_ardupilot_port': 9002 + k * 10, + 'to_ardupilot_ip': '127.0.0.1'}) + env = Environment(loader=FileSystemLoader(os.path.join(autotest_dir, 'template'))) + mission = env.get_template('scrimmage.xml').render(**config) + tmp = mkstemp() + atexit.register(os.remove, tmp[1]) + + with os.fdopen(tmp[0], 'w') as fd: + fd.write(mission) + run_in_terminal_window('SCRIMMAGE', ['scrimmage', tmp[1]]) + + if cmd_opts.delay_start: progress("Sleeping for %f seconds" % (cmd_opts.delay_start,)) time.sleep(float(cmd_opts.delay_start)) diff --git a/Tools/autotest/template/scrimmage.xml b/Tools/autotest/template/scrimmage.xml new file mode 100644 index 0000000000..4809eaed7d --- /dev/null +++ b/Tools/autotest/template/scrimmage.xml @@ -0,0 +1,101 @@ + + + + + + + + 50051 + localhost + + all_dead + + 10 + 1000 + + {% if terrain %}{{ terrain }}{% else %}mcmillan{% endif %} + 191 191 191 + 10 + + false + all + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + + {% if plane %} + {% if lat %}{{ lat }}{% else %}32.42553{% endif %} + {% if lon %}{{ lon }}{% else %}-84.79109{% endif %} + {% if alt %}{{ alt }}{% else %}75{% endif %} + {% else %} + {% if lat %}{{ lat }}{% else %}34.458281{% endif %} + {% if lon %}{{ lon }}{% else %}-84.180209{% endif %} + {% if alt %}{{ alt }}{% else %}450{% endif %} + {% endif %} + + true + 10 + + LocalNetwork + GlobalNetwork + + SimpleCollision + GroundCollision + + + + + + {% for e in entities %} + + 1 + 77 77 255 + 1 + 1 + 1 + + {% if e.x %}{{ e.x }}{% else %}0{% endif %} + {% if e.y %}{{ e.y }}{% else %}0{% endif %} + {% if e.z %}{{ e.z }}{% else %}0{% endif %} + {% if plane %}-20{% endif %} + {% if e.heading %}{{ e.heading }}{% else %}0{% endif %} + + + RigidBody6DOFStateSensor + DirectController + + + ArduPilot + + + + {% if e.motion_model %}{{ e.motion_model }}{% else %}JSBSimControl{% endif %} + rascal_no_autopilot.xml + {% if e.visual_model %}{{ e.visual_model }}{% else %}zephyr-blue{% endif %} + {% else %} + servo_map="[ motor_0 0 1000 +2000 346.41 1200.0 +1 ] + [ motor_1 1 1000 +2000 346.41 1200.0 +1 ] + [ motor_2 2 1000 +2000 346.41 1200.0 +1 ] + [ motor_3 3 1000 +2000 346.41 1200.0 +1 ]" + >ArduPilot + {% if e.motion_model %}{{ e.motion_model }}{% else %}Multirotor{% endif %} + {% if e.visual_model %}{{ e.visual_model }}{% else %}iris{% endif %} + {% endif %} + + {% endfor %} +