CrazyRadio.py 17.5 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)

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


TYPE_PPS_MOTORS = 6
TYPE_PPS_RATE =   7
TYPE_PPS_ANGLE =  8

65
RAD_TO_DEG = 57.296
66

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

# Commands coming
CMD_RECONNECT = 0
74
CMD_DISCONNECT = 1
75

76
77
78
79
80
81
82
# 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

83
84
85
# max Number of UWB Anchors
UWB_NUM_ANCHORS = 6

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

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

        # 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
108
        self._status = DISCONNECTED
109
        self.link_uri = ""
bucyril's avatar
bucyril committed
110

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

bucyril's avatar
bucyril committed
115
        # Initialize the CrazyFlie and add callbacks
roangel's avatar
roangel committed
116
117
118
119
120
121
        self._init_cf()

        # Connect to the Crazyflie
        self.connect()

    def _init_cf(self):
bucyril's avatar
bucyril committed
122
123
        self._cf = Crazyflie()

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

130
131
        self.change_status_to(DISCONNECTED)

132

133
134
    """
    ---------------------------------------
135

136
        Logging functions
michaero's avatar
michaero committed
137

138
139
140
    ---------------------------------------
    """
    def _start_logging(self):
141

142
        self.init_log_battery()
143

144
145
146
147
148
149
150
151
        self.init_log_rpy()
        
        self.init_log_uwb()

        self.UWBUpdateCallback(1)
        # if enableUWB:
        #     self.init_log_uwb()
        #     self.init_log_rpy()
152

153
    def _stop_logging(self):
154

155
        self.stop_log_battery()
156

157
158
159
160
161
162
163
164
165
        self.delete_log_uwb()

        slef.delete_log_rpy()



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

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

roangel's avatar
roangel committed
170

171
    ### Battery log
172
    def init_log_battery(self):
173
174
175
176

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

177
178
        self.batLog = LogConfig("BatteryLog", battery_polling_period) # second variable is period in ms
        self.batLog.add_variable("pm.vbat", "float");
179
        
180
        self._cf.log.add_config(self.batLog)
181

182
183
        if self.batLog.valid:
            self.batLog.data_received_cb.add_callback(self._vbat_received_callback)
184
            self.batLog.error_cb.add_callback(self._logging_error)
185
            rospy.loginfo("[CrazyRadio] batLog valid")
186
        else:
187
            rospy.logwarn("[CrazyRadio] batLog invalid")
188
189

        self.batLog.start()
190
        rospy.loginfo("[CrazyRadio] batLog started")
191

192
193
194
195
196
197
198
199
200
201
202
203
204
    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)
205

206
207
208
209
210
    def stop_log_battery(self):
        self.batLog.stop()
        rospy.loginfo("[CrazyRadio] batLog stopped")
        self.batLog.delete()
        rospy.loginfo("[CrazyRadio] batLog deleted")
211
212


213
214
    ### UWB log
    def init_log_uwb(self):
215

216
217
        polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/positioning_polling_period")
        self.anchorLog = LogConfig("AnchorDistances", polling_period)
218
219

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

222
        self._cf.log.add_config(self.anchorLog)
223

224
225
226
227
        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")
228
        else:
229
            rospy.logwarn("[CrazyRadio] anchorLog invalid")
230

231
232
233
        
    def start_log_uwb(self):

234
        self.anchorLog.start()
235
        self.anchorLog._set_started(True)
236
        rospy.loginfo("[CrazyRadio] anchorLog started")
237

238
    def _anchors_received_callback(self, timestamp, data, anchorDist):
239

240
241
        dist = UInt32MultiArray()
        dist.data = []
242

243
        for i in range(UWB_NUM_ANCHORS):
244
            dist.data.insert(i, data["UWB_Anchor_Pos.anchor" + str(i)])
245

246
247
        uwb_location_pub.publish(dist)
        #bag.write('anchorTime', dist)     # needs a lot of time
roangel's avatar
roangel committed
248

249
250
251
        # for debugging
        #for i in range(UWB_NUM_ANCHORS):
        #    rospy.loginfo("Anchor " + str(i) + ": " + str(dist.data[i]))
252

253
    def stop_log_uwb(self):
254

255
        self.anchorLog._set_started(False)
256
257
        self.anchorLog.stop()
        rospy.loginfo("[CrazyRadio] anchorLog stopped")
258
259
260
261

    def delete_log_uwb(self):
        if (self.anchorLog._get_started()):
            self.stop_log_uwb()
262
263
        self.anchorLog.delete()
        rospy.loginfo("[CrazyRadio] anchorLog deleted")
264
        
265

266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
    ### 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")
284
285
286
        

    def start_log_rpy(self):
287
288

        self.rpyLog.start()
289
        self.rpyLog._set_started(True)
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
        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):

305
        self.rpyLog._set_started(False)
306
307
        self.rpyLog.stop()
        rospy.loginfo("[CrazyRadio] rpyLog stopped")
308
309
310
311

    def delete_log_rpy(self):
        if (self.rpyLog._get_started()):
            self.stop_log_rpy()
312
313
314
        self.rpyLog.delete()
        rospy.loginfo("[CrazyRadio] rpyLog deleted")

315
        
316
317
318
319
320
321
322
    """
    ---------------------------------------

        Callback functions

    ---------------------------------------
    """
bucyril's avatar
bucyril committed
323
324
325
326
327
    def _connected(self, link_uri):
        """
            This callback is executed as soon as the connection to the
            quadrotor is established.
        """
328
        self.change_status_to(CONNECTED)
329
330
331
        # change state to motors OFF
        cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)

332
        rospy.loginfo("[CrazyRadio] Connection to %s successful: " % link_uri)
333
        # Config for Logging
roangel's avatar
roangel committed
334
        self._start_logging()
335

336
337
338
339
340
341
342
343
344
345
    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
346
347
348
    def _connection_failed(self, link_uri, msg):
        """Callback when connection initial connection fails (i.e no Crazyflie
        at the specified address)"""
349
        self.change_status_to(DISCONNECTED)
350
        rospy.logerr("[CrazyRadio] Connection to %s failed: %s" % (link_uri, msg))
bucyril's avatar
bucyril committed
351
352
353
354

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

358
359
360
361
362
363
364
365
366
367
368
369
370
371
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)
            resp = uwbdataproxy(0,0,0)
            if (resp.enableUWB == False):
                if (self.anchorLog._get_started()):
                    self.stop_log_uwb()
                if (self.rpyLog._get_started):
                    self.stop_log_rpy()
            else:
                if not (self.anchorLog._get_started()):
                    self.start_log_uwb()
                if not (self.rpyLog._get_started()):
                    self.start_log_rpy()
        except rospy.ServiceException, e:
            rospy.logerr("[CrazyRadio] Unable to activate Loggings from Service");


roangel's avatar
roangel committed
380

381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411

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

        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)
412
413
        # change state to motors OFF
        self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
414
415
416
417
        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
418

roangel's avatar
roangel committed
419
420
421
422
423
424
425
426
427
    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
428
        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
429
        self._cf.send_packet(pk)
bucyril's avatar
bucyril committed
430

roangel's avatar
roangel committed
431
    def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
bucyril's avatar
bucyril committed
432
        pk = CRTPPacket()
roangel's avatar
roangel committed
433
        pk.port = CRTPPort.COMMANDER_GENERIC
roangel's avatar
roangel committed
434
        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
435
436
        self._cf.send_packet(pk)

roangel's avatar
roangel committed
437
438
439
440
441
442
    # 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)

443
    def crazyRadioCommandCallback(self, msg):
444
        """Callback to tell CrazyRadio to reconnect"""
445
        print "crazyRadio command received %s" % msg.data
446

447
        if msg.data == CMD_RECONNECT:            # reconnect, check _status first and then do whatever needs to be done
448
            print "entered reconnect"
449
450
            print "_status: %s" % self._status
            if self.get_status() == DISCONNECTED:
451
452
                print "entered disconnected"
                self.connect()
453
454
            if self.get_status() == CONNECTED:
                self.status_pub.publish(CONNECTED)
455

456
457
458
459
460
461
462
        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)

463
def controlCommandCallback(data):
464
    
465
    """Callback for controller actions"""
466
    #rospy.loginfo("controller callback : %s, %s, %s", data.roll, data.pitch, data.yaw)
bucyril's avatar
bucyril committed
467

468
    #cmd1..4 must not be 0, as crazyflie onboard controller resets!
469
    #pitch and yaw are inverted on crazyflie controller
roangel's avatar
roangel committed
470
471
472
473
474
    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)
475

roangel's avatar
roangel committed
476
477
478
    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)
479
480
481



482
483
484
485
"""
---------------------------------------

    main function
bucyril's avatar
bucyril committed
486

487
488
---------------------------------------
"""
bucyril's avatar
bucyril committed
489
if __name__ == '__main__':
490

roangel's avatar
roangel committed
491
492
493
    global node_name
    node_name = "CrazyRadio"
    rospy.init_node(node_name, anonymous=True)
494
495
496

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

500
    #wait until address parameter is set by PPSClient
501
    while not rospy.has_param("~crazyFlieAddress"):
bucyril's avatar
bucyril committed
502
503
        time.sleep(0.05)

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

508
    
michaero's avatar
michaero committed
509

510
    
511
    global cfbattery_pub
512
    cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
513

514
515
516
517
518
    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
519
    
520

521
    global cf_client
522

523
    cf_client = PPSRadioClient()
524
    #cf_client.UWBUpdateCallback(1)
525
    rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
526

527
    time.sleep(1.0)
528

529
    rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
530

531
532
    rospy.Subscriber("/my_GUI/UWBUpdate", Int32, cf_client.UWBUpdateCallback)

533
534
    rospy.spin()
    rospy.loginfo("Turning off crazyflie")
535

roangel's avatar
roangel committed
536
    cf_client._send_to_commander_motor(0, 0, 0, 0)
537
538
    # change state to motors OFF
    cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
539
540
    #wait for client to send its commands
    time.sleep(1.0)
541

542

roangel's avatar
roangel committed
543
544
545
    bag.close()
    rospy.loginfo("bag closed")

546
547
    cf_client._cf.close_link()
    rospy.loginfo("Link closed")