CrazyRadio.py 16.7 KB
Newer Older
bucyril's avatar
bucyril committed
1
2
3
#!/usr/bin/env python
# -*- coding: utf-8 -*-

roangel's avatar
roangel committed
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
# Alternate controller that is expected to work.
# Copyright (C) 2017  Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli

# This program 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.

# This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.

20
import roslib; roslib.load_manifest('d_fall_pps')
bucyril's avatar
bucyril committed
21
import rospy
22
from d_fall_pps.msg import ControlCommand
23
from std_msgs.msg import Int32
bucyril's avatar
bucyril committed
24
25
26
27
28
29
30


# General import
import time, sys
import struct
import logging

31
import rosbag
32
from rospkg import RosPack
33
34
from std_msgs.msg import Float32
from std_msgs.msg import String
35
from std_msgs.msg import Float32MultiArray
36

bucyril's avatar
bucyril committed
37
38
39
40
41
42
43
44
45
46
47
# Add library
#sys.path.append("lib")

# CrazyFlie client imports
import cflib

from cflib.crazyflie import Crazyflie
from cflib.crtp.crtpstack import CRTPPacket, CRTPPort

import cflib.drivers.crazyradio

48
# Logging import(*
bucyril's avatar
bucyril committed
49
50
51
52
53
from cflib.crazyflie.log import LogConfig

# Logging settings
logging.basicConfig(level=logging.ERROR)

roangel's avatar
roangel committed
54
55
56
57
58
59
60
61
62
# CONTROLLER_MOTOR = 2
# CONTROLLER_ANGLE = 1
# CONTROLLER_RATE = 0


TYPE_PPS_MOTORS = 6
TYPE_PPS_RATE =   7
TYPE_PPS_ANGLE =  8

63
RAD_TO_DEG = 57.296
64

65
66
67
68
69
70
71
# CrazyRadio states:
CONNECTED = 0
CONNECTING = 1
DISCONNECTED = 2

# Commands coming
CMD_RECONNECT = 0
72
CMD_DISCONNECT = 1
73

74
75
76
77
78
79
80
# Commands for PPSClient
CMD_USE_SAFE_CONTROLLER =   1
CMD_USE_CUSTOM_CONTROLLER = 2
CMD_CRAZYFLY_TAKE_OFF =     3
CMD_CRAZYFLY_LAND =         4
CMD_CRAZYFLY_MOTORS_OFF =   5

81
82
rp = RosPack()
record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag'
bucyril's avatar
bucyril committed
83
84
rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
rospy.loginfo(record_file)
85
bag = rosbag.Bag(record_file, 'w')
86

87
class PPSRadioClient:
bucyril's avatar
bucyril committed
88
    """
89
90
       CrazyRadio client that recieves the commands from the controller and
       sends them in a CRTP package to the crazyflie with the specified
91
       address.
bucyril's avatar
bucyril committed
92
    """
93
    def __init__(self):
bucyril's avatar
bucyril committed
94
95
96
97
98
99
100
101
102

        # Setpoints to be sent to the CrazyFlie
        self.roll = 0.0
        self.pitch = 0.0
        self.yaw = 0.0
        self.motor1cmd = 0.0
        self.motor2cmd = 0.0
        self.motor3cmd = 0.0
        self.motor4cmd = 0.0
103
        self._status = DISCONNECTED
104
        self.link_uri = ""
bucyril's avatar
bucyril committed
105

roangel's avatar
roangel committed
106
        self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
107
        self.PPSClient_command_pub = rospy.Publisher('PPSClient/Command', Int32, queue_size=1)
108
        time.sleep(1.0)
109

bucyril's avatar
bucyril committed
110
        # Initialize the CrazyFlie and add callbacks
roangel's avatar
roangel committed
111
112
113
114
115
116
        self._init_cf()

        # Connect to the Crazyflie
        self.connect()

    def _init_cf(self):
bucyril's avatar
bucyril committed
117
118
        self._cf = Crazyflie()

119
        # Add callbacks that get executed depending on the connection _status.
bucyril's avatar
bucyril committed
120
121
122
123
124
        self._cf.connected.add_callback(self._connected)
        self._cf.disconnected.add_callback(self._disconnected)
        self._cf.connection_failed.add_callback(self._connection_failed)
        self._cf.connection_lost.add_callback(self._connection_lost)

125
126
127
        self.change_status_to(DISCONNECTED)

    def change_status_to(self, new_status):
128
        print "status changed to: %s" % new_status
129
130
        self._status = new_status
        self.status_pub.publish(new_status)
131

132
133
    def get_status(self):
        return self._status
134

135
136
137
    def update_link_uri(self):
        self.link_uri = "radio://" + rospy.get_param("~crazyFlieAddress")

138
    def connect(self):
139
140
141
        # update link from ros params
        self.update_link_uri()

roangel's avatar
roangel committed
142
        print "Connecting to %s" % self.link_uri
143
        self.change_status_to(CONNECTING)
144
145
        rospy.loginfo("connecting...")
        self._cf.open_link(self.link_uri)
roangel's avatar
roangel committed
146

147
    def disconnect(self):
148
        print "Motors OFF"
roangel's avatar
roangel committed
149
        self._send_to_commander_motor(0, 0, 0, 0)
150
151
        # change state to motors OFF
        self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
152
        time.sleep(0.1)
153
154
155
156
        print "Disconnecting from %s" % self.link_uri
        self._cf.close_link()
        self.change_status_to(DISCONNECTED)

157
    def _anchorDist_received_callback(self, timestamp, data, anchorDist):
158
159
160
        distances = Float32MultiArray()
        distances.data = [data["ranging.distance1"],data["ranging.distance2"],data["ranging.distance3"],data["ranging.distance4"],data["ranging.distance5"],data["ranging.distance6"]]

michaero's avatar
michaero committed
161
        bag.write('anchorDistances',distances)
162
        distances_pub.publish(distances)
163

164
    def _xyzrpy_received_callback(self, timestamp, data, xyzrpy):
165

166
        #if onboardPosition:
michaero's avatar
michaero committed
167
168
169
170
            #arrXYZ = Float32MultiArray()
            #arrXYZ.data = [data["xxx.x"], fdata["xxx.y"],data["xxx.z"]]
            #bag.write('XYZ',arrXYZ)
            #xyz_pub.publish(arrXYZ)
171
172
173
174
175
176
177
178
        #print "anchor0: %s" % data["UWB_Anchor_Pos.anchor0"]
        #print "anchor1: %s" % data["UWB_Anchor_Pos.anchor1"]

        dist0 = data["UWB_Anchor_Pos.anchor0"] * 75.0 * (1.0 / (128.0 * 499.2))
        print "dist0: %s" % dist0
        dist1 = data["UWB_Anchor_Pos.anchor1"] * 75.0 * (1.0 / (128.0 * 499.2))
        print "dist1: %s" % dist1

179

180
181
        arrRPY = Float32MultiArray()
        arrRPY.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]
182

michaero's avatar
michaero committed
183
184
        bag.write('rollPitchYaw',arrRPY)

185
        rpy_pub.publish(arrRPY)
186

187
        #print arrRPY
188

189
190
191
192
193

    #def _data_received_callback(self, timestamp, data, logconf):
    def _vbat_received_callback(self, timestamp, data, batLog):
        #print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
        batteryVolt = Float32()
194
        
195
        batteryVolt.data = data["pm.vbat"]
196
       
197
        bag.write('batteryVoltage', batteryVolt)
198
        
199
        #publish battery voltage for GUI
200
        #cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
201
        #print "batteryVolt: %s" % batteryVolt
202
203
        cfbattery_pub.publish(batteryVolt)

204
205


206
207
    def _logging_error(self, logconf, msg):
        print "Error when logging %s" % logconf.name
Yvan Bosshard's avatar
Yvan Bosshard committed
208

roangel's avatar
roangel committed
209
210
    # def _init_logging(self):

211
    def init_log_battery(self):
212
213
214
215

        global battery_polling_period
        battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/battery_polling_period")

216
217
        self.batLog = LogConfig("BatteryLog", battery_polling_period) # second variable is period in ms
        self.batLog.add_variable("pm.vbat", "float");
218
        
219
220
221
        self._cf.log.add_config(self.batLog)
        if self.batLog.valid:
            self.batLog.data_received_cb.add_callback(self._vbat_received_callback)
222
            self.batLog.error_cb.add_callback(self._logging_error)
223
224
225
226
227
228
229
230
            print "batLog valid"
        else:
            print "batLog invalid"

        self.batLog.start()
        print "batLog start"

    def init_log_xyzrpy(self):
231
232
233
234
235

        global xyzrpy_polling_period
        xyzrpy_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/xyzrpy_polling_period")

        self.xyzrpy = LogConfig("XYZRPYLog", xyzrpy_polling_period)
236
        #TODO: where are coordinates published?
237
238
239
240
241
242
243
244
        #if onboardPosition:
            #self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor0", "uint32_t");
            #self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor1", "uint32_t");
            #self.xyzrpy.add_variable("xxx.z", "float")

        self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor0", "uint32_t");
        self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor1", "uint32_t");

245
246
247
248
249
250
251
        self.xyzrpy.add_variable("stabilizer.roll", "float");
        self.xyzrpy.add_variable("stabilizer.pitch", "float");
        self.xyzrpy.add_variable("stabilizer.yaw", "float");

        self._cf.log.add_config(self.xyzrpy)
        if self.xyzrpy.valid:
            self.xyzrpy.data_received_cb.add_callback(self._xyzrpy_received_callback)
252
            self.xyzrpy.error_cb.add_callback(self._logging_error)
253
            print "xyzrpy valid"
roangel's avatar
roangel committed
254
        else:
255
256
257
258
259
            print "xyzrpy invalid"

        self.xyzrpy.start()
        print "xyzrpy start"

260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
    def init_log_anchordist(self):
        global anchordist_polling_period
        anchordist_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/anchordist_polling_period")

        self.anchorDist = LogConfig("AnchorDistances", anchordist_polling_period)
        self.anchorDist.add_variable("ranging.distance1","float");
        self.anchorDist.add_variable("ranging.distance2","float");
        self.anchorDist.add_variable("ranging.distance3","float");
        self.anchorDist.add_variable("ranging.distance4","float");
        self.anchorDist.add_variable("ranging.distance5","float");
        self.anchorDist.add_variable("ranging.distance6","float");

        self._cf.log.add_config(self.anchorDist)
        if self.anchorDist.valid:
            self.anchorDist.data_received_cb.add_callback(self._anchorDist_received_callback)
            self.anchorDist.error_cb.add_callback(self._logging_error)
            print "anchorDist valid"
        else:
            print "anchorDist invalid"

        self.anchorDist.start()
        print "anchorDist start"

283
284
285
286

    def _start_logging(self):

        self.init_log_battery()
roangel's avatar
roangel committed
287

michaero's avatar
michaero committed
288
289
        if enableUWB:
            self.init_log_xyzrpy()
290
291
            #if not onboardPosition:
                #self.init_log_anchordist()             #TODO: test
292

293
294
295
296
297
298
299
    def _stop_logging(self):

        self.batLog.stop()
        rospy.loginfo("batLog stopped")
        self.batLog.delete()
        rospy.loginfo("batLog deleted")

michaero's avatar
michaero committed
300
301
302
303
304
        if enableUWB:
            self.xyzrpy.stop()
            rospy.loginfo("xyzrpy stopped")
            self.xyzrpy.delete()
            rospy.loginfo("xyzrpy deleted")
305

306
            '''if not onboardPosition:
michaero's avatar
michaero committed
307
308
309
                self.anchorDist.stop()
                rospy.loginfo("anchorDist stopped")
                self.anchorDist.delete()
310
                rospy.loginfo("anchorDist deleted")'''
311

bucyril's avatar
bucyril committed
312
313
314
315
316
    def _connected(self, link_uri):
        """
            This callback is executed as soon as the connection to the
            quadrotor is established.
        """
317
        self.change_status_to(CONNECTED)
318
319
320
        # change state to motors OFF
        cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)

bucyril's avatar
bucyril committed
321
        rospy.loginfo("Connection to %s successful: " % link_uri)
322
        # Config for Logging
roangel's avatar
roangel committed
323
        self._start_logging()
324

bucyril's avatar
bucyril committed
325
326
327
    def _connection_failed(self, link_uri, msg):
        """Callback when connection initial connection fails (i.e no Crazyflie
        at the specified address)"""
328
        self.change_status_to(DISCONNECTED)
bucyril's avatar
bucyril committed
329
330
331
332
333
        rospy.logerr("Connection to %s failed: %s" % (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)"""
334
        self.change_status_to(DISCONNECTED)
bucyril's avatar
bucyril committed
335
336
337
338
        rospy.logerr("Connection to %s lost: %s" % (link_uri, msg))

    def _disconnected(self, link_uri):
        """Callback when the Crazyflie is disconnected (called in all cases)"""
339
        self.change_status_to(DISCONNECTED)
bucyril's avatar
bucyril committed
340
        rospy.logwarn("Disconnected from %s" % link_uri)
roangel's avatar
roangel committed
341

342
343
344
        # change state to motors OFF
        self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)

345
        self._stop_logging()
Yvan Bosshard's avatar
Yvan Bosshard committed
346

roangel's avatar
roangel committed
347
348
349
350
351
352
353
354
355
    def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
        pk = CRTPPacket()
        pk.port = CRTPPort.COMMANDER_GENERIC
        pk.data = struct.pack('<BHHHH', TYPE_PPS_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):
        pk = CRTPPacket()
        pk.port = CRTPPort.COMMANDER_GENERIC
roangel's avatar
roangel committed
356
        pk.data = struct.pack('<BHHHHfff', TYPE_PPS_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate * RAD_TO_DEG, pitch_rate * RAD_TO_DEG, yaw_rate * RAD_TO_DEG)
roangel's avatar
roangel committed
357
        self._cf.send_packet(pk)
bucyril's avatar
bucyril committed
358

roangel's avatar
roangel committed
359
    def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
bucyril's avatar
bucyril committed
360
        pk = CRTPPacket()
roangel's avatar
roangel committed
361
        pk.port = CRTPPort.COMMANDER_GENERIC
roangel's avatar
roangel committed
362
        pk.data = struct.pack('<BHHHHfff', TYPE_PPS_ANGLE, cmd1, cmd2, cmd3, cmd4, roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG)
bucyril's avatar
bucyril committed
363
364
        self._cf.send_packet(pk)

roangel's avatar
roangel committed
365
366
367
368
369
370
    # def _send_to_commander(self,roll, pitch, yaw, thrust, cmd1, cmd2, cmd3, cmd4, mode):
    #     pk = CRTPPacket()
    #     pk.port = CRTPPort.COMMANDER
    #     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)

371
    def crazyRadioCommandCallback(self, msg):
372
        """Callback to tell CrazyRadio to reconnect"""
373
        print "crazyRadio command received %s" % msg.data
374

375
        if msg.data == CMD_RECONNECT:            # reconnect, check _status first and then do whatever needs to be done
376
            print "entered reconnect"
377
378
            print "_status: %s" % self._status
            if self.get_status() == DISCONNECTED:
379
380
                print "entered disconnected"
                self.connect()
381
382
            if self.get_status() == CONNECTED:
                self.status_pub.publish(CONNECTED)
383

384
385
386
387
388
389
390
        elif msg.data == CMD_DISCONNECT:
            print "disconnect received"
            if self.get_status() != DISCONNECTED: # what happens if we disconnect while we are in connecting state?
                self.disconnect()
            else:
                self.status_pub.publish(DISCONNECTED)

391
392
def controlCommandCallback(data):
    """Callback for controller actions"""
393
    #rospy.loginfo("controller callback : %s, %s, %s", data.roll, data.pitch, data.yaw)
bucyril's avatar
bucyril committed
394

395
    #cmd1..4 must not be 0, as crazyflie onboard controller resets!
396
    #pitch and yaw are inverted on crazyflie controller
roangel's avatar
roangel committed
397
398
399
400
401
    if data.onboardControllerType == TYPE_PPS_MOTORS:
        cf_client._send_to_commander_motor(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4)

    elif data.onboardControllerType == TYPE_PPS_RATE:
        cf_client._send_to_commander_rate(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
402

roangel's avatar
roangel committed
403
404
405
    elif data.onboardControllerType == TYPE_PPS_ANGLE:
        cf_client._send_to_commander_angle(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
        # cf_client._send_to_commander(data.roll, -data.pitch, -data.yaw, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
406
407
408



bucyril's avatar
bucyril committed
409
410

if __name__ == '__main__':
roangel's avatar
roangel committed
411
412
413
    global node_name
    node_name = "CrazyRadio"
    rospy.init_node(node_name, anonymous=True)
414
415
416

    global ros_namespace
    ros_namespace = rospy.get_namespace()
417
    # Initialize the low-level drivers (don't list the debug drivers)
bucyril's avatar
bucyril committed
418
419
    cflib.crtp.init_drivers(enable_debug_driver=False)

420
    #wait until address parameter is set by PPSClient
421
    while not rospy.has_param("~crazyFlieAddress"):
bucyril's avatar
bucyril committed
422
423
        time.sleep(0.05)

424
    #use this following two lines to connect without data from CentralManager
roangel's avatar
roangel committed
425
426
    # radio_address = "radio://0/72/2M"
    # rospy.loginfo("manual address loaded")
michaero's avatar
michaero committed
427
428

    # Bool if using UWB, TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
429
430
431
432
433
434
    global enableUWB
    enableUWB = True
    global useUWB 
    useUWB = False
    global onboardPosition
    onboardPosition = False
michaero's avatar
michaero committed
435

436
    
437
    global cfbattery_pub
438
    cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
439

michaero's avatar
michaero committed
440
441
442
    if enableUWB:
        global rpy_pub
        rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)
443
444


michaero's avatar
michaero committed
445
446
447
448
449
450
451
452
453
454
        #TODO: To publish position, fix _xyzrpy_received_callback and _init_log_xyzrpy
        if onboardPosition:
            global xyz_pub
            xyz_pub = rospy.Publisher(node_name + '/XYZ', Float32MultiArray, queue_size=10)

        #TODO: To publish distances, uncomment these lines, uncomment self.init_log_anchordist() in _start_logging, uncomment stop functions in _stop_logging
        else:
            global distances_pub
            distances_pub = rospy.Publisher(node_name + '/distances', Float32MultiArray, queue_size=10)
    
455

456
    global cf_client
457

458
    cf_client = PPSRadioClient()
459
    rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
460

461
    time.sleep(1.0)
462

463
    rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
464

465
466
    rospy.spin()
    rospy.loginfo("Turning off crazyflie")
467

roangel's avatar
roangel committed
468
    cf_client._send_to_commander_motor(0, 0, 0, 0)
469
470
    # change state to motors OFF
    cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
471
472
    #wait for client to send its commands
    time.sleep(1.0)
473

474

roangel's avatar
roangel committed
475
476
477
    bag.close()
    rospy.loginfo("bag closed")

478
479
    cf_client._cf.close_link()
    rospy.loginfo("Link closed")