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

dfall_ws/src/dfall_pkg/launch/qualisys_datastream.launch

@@ -20,7 +20,7 @@
 		<!-- QUALISYS DATA PUBLISHER -->
 		<node
 			pkg    = "dfall_pkg"
-			name   = "QualisysDataPublisher"
+			name   = "ViconDataPublisher"
 			output = "screen"
 			type   = "QualisysDataPublisher.py"
 			>

dfall_ws/src/dfall_pkg/launch/qualisys_datastream_WIP.launch

-<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>

dfall_ws/src/dfall_pkg/param/QualisysConfig.yaml

-hostName: "10.42.00.15:801"
-
+qtm_host_ip_address: "10.42.00.15"
\ No newline at end of file

dfall_ws/src/dfall_pkg/src/qualisys/QualisysDataPublisher.py

@@ -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)