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

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
parent c89b2d28
<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>
......@@ -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")
......
#!/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()
"""
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())
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