To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 83b17439 authored by beuchatp's avatar beuchatp
Browse files

Now working with the quadlisys node as a python class. Next step to make a...

Now working with the quadlisys node as a python class. Next step to make a teacher launch file for testing
parent e1a8845b
......@@ -54,16 +54,19 @@ from std_msgs.msg import String
# Import the D-FaLL message types
from dfall_pkg.msg import ViconData
from dfall_pkg.msg import CrazyflieData
from dfall_pkg.msg import UnlabeledMarker
# General import
#import logging
import time
import datetime
import math
from enum import Enum
import sys
import struct
# Not sure if any of these are required
#import logging
#import datetime
#from enum import Enum
#import sys
#import struct
# ROS imports
#import rosbag
......@@ -118,6 +121,11 @@ class QualisysQtmClient:
self._qtm_status = "empty"
self._qtm_connection = None
self._body_to_index_dictionary = None
self._list_of_body_names = None
self._qualisys_data_publisher = rospy.Publisher(node_name + '/ViconData', ViconData, queue_size=1)
# WAIT FOR A SHORT TIME BEFORE PROCEEDING WITH INITIALISATION
time.sleep(0.1)
......@@ -125,9 +133,8 @@ class QualisysQtmClient:
# 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
......@@ -141,16 +148,16 @@ class QualisysQtmClient:
# 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 )
# Connect to QTM
self._qtm_connection = await qtm.connect( self.qtm_ip )
# Inform the user is the connection failed
if connection is None:
# Inform the user is the QTM connection failed
if self._qtm_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"):
async with qtm.TakeControl(self._qtm_connection, "password"):
# Specify whether to:
# True = get real-time QTM
......@@ -160,7 +167,7 @@ class QualisysQtmClient:
if get_realtime_data:
# Get the current connection state to the QTM server
current_connection_state = await connection.get_state()
current_connection_state = await self._qtm_connection.get_state()
# Inform the user of the current connection state
print( "[QUALISYS DATA PUBLISHER] QTM connection State = " , current_connection_state )
......@@ -168,24 +175,137 @@ class QualisysQtmClient:
# 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()
await self._qtm_connection.new()
else:
# Load qtm file
await connection.load(QTM_FILE)
await self._qtm_connection.load(QTM_FILE)
# start rtfromfile
await connection.start(rtfromfile=True)
await self._qtm_connection.start(rtfromfile=True)
# Get 6dof settings from QTM
parameters_6d_as_xml_string = await connection.get_parameters(parameters=["6d"])
parameters_6d_as_xml_string = await self._qtm_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)
self._body_to_index_dictionary = self.create_body_index(parameters_6d_as_xml_string)
self._list_of_body_names = self.create_list_of_body_names(parameters_6d_as_xml_string)
print("[QUALISYS DATA PUBLISHER] Index of 6DOF bodies available from QTM: " , self._body_to_index_dictionary )
# START STREAMING FRAMES RECIEVED FROM QTM
async def start_streaming(self):
# Inform the user
print("[QUALISYS DATA PUBLISHER] Now starting to stream 6DOF data" )
# Start streaming frames:
# > with position and rotation matrix
#await connection.stream_frames(components=["6d"], on_packet=on_6d_packet)
# > with position and euler angles
# Take control of qtm, context manager will automatically release control after scope end
await self._qtm_connection.stream_frames(frames="allframes",components=["6deuler"], on_packet=self.on_6deuler_packet)
# Options for .stream_frames:
# frames=| allframes | frequency:n | frequencydivisor:n |
# where n should be the desired value
# STOP STREAMING FRAMES RECIEVED FROM QTM
async def stop_streaming(self):
# Stop streaming
await self._qtm_connection.stream_frames_stop()
def on_6deuler_packet(self,packet):
# DEBUGGING: Print the frame number
#print("[DEBUGGING] Framenumber: {}".format(packet.framenumber))
# GET THE DATA FROM THE PACKET
#info, bodies = packet.get_6d()
info, bodies = packet.get_6d_euler()
# DEBUGGING: Print the body count and position of an object
# print("[DEBUGGING] Body count = ", info.body_count )
# if info.body_count > 0:
# position, rotation = bodies[0]
# print( "position.x = " , position[0] )
if info.body_count > 0:
# Initilise a "ViconData" struct
# > This is defined in the "ViconData.msg" file
viconData = ViconData()
# Initalise a counter for the bodies
current_body_index = 0
# Iterate through the bodies
for body in bodies:
# Get the position and rotation data of the body
position, rotation = body
# DEBUGGING: to check the naming of rotation angles
#print( "rotation = ", rotation )
# Initialise a "ViconData" struct
# > This is defined in the "CrazyflieData.msg" file
this_crazyflie_data = CrazyflieData();
# Add the name of this object
if current_body_index < len(self._list_of_body_names):
this_crazyflie_data.crazyflieName = self._list_of_body_names[current_body_index]
else:
this_crazyflie_data.crazyflieName = ''
# Check for "nan" as an indication of occulsion
if math.isnan(position.x):
# Fill in the occlusion flag
this_crazyflie_data.occluded = True
else:
# Fill in the occlusion flag
this_crazyflie_data.occluded = False
# Fill in the position data
this_crazyflie_data.x = position.x
this_crazyflie_data.y = position.y
this_crazyflie_data.z = position.z
# Fill in the position data
this_crazyflie_data.roll = rotation.a3 * DEG_TO_RAD
this_crazyflie_data.pitch = rotation.a2 * DEG_TO_RAD
this_crazyflie_data.yaw = rotation.a1 * DEG_TO_RAD
# DEBUGGIN: print out the crazyflie data
#print(this_crazyflie_data)
# Add the data for this body to the ViconData struct
viconData.crazyflies.append(this_crazyflie_data)
# Increment the indexing
current_body_index = current_body_index + 1
# PUBLISH THE VICON DATA
self._qualisys_data_publisher.publish(viconData)
# DEBUGGIN:
#print(viconData)
print("[QUALISYS DATA PUBLISHER] Index of 6DOF bodies available from QTM: " , body_to_index_dictionary )
# EXTRACT THE INDEX DICTIONARY FROM 6DOF SETTING XML
......@@ -204,63 +324,26 @@ class QualisysQtmClient:
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)))
# EXTRACT THE INDEX DICTIONARY FROM 6DOF SETTING XML
def create_list_of_body_names(self, xml_string):
# Parse the element tree of the XML string
xml = ET.fromstring(xml_string)
# if connection is None:
# start_async_task(self.on_qtm_disconnect("Failed to connect"))
# return
# Initliase the "list_of_names" return variable
list_of_names = list()
# 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 to the list
list_of_names.append(body.text.strip())
# self._qtm_connection = connection
# await self.setup_qtm_connection()
# Return the "list_of_names" variable
return list_of_names
# 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
......@@ -276,8 +359,8 @@ if __name__ == '__main__':
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)
#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
......@@ -306,9 +389,16 @@ if __name__ == '__main__':
global qtm_client
qtm_client = QualisysQtmClient()
# Connect to the QTM server
asyncio.get_event_loop().run_until_complete( qtm_client.connect_to_qtm() )
#asyncio.get_event_loop().run_until_complete( qtm_client.start_streaming() )
# Start streaming the data
asyncio.ensure_future( qtm_client.start_streaming() )
asyncio.get_event_loop().run_forever()
# PAUSE FOR A SECOND AND THEN SPIN
time.sleep(1.0)
......@@ -317,7 +407,7 @@ if __name__ == '__main__':
# DISCONNECT FROM THE QTM SERVER
#print("[QUALISYS DATA PUBLISHER] Disconnecting from QTM server")
#cf_client._cf.close_link()
#qtm_client.stop_streaming()
# Wait for diconnection to complete
#time.sleep(1.0)
......@@ -325,116 +415,4 @@ if __name__ == '__main__':
# 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")
#rospy.loginfo("[QUALISYS DATA PUBLISHER] ROS logging bag is closed")
\ No newline at end of file
......@@ -39,7 +39,7 @@ import roslib;
import rospy
import rosbag
#import rosbag
from rospkg import RosPack
......@@ -54,22 +54,28 @@ import qtm
def on_packet(packet):
""" Callback function that is called everytime a data packet arrives from QTM """
print("Framenumber: {}".format(packet.framenumber))
header, markers = packet.get_3d_markers()
print("Component info: {}".format(header))
for marker in markers:
print("\t", marker)
#header, markers = packet.get_3d_markers()
#print("Component info: {}".format(header))
#for marker in markers:
# print("\t", marker)
async def setup():
""" Main function """
connection = await qtm.connect("127.0.0.1")
connection = await qtm.connect("10.42.0.15")
if connection is None:
print("[QUALISYS DATA PUBLISHER] connection is NONE")
return
async with qtm.TakeControl(connection, "password"):
await connection.stream_frames(components=["3d"], on_packet=on_packet)
await connection.stream_frames(frames="frequencydivisor:50",components=["3d"], on_packet=on_packet)
# Wait asynchronously 5 seconds
await asyncio.sleep(2)
# Stop streaming
await self._qtm_connection.stream_frames_stop()
if __name__ == "__main__":
......
#!/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.
"""