Commit c99f89e1 authored by beuchatp's avatar beuchatp
Browse files

qtm and asyncio are somehow working with python3 and ROS kinetic...

parent 39b458bd
#!/usr/bin/env python #!/usr/bin/env python3
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
# Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat # Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
...@@ -33,6 +33,8 @@ ...@@ -33,6 +33,8 @@
# ---------------------------------------------------------------------------------- # ----------------------------------------------------------------------------------
import roslib; roslib.load_manifest('dfall_pkg') import roslib; roslib.load_manifest('dfall_pkg')
import rospy import rospy
...@@ -57,6 +59,8 @@ import struct ...@@ -57,6 +59,8 @@ import struct
import rosbag import rosbag
from rospkg import RosPack from rospkg import RosPack
# Import the library for asynchronous I/O # Import the library for asynchronous I/O
import asyncio import asyncio
...@@ -64,7 +68,6 @@ import asyncio ...@@ -64,7 +68,6 @@ import asyncio
import qtm import qtm
# # Add library # # Add library
# #sys.path.append("lib") # #sys.path.append("lib")
...@@ -386,12 +389,12 @@ class QualisysDataPublisher: ...@@ -386,12 +389,12 @@ class QualisysDataPublisher:
reason if reason is not None else '') reason if reason is not None else '')
# GETTERS AND SETTERS # # GETTERS AND SETTERS
@qtmStatus.setter # @qtmStatus.setter
def qtmStatus(self, value): # def qtmStatus(self, value):
if value != self._qtm_status: # if value != self._qtm_status:
self._qtm_status = value # self._qtm_status = value
print "[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)
...@@ -413,7 +416,29 @@ class QualisysDataPublisher: ...@@ -413,7 +416,29 @@ class QualisysDataPublisher:
# # cf_client._send_to_commander(data.roll, -data.pitch, -data.yaw, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType) # # cf_client._send_to_commander(data.roll, -data.pitch, -data.yaw, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
# 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)))
# if connection is None:
# start_async_task(self.on_qtm_disconnect("Failed to connect"))
# return
# self._qtm_connection = connection
# await self.setup_qtm_connection()
async def setup():
""" Main function """
connection = await qtm.connect("127.0.0.1")
if connection is None:
print("[QUALISYS DATA PUBLISHER] connection is NONE")
return
# THIS INITIALISES THE ROS NODE # THIS INITIALISES THE ROS NODE
...@@ -428,9 +453,7 @@ if __name__ == '__main__': ...@@ -428,9 +453,7 @@ if __name__ == '__main__':
global ros_namespace global ros_namespace
ros_namespace = rospy.get_namespace() ros_namespace = rospy.get_namespace()
# PUBLISHER FOR THE MOTION CAPTURE DATA # PUBLISHER FOR THE MOTION CAPTURE DATA
global qualisys_data_publisher global qualisys_data_publisher
qualisys_data_publisher = rospy.Publisher(node_name + '/ViconData', ViconData, queue_size=1) qualisys_data_publisher = rospy.Publisher(node_name + '/ViconData', ViconData, queue_size=1)
...@@ -441,42 +464,46 @@ if __name__ == '__main__': ...@@ -441,42 +464,46 @@ if __name__ == '__main__':
# > The "Qualisys Computer" runs the "QTM" software that # > The "Qualisys Computer" runs the "QTM" software that
# is the heart of the Qualisys Motion Capture system # is the heart of the Qualisys Motion Capture system
if not rospy.has_param("~hostName"): if not rospy.has_param("~hostName"):
print"[QUALISYS DATA PUBLISHER] ERROR, hostName parameter not found." print("[QUALISYS DATA PUBLISHER] ERROR, hostName parameter not found.")
qtm_ip = rospy.get_param("~hostName") qtm_ip = rospy.get_param("~hostName")
# ENTER A WHILE LOOP FOR ATTEMPTING TO CONNECT asyncio.ensure_future(setup())
qtm_is_connected = False asyncio.get_event_loop().run_forever()
# # ENTER A WHILE LOOP FOR ATTEMPTING TO CONNECT
# qtm_is_connected = False
while not(qtm_is_connected): # while not(qtm_is_connected):
# ATTEMPT TO CONNECT TO THE QTM MANAGER # # ATTEMPT TO CONNECT TO THE QTM MANAGER
connection = await qtm.connect( # connection = await qtm.connect("127.0.0.1")
qtm_ip, # # connection = await qtm.connect(
on_event=self.on_qtm_event, # # qtm_ip,
on_disconnect=lambda reason: start_async_task( # # on_event=self.on_qtm_event,
self.on_qtm_disconnect(reason))) # # on_disconnect=lambda reason: start_async_task(
# # self.on_qtm_disconnect(reason)))
# CHECK IF THE CONNECTION WAS SUCCESSFUL # # CHECK IF THE CONNECTION WAS SUCCESSFUL
if connection is None: # if connection is None:
# Update the status # # Update the status
start_async_task(self.on_qtm_disconnect("failed to connect")) # start_async_task(self.on_qtm_disconnect("failed to connect"))
# Wait for a second before trying again # # Wait for a second before trying again
time.sleep(1.0) # time.sleep(1.0)
# ELSE # # ELSE
else: # else:
# Set the flag to exit the while loop # # Set the flag to exit the while loop
qtm_is_connected = True # qtm_is_connected = True
# PUT THE CONNECTION OBJECT INTO "self" # # PUT THE CONNECTION OBJECT INTO "self"
self._qtm_connection = connection # self._qtm_connection = connection
#await self.setup_qtm_connection() # #await self.setup_qtm_connection()
# PAUSE FOR A SECOND AND THEN SPIN # PAUSE FOR A SECOND AND THEN SPIN
time.sleep(1.0) time.sleep(1.0)
print "[QUALISYS DATA PUBLISHER] Node READY :-)" print("[QUALISYS DATA PUBLISHER] Node READY :-)")
rospy.spin() rospy.spin()
# #
......
#!/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;
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(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)
async def setup():
""" Main function """
connection = await qtm.connect("127.0.0.1")
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)
if __name__ == "__main__":
# Starting the ROS-node
global node_name
node_name = "QualisysDataPublisherMWE"
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()
Supports Markdown
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