WIP: a bit of a mess. The test_stream_6dof_example file works. Next step to...

WIP: a bit of a mess. The test_stream_6dof_example file works. Next step to start a python node from scratch based on the test file

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/src/qualisys/QualisysDataPublisher.py

@@ -445,7 +445,7 @@ class QualisysDataPublisher:
 
 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")

dfall_ws/src/dfall_pkg/src/qualisys/QualisysDataPublisherWIP.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+#    Copyright (C) 2017, 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 roslib; roslib.load_manifest('dfall_pkg')
+import rospy
+
+
+import rosbag
+from rospkg import RosPack
+
+
+
+# Import the library for asynchronous I/O
+import asyncio
+
+# Import the Qualisys Track Manager (QTM) Library
+import qtm
+
+
+def on_packet_3d(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)
+
+
+def on_packet_6d(packet):
+        info, bodies = packet.get_6d()
+        print(
+            "Framenumber: {} - Body count: {}".format(
+                packet.framenumber, info.body_count
+            )
+        )
+
+        print( "Bodies = " , bodies )
+
+        if info.body_count > 0:
+
+            if wanted_body is not None and wanted_body in body_index:
+                # Extract one specific body
+                wanted_index = body_index[wanted_body]
+                position, rotation = bodies[wanted_index]
+                print("{} - Pos: {} - Rot: {}".format(wanted_body, position, rotation))
+            else:
+                # Print all bodies
+                for position, rotation in bodies:
+                    print("Pos: {} - Rot: {}".format(position, rotation))
+
+
+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
+
+    async with qtm.TakeControl(connection, "password"):
+
+
+        # Start new realtime
+        temp_connection_state = await connection.get_state()
+
+        print( "[QUALISYS DATA PUBLISHER] connection State = " , temp_connection_state )
+
+        if (temp_connection_state != qtm.QRTEvent.EventConnected):
+            await connection.new()
+
+    print( "DEBUG 1" )
+
+    # Get 6dof settings from qtm
+    xml_string = await connection.get_parameters(parameters=["6d"])
+    print( "DEBUG 2" , xml_string )
+    body_index = create_body_index(xml_string)
+
+    print( "DEBUG 3" )
+
+    print( "Body Index = " , body_index )
+
+    #wanted_body = "L-frame"
+    wanted_body = "CF12"
+
+    print( "DEBUG 5" )
+
+    # Start streaming frames
+    await connection.stream_frames(components=["6d"], on_packet=on_packet_6d)
+
+    #await connection.stream_frames(components=["3d"], on_packet=on_packet_3d)
+
+
+if __name__ == "__main__":
+	# Starting the ROS-node
+    global node_name
+    node_name = "QualisysDataPublisherWIP"
+    rospy.init_node(node_name, anonymous=True)
+
+    # Get the namespace of this node
+    global ros_namespace
+    ros_namespace = rospy.get_namespace()
+
+    asyncio.ensure_future(setup())
+    asyncio.get_event_loop().run_forever()
+
+    # PAUSE FOR A SECOND AND THEN SPIN
+    time.sleep(1.0)
+    print("[QUALISYS DATA PUBLISHER] Node READY :-)")
+    rospy.spin()

dfall_ws/src/dfall_pkg/src/qualisys/test_stream_6dof_example.py

+"""
+    Streaming 6Dof from QTM
+"""
+
+import asyncio
+import xml.etree.ElementTree as ET
+import pkg_resources
+
+import qtm
+
+QTM_FILE = pkg_resources.resource_filename("qtm", "data/Demo.qtm")
+
+
+def create_body_index(xml_string):
+    """ Extract a name to index dictionary from 6dof settings xml """
+    xml = ET.fromstring(xml_string)
+
+    body_to_index = {}
+    for index, body in enumerate(xml.findall("*/Body/Name")):
+        body_to_index[body.text.strip()] = index
+
+    return body_to_index
+
+
+async def main():
+
+    # Connect to qtm
+    connection = await qtm.connect("10.42.0.15")
+
+    # Connection failed?
+    if connection is None:
+        print("Failed to connect")
+        return
+
+    # Take control of qtm, context manager will automatically release control after scope end
+    async with qtm.TakeControl(connection, "password"):
+
+        realtime = True
+
+        if realtime:
+            # Start new realtime
+            temp_connection_state = await connection.get_state()
+
+            print( "Connection State = " , temp_connection_state )
+
+            if (temp_connection_state != qtm.QRTEvent.EventConnected):
+                await connection.new()
+
+        else:
+            # Load qtm file
+            await connection.load(QTM_FILE)
+
+            # start rtfromfile
+            await connection.start(rtfromfile=True)
+
+    # Get 6dof settings from qtm
+    xml_string = await connection.get_parameters(parameters=["6d"])
+
+    print( "DEBUG 2" , xml_string )
+    
+    body_index = create_body_index(xml_string)
+
+    print( "Body Index = " , body_index )
+
+    #wanted_body = "L-frame"
+    wanted_body = "CF12"
+
+    def on_packet(packet):
+        
+        #info, bodies = packet.get_6d()
+        info, bodies  = packet.get_6d_euler()
+
+        print(
+            "\n\n\nFramenumber: {} - Body count: {}".format(
+                packet.framenumber, info.body_count
+            )
+        )
+
+        print( "\nBodies = " , bodies )
+
+        #print( "\nEuler = " , data_euler )
+
+        print( "\n" )
+        if info.body_count > 0:
+
+            if wanted_body is not None and wanted_body in body_index:
+                # Extract one specific body
+                wanted_index = body_index[wanted_body]
+                position, rotation = bodies[wanted_index]
+                #print("{} - Pos: {} - Rot: {}".format(wanted_body, position, rotation))
+                print("{} - Pos: {}".format(wanted_body, position))
+                print("{} - Rot: {}".format(wanted_body, rotation))
+            #else:
+                # Print all bodies
+                #for position, rotation in bodies:
+                #    print("Pos: {} - Rot: {}".format(position, rotation))
+
+
+    # Start streaming frames
+    #await connection.stream_frames(components=["6d"], on_packet=on_packet)
+    await connection.stream_frames(components=["6deuler"], on_packet=on_packet)
+
+    # Wait asynchronously 5 seconds
+    await asyncio.sleep(5)
+
+    # Stop streaming
+    await connection.stream_frames_stop()
+
+
+if __name__ == "__main__":
+    # Run our asynchronous function until complete
+    asyncio.get_event_loop().run_until_complete(main())