QualisysDataPublisher.py

#!/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 <http://www.gnu.org/licenses/>.
#    
#
#    ----------------------------------------------------------------------------------
#    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")
QTM_FILE = "none"


# 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(0.1)

        # Initialize the CrazyFlie and add callbacks
        self._init_qtm_client()

        # Connect to the Crazyflie
        #asyncio.ensure_future( self.connect_to_qtm() )
        asyncio.get_event_loop().run_until_complete( self.connect_to_qtm() )


    # INITIALISE THE QTM CLIENT OBJECT
    def _init_qtm_client(self):
        # Get the "qtm_host_ip_address" parameter that is the IP address
        self.qtm_ip = rospy.get_param("~qtm_host_ip_address")


    # CONNECT TO THE QTM SERVER
    async def connect_to_qtm(self):
        # Inform the user
        print("[QUALISYS DATA PUBLISHER] now attempting to connect to QTM at IP address ", self.qtm_ip )

        # 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( "[QUALISYS DATA PUBLISHER] QTM 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 = self.create_body_index(parameters_6d_as_xml_string)

        print("[QUALISYS DATA PUBLISHER] Index of 6DOF bodies available from QTM: " , body_to_index_dictionary )


    # EXTRACT THE INDEX DICTIONARY FROM 6DOF SETTING XML
    def create_body_index(self, 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)

    # IP ADDRESS OF THE HOST "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
    number_of_fails = 0
    while not rospy.has_param("~qtm_host_ip_address"):
        number_of_fails = number_of_fails + 1 
        if number_of_fails > 9:
            print("[QUALISYS DATA PUBLISHER] ERROR, qtm_host_ip_address parameter not found.")
            number_of_fails = 0;
        time.sleep(0.05)

    #global qtm_ip
    qtm_ip = rospy.get_param("~qtm_host_ip_address")

    print("[QUALISYS DATA PUBLISHER] qtm_host_ip_address 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")