From 9b29a9cd5d1e3ca31e9c0bff5f6e43567399be0d Mon Sep 17 00:00:00 2001 From: Georacer Date: Mon, 16 Feb 2015 18:31:27 +0200 Subject: [PATCH] autotest: Enable SITL code to call an external ROS simulator for ArduPlane sim_vehicle.sh code was modified to call the ROS/runsim.py file instead of the default /jsbsim/runsim.py. ROS/runsim.py uses roslaunch command to launch the last_letter simulator https://github.com/Georacer/last_letter, which should be cloned and compiled in the system. GIT_Success.txt file was modified to depict the author's current mindstate. The location LGAT (Former National Airport of Athens, now an unofficial model park) was added in the locations.txt file, since this is my base of operations and start for my missions. --- Tools/GIT_Test/GIT_Success.txt | 3 +- Tools/autotest/ROS/runsim.py | 260 +++++++++++++++++++++++++++++++++ Tools/autotest/locations.txt | 2 +- Tools/autotest/sim_vehicle.sh | 8 +- 4 files changed, 270 insertions(+), 3 deletions(-) create mode 100755 Tools/autotest/ROS/runsim.py diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt index 679255e44a..0aa5b33e90 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -17,4 +17,5 @@ Nils H Linus Penzlien Hazy Benoit GRUSON -Dario Lindo Andres \ No newline at end of file +Dario Lindo Andres +George Zogopoulos Papaliakos, struggling with Git \ No newline at end of file diff --git a/Tools/autotest/ROS/runsim.py b/Tools/autotest/ROS/runsim.py new file mode 100755 index 0000000000..f746b28534 --- /dev/null +++ b/Tools/autotest/ROS/runsim.py @@ -0,0 +1,260 @@ +#!/usr/bin/env python +# run a ROS simulator as a child process + +import sys, os, pexpect, socket +import math, time, select, struct, signal, errno + +sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'pysim')) + +import util, atexit +from pymavlink import fgFDM + +class control_state(object): + def __init__(self): + self.aileron = 0 + self.elevator = 0 + self.throttle = 0 + self.rudder = 0 + self.ground_height = 0 + +sitl_state = control_state() + + +def interpret_address(addrstr): + '''interpret a IP:port string''' + a = addrstr.split(':') + a[1] = int(a[1]) + return tuple(a) + +def process_sitl_input(buf): + '''process control changes from SITL sim''' + + global ros_out + + control = list(struct.unpack('<14H', buf)) + pwm = control[:11] + (speed, direction, turbulance) = control[11:] + + global wind + wind.speed = speed*0.01 + wind.direction = direction*0.01 + wind.turbulance = turbulance*0.01 + + aileron = (pwm[0]-1500)/500.0 + elevator = (pwm[1]-1500)/500.0 + throttle = (pwm[2]-1000)/1000.0 + if opts.revthr: + throttle = 1.0 - throttle + rudder = (pwm[3]-1500)/500.0 + + if opts.elevon: + # fake an elevon plane + ch1 = aileron + ch2 = elevator + aileron = (ch2-ch1)/2.0 + # the minus does away with the need for RC2_REV=-1 + elevator = -(ch2+ch1)/2.0 + + if opts.vtail: + # fake an elevon plane + ch1 = elevator + ch2 = rudder + # this matches VTAIL_OUTPUT==2 + elevator = (ch2-ch1)/2.0 + rudder = (ch2+ch1)/2.0 + + if aileron != sitl_state.aileron: + fdm_ctrls.set('left_aileron', pwm[0]) + sitl_state.aileron = aileron + if elevator != sitl_state.elevator: + fdm_ctrls.set('elevator', pwm[1]) + sitl_state.elevator = elevator + if rudder != sitl_state.rudder: + fdm_ctrls.set('rudder', pwm[3]) + sitl_state.rudder = rudder + if throttle != sitl_state.throttle: + fdm_ctrls.set('rpm', pwm[2]) + sitl_state.throttle = throttle + + try: + ros_out.send(fdm_ctrls.pack()) + except socket.error as e: + if e.errno not in [ errno.ECONNREFUSED ]: + raise + +def process_ros_input(buf,frame_count): + '''process FG FDM input from ROS Simulator''' + global fdm, fg_out, sim_out + FG_FDM_FPS = 30 + + fdm.parse(buf) + if (fg_out and ((frame_count % FG_FDM_FPS) == 0)) : + try: + agl = fdm.get('agl', units='meters') + fdm.set('altitude', agl+sitl_state.ground_height, units='meters') + fdm.set('rpm', sitl_state.throttle*1000) + fg_out.send(fdm.pack()) + except socket.error as e: + if e.errno not in [ errno.ECONNREFUSED ]: + raise + + simbuf = struct.pack('<17dI', + fdm.get('latitude', units='degrees'), + fdm.get('longitude', units='degrees'), + fdm.get('altitude', units='meters'), + fdm.get('psi', units='degrees'), + fdm.get('v_north', units='mps'), + fdm.get('v_east', units='mps'), + fdm.get('v_down', units='mps'), + fdm.get('A_X_pilot', units='mpss'), + fdm.get('A_Y_pilot', units='mpss'), + fdm.get('A_Z_pilot', units='mpss'), + fdm.get('phidot', units='dps'), + fdm.get('thetadot', units='dps'), + fdm.get('psidot', units='dps'), + fdm.get('phi', units='degrees'), + fdm.get('theta', units='degrees'), + fdm.get('psi', units='degrees'), + fdm.get('vcas', units='mps'), + 0x4c56414f) + try: + sim_out.send(simbuf) + except socket.error as e: + if e.errno not in [ errno.ECONNREFUSED ]: + raise + + + +################## +# main program +from optparse import OptionParser +parser = OptionParser("runsim.py [options]") +parser.add_option("--simin", help="SITL input (IP:port)", default="127.0.0.1:5502") +parser.add_option("--simout", help="SITL output (IP:port)", default="127.0.0.1:5501") +parser.add_option("--fgout", help="FG display output (IP:port)", default="127.0.0.1:5503") +parser.add_option("--home", type='string', help="home lat,lng,alt,hdg") # Not implemented yet +parser.add_option("--script", type='string', help='jsbsim model script', default='jsbsim/rascal_test.xml') # Not implemented yet +parser.add_option("--options", type='string', help='ROS startup options') +parser.add_option("--elevon", action='store_true', default=False, help='assume elevon input') +parser.add_option("--revthr", action='store_true', default=False, help='reverse throttle') +parser.add_option("--vtail", action='store_true', default=False, help='assume vtail input') +parser.add_option("--wind", dest="wind", help="Simulate wind (speed,direction,turbulance)", default='0,0,0') # Not implemented yet + +(opts, args) = parser.parse_args() + +os.chdir(util.reltopdir('Tools/autotest')) + +# kill off child when we exit +atexit.register(util.pexpect_close_all) + +# start child +cmd = "roslaunch last_letter launcher.launch ArduPlane:=true" +if opts.options: + cmd += ' %s' % opts.options + +ros = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10) +ros.delaybeforesend = 0 +util.pexpect_autoclose(ros) + +ros_out_address = interpret_address("127.0.0.1:5505") +ros_in_address = interpret_address("127.0.0.1:5504") + +# setup output to ROS +print("ROS listens for FG-FDM packets at %s" % str(ros_out_address)) +ros_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +ros_out.connect(ros_out_address) + +# setup input from ROS +print("ROS sends FG-FDM packetes for the SITL at %s" % str(ros_in_address)) +ros_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +ros_in.bind(ros_in_address) +ros_in.setblocking(0) + +# socket addresses +sim_out_address = interpret_address(opts.simout) +sim_in_address = interpret_address(opts.simin) + +# setup input from SITL sim +sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +sim_in.bind(sim_in_address) +sim_in.setblocking(0) + +# setup output to SITL sim +sim_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +sim_out.connect(interpret_address(opts.simout)) +sim_out.setblocking(0) + +# setup possible output to FlightGear for display +fg_out = None +if opts.fgout: + fg_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + fg_out.connect(interpret_address(opts.fgout)) + +# setup wind generator +wind = util.Wind(opts.wind) + +fdm = fgFDM.fgFDM() +fdm_ctrls = fgFDM.fgFDM() # Setup another fdm object to send ctrls to ROS + +time.sleep(1.5) + +print("Simulator ready to fly") + +def main_loop(): + '''run main loop''' + tnow = time.time() + last_report = tnow + last_sim_input = tnow + last_wind_update = tnow + frame_count = 0 + paused = False + + while True: + rin = [ros_in.fileno(), sim_in.fileno()] + + try: + (rin, win, xin) = select.select(rin, [], [], 1.0) + except select.error: + util.check_parent() + continue + + tnow = time.time() + + if ros_in.fileno() in rin: + buf = ros_in.recv(fdm.packet_size()) + process_ros_input(buf,frame_count) + frame_count += 1 + + if sim_in.fileno() in rin: + simbuf = sim_in.recv(28) + process_sitl_input(simbuf) + last_sim_input = tnow + + if tnow - last_report > 3: + print("FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)" % ( + frame_count / (time.time() - last_report), + fdm.get('altitude', units='meters'), + fdm.get('agl', units='meters'), + fdm.get('phi', units='degrees'), + fdm.get('theta', units='degrees'), + fdm.get('A_X_pilot', units='mpss'), + fdm.get('A_Y_pilot', units='mpss'), + fdm.get('A_Z_pilot', units='mpss'))) + + frame_count = 0 + last_report = time.time() + +def exit_handler(): + '''exit the sim''' + signal.signal(signal.SIGINT, signal.SIG_IGN) + signal.signal(signal.SIGTERM, signal.SIG_IGN) + sys.exit(1) + +signal.signal(signal.SIGINT, exit_handler) +signal.signal(signal.SIGTERM, exit_handler) + +try: + main_loop() +except: + exit_handler() + raise diff --git a/Tools/autotest/locations.txt b/Tools/autotest/locations.txt index b30234392e..aeba769825 100644 --- a/Tools/autotest/locations.txt +++ b/Tools/autotest/locations.txt @@ -10,4 +10,4 @@ TomCarpark=26.048950,-80.254219,6,270 Ballarat=-37.598705,143.881744,485,165 3DRBerkeley=37.872991,-122.302348,20,260 BMAC=-35.226343,149.132122,588,301 - +LGAT=37.889063,23.731863,21,330 \ No newline at end of file diff --git a/Tools/autotest/sim_vehicle.sh b/Tools/autotest/sim_vehicle.sh index a288cf6ca8..6f3cb99d64 100755 --- a/Tools/autotest/sim_vehicle.sh +++ b/Tools/autotest/sim_vehicle.sh @@ -337,7 +337,13 @@ if [ $EXTERNAL_SIM == 0 ]; then } sleep 2 else - echo "Using external simulator" + echo "Using external ROS simulator" + RUNSIM="$autotest/ROS/runsim.py --simin=$SIMIN_PORT --simout=$SIMOUT_PORT --fgout=$FG_PORT $EXTRA_SIM" + $autotest/run_in_terminal_window.sh "ROS Simulator" $RUNSIM || { + echo "Failed to start simulator: $RUNSIM" + exit 1 + } + sleep 2 fi # mavproxy.py --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --out 127.0.0.1:14550 --out 127.0.0.1:14551