CrazyRadio.py 15.6 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
        #arrXYZ = Float32MultiArray()
        #arrXYZ.data = [data["xxx.x"], fdata["xxx.y"],data["xxx.z"]]
michaero's avatar
michaero committed
167
        #bag.write('XYZ',arrXYZ)
168
169
        #xyz_pub.publish(arrXYZ)

170

171
172
        arrRPY = Float32MultiArray()
        arrRPY.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]
173

michaero's avatar
michaero committed
174
175
        bag.write('rollPitchYaw',arrRPY)

176
        rpy_pub.publish(arrRPY)
177

178
        #print arrRPY
179

180
181
182
183
184

    #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()
185
        
186
        batteryVolt.data = data["pm.vbat"]
187
       
188
        bag.write('batteryVoltage', batteryVolt)
189
        
190
        #publish battery voltage for GUI
191
        #cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
192
        #print "batteryVolt: %s" % batteryVolt
193
194
        cfbattery_pub.publish(batteryVolt)

195
196


197
198
    def _logging_error(self, logconf, msg):
        print "Error when logging %s" % logconf.name
Yvan Bosshard's avatar
Yvan Bosshard committed
199

roangel's avatar
roangel committed
200
201
    # def _init_logging(self):

202
    def init_log_battery(self):
203
204
205
206

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

207
208
        self.batLog = LogConfig("BatteryLog", battery_polling_period) # second variable is period in ms
        self.batLog.add_variable("pm.vbat", "float");
209
        
210
211
212
        self._cf.log.add_config(self.batLog)
        if self.batLog.valid:
            self.batLog.data_received_cb.add_callback(self._vbat_received_callback)
213
            self.batLog.error_cb.add_callback(self._logging_error)
214
215
216
217
218
219
220
221
            print "batLog valid"
        else:
            print "batLog invalid"

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

    def init_log_xyzrpy(self):
222
223
224
225
226

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

        self.xyzrpy = LogConfig("XYZRPYLog", xyzrpy_polling_period)
227
        #TODO: where are coordinates published?
228
229
230
231
232
233
234
235
236
237
        #self.xyzrpy.add_variable("xxx.x", "float");
        #self.xyzrpy.add_variable("xxx.y", "float");
        #self.xyzrpy.add_variable("xxx.z", "float");
        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)
238
            self.xyzrpy.error_cb.add_callback(self._logging_error)
239
            print "xyzrpy valid"
roangel's avatar
roangel committed
240
        else:
241
242
243
244
245
            print "xyzrpy invalid"

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

246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
    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"

269
270
271
272

    def _start_logging(self):

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

274
275
276
        self.init_log_xyzrpy()

        #self.init_log_anchordist()             #TODO: test
277

278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
    def _stop_logging(self):

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

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

        #self.anchorDist.stop()
        #rospy.loginfo("anchorDist stopped")
        #self.anchorDist.delete()
        #rospy.loginfo("anchorDist deleted")

bucyril's avatar
bucyril committed
295
296
297
298
299
    def _connected(self, link_uri):
        """
            This callback is executed as soon as the connection to the
            quadrotor is established.
        """
300
        self.change_status_to(CONNECTED)
301
302
303
        # change state to motors OFF
        cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)

bucyril's avatar
bucyril committed
304
        rospy.loginfo("Connection to %s successful: " % link_uri)
305
        # Config for Logging
roangel's avatar
roangel committed
306
        self._start_logging()
307

bucyril's avatar
bucyril committed
308
309
310
    def _connection_failed(self, link_uri, msg):
        """Callback when connection initial connection fails (i.e no Crazyflie
        at the specified address)"""
311
        self.change_status_to(DISCONNECTED)
bucyril's avatar
bucyril committed
312
313
314
315
316
        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)"""
317
        self.change_status_to(DISCONNECTED)
bucyril's avatar
bucyril committed
318
319
320
321
        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)"""
322
        self.change_status_to(DISCONNECTED)
bucyril's avatar
bucyril committed
323
        rospy.logwarn("Disconnected from %s" % link_uri)
roangel's avatar
roangel committed
324

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

328
        self._stop_logging()
Yvan Bosshard's avatar
Yvan Bosshard committed
329

roangel's avatar
roangel committed
330
331
332
333
334
335
336
337
338
    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
339
        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
340
        self._cf.send_packet(pk)
bucyril's avatar
bucyril committed
341

roangel's avatar
roangel committed
342
    def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
bucyril's avatar
bucyril committed
343
        pk = CRTPPacket()
roangel's avatar
roangel committed
344
        pk.port = CRTPPort.COMMANDER_GENERIC
roangel's avatar
roangel committed
345
        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
346
347
        self._cf.send_packet(pk)

roangel's avatar
roangel committed
348
349
350
351
352
353
    # 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)

354
    def crazyRadioCommandCallback(self, msg):
355
        """Callback to tell CrazyRadio to reconnect"""
356
        print "crazyRadio command received %s" % msg.data
357

358
        if msg.data == CMD_RECONNECT:            # reconnect, check _status first and then do whatever needs to be done
359
            print "entered reconnect"
360
361
            print "_status: %s" % self._status
            if self.get_status() == DISCONNECTED:
362
363
                print "entered disconnected"
                self.connect()
364
365
            if self.get_status() == CONNECTED:
                self.status_pub.publish(CONNECTED)
366

367
368
369
370
371
372
373
        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)

374
375
def controlCommandCallback(data):
    """Callback for controller actions"""
376
    #rospy.loginfo("controller callback : %s, %s, %s", data.roll, data.pitch, data.yaw)
bucyril's avatar
bucyril committed
377

378
    #cmd1..4 must not be 0, as crazyflie onboard controller resets!
379
    #pitch and yaw are inverted on crazyflie controller
roangel's avatar
roangel committed
380
381
382
383
384
    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)
385

roangel's avatar
roangel committed
386
387
388
    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)
389
390
391



bucyril's avatar
bucyril committed
392
393

if __name__ == '__main__':
roangel's avatar
roangel committed
394
395
396
    global node_name
    node_name = "CrazyRadio"
    rospy.init_node(node_name, anonymous=True)
397
398
399

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

403
    #wait until address parameter is set by PPSClient
404
    while not rospy.has_param("~crazyFlieAddress"):
bucyril's avatar
bucyril committed
405
406
        time.sleep(0.05)

407
    #use this following two lines to connect without data from CentralManager
roangel's avatar
roangel committed
408
409
    # radio_address = "radio://0/72/2M"
    # rospy.loginfo("manual address loaded")
410
    
411
    global cfbattery_pub
412
    cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
413

414
415
416
    global rpy_pub
    rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)

417
    #TODO: To publish position, fix _xyzrpy_received_callback and _init_log_xyzrpy
418
419
420
    #global xyz_pub
    #xyz_pub = rospy.Publisher(node_name + '/XYZ', Float32MultiArray, queue_size=10)

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

425
    global cf_client
426

427
    cf_client = PPSRadioClient()
428
    rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
429

430
    time.sleep(1.0)
431

432
    rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
433

434
435
    rospy.spin()
    rospy.loginfo("Turning off crazyflie")
436

roangel's avatar
roangel committed
437
    cf_client._send_to_commander_motor(0, 0, 0, 0)
438
439
    # change state to motors OFF
    cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
440
441
    #wait for client to send its commands
    time.sleep(1.0)
442

443

roangel's avatar
roangel committed
444
445
446
    bag.close()
    rospy.loginfo("bag closed")

447
448
    cf_client._cf.close_link()
    rospy.loginfo("Link closed")