CrazyRadio.py 18 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
from std_msgs.msg import UInt32MultiArray
37
from d_fall_pps.srv import Anchors
38

bucyril's avatar
bucyril committed
39
40
41
42
43
44
45
46
47
48
49
# 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

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

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

56
57
58
# enableUWB parameter import
from cflib.crazyflie.param import Param

roangel's avatar
roangel committed
59
60
61
62
63
64
65
66
67
# CONTROLLER_MOTOR = 2
# CONTROLLER_ANGLE = 1
# CONTROLLER_RATE = 0


TYPE_PPS_MOTORS = 6
TYPE_PPS_RATE =   7
TYPE_PPS_ANGLE =  8

68
RAD_TO_DEG = 57.296
69

70
71
72
73
74
75
76
# CrazyRadio states:
CONNECTED = 0
CONNECTING = 1
DISCONNECTED = 2

# Commands coming
CMD_RECONNECT = 0
77
CMD_DISCONNECT = 1
78

79
80
81
82
83
84
85
# 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

86
87
88
# max Number of UWB Anchors
UWB_NUM_ANCHORS = 6

89
90
rp = RosPack()
record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag'
bucyril's avatar
bucyril committed
91
92
rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
rospy.loginfo(record_file)
93
bag = rosbag.Bag(record_file, 'w')
94

95
class PPSRadioClient:
bucyril's avatar
bucyril committed
96
    """
97
98
       CrazyRadio client that recieves the commands from the controller and
       sends them in a CRTP package to the crazyflie with the specified
99
       address.
bucyril's avatar
bucyril committed
100
    """
101
    def __init__(self):
bucyril's avatar
bucyril committed
102
103
104
105
106
107
108
109
110

        # 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
111
        self._status = DISCONNECTED
112
        self.link_uri = ""
bucyril's avatar
bucyril committed
113

roangel's avatar
roangel committed
114
        self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
115
        self.PPSClient_command_pub = rospy.Publisher('PPSClient/Command', Int32, queue_size=1)
116
        time.sleep(1.0)
117

bucyril's avatar
bucyril committed
118
        # Initialize the CrazyFlie and add callbacks
roangel's avatar
roangel committed
119
120
121
122
123
124
        self._init_cf()

        # Connect to the Crazyflie
        self.connect()

    def _init_cf(self):
bucyril's avatar
bucyril committed
125
126
        self._cf = Crazyflie()

127
        # Add callbacks that get executed depending on the connection _status.
bucyril's avatar
bucyril committed
128
129
130
131
132
        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)

133
134
        self.change_status_to(DISCONNECTED)

135

136
137
    """
    ---------------------------------------
138

139
        Logging functions
michaero's avatar
michaero committed
140

141
142
143
    ---------------------------------------
    """
    def _start_logging(self):
144

145
        self.init_log_battery()
146

147
148
149
150
        self.init_log_rpy()
        
        self.init_log_uwb()

151
152
153
154
155
        # self.init_params()
        # _cf.params.refresh_toc(None, )
        # self.params.request_update_of_all_params()

        # rospy.logwarn(self._cf.param.request_param_update("firmware.modified"))
156

157
158
159
160
        self.UWBUpdateCallback(1)
        # if enableUWB:
        #     self.init_log_uwb()
        #     self.init_log_rpy()
161

162
    def _stop_logging(self):
163

164
        self.stop_log_battery()
165

166
167
        self.delete_log_uwb()

168
        self.delete_log_rpy()
169
170
171
172
173
174



        # if enableUWB:
        #     self.stop_log_uwb()
        #     self.stop_log_rpy()
175

176
    def _logging_error(self, logconf, msg):
177
        rospy.logerr("[CrazyRadio] Error when logging %s", logconf.name)
Yvan Bosshard's avatar
Yvan Bosshard committed
178

roangel's avatar
roangel committed
179

180
    ### Battery log
181
    def init_log_battery(self):
182
183
184
185

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

186
187
        self.batLog = LogConfig("BatteryLog", battery_polling_period) # second variable is period in ms
        self.batLog.add_variable("pm.vbat", "float");
188
        
189
        self._cf.log.add_config(self.batLog)
190

191
192
        if self.batLog.valid:
            self.batLog.data_received_cb.add_callback(self._vbat_received_callback)
193
            self.batLog.error_cb.add_callback(self._logging_error)
194
            rospy.loginfo("[CrazyRadio] batLog valid")
195
        else:
196
            rospy.logwarn("[CrazyRadio] batLog invalid")
197
198

        self.batLog.start()
199
        rospy.loginfo("[CrazyRadio] batLog started")
200

201
202
203
204
205
206
207
208
209
210
211
212
213
    def _vbat_received_callback(self, timestamp, data, batLog):
        
        #print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
        batteryVolt = Float32()
        
        batteryVolt.data = data["pm.vbat"]
       
        bag.write('batteryVoltage', batteryVolt)
        
        #publish battery voltage for GUI
        #cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
        #print "batteryVolt: %s" % batteryVolt
        cfbattery_pub.publish(batteryVolt)
214

215
216
217
218
219
    def stop_log_battery(self):
        self.batLog.stop()
        rospy.loginfo("[CrazyRadio] batLog stopped")
        self.batLog.delete()
        rospy.loginfo("[CrazyRadio] batLog deleted")
220
221


222
223
    ### UWB log
    def init_log_uwb(self):
224

225
226
        polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/positioning_polling_period")
        self.anchorLog = LogConfig("AnchorDistances", polling_period)
227
228

        for i in range(UWB_NUM_ANCHORS):
229
            self.anchorLog.add_variable("UWB_Anchor_Pos.anchor" + str(i), "uint32_t");
230

231
        self._cf.log.add_config(self.anchorLog)
232

233
234
235
236
        if self.anchorLog.valid:
            self.anchorLog.data_received_cb.add_callback(self._anchors_received_callback)
            self.anchorLog.error_cb.add_callback(self._logging_error)
            rospy.loginfo("[CrazyRadio] anchorLog valid")
237
        else:
238
            rospy.logwarn("[CrazyRadio] anchorLog invalid")
239

240
241
242
        
    def start_log_uwb(self):

243
        self.anchorLog.start()
244
        self.anchorLog._set_started(True)
245
        rospy.loginfo("[CrazyRadio] anchorLog started")
246

247
    def _anchors_received_callback(self, timestamp, data, anchorDist):
248

249
250
        dist = UInt32MultiArray()
        dist.data = []
251

252
        for i in range(UWB_NUM_ANCHORS):
253
            dist.data.insert(i, data["UWB_Anchor_Pos.anchor" + str(i)])
254

255
256
        uwb_location_pub.publish(dist)
        #bag.write('anchorTime', dist)     # needs a lot of time
roangel's avatar
roangel committed
257

258
259
260
        # for debugging
        #for i in range(UWB_NUM_ANCHORS):
        #    rospy.loginfo("Anchor " + str(i) + ": " + str(dist.data[i]))
261

262
    def stop_log_uwb(self):
263

264
        self.anchorLog._set_started(False)
265
266
        self.anchorLog.stop()
        rospy.loginfo("[CrazyRadio] anchorLog stopped")
267
268
269
270

    def delete_log_uwb(self):
        if (self.anchorLog._get_started()):
            self.stop_log_uwb()
271
272
        self.anchorLog.delete()
        rospy.loginfo("[CrazyRadio] anchorLog deleted")
273
        
274

275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
    ### RPY log
    def init_log_rpy(self):

        polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/positioning_polling_period")
        self.rpyLog = LogConfig("RPY", polling_period)

        self.rpyLog.add_variable("stabilizer.roll", "float");
        self.rpyLog.add_variable("stabilizer.pitch", "float");
        self.rpyLog.add_variable("stabilizer.yaw", "float");

        self._cf.log.add_config(self.rpyLog)

        if self.rpyLog.valid:
            self.rpyLog.data_received_cb.add_callback(self._rpy_received_callback)
            self.rpyLog.error_cb.add_callback(self._logging_error)
            rospy.loginfo("[CrazyRadio] rpyLog valid")
        else:
            rospy.logwarn("[CrazyRadio] rpyLog invalid")
293
294
295
        

    def start_log_rpy(self):
296
297

        self.rpyLog.start()
298
        self.rpyLog._set_started(True)
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
        rospy.loginfo("[CrazyRadio] rpyLog started")

    def _rpy_received_callback(self, timestamp, data, rpy):
        
        rpy_data = Float32MultiArray()
        rpy_data.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]

        rpy_pub.publish(rpy_data)
        #bag.write('RPY', rpy_data)     # needs a lot of time

        # for debugging
        #rospy.loginfo("Roll: %f\nPitch: %f\nYaw: %f", rpy_data.data[0], rpy_data.data[1], rpy_data.data[2])

    def stop_log_rpy(self):

314
        self.rpyLog._set_started(False)
315
316
        self.rpyLog.stop()
        rospy.loginfo("[CrazyRadio] rpyLog stopped")
317
318
319
320

    def delete_log_rpy(self):
        if (self.rpyLog._get_started()):
            self.stop_log_rpy()
321
322
323
        self.rpyLog.delete()
        rospy.loginfo("[CrazyRadio] rpyLog deleted")

324
325

    ### enableUWB parameter on CF
326
327
    # def init_params(self):
        # self._cf.params = Param()
328
329


330
        
331
332
333
334
335
336
337
    """
    ---------------------------------------

        Callback functions

    ---------------------------------------
    """
bucyril's avatar
bucyril committed
338
339
340
341
342
    def _connected(self, link_uri):
        """
            This callback is executed as soon as the connection to the
            quadrotor is established.
        """
343
        self.change_status_to(CONNECTED)
344
345
346
        # change state to motors OFF
        cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)

347
        rospy.loginfo("[CrazyRadio] Connection to %s successful: " % link_uri)
348
        # Config for Logging
roangel's avatar
roangel committed
349
        self._start_logging()
350

351
352
353
354
355
356
357
358
359
360
    def _disconnected(self, link_uri):
        """Callback when the Crazyflie is disconnected (called in all cases)"""
        self.change_status_to(DISCONNECTED)
        rospy.logwarn("[CrazyRadio] Disconnected from %s" % link_uri)

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

        self._stop_logging()

bucyril's avatar
bucyril committed
361
362
363
    def _connection_failed(self, link_uri, msg):
        """Callback when connection initial connection fails (i.e no Crazyflie
        at the specified address)"""
364
        self.change_status_to(DISCONNECTED)
365
        rospy.logerr("[CrazyRadio] Connection to %s failed: %s" % (link_uri, msg))
bucyril's avatar
bucyril committed
366
367
368
369

    def _connection_lost(self, link_uri, msg):
        """Callback when disconnected after a connection has been made (i.e
        Crazyflie moves out of range)"""
370
        self.change_status_to(DISCONNECTED)
371
        rospy.logerr("[CrazyRadio] Connection to %s lost: %s" % (link_uri, msg))
bucyril's avatar
bucyril committed
372

373
374
375
376
377
378
379
    def UWBUpdateCallback(self, data):
        """Callback for change in UWB Settings"""
        # TODO: Tiago? Add command to CF to enable/disable ranging
        rospy.wait_for_service('/UWBManagerService/UWBData')

        try:
            uwbdataproxy = rospy.ServiceProxy('/UWBManagerService/UWBData',Anchors)
380
            resp = uwbdataproxy(0,0,0,0)
381
382
383
384
385
            if (resp.enableUWB == False):
                if (self.anchorLog._get_started()):
                    self.stop_log_uwb()
                if (self.rpyLog._get_started):
                    self.stop_log_rpy()
386
                self._cf.param.set_value('activate_anchors.enable', '0')
387
388
389
390
391
            else:
                if not (self.anchorLog._get_started()):
                    self.start_log_uwb()
                if not (self.rpyLog._get_started()):
                    self.start_log_rpy()
392
                self._cf.param.set_value('activate_anchors.enable', '1')
393
394
395
396
        except rospy.ServiceException, e:
            rospy.logerr("[CrazyRadio] Unable to activate Loggings from Service");


roangel's avatar
roangel committed
397

398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428

    """
    ---------------------------------------

        CrazyRadio interface functions

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

    def get_status(self):
        return self._status

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

    def connect(self):
        # update link from ros params
        self.update_link_uri()

        print "Connecting to %s" % self.link_uri
        self.change_status_to(CONNECTING)
        rospy.loginfo("connecting...")
        self._cf.open_link(self.link_uri)

    def disconnect(self):
        print "Motors OFF"
        self._send_to_commander_motor(0, 0, 0, 0)
429
430
        # change state to motors OFF
        self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
431
432
433
434
        time.sleep(0.1)
        print "Disconnecting from %s" % self.link_uri
        self._cf.close_link()
        self.change_status_to(DISCONNECTED)
Yvan Bosshard's avatar
Yvan Bosshard committed
435

roangel's avatar
roangel committed
436
437
438
439
440
441
442
443
444
    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
445
        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
446
        self._cf.send_packet(pk)
bucyril's avatar
bucyril committed
447

roangel's avatar
roangel committed
448
    def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
bucyril's avatar
bucyril committed
449
        pk = CRTPPacket()
roangel's avatar
roangel committed
450
        pk.port = CRTPPort.COMMANDER_GENERIC
roangel's avatar
roangel committed
451
        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
452
453
        self._cf.send_packet(pk)

roangel's avatar
roangel committed
454
455
456
457
458
459
    # 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)

460
    def crazyRadioCommandCallback(self, msg):
461
        """Callback to tell CrazyRadio to reconnect"""
462
        print "crazyRadio command received %s" % msg.data
463

464
        if msg.data == CMD_RECONNECT:            # reconnect, check _status first and then do whatever needs to be done
465
            print "entered reconnect"
466
467
            print "_status: %s" % self._status
            if self.get_status() == DISCONNECTED:
468
469
                print "entered disconnected"
                self.connect()
470
471
            if self.get_status() == CONNECTED:
                self.status_pub.publish(CONNECTED)
472

473
474
475
476
477
478
479
        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)

480
def controlCommandCallback(data):
481
    
482
    """Callback for controller actions"""
483
    #rospy.loginfo("controller callback : %s, %s, %s", data.roll, data.pitch, data.yaw)
bucyril's avatar
bucyril committed
484

485
    #cmd1..4 must not be 0, as crazyflie onboard controller resets!
486
    #pitch and yaw are inverted on crazyflie controller
roangel's avatar
roangel committed
487
488
489
490
491
    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)
492

roangel's avatar
roangel committed
493
494
495
    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)
496
497
498



499
500
501
502
"""
---------------------------------------

    main function
bucyril's avatar
bucyril committed
503

504
505
---------------------------------------
"""
bucyril's avatar
bucyril committed
506
if __name__ == '__main__':
507

roangel's avatar
roangel committed
508
509
510
    global node_name
    node_name = "CrazyRadio"
    rospy.init_node(node_name, anonymous=True)
511
512
513

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

517
    #wait until address parameter is set by PPSClient
518
    while not rospy.has_param("~crazyFlieAddress"):
bucyril's avatar
bucyril committed
519
520
        time.sleep(0.05)

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

525
    
michaero's avatar
michaero committed
526

527
    
528
    global cfbattery_pub
529
    cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
530

531
532
533
534
535
    global uwb_location_pub
    uwb_location_pub = rospy.Publisher(node_name + '/AnchorDistances', UInt32MultiArray, queue_size=10)
    global rpy_pub
    rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)

michaero's avatar
michaero committed
536
    
537

538
    global cf_client
539

540
    cf_client = PPSRadioClient()
541
    #cf_client.UWBUpdateCallback(1)
542
    rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
543

544
    time.sleep(1.0)
545

546
    rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
547

548
549
    rospy.Subscriber("/my_GUI/UWBUpdate", Int32, cf_client.UWBUpdateCallback)

550
551
    rospy.spin()
    rospy.loginfo("Turning off crazyflie")
552

roangel's avatar
roangel committed
553
    cf_client._send_to_commander_motor(0, 0, 0, 0)
554
555
    # change state to motors OFF
    cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
556
557
    #wait for client to send its commands
    time.sleep(1.0)
558

559

roangel's avatar
roangel committed
560
561
562
    bag.close()
    rospy.loginfo("bag closed")

563
564
    cf_client._cf.close_link()
    rospy.loginfo("Link closed")