Commit f9959856 authored by beuchatp's avatar beuchatp
Browse files

Started with making a clean qualisys python node

parent a31c1dfd
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# OPTIONS: #!/usr/bin/env python
# OR: #!/usr/bin/python3
# OR: #!/usr/bin/python3.5
......@@ -11,7 +10,7 @@
# pip3 install rospkg catkin_pkg --user
# pip3 install qtm --user
# Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
# Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
#
# This file is part of D-FaLL-System.
#
......@@ -44,7 +43,7 @@
# Import ros and the D-FaLL package
import roslib; roslib.load_manifest('dfall_pkg')
import rospy
......@@ -58,7 +57,7 @@ from dfall_pkg.msg import ViconData
from dfall_pkg.msg import UnlabeledMarker
# General import
import logging
#import logging
import time
import datetime
import math
......@@ -66,75 +65,46 @@ from enum import Enum
import sys
import struct
import rosbag
# ROS imports
#import rosbag
from rospkg import RosPack
# Import the library for asynchronous I/O
import asyncio
# Import the library for parsing XML trees
import xml.etree.ElementTree as ET
# Import the Qualisys Track Manager (QTM) Library
import qtm
# # Add library
# #sys.path.append("lib")
# # CrazyFlie client imports
# import cflib
# from cflib.crazyflie import Crazyflie
# from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
# import cflib.drivers.crazyradio
# DEFINE CONSTANTS
# # Logging import(*
# from cflib.crazyflie.log import LogConfig
# Conversion between radians and degrees
RAD_TO_DEG = 57.296
DEG_TO_RAD = 0.01745329
# # Logging settings
# logging.basicConfig(level=logging.ERROR)
# Path to the file with "fake" data for testing QTM
QTM_FILE = pkg_resources.resource_filename("qtm", "data/Demo.qtm")
# # CONTROLLER_MOTOR = 2
# # CONTROLLER_ANGLE = 1
# # CONTROLLER_RATE = 0
# CF_COMMAND_TYPE_MOTORS = 6
# CF_COMMAND_TYPE_RATE = 7
# CF_COMMAND_TYPE_ANGLE = 8
# RAD_TO_DEG = 57.296
# # CrazyRadio states:
# CONNECTED = 0
# CONNECTING = 1
# DISCONNECTED = 2
# OPEN THE ROS BAG FOR WRITING
#rp = RosPack()
#record_file = rp.get_path('dfall_pkg') + '/LoggingOnboard.bag'
#rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
#rospy.loginfo(record_file)
#bag = rosbag.Bag(record_file, 'w')
# # Commands coming
# CMD_RECONNECT = 0
# CMD_DISCONNECT = 1
# # Commands for FlyingAgentClient
# #CMD_USE_SAFE_CONTROLLER = 1
# #CMD_USE_DEMO_CONTROLLER = 2
# #CMD_USE_STUDENT_CONTROLLER = 3
# #CMD_USE_MPC_CONTROLLER = 4
# #CMD_USE_REMOTE_CONTROLLER = 5
# #CMD_USE_TUNING_CONTROLLER = 6
# CMD_CRAZYFLY_TAKE_OFF = 11
# CMD_CRAZYFLY_LAND = 12
# CMD_CRAZYFLY_MOTORS_OFF = 13
# Setup the ROS logging
rp = RosPack()
record_file = rp.get_path('dfall_pkg') + '/LoggingOnboard.bag'
#rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
rospy.loginfo(record_file)
bag = rosbag.Bag(record_file, 'w')
class QualisysDataPublisher:
class QualisysQtmClient:
"""
Qualisys Data Publisher that connects to the
Qualisys Track Manager (QTM) server, receives
......@@ -144,259 +114,113 @@ class QualisysDataPublisher:
def __init__(self):
# QTM CONNECTION STATUS AND OBJECT
self.qtm_ip = "10.42.00.15:801"
self._qtm_status = "empty"
self._qtm_connection = None
# Setpoints to be sent to the CrazyFlie
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
self.motor1cmd = 0.0
self.motor2cmd = 0.0
self.motor3cmd = 0.0
self.motor4cmd = 0.0
self._status = DISCONNECTED
self.link_uri = ""
self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
self.FlyingAgentClient_command_pub = rospy.Publisher('FlyingAgentClient/Command', IntWithHeader, queue_size=1)
# WAIT FOR A SHORT TIME BEFORE PROCEEDING WITH INITIALISATION
time.sleep(1.0)
# Initialize the CrazyFlie and add callbacks
self._init_cf()
self._init_qtm_client()
# Connect to the Crazyflie
self.connect()
self.connect_to_qtm()
# INITIALISE THE QTM CLIENT OBJECT
def _init_qtm_client(self):
# Get the "hostName" parameter that is the IP address
self.qtm_ip = rospy.get_param("~hostName")
# CONNECT TO THE QTM SERVER
def connect(self)
# Connect to qtm
connection = await qtm.connect( self.qtm_ip )
# Inform the user is the connection failed
if connection is None:
print("[QUALISYS DATA PUBLISHER] connection is NONE for IP address = " , self.qtm_ip )
return
# Take control of qtm, context manager will automatically release control after scope end
async with qtm.TakeControl(connection, "password"):
# Specify whether to:
# True = get real-time QTM
# False = get data from file
get_realtime_data = True
if get_realtime_data:
# Get the current connection state to the QTM server
current_connection_state = await connection.get_state()
# Inform the user of the current connection state
print( "Connection State = " , current_connection_state )
#def _init_cf(self):
# self._cf = Crazyflie()
# If not currently connected to the QTM serever...
if (current_connection_state != qtm.QRTEvent.EventConnected):
# ... then start new connection to the QTM server
await connection.new()
# # Add callbacks that get executed depending on the connection _status.
# self._cf.connected.add_callback(self._connected)
# self._cf.disconnected.add_callback(self._disconnected)
# self._cf.connection_failed.add_callback(self._connection_failed)
# self._cf.connection_lost.add_callback(self._connection_lost)
else:
# Load qtm file
await connection.load(QTM_FILE)
# start rtfromfile
await connection.start(rtfromfile=True)
# Get 6dof settings from QTM
parameters_6d_as_xml_string = await connection.get_parameters(parameters=["6d"])
# Convert the XML string to a index of the 6DOF bodies available from QTM
# This should be a dictionary that has the name of the object, and its index,
# for example: {'CF01',0 , 'CF02',1}
body_to_index_dictionary = create_body_index(xml_string)
print("[QUALISYS DATA PUBLISHER] Index of 6DOF bodies available from QTM: " , body_index )
# EXTRACT THE INDEX DICTIONARY FROM 6DOF SETTING XML
def create_body_index(xml_string):
# Parse the element tree of the XML string
xml = ET.fromstring(xml_string)
# Initliase the "body_to_index" return variable
body_to_index = {}
# Iterate through the approriate elements of the XML element tree
for index, body in enumerate(xml.findall("*/Body/Name")):
# Add the name of this body and its index
body_to_index[body.text.strip()] = index
# Return the "body_to_index" variable
return body_to_index
# self.change_status_to(DISCONNECTED)
# "on_event" CALLBACK FROM THE "qtm.connect" FUNCTION
def on_qtm_event(self, event):
logger.info(event)
if event == qtm.QRTEvent.EventRTfromFileStarted:
self.qtmStatus = 'connected'
elif event == qtm.QRTEvent.EventRTfromFileStopped:
self.qtmStatus = 'connected : Waiting QTM to start sending data'
#def change_status_to(self, new_status):
# print "[CRAZY RADIO] status changed to: %s" % new_status
# self._status = new_status
# self.status_pub.publish(new_status)
#def get_status(self):
# return self._status
#def update_link_uri(self):
# self.link_uri = "radio://" + rospy.get_param("~crazyFlieAddress")
#def connect(self):
# # update link from ros params
# self.update_link_uri()
# print "[CRAZY RADIO] Connecting to %s" % self.link_uri
# self.change_status_to(CONNECTING)
# rospy.loginfo("[CRAZY RADIO] connecting...")
# self._cf.open_link(self.link_uri)
#def disconnect(self):
# print "[CRAZY RADIO] sending Motors OFF command before disconnecting"
# self._send_to_commander_motor(0, 0, 0, 0)
# # change state to motors OFF
# msg = IntWithHeader()
# msg.shouldCheckForAgentID = False
# msg.data = CMD_CRAZYFLY_MOTORS_OFF
# self.FlyingAgentClient_command_pub.publish(msg)
# time.sleep(0.1)
# print "[CRAZY RADIO] Disconnecting from %s" % self.link_uri
# self._cf.close_link()
# self.change_status_to(DISCONNECTED)
#def _data_received_callback(self, timestamp, data, logconf):
# #print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
# batteryVolt = Float32()
# stabilizerYaw = Float32()
# stabilizerPitch = Float32()
# stabilizerRoll = Float32()
# batteryVolt.data = data["pm.vbat"]
# stabilizerYaw.data = data["stabilizer.yaw"]
# stabilizerPitch.data = data["stabilizer.pitch"]
# bag.write('batteryVoltage', batteryVolt)
# bag.write('stabilizerYaw', stabilizerYaw)
# bag.write('stabilizerPitch', stabilizerPitch)
# bag.write('stabilizerRoll', stabilizerRoll)
# #publish battery voltage for GUI
# #cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
# # print "batteryVolt: %s" % batteryVolt
# cfbattery_pub.publish(batteryVolt)
#def _logging_error(self, logconf, msg):
# print "[CRAZY RADIO] Error when logging %s" % logconf.name
# def _init_logging(self):
#def _start_logging(self):
# self.logconf = LogConfig("LoggingTest", battery_polling_period) # second variable is period in ms
# self.logconf.add_variable("stabilizer.roll", "float");
# self.logconf.add_variable("stabilizer.pitch", "float");
# self.logconf.add_variable("stabilizer.yaw", "float");
# self.logconf.add_variable("pm.vbat", "float");
# self._cf.log.add_config(self.logconf)
# if self.logconf.valid:
# self.logconf.data_received_cb.add_callback(self._data_received_callback)
# self.logconf.error_cb.add_callback(self._logging_error)
# print "[CRAZY RADIO] logconf valid"
# else:
# print "[CRAZY RADIO] logconf invalid"
# self.logconf.start()
# print "[CRAZY RADIO] logconf start"
#def _connected(self, link_uri):
"""
This callback is executed as soon as the connection to the
quadrotor is established.
"""
# self.change_status_to(CONNECTED)
# # change state to motors OFF
# msg = IntWithHeader()
# msg.shouldCheckForAgentID = False
# msg.data = CMD_CRAZYFLY_MOTORS_OFF
# cf_client.FlyingAgentClient_command_pub.publish(msg)
# rospy.loginfo("[CRAZY RADIO] Connection to %s successful: " % link_uri)
# # Config for Logging
# self._start_logging()
#def _connection_failed(self, link_uri, msg):
# """Callback when connection initial connection fails (i.e no Crazyflie
# at the specified address)"""
# self.change_status_to(DISCONNECTED)
# rospy.logerr("[CRAZY RADIO] Connection to %s failed: %s" % (link_uri, msg))
#def _connection_lost(self, link_uri, msg):
# """Callback when disconnected after a connection has been made (i.e
# Crazyflie moves out of range)"""
# self.change_status_to(DISCONNECTED)
# rospy.logerr("[CRAZY RADIO] Connection to %s lost: %s" % (link_uri, msg))
#def _disconnected(self, link_uri):
# """Callback when the Crazyflie is disconnected (called in all cases)"""
# self.change_status_to(DISCONNECTED)
# rospy.logwarn("[CRAZY RADIO] Disconnected from %s" % link_uri)
# # change state to motors OFF
# msg = IntWithHeader()
# msg.shouldCheckForAgentID = False
# msg.data = CMD_CRAZYFLY_MOTORS_OFF
# self.FlyingAgentClient_command_pub.publish(msg)
# self.logconf.stop()
# rospy.loginfo("logconf stopped")
# self.logconf.delete()
# rospy.loginfo("logconf deleted")
#def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
# pk = CRTPPacket()
# pk.port = CRTPPort.COMMANDER_GENERIC
# pk.data = struct.pack('<BHHHH', CF_COMMAND_TYPE_MOTORS, cmd1, cmd2, cmd3, cmd4)
# self._cf.send_packet(pk)
#def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate):
# pk = CRTPPacket()
# pk.port = CRTPPort.COMMANDER_GENERIC
# pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate * RAD_TO_DEG, pitch_rate * RAD_TO_DEG, yaw_rate * RAD_TO_DEG)
# self._cf.send_packet(pk)
#def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
# pk = CRTPPacket()
# pk.port = CRTPPort.COMMANDER_GENERIC
# pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_ANGLE, cmd1, cmd2, cmd3, cmd4, roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG)
# self._cf.send_packet(pk)
# def _send_to_commander(self,roll, pitch, yaw, thrust, cmd1, cmd2, cmd3, cmd4, mode):
# pk = CRTPPacket()
# pk.port = CRTPPort.COMMANDER
# pk.data = struct.pack('<fffHHHHHH', roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG, thrust, cmd1, cmd2, cmd3, cmd4, mode)
# self._cf.send_packet(pk)
#def crazyRadioCommandCallback(self, msg):
# """Callback to tell CrazyRadio to reconnect"""
# # Initialise a boolean flag that the command is NOT relevant
# command_is_relevant = False
# # Check the header details of the message for it relevance
# if (msg.shouldCheckForAgentID == False):
# command_is_relevant = True
# else:
# for this_ID in msg.agentIDs:
# if (this_ID == m_agentID):
# command_is_relevant = True
# break
# # Only consider the command if it is relevant
# if (command_is_relevant):
# #print "[CRAZY RADIO] received command to: %s" % msg.data
# if msg.data == CMD_RECONNECT:
# if self.get_status() == DISCONNECTED:
# print "[CRAZY RADIO] received command to CONNECT (current status is DISCONNECTED)"
# self.connect()
# elif self.get_status() == CONNECTING:
# print "[CRAZY RADIO] received command to CONNECT (current status is CONNECTING)"
# #self.status_pub.publish(CONNECTING)
# elif self.get_status() == CONNECTED:
# print "[CRAZY RADIO] received command to CONNECT (current status is CONNECTED)"
# #self.status_pub.publish(CONNECTED)
# elif msg.data == CMD_DISCONNECT:
# if self.get_status() == CONNECTED:
# print "[CRAZY RADIO] received command to DISCONNECT (current status is CONNECTED)"
# self.disconnect()
# elif self.get_status() == CONNECTING:
# print "[CRAZY RADIO] received command to DISCONNECT (current status is CONNECTING)"
# #self.status_pub.publish(CONNECTING)
# elif self.get_status() == DISCONNECTED:
# print "[CRAZY RADIO] received command to DISCONNECT (current status is DISCONNECTED)"
# #self.status_pub.publish(DISCONNECTED)
#def getCurrentCrazyRadioStatusServiceCallback(self, req):
# """Callback to service the request for the connection status"""
# # Directly return the current status
# return self._status
# def on_qtm_event(self, event):
# logger.info(event)
# if event == qtm.QRTEvent.EventRTfromFileStarted:
# self.qtmStatus = 'connected'
# elif event == qtm.QRTEvent.EventRTfromFileStopped:
# self.qtmStatus = 'connected : Waiting QTM to start sending data'
# CALLBACK FOR QTM DISCONNECTED
async def on_qtm_disconnect(self, reason):
#def on_qtm_disconnect(self, reason):
# "Clear" the QTM connection object
self._qtm_connection = None
# async def on_qtm_disconnect(self, reason):
# #def on_qtm_disconnect(self, reason):
# # "Clear" the QTM connection object
# self._qtm_connection = None
# Log the reason for the disconnection
#logger.info(reason)
# # Log the reason for the disconnection
# #logger.info(reason)
# Update the QTM status
self.qtmStatus = 'not connected : {}'.format(
reason if reason is not None else '')
# # Update the QTM status
# self.qtmStatus = 'not connected : {}'.format(
# reason if reason is not None else '')
# GETTERS AND SETTERS
......@@ -410,23 +234,6 @@ class QualisysDataPublisher:
#def controlCommandCallback(data):
# """Callback for controller actions"""
# #rospy.loginfo("controller callback : %s, %s, %s", data.roll, data.pitch, data.yaw)
# #cmd1..4 must not be 0, as crazyflie onboard controller resets!
# #pitch and yaw are inverted on crazyflie controller
# if data.onboardControllerType == CF_COMMAND_TYPE_MOTORS:
# cf_client._send_to_commander_motor(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4)
# elif data.onboardControllerType == CF_COMMAND_TYPE_RATE:
# cf_client._send_to_commander_rate(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
# elif data.onboardControllerType == CF_COMMAND_TYPE_ANGLE:
# cf_client._send_to_commander_angle(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
# # cf_client._send_to_commander(data.roll, -data.pitch, -data.yaw, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
# async def qtm_connect(self, ip):
# connection = await qtm.connect(
......@@ -443,27 +250,26 @@ class QualisysDataPublisher:
# await self.setup_qtm_connection()
async def setup():
""" Main function """
connection = await qtm.connect("10.42.0.15")
# async def setup():
# """ Main function """
# connection = await qtm.connect("10.42.0.15")
if connection is None:
print("[QUALISYS DATA PUBLISHER] connection is NONE")
return
# if connection is None:
# print("[QUALISYS DATA PUBLISHER] connection is NONE")
# return
# THIS INITIALISES THE ROS NODE
if __name__ == '__main__':
# Starting the ROS-node
# STARTING THE ROS-NODE
global node_name
node_name = "QualisysDataPublisher"
node_name = "ViconDataPublisher"
rospy.init_node(node_name, anonymous=True)
# Get the namespace of this node
# GET THE NAMESPACE OF THIS NODE
global ros_namespace
ros_namespace = rospy.get_namespace()
# PUBLISHER FOR THE MOTION CAPTURE DATA
global qualisys_data_publisher
......@@ -474,20 +280,52 @@ if __name__ == '__main__':
# > That yaml file is added to this node during launch
# > The "Qualisys Computer" runs the "QTM" software that
# is the heart of the Qualisys Motion Capture system
if not rospy.has_param("~hostName"):
rospy.loginfo("[QUALISYS DATA PUBLISHER] ERROR, hostName parameter not found.")
while not rospy.has_param("~hostName"):
print("[QUALISYS DATA PUBLISHER] ERROR, hostName parameter not found.")
time.sleep(0.05)
#global qtm_ip
qtm_ip = rospy.get_param("~hostName")
print("[QUALISYS DATA PUBLISHER] ERROR, hostName parameter = " , qtm_ip )
# INFORM THE USER
rospy.loginfo("[QUALISYS DATA PUBLISHER] Now attempting to connection to QTM server at IP address: %s", qtm_ip)
print("[QUALISYS DATA PUBLISHER] ERROR, hostName parameter not found.")
#rospy.loginfo("[QUALISYS DATA PUBLISHER] Now attempting to connection to QTM server at IP address: %s", qtm_ip)
# Initialise a "CrazyRadioClient" type variable that handles
# all communication over the CrazyRadio
global qtm_client
qtm_client = QualisysQtmClient()
qtm_ip = rospy.get_param("~hostName")
# PAUSE FOR A SECOND AND THEN SPIN
time.sleep(1.0)
print("[QUALISYS DATA PUBLISHER] Node READY :-)")
rospy.spin()
asyncio.ensure_future(setup())
asyncio.get_event_loop().run_forever()
# DISCONNECT FROM THE QTM SERVER
#print("[QUALISYS DATA PUBLISHER] Disconnecting from QTM server")
#cf_client._cf.close_link()
# Wait for diconnection to complete
#time.sleep(1.0)
#print("[QUALISYS DATA PUBLISHER] Disconnected from QTM server")
# CLOSE THE LOGGING "BAG"
#bag.close()
#rospy.loginfo("[QUALISYS DATA PUBLISHER] ROS logging bag is closed")
#asyncio.ensure_future(setup())
#asyncio.get_event_loop().run_forever()
# # ENTER A WHILE LOOP FOR ATTEMPTING TO CONNECT
# qtm_is_connected = False
......@@ -518,27 +356,6 @@ if __name__ == '__main__':
# #await self.setup_qtm_connection()
# PAUSE FOR A SECOND AND THEN SPIN
time.sleep(1.0)
rospy.loginfo("[QUALISYS DATA PUBLISHER] Node READY :-)")
print("[QUALISYS DATA PUBLISHER] Node READY :-)")
rospy.spin()
#
rospy.loginfo("[QUALISYS DATA PUBLISHER] Disconnecting from QTM server")
cf_client._cf.close_link()
rospy.loginfo("[QUALISYS DATA PUBLISHER] Link closed")