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 e1a8845b authored by beuchatp's avatar beuchatp
Browse files

Tested and working for making a connection to QTM and getting the list of available bodies

parent f9959856
......@@ -20,7 +20,7 @@
<!-- QUALISYS DATA PUBLISHER -->
<node
pkg = "dfall_pkg"
name = "QualisysDataPublisher"
name = "ViconDataPublisher"
output = "screen"
type = "QualisysDataPublisher.py"
>
......
<launch>
<!-- INPUT ARGUMENT OF THE AGENT's ID -->
<arg name="agentID" default="$(optenv DFALL_DEFAULT_AGENT_ID)" />
<!-- INPUT ARGUMENT OF THE COORDINATOR's ID -->
<arg name="coordID" default="$(optenv DFALL_DEFAULT_COORD_ID)" />
<!-- INPUT ARGUMENT FOR LAUNCHING A GUI OR NOT -->
<arg name="withGUI" default="true" />
<!-- Example of how to use the value in agentID -->
<!-- <param name="param" value="$(arg agentID)"/> -->
<!-- Example of how to specify the agentID from command line -->
<!-- roslaunch dfall_pkg agentID:=1 -->
<group ns="$(eval 'agent' + str(agentID).zfill(3))">
<!-- QUALISYS DATA PUBLISHER -->
<node
pkg = "dfall_pkg"
name = "QualisysDataPublisherWIP"
output = "screen"
type = "QualisysDataPublisherWIP.py"
>
<rosparam command="load" file="$(find dfall_pkg)/param/QualisysConfig.yaml" />
</node>
</group>
</launch>
hostName: "10.42.00.15:801"
qtm_host_ip_address: "10.42.00.15"
\ No newline at end of file
......@@ -89,8 +89,8 @@ 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 = pkg_resources.resource_filename("qtm", "data/Demo.qtm")
QTM_FILE = "none"
# OPEN THE ROS BAG FOR WRITING
......@@ -120,23 +120,27 @@ class QualisysQtmClient:
# WAIT FOR A SHORT TIME BEFORE PROCEEDING WITH INITIALISATION
time.sleep(1.0)
time.sleep(0.1)
# Initialize the CrazyFlie and add callbacks
self._init_qtm_client()
# Connect to the Crazyflie
self.connect_to_qtm()
#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 "hostName" parameter that is the IP address
self.qtm_ip = rospy.get_param("~hostName")
# 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
def connect(self)
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 )
......@@ -159,7 +163,7 @@ class QualisysQtmClient:
current_connection_state = await connection.get_state()
# Inform the user of the current connection state
print( "Connection State = " , 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):
......@@ -179,13 +183,13 @@ class QualisysQtmClient:
# 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)
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_index )
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(xml_string):
def create_body_index(self, xml_string):
# Parse the element tree of the XML string
xml = ET.fromstring(xml_string)
......@@ -275,19 +279,23 @@ if __name__ == '__main__':
global qualisys_data_publisher
qualisys_data_publisher = rospy.Publisher(node_name + '/ViconData', ViconData, queue_size=1)
# HOSTNAME (IP ADDRESS) OF THE "QUALISYS COMPUTER"
# 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
while not rospy.has_param("~hostName"):
print("[QUALISYS DATA PUBLISHER] ERROR, hostName parameter not found.")
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("~hostName")
qtm_ip = rospy.get_param("~qtm_host_ip_address")
print("[QUALISYS DATA PUBLISHER] ERROR, hostName parameter = " , qtm_ip )
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)
......
File mode changed from 100755 to 100644
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment