def test_packparse(self):
"""Test the packing and parsing of an fgFDM packet"""
fdm = fgFDM()
fdm.set('latitude', 67.4, units='degrees')
fdm.set('longitude', 120.6, units='degrees')
fdm.set('num_engines', 1)
fdm.set('vcas', 44, units='mps')
packedBytes = fdm.pack()
parsedObj = fgFDM()
parsedObj.parse(packedBytes)
assert parsedObj.get('latitude', units='degrees') == 67.4
assert round(parsedObj.get('vcas', units='knots'), 2) == 85.53
def run_node():
global pub
global fdm
global jsb_in
global jsb_console
global fg_out
rospy.init_node('simulation', anonymous=True)
rospy.Subscriber("mixer_out", mixer_out, mixer_out_cb)
pub = rospy.Publisher("simout", simout, queue_size=1)
"""Start JSBsim and treat it as a child proccess"""
cmd = "JSBSim --realtime --suspend --nice --simulation-rate=400 --logdirectivefile=jsbsim/fgout.xml --script=jsbsim/rascal_test.xml"
jsb = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
jsb.delaybeforesend = 0
util.pexpect_autoclose(jsb)
i = jsb.expect([
"Successfully bound to socket for input on port (\d+)",
"Could not bind to socket for input"
])
if i == 1:
print("Failed to start JSBSim - is another copy running?")
sys.exit(1)
jsb_out_address = ('127.0.0.1', 5002) #port on which jsbsim takes data
jsb.expect("Creating UDP socket on port (\d+)")
jsb_in_address = ('127.0.0.1', 5505) #port on which jsbsim outputs data
jsb.expect("Successfully connected to socket for output")
jsb.expect("JSBSim Execution beginning")
# setup output to jsbsim
print("JSBSim console on %s" % str(jsb_in_address))
jsb_out = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
jsb_out.connect(jsb_in_address)
jsb_console = fdpexpect.fdspawn(jsb_out.fileno(), logfile=sys.stdout)
jsb_console.delaybeforesend = 0
# setup input from jsbsim
print("JSBSim FG FDM input on %s" % str(jsb_out_address))
jsb_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
jsb_in.bind(jsb_out_address)
#jsb_in.setblocking(0) #we want this to be blocking
#setput input to flightgear for visualization
fg_address = ('127.0.0.1', 5503)
fg_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
fg_out.connect(fg_address)
fdm = fgFDM.fgFDM()
jsb_console.send('info\n')
jsb_console.send('resume\n')
jsb.expect(["trim computation time", "Trim Results"])
time.sleep(1.5)
jsb_console.logfile = None
print("Simulator ready to fly")
rospy.spin()
def __init__(self, filename):
self.root = Tkinter.Tk()
self.filesize = os.path.getsize(filename)
self.filepos = 0.0
self.mlog = mavutil.mavlink_connection(filename, planner_format=opts.planner,
robust_parsing=True)
self.mout = []
for m in opts.out:
self.mout.append(mavutil.mavlink_connection(m, input=False, baud=opts.baudrate))
self.fgout = []
for f in opts.fgout:
self.fgout.append(mavutil.mavudp(f, input=False))
self.fdm = fgFDM.fgFDM()
self.msg = self.mlog.recv_match(condition=opts.condition)
if self.msg is None:
sys.exit(1)
self.last_timestamp = getattr(self.msg, '_timestamp')
self.paused = False
self.topframe = Tkinter.Frame(self.root)
self.topframe.pack(side=Tkinter.TOP)
self.frame = Tkinter.Frame(self.root)
self.frame.pack(side=Tkinter.LEFT)
self.slider = Tkinter.Scale(self.topframe, from_=0, to=1.0, resolution=0.01,
orient=Tkinter.HORIZONTAL, command=self.slew)
self.slider.pack(side=Tkinter.LEFT)
self.clock = Tkinter.Label(self.topframe,text="")
self.clock.pack(side=Tkinter.RIGHT)
self.playback = Tkinter.Spinbox(self.topframe, from_=0, to=20, increment=0.1, width=3)
self.playback.pack(side=Tkinter.BOTTOM)
self.playback.delete(0, "end")
self.playback.insert(0, 1)
self.buttons = {}
self.button('quit', 'gtk-quit.gif', self.frame.quit)
self.button('pause', 'media-playback-pause.gif', self.pause)
self.button('rewind', 'media-seek-backward.gif', self.rewind)
self.button('forward', 'media-seek-forward.gif', self.forward)
self.button('status', 'Status', self.status)
self.flightmode = Tkinter.Label(self.frame,text="")
self.flightmode.pack(side=Tkinter.RIGHT)
self.next_message()
self.root.mainloop()
def __init__(self, filename):
self.root = Tkinter.Tk()
self.filesize = os.path.getsize(filename)
self.filepos = 0.0
self.mlog = mavutil.mavlink_connection(filename, planner_format=args.planner,
robust_parsing=True)
self.mout = []
for m in args.out:
self.mout.append(mavutil.mavlink_connection(m, input=False, baud=args.baudrate))
self.fgout = []
for f in args.fgout:
self.fgout.append(mavutil.mavudp(f, input=False))
self.fdm = fgFDM.fgFDM()
self.msg = self.mlog.recv_match(condition=args.condition)
if self.msg is None:
sys.exit(1)
self.last_timestamp = getattr(self.msg, '_timestamp')
self.paused = False
self.topframe = Tkinter.Frame(self.root)
self.topframe.pack(side=Tkinter.TOP)
self.frame = Tkinter.Frame(self.root)
self.frame.pack(side=Tkinter.LEFT)
self.slider = Tkinter.Scale(self.topframe, from_=0, to=1.0, resolution=0.01,
orient=Tkinter.HORIZONTAL, command=self.slew)
self.slider.pack(side=Tkinter.LEFT)
self.clock = Tkinter.Label(self.topframe,text="")
self.clock.pack(side=Tkinter.RIGHT)
self.playback = Tkinter.Spinbox(self.topframe, from_=0, to=20, increment=0.1, width=3)
self.playback.pack(side=Tkinter.BOTTOM)
self.playback.delete(0, "end")
self.playback.insert(0, 1)
self.buttons = {}
self.button('quit', 'gtk-quit.gif', self.frame.quit)
self.button('pause', 'media-playback-pause.gif', self.pause)
self.button('rewind', 'media-seek-backward.gif', self.rewind)
self.button('forward', 'media-seek-forward.gif', self.forward)
self.button('status', 'Status', self.status)
self.flightmode = Tkinter.Label(self.frame,text="")
self.flightmode.pack(side=Tkinter.RIGHT)
self.next_message()
self.root.mainloop()
def test_getset(self):
"""Test the getting and setting and unit conversion of variables"""
fdm = fgFDM()
fdm.set('latitude', 67.4, units='degrees')
fdm.set('longitude', 120.6, units='degrees')
fdm.set('num_engines', 1)
fdm.set('vcas', 44, units='mps')
assert fdm.get('latitude', units='degrees') == 67.4
assert round(fdm.get('vcas', units='knots'), 2) == 85.53
def setup_fdm(self):
self.fdm = fgFDM.fgFDM()
jsb_console = self.jsb_console
jsb = self.jsb
jsb_console.send('info\n')
jsb_console.send('resume\n')
jsb.expect(["trim computation time", "Trim Results"])
time.sleep(1.5)
jsb_console.send('step\n')
jsb_console.logfile = None
print "read to fly"
def setup_fdm(self):
self.fdm = fgFDM.fgFDM()
jsb_console = self.jsb_console
jsb = self.jsb
jsb_console.send("info\n")
jsb_console.send("resume\n")
jsb.expect(["trim computation time", "Trim Results"])
time.sleep(1.5)
jsb_console.send("step\n")
jsb_console.logfile = None
print "read to fly"
def __init__(self, fg_in_address, fg_out_address):
self.fg_in_address = fg_in_address
self.fg_out_address = fg_out_address
#Startup socket for incomming messages from FG
self.fg_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.fg_in.bind(fg_in_address)
self.fg_in.setblocking(0)
#Startup socket for outgoing messages to FG
self.fg_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.fg_out.connect(fg_out_address)
self.fg_out.setblocking(0)
self.fdm = fgFDM.fgFDM()
def init_JSBSim(self):
cmd = "JSBSim --realtime --suspend --simulation-rate=400 --script=%s --logdirectivefile=data/fgout.xml" % self.script
jsb = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
jsb.delaybeforesend = 0
util.pexpect_autoclose(jsb)
# time.sleep(10)
print 'Waiting for JSBSim'
jsb.expect("JSBSim startup beginning")
i = jsb.expect([
"Creating input TCP socket on port (\d+)",
"Could not bind to socket for input"
])
if i == 1:
print("Failed to start JSBSim - is another copy running?")
sys.exit(1)
jsb.expect("Creating UDP socket on port (\d+)")
jsb.expect("Successfully connected to socket for output")
jsb.expect("JSBSim Execution beginning")
# setup output to jsbsim
jsb_out_address = ('127.0.0.1', 5124)
print("JSBSim console on %s" % str(jsb_out_address))
jsb_out = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
jsb_out.connect(jsb_out_address)
# jsb_out.bind(jsb_out_address)
# jsb_console = fdpexpect.fdspawn(jsb_out.fileno(), logfile=sys.stdout)
jsb_console = fdpexpect.fdspawn(jsb_out.fileno(), logfile=sys.stdout)
jsb_console.delaybeforesend = 0
jsb_console.logfile = open('/home/mvacanti/logf.txt', 'wt')
jsb_console.expect('Connected to JSBSim server')
# setup input from jsbsim
jsb_in_address = ('127.0.0.1', 5123)
print("JSBSim FG FDM input on %s" % str(jsb_in_address))
jsb_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
jsb_in.bind(jsb_in_address)
jsb_in.setblocking(0)
# set class data
self.jsb = jsb
self.jsb_in = jsb_in
self.jsb_out = jsb_out
self.jsb_console = jsb_console
self.fdm = fgFDM.fgFDM()
def __init__(self, filename):
self.mlog = mavutil.mavlink_connection(filename)
self.fgout = []
for f in args.fgout:
self.fgout.append(mavutil.mavudp(f, input=False))
self.fdm = fgFDM.fgFDM()
self.chute_released = False
self.msg = self.next_msg()
if self.msg is None:
sys.exit(1)
self.last_timestamp = self.msg.TimeUS*1.0e-6
while True:
self.next_message()
if self.msg is None:
break
def __init__(self, filename):
self.mlog = mavutil.mavlink_connection(filename)
self.fgout = []
for f in args.fgout:
self.fgout.append(mavutil.mavudp(f, input=False))
self.fdm = fgFDM.fgFDM()
self.chute_released = False
self.msg = self.next_msg()
if self.msg is None:
sys.exit(1)
self.last_timestamp = self.msg.TimeUS * 1.0e-6
while True:
self.next_message()
if self.msg is None:
break
def init_jsbsim(self):
cmd = "JSBSim --realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=data/flightgear.xml --script=%s" % self.script
if self.options:
cmd += ' %s' % self.options
jsb = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
jsb.delaybeforesend = 0
util.pexpect_autoclose(jsb)
i = jsb.expect([
"Successfully bound to socket for input on port (\d+)",
"Could not bind to socket for input"
])
if i == 1:
print("Failed to start JSBSim - is another copy running?")
sys.exit(1)
jsb_out_address = self.interpret_address("127.0.0.1:%u" %
int(jsb.match.group(1)))
jsb.expect("Creating UDP socket on port (\d+)")
jsb_in_address = self.interpret_address("127.0.0.1:%u" %
int(jsb.match.group(1)))
jsb.expect("Successfully connected to socket for output")
jsb.expect("JSBSim Execution beginning")
# setup output to jsbsim
print("JSBSim console on %s" % str(jsb_out_address))
jsb_out = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
jsb_out.connect(jsb_out_address)
jsb_console = fdpexpect.fdspawn(jsb_out.fileno(), logfile=sys.stdout)
jsb_console.delaybeforesend = 0
jsb_console.logfile = None
# setup input from jsbsim
print("JSBSim FG FDM input on %s" % str(jsb_in_address))
jsb_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
jsb_in.bind(jsb_in_address)
jsb_in.setblocking(0)
# set class data
self.jsb = jsb
self.jsb_in = jsb_in
self.jsb_out = jsb_out
self.jsb_console = jsb_console
self.fdm = fgFDM.fgFDM()
def init_jsbsim(self):
cmd = (
"JSBSim --realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=data/flightgear.xml --script=%s"
% self.script
)
if self.options:
cmd += " %s" % self.options
jsb = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
jsb.delaybeforesend = 0
util.pexpect_autoclose(jsb)
i = jsb.expect(["Successfully bound to socket for input on port (\d+)", "Could not bind to socket for input"])
if i == 1:
print ("Failed to start JSBSim - is another copy running?")
sys.exit(1)
jsb_out_address = self.interpret_address("127.0.0.1:%u" % int(jsb.match.group(1)))
jsb.expect("Creating UDP socket on port (\d+)")
jsb_in_address = self.interpret_address("127.0.0.1:%u" % int(jsb.match.group(1)))
jsb.expect("Successfully connected to socket for output")
jsb.expect("JSBSim Execution beginning")
# setup output to jsbsim
print ("JSBSim console on %s" % str(jsb_out_address))
jsb_out = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
jsb_out.connect(jsb_out_address)
jsb_console = fdpexpect.fdspawn(jsb_out.fileno(), logfile=sys.stdout)
jsb_console.delaybeforesend = 0
jsb_console.logfile = None
# setup input from jsbsim
print ("JSBSim FG FDM input on %s" % str(jsb_in_address))
jsb_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
jsb_in.bind(jsb_in_address)
jsb_in.setblocking(0)
# set class data
self.jsb = jsb
self.jsb_in = jsb_in
self.jsb_out = jsb_out
self.jsb_console = jsb_console
self.fdm = fgFDM.fgFDM()
def test_constructor(self):
"""Test the constructor"""
fdm = fgFDM()
assert fdm.FG_NET_FDM_VERSION == 24
def kt2mps(x):
return x * 0.514444444
def mps2kt(x):
return x / 0.514444444
udp = udp_socket("127.0.0.1:5123")
fgout = udp_socket("127.0.0.1:5124", is_input=False)
tlast = time.time()
count = 0
fg = fgFDM.fgFDM()
while True:
udp_buffer = udp.recv(1000)
fg.parse(udp_buffer)
fgout.write(fg.pack())
count += 1
if time.time() - tlast > 1.0:
print("%u FPS len=%u" % (count, len(udp_buffer)))
count = 0
tlast = time.time()
print(fg.get('latitude', units='degrees'),
fg.get('longitude', units='degrees'),
fg.get('altitude', units='meters'), fg.get('vcas', units='mps'))
# 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
def m2ft(x):
return x / 0.3048
def kt2mps(x):
return x * 0.514444444
def mps2kt(x):
return x / 0.514444444
udp = udp_socket("127.0.0.1:5123")
fgout = udp_socket("127.0.0.1:5124", input=False)
tlast = time.time()
count = 0
fg = fgFDM.fgFDM()
while True:
buf = udp.recv(1000)
fg.parse(buf)
fgout.write(fg.pack())
count += 1
if time.time() - tlast > 1.0:
print("%u FPS len=%u" % (count, len(buf)))
count = 0
tlast = time.time()
print(fg.get('latitude', units='degrees'),
fg.get('longitude', units='degrees'),
fg.get('altitude', units='meters'),
fg.get('vcas', units='mps'))
airspeed.y = stateStorage.velocity.linear.y - environment.wind.y
airspeed.z = stateStorage.velocity.linear.z - environment.wind.z
vcas = np.sqrt(airspeed.x*airspeed.x + airspeed.y*airspeed.y + airspeed.z*airspeed.z)
fdm.set('vcas', vcas, units='mps')
# rospy.logerr('vcas: %g',vcas)
########
## Setup
########
ros_in_address = "127.0.0.1:5504" # FDM data coming out of the ROS Simulator
ros_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
ros_in.connect(interpret_address(ros_in_address)) # Should I use connect?
fdm = fgFDM.fgFDM() # Create fdm objects
###############
## Main Program
###############
if __name__ == '__main__':
try:
rospy.init_node('fdmUDPSend')
rospy.Subscriber('states', SimStates, state_callback, queue_size=1)
rospy.Subscriber('linearAcc', Vector3, accel_callback, queue_size=1)
rospy.Subscriber('environment', Environment, env_callback, queue_size=1)
timer = rospy.Rate(1000)
stateStorage = SimStates()
rospy.loginfo('fdmUDPSend node up')
airspeed.z = stateStorage.velocity.linear.z - environment.wind.z
vcas = np.sqrt(airspeed.x * airspeed.x + airspeed.y * airspeed.y +
airspeed.z * airspeed.z)
fdm.set('vcas', vcas, units='mps')
# rospy.logerr('vcas: %g',vcas)
########
## Setup
########
ros_in_address = "127.0.0.1:5504" # FDM data coming out of the ROS Simulator
ros_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
ros_in.connect(interpret_address(ros_in_address)) # Should I use connect?
fdm = fgFDM.fgFDM() # Create fdm objects
###############
## Main Program
###############
if __name__ == '__main__':
try:
rospy.init_node('fdmUDPSend')
rospy.Subscriber('states', SimStates, state_callback, queue_size=1)
rospy.Subscriber('linearAcc', Vector3, accel_callback, queue_size=1)
rospy.Subscriber('environment',
Environment,
env_callback,
queue_size=1)
timer = rospy.Rate(1000)
def processFgInput(buf): fullState = fgFDM.fgFDM() print buf fullState.parse(buf) return fullState
ctrls.value[0] = fdm_ctrls.get('left_aileron')
ctrls.value[1] = fdm_ctrls.get('elevator')
ctrls.value[2] = fdm_ctrls.get('rpm')
ctrls.value[3] = fdm_ctrls.get('rudder')
# ctrls.value[5] = fdm_ctrls.get('speedbrake')
ctrls.value[5] = 1000
# print ctrls
pub.publish(ctrls)
jsb_out_address = "127.0.0.1:5505" # FDM control data coming into the ROS Simulator from Ardupilot
jsb_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
jsb_out.bind(interpret_address(jsb_out_address))
# jsb_out.setblocking(0)
fdm_ctrls = fgFDM.fgFDM()
###############
## Main Program
###############
if __name__ == '__main__':
try:
rospy.init_node('fdmUDPReceive')
# pub = rospy.Publisher('/fw1/ctrlPWM',SimPWM)
pub = rospy.Publisher('ctrlPWM',SimPWM, queue_size=10)
timer = rospy.Rate(1000)
while not rospy.is_shutdown():
# 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()
jsb_console.send('info\n')
jsb_console.send('resume\n')
jsb.expect(["trim computation time","Trim Results"])
time.sleep(1.5)
jsb_console.send('step\n')
jsb_console.logfile = None
print("Simulator ready to fly")
def main_loop():
'''run main loop'''
tnow = time.time()
last_report = tnow
last_sim_input = tnow
# 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
timestamp = 0
