ivy.send(gr)
# start natnet interface
natnet = NatNetClient(
server=args.server,
rigidBodyListListener=receiveRigidBodyList,
dataPort=args.data_port,
commandPort=args.command_port,
verbose=args.verbose)
print("Starting Natnet3.x to Ivy interface at %s" % (args.server))
try:
# Start up the streaming client.
# This will run perpetually, and operate on a separate thread.
natnet.run()
while True:
sleep(1)
except (KeyboardInterrupt, SystemExit):
print("Shutting down ivy and natnet interfaces...")
natnet.stop()
ivy.shutdown()
except OSError:
print("Natnet connection error")
natnet.stop()
ivy.stop()
exit(-1)
# convert quaternion to psi euler angle
dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
ivy.send(msg)
# start natnet interface
natnet = NatNetClient(server=args.server,
rigidBodyListListener=receiveRigidBodyList,
dataPort=args.data_port,
commandPort=args.command_port,
verbose=args.verbose)
print("Starting Natnet3.x to Ivy interface at %s" % (args.server))
try:
# Start up the streaming client.
# This will run perpetually, and operate on a separate thread.
natnet.run()
while True:
sleep(1)
except (KeyboardInterrupt, SystemExit):
print("Shutting down ivy and natnet interfaces...")
natnet.stop()
ivy.shutdown()
except OSError:
print("Natnet connection error")
natnet.stop()
ivy.stop()
exit(-1)
from pprzlink import messages_xml_map
try:
msgs = messages_xml_map.get_msgs('test')
except Exception as e:
print(e)
dico = messages_xml_map.message_dictionary
for msg_type in dico.keys():
for msg in dico[msg_type]:
print(msg_type, ":", msg)
ac_id = 24
ivyInterface = IvyMessagesInterface()
time.sleep(0.5)
world = None
uavid = None
def callback01(ac_id, msg, request_id):
print(request_id, msg)
def callback02(ac_id, msg):
print(msg)
ivyInterface.subscribe(callback01, '(.* WORLD_ENV_REQ .*)')
ivyInterface.subscribe(callback02, '(.* GPS .*)')
signal.signal(signal.SIGINT, lambda frame, sig: ivyInterface.stop())
