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

Small changes. Main error is that import qtm does not seem to work. Likely a...

Small changes. Main error is that import qtm does not seem to work. Likely a compatability problem between ROS using python 2.7 and not using python 3.5
parent c0800fb6
......@@ -57,11 +57,12 @@ import struct
import rosbag
from rospkg import RosPack
# Import the library for asynchronous I/O
import asyncio
# Import the Qualisys Track Manager (QTM) Library
import qtm
# Import the library for asynchronous I/O
import asyncio
# # Add library
......@@ -154,7 +155,7 @@ class QualisysDataPublisher:
# Connect to the Crazyflie
self.connect()
def _init_cf(self):
#def _init_cf(self):
# self._cf = Crazyflie()
# # Add callbacks that get executed depending on the connection _status.
......@@ -174,18 +175,18 @@ class QualisysDataPublisher:
elif event == qtm.QRTEvent.EventRTfromFileStopped:
self.qtmStatus = 'connected : Waiting QTM to start sending data'
def change_status_to(self, new_status):
#def change_status_to(self, new_status):
# print "[CRAZY RADIO] status changed to: %s" % new_status
# self._status = new_status
# self.status_pub.publish(new_status)
def get_status(self):
#def get_status(self):
# return self._status
def update_link_uri(self):
#def update_link_uri(self):
# self.link_uri = "radio://" + rospy.get_param("~crazyFlieAddress")
def connect(self):
#def connect(self):
# # update link from ros params
# self.update_link_uri()
......@@ -194,7 +195,7 @@ class QualisysDataPublisher:
# rospy.loginfo("[CRAZY RADIO] connecting...")
# self._cf.open_link(self.link_uri)
def disconnect(self):
#def disconnect(self):
# print "[CRAZY RADIO] sending Motors OFF command before disconnecting"
# self._send_to_commander_motor(0, 0, 0, 0)
# # change state to motors OFF
......@@ -207,7 +208,7 @@ class QualisysDataPublisher:
# self._cf.close_link()
# self.change_status_to(DISCONNECTED)
def _data_received_callback(self, timestamp, data, logconf):
#def _data_received_callback(self, timestamp, data, logconf):
# #print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
# batteryVolt = Float32()
# stabilizerYaw = Float32()
......@@ -228,12 +229,14 @@ class QualisysDataPublisher:
def _logging_error(self, logconf, msg):
#def _logging_error(self, logconf, msg):
# print "[CRAZY RADIO] Error when logging %s" % logconf.name
# def _init_logging(self):
def _start_logging(self):
#def _start_logging(self):
# self.logconf = LogConfig("LoggingTest", battery_polling_period) # second variable is period in ms
# self.logconf.add_variable("stabilizer.roll", "float");
# self.logconf.add_variable("stabilizer.pitch", "float");
......@@ -251,7 +254,7 @@ class QualisysDataPublisher:
# self.logconf.start()
# print "[CRAZY RADIO] logconf start"
def _connected(self, link_uri):
#def _connected(self, link_uri):
"""
This callback is executed as soon as the connection to the
quadrotor is established.
......@@ -270,19 +273,19 @@ class QualisysDataPublisher:
def _connection_failed(self, link_uri, msg):
#def _connection_failed(self, link_uri, msg):
# """Callback when connection initial connection fails (i.e no Crazyflie
# at the specified address)"""
# self.change_status_to(DISCONNECTED)
# rospy.logerr("[CRAZY RADIO] Connection to %s failed: %s" % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
#def _connection_lost(self, link_uri, msg):
# """Callback when disconnected after a connection has been made (i.e
# Crazyflie moves out of range)"""
# self.change_status_to(DISCONNECTED)
# rospy.logerr("[CRAZY RADIO] Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
#def _disconnected(self, link_uri):
# """Callback when the Crazyflie is disconnected (called in all cases)"""
# self.change_status_to(DISCONNECTED)
# rospy.logwarn("[CRAZY RADIO] Disconnected from %s" % link_uri)
......@@ -298,19 +301,19 @@ class QualisysDataPublisher:
# self.logconf.delete()
# rospy.loginfo("logconf deleted")
def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
#def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
# pk = CRTPPacket()
# pk.port = CRTPPort.COMMANDER_GENERIC
# pk.data = struct.pack('<BHHHH', CF_COMMAND_TYPE_MOTORS, cmd1, cmd2, cmd3, cmd4)
# self._cf.send_packet(pk)
def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate):
#def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate):
# pk = CRTPPacket()
# pk.port = CRTPPort.COMMANDER_GENERIC
# pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate * RAD_TO_DEG, pitch_rate * RAD_TO_DEG, yaw_rate * RAD_TO_DEG)
# self._cf.send_packet(pk)
def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
#def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
# pk = CRTPPacket()
# pk.port = CRTPPort.COMMANDER_GENERIC
# pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_ANGLE, cmd1, cmd2, cmd3, cmd4, roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG)
......@@ -322,7 +325,7 @@ class QualisysDataPublisher:
# pk.data = struct.pack('<fffHHHHHH', roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG, thrust, cmd1, cmd2, cmd3, cmd4, mode)
# self._cf.send_packet(pk)
def crazyRadioCommandCallback(self, msg):
#def crazyRadioCommandCallback(self, msg):
# """Callback to tell CrazyRadio to reconnect"""
# # Initialise a boolean flag that the command is NOT relevant
......@@ -364,13 +367,14 @@ class QualisysDataPublisher:
def getCurrentCrazyRadioStatusServiceCallback(self, req):
#def getCurrentCrazyRadioStatusServiceCallback(self, req):
# """Callback to service the request for the connection status"""
# # Directly return the current status
# return self._status
# CALLBACK FOR QTM DISCONNECTED
async def on_qtm_disconnect(self, reason):
#async def on_qtm_disconnect(self, reason):
def on_qtm_disconnect(self, reason):
# "Clear" the QTM connection object
self._qtm_connection = None
......@@ -392,7 +396,7 @@ class QualisysDataPublisher:
def controlCommandCallback(data):
#def controlCommandCallback(data):
# """Callback for controller actions"""
# #rospy.loginfo("controller callback : %s, %s, %s", data.roll, data.pitch, data.yaw)
......
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