#!/usr/bin/env python3 # -*- coding: utf-8 -*- # OPTIONS: #!/usr/bin/env python # OR: #!/usr/bin/python3 # OR: #!/usr/bin/python3.5 # WHERE THE KEY INSTALLATIONS TO USE python3 ARE: # sudo apt-get install python3-pip python3-yaml # pip3 install rospkg catkin_pkg --user # pip3 install qtm --user # Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat # # This file is part of D-FaLL-System. # # D-FaLL-System is free software: you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by # the Free Software Foundation, either version 3 of the License, or # (at your option) any later version. # # D-FaLL-System is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # # You should have received a copy of the GNU General Public License # along with D-FaLL-System. If not, see . # # # ---------------------------------------------------------------------------------- # DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M # D D F aaa L L S Y Y S T E MM MM # D D --- FFFF a a L L --- SSS Y SSS T EEE M M M # D D F a aa L L S Y S T E M M # DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M # # # DESCRIPTION: # The publisher for the Qualisys motion capture data # # ---------------------------------------------------------------------------------- # Import ros and the D-FaLL package import roslib; roslib.load_manifest('dfall_pkg') import rospy # Import the standard message types from std_msgs.msg import Int32 from std_msgs.msg import Float32 from std_msgs.msg import String # Import the D-FaLL message types from dfall_pkg.msg import ViconData from dfall_pkg.msg import UnlabeledMarker # General import #import logging import time import datetime import math from enum import Enum import sys import struct # 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 # DEFINE CONSTANTS # Conversion between radians and degrees RAD_TO_DEG = 57.296 DEG_TO_RAD = 0.01745329 # Path to the file with "fake" data for testing QTM QTM_FILE = pkg_resources.resource_filename("qtm", "data/Demo.qtm") # 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') class QualisysQtmClient: """ Qualisys Data Publisher that connects to the Qualisys Track Manager (QTM) server, receives the 6DOF data, and publishs it for other nodes to use. """ def __init__(self): # QTM CONNECTION STATUS AND OBJECT self.qtm_ip = "10.42.00.15:801" self._qtm_status = "empty" self._qtm_connection = None # WAIT FOR A SHORT TIME BEFORE PROCEEDING WITH INITIALISATION time.sleep(1.0) # Initialize the CrazyFlie and add callbacks self._init_qtm_client() # Connect to the Crazyflie 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 ) # 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() 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 # "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' # 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 # # 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 '') # GETTERS AND SETTERS # @qtmStatus.setter # def qtmStatus(self, value): # if value != self._qtm_status: # self._qtm_status = value # rospy.loginfo("[QUALISYS DATA PUBLISHER] QTM Connection status changed to: %s", self._qtm_status) # print("[QUALISYS DATA PUBLISHER] QTM Connection status changed to: %s", self._qtm_status) # async def qtm_connect(self, ip): # connection = await qtm.connect( # ip, # on_event=self.on_qtm_event, # on_disconnect=lambda reason: start_async_task( # self.on_qtm_disconnect(reason))) # if connection is None: # start_async_task(self.on_qtm_disconnect("Failed to connect")) # return # self._qtm_connection = connection # await self.setup_qtm_connection() # 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 # THIS INITIALISES THE ROS NODE if __name__ == '__main__': # STARTING THE ROS-NODE global node_name node_name = "ViconDataPublisher" rospy.init_node(node_name, anonymous=True) # GET THE NAMESPACE OF THIS NODE global ros_namespace ros_namespace = rospy.get_namespace() # PUBLISHER FOR THE MOTION CAPTURE DATA global qualisys_data_publisher qualisys_data_publisher = rospy.Publisher(node_name + '/ViconData', ViconData, queue_size=1) # HOSTNAME (IP ADDRESS) OF THE "QUALISYS COMPUTER" # > This is specified in the "QualisysConfig.yaml" file # > 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 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) # Initialise a "CrazyRadioClient" type variable that handles # all communication over the CrazyRadio global qtm_client qtm_client = QualisysQtmClient() # PAUSE FOR A SECOND AND THEN SPIN time.sleep(1.0) print("[QUALISYS DATA PUBLISHER] Node READY :-)") rospy.spin() # 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 # while not(qtm_is_connected): # # ATTEMPT TO CONNECT TO THE QTM MANAGER # connection = await qtm.connect("127.0.0.1") # # connection = await qtm.connect( # # qtm_ip, # # on_event=self.on_qtm_event, # # on_disconnect=lambda reason: start_async_task( # # self.on_qtm_disconnect(reason))) # # CHECK IF THE CONNECTION WAS SUCCESSFUL # if connection is None: # # Update the status # start_async_task(self.on_qtm_disconnect("failed to connect")) # # Wait for a second before trying again # time.sleep(1.0) # # ELSE # else: # # Set the flag to exit the while loop # qtm_is_connected = True # # PUT THE CONNECTION OBJECT INTO "self" # self._qtm_connection = connection # #await self.setup_qtm_connection() # # Initialize the low-level drivers (don't list the debug drivers) # cflib.crtp.init_drivers(enable_debug_driver=False) # #wait until address parameter is set by FlyingAgentClient # while not rospy.has_param("~crazyFlieAddress"): # time.sleep(0.05) # #use this following two lines to connect without data from CentralManager # # radio_address = "radio://0/72/2M" # # rospy.loginfo("manual address loaded") # # Fetch the YAML paramter "battery polling period" # global battery_polling_period # battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/battery_polling_period") # # Fetch the YAML paramter "agentID" and "coordID" # global m_agentID # m_agentID = rospy.get_param(ros_namespace + "/FlyingAgentClient/agentID") # coordID = rospy.get_param(ros_namespace + "/FlyingAgentClient/coordID") # # Convert the coordinator ID to a zero-padded string # coordID_as_string = format(coordID, '03') # # Initialise a publisher for the battery voltage # global cfbattery_pub # cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10) # # Initialise a "CrazyRadioClient" type variable that handles # # all communication over the CrazyRadio # global cf_client # cf_client = CrazyRadioClient() # print "[CRAZY RADIO] DEBUG 2" # # Subscribe to the commands for when to (dis-)connect the # # CrazyRadio connection with the Crazyflie # # > For the radio commands from the FlyingAgentClient of this agent # rospy.Subscriber("FlyingAgentClient/crazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback) # # > For the radio command from the coordinator # rospy.Subscriber("/dfall/coord" + coordID_as_string + "/FlyingAgentClient/crazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback) # # Advertise a Serice for the current status # # of the Crazy Radio connect # s = rospy.Service(node_name + "/getCurrentCrazyRadioStatus", IntIntService , cf_client.getCurrentCrazyRadioStatusServiceCallback) # time.sleep(1.0) # rospy.Subscriber("FlyingAgentClient/ControlCommand", ControlCommand, controlCommandCallback) # print "[CRAZY RADIO] Node READY :-)" # rospy.spin() # rospy.loginfo("[CRAZY RADIO] Turning off crazyflie") # cf_client._send_to_commander_motor(0, 0, 0, 0) # # change state to motors OFF # msg = IntWithHeader() # msg.shouldCheckForAgentID = False # msg.data = CMD_CRAZYFLY_MOTORS_OFF # cf_client.FlyingAgentClient_command_pub.publish(msg) # #wait for client to send its commands # time.sleep(1.0) # bag.close() # rospy.loginfo("[CRAZY RADIO] bag closed") # cf_client._cf.close_link() # rospy.loginfo("[CRAZY RADIO] Link closed")