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 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 -*-
# Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
......@@ -33,6 +33,8 @@
# ----------------------------------------------------------------------------------
import roslib; roslib.load_manifest('dfall_pkg')
import rospy
......@@ -57,6 +59,8 @@ import struct
import rosbag
from rospkg import RosPack
# Import the library for asynchronous I/O
import asyncio
......@@ -64,7 +68,6 @@ import asyncio
import qtm
# # Add library
# #sys.path.append("lib")
......@@ -386,12 +389,12 @@ class QualisysDataPublisher:
reason if reason is not None else '')
# GETTERS AND SETTERS
@qtmStatus.setter
def qtmStatus(self, value):
if value != self._qtm_status:
self._qtm_status = value
print "[QUALISYS DATA PUBLISHER] QTM Connection status changed to: %s" % self._qtm_status
# # GETTERS AND SETTERS
# @qtmStatus.setter
# def qtmStatus(self, value):
# if value != self._qtm_status:
# self._qtm_status = value
# print("[QUALISYS DATA PUBLISHER] QTM Connection status changed to: %s", self._qtm_status)
......@@ -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)
# 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
......@@ -428,9 +453,7 @@ if __name__ == '__main__':
global ros_namespace
ros_namespace = rospy.get_namespace()
# PUBLISHER FOR THE MOTION CAPTURE DATA
global qualisys_data_publisher
qualisys_data_publisher = rospy.Publisher(node_name + '/ViconData', ViconData, queue_size=1)
......@@ -441,42 +464,46 @@ if __name__ == '__main__':
# > The "Qualisys Computer" runs the "QTM" software that
# is the heart of the Qualisys Motion Capture system
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")
# ENTER A WHILE LOOP FOR ATTEMPTING TO CONNECT
qtm_is_connected = False
asyncio.ensure_future(setup())
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
connection = await qtm.connect(
qtm_ip,
on_event=self.on_qtm_event,
on_disconnect=lambda reason: start_async_task(
self.on_qtm_disconnect(reason)))
# # ATTEMPT TO CONNECT TO THE QTM MANAGER
# connection = await qtm.connect("127.0.0.1")
# # connection = await qtm.connect(
# # qtm_ip,
# # on_event=self.on_qtm_event,
# # on_disconnect=lambda reason: start_async_task(
# # self.on_qtm_disconnect(reason)))
# CHECK IF THE CONNECTION WAS SUCCESSFUL
if connection is None:
# Update the status
start_async_task(self.on_qtm_disconnect("failed to connect"))
# Wait for a second before trying again
time.sleep(1.0)
# ELSE
else:
# Set the flag to exit the while loop
qtm_is_connected = True
# # CHECK IF THE CONNECTION WAS SUCCESSFUL
# if connection is None:
# # Update the status
# start_async_task(self.on_qtm_disconnect("failed to connect"))
# # Wait for a second before trying again
# time.sleep(1.0)
# # ELSE
# else:
# # Set the flag to exit the while loop
# qtm_is_connected = True
# PUT THE CONNECTION OBJECT INTO "self"
self._qtm_connection = connection
#await self.setup_qtm_connection()
# # PUT THE CONNECTION OBJECT INTO "self"
# self._qtm_connection = connection
# #await self.setup_qtm_connection()
# PAUSE FOR A SECOND AND THEN SPIN
time.sleep(1.0)
print "[QUALISYS DATA PUBLISHER] Node READY :-)"
print("[QUALISYS DATA PUBLISHER] Node READY :-)")
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()
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