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

4
import roslib; roslib.load_manifest('d_fall_pps')
bucyril's avatar
bucyril committed
5
import rospy
6
from d_fall_pps.msg import ControlCommand
7
from std_msgs.msg import Int32
bucyril's avatar
bucyril committed
8
9
10
11
12
13
14


# General import
import time, sys
import struct
import logging

15
import rosbag
16
from rospkg import RosPack
17
18
19
from std_msgs.msg import Float32
from std_msgs.msg import String

bucyril's avatar
bucyril committed
20
21
22
23
24
25
26
27
28
29
30
# 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

31
# Logging import(*
bucyril's avatar
bucyril committed
32
33
34
35
36
from cflib.crazyflie.log import LogConfig

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

roangel's avatar
roangel committed
37
38
39
40
41
42
43
44
45
# CONTROLLER_MOTOR = 2
# CONTROLLER_ANGLE = 1
# CONTROLLER_RATE = 0


TYPE_PPS_MOTORS = 6
TYPE_PPS_RATE =   7
TYPE_PPS_ANGLE =  8

46
RAD_TO_DEG = 57.296
47

48
49
50
51
52
53
54
# CrazyRadio states:
CONNECTED = 0
CONNECTING = 1
DISCONNECTED = 2

# Commands coming
CMD_RECONNECT = 0
55
CMD_DISCONNECT = 1
56

57
58
59
60
61
62
63
# 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

64
65
rp = RosPack()
record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag'
bucyril's avatar
bucyril committed
66
67
rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
rospy.loginfo(record_file)
68
bag = rosbag.Bag(record_file, 'w')
69

70
class PPSRadioClient:
bucyril's avatar
bucyril committed
71
    """
72
73
       CrazyRadio client that recieves the commands from the controller and
       sends them in a CRTP package to the crazyflie with the specified
74
       address.
bucyril's avatar
bucyril committed
75
    """
76
    def __init__(self):
bucyril's avatar
bucyril committed
77
78
79
80
81
82
83
84
85

        # 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
86
        self._status = DISCONNECTED
87
        self.link_uri = ""
bucyril's avatar
bucyril committed
88

roangel's avatar
roangel committed
89
        self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
90
        self.PPSClient_command_pub = rospy.Publisher('PPSClient/Command', Int32, queue_size=1)
91
        time.sleep(1.0)
92

bucyril's avatar
bucyril committed
93
        # Initialize the CrazyFlie and add callbacks
roangel's avatar
roangel committed
94
95
96
97
98
99
        self._init_cf()

        # Connect to the Crazyflie
        self.connect()

    def _init_cf(self):
bucyril's avatar
bucyril committed
100
101
        self._cf = Crazyflie()

102
        # Add callbacks that get executed depending on the connection _status.
bucyril's avatar
bucyril committed
103
104
105
106
107
        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)

108
109
110
        self.change_status_to(DISCONNECTED)

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

115
116
    def get_status(self):
        return self._status
117

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

121
    def connect(self):
122
123
124
        # update link from ros params
        self.update_link_uri()

roangel's avatar
roangel committed
125
        print "Connecting to %s" % self.link_uri
126
        self.change_status_to(CONNECTING)
127
128
        rospy.loginfo("connecting...")
        self._cf.open_link(self.link_uri)
roangel's avatar
roangel committed
129

130
    def disconnect(self):
131
        print "Motors OFF"
roangel's avatar
roangel committed
132
        self._send_to_commander_motor(0, 0, 0, 0)
133
134
        # change state to motors OFF
        self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
135
        time.sleep(0.1)
136
137
138
139
        print "Disconnecting from %s" % self.link_uri
        self._cf.close_link()
        self.change_status_to(DISCONNECTED)

140
    def _data_received_callback(self, timestamp, data, logconf):
141
        #print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
142
        batteryVolt = Float32()
143
144
145
        stabilizerYaw = Float32()
        stabilizerPitch = Float32()
        stabilizerRoll = Float32()
146
        batteryVolt.data = data["pm.vbat"]
147
148
149
150
151
152
        stabilizerYaw.data = data["stabilizer.yaw"]
        stabilizerPitch.data = data["stabilizer.pitch"]
        bag.write('batteryVoltage', batteryVolt)
        bag.write('stabilizerYaw', stabilizerYaw)
        bag.write('stabilizerPitch', stabilizerPitch)
        bag.write('stabilizerRoll', stabilizerRoll)
153
154

        #publish battery voltage for GUI
155
        #cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
156
        # print "batteryVolt: %s" % batteryVolt
157
158
        cfbattery_pub.publish(batteryVolt)

159
160


161
162
    def _logging_error(self, logconf, msg):
        print "Error when logging %s" % logconf.name
bucyril's avatar
bucyril committed
163

roangel's avatar
roangel committed
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
    # def _init_logging(self):

    def _start_logging(self):
        self.logconf = LogConfig("LoggingTest", 100) # second variable is freq in ms
        self.logconf.add_variable("stabilizer.roll", "float");
        self.logconf.add_variable("stabilizer.pitch", "float");
        self.logconf.add_variable("stabilizer.yaw", "float");
        self.logconf.add_variable("pm.vbat", "float");

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

        self.logconf.start()
        print "logconf start"

bucyril's avatar
bucyril committed
184
185
186
187
188
    def _connected(self, link_uri):
        """
            This callback is executed as soon as the connection to the
            quadrotor is established.
        """
189
        self.change_status_to(CONNECTED)
190
191
192
        # change state to motors OFF
        cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)

bucyril's avatar
bucyril committed
193
        rospy.loginfo("Connection to %s successful: " % link_uri)
194
        # Config for Logging
roangel's avatar
roangel committed
195
        self._start_logging()
196

bucyril's avatar
bucyril committed
197

198
199


bucyril's avatar
bucyril committed
200
201
202
    def _connection_failed(self, link_uri, msg):
        """Callback when connection initial connection fails (i.e no Crazyflie
        at the specified address)"""
203
        self.change_status_to(DISCONNECTED)
bucyril's avatar
bucyril committed
204
205
206
207
208
        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)"""
209
        self.change_status_to(DISCONNECTED)
bucyril's avatar
bucyril committed
210
211
212
213
        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)"""
214
        self.change_status_to(DISCONNECTED)
bucyril's avatar
bucyril committed
215
        rospy.logwarn("Disconnected from %s" % link_uri)
roangel's avatar
roangel committed
216

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

roangel's avatar
roangel committed
220
221
222
223
        self.logconf.stop()
        rospy.loginfo("logconf stopped")
        self.logconf.delete()
        rospy.loginfo("logconf deleted")
224

roangel's avatar
roangel committed
225
226
227
228
229
230
231
232
233
234
235
    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
        pk.data = struct.pack('<BHHHHfff', TYPE_PPS_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate)
        self._cf.send_packet(pk)
bucyril's avatar
bucyril committed
236

roangel's avatar
roangel committed
237
    def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
bucyril's avatar
bucyril committed
238
        pk = CRTPPacket()
roangel's avatar
roangel committed
239
240
        pk.port = CRTPPort.COMMANDER_GENERIC
        pk.data = struct.pack('<BHHHHfff', TYPE_PPS_ANGLE, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw)
bucyril's avatar
bucyril committed
241
242
        self._cf.send_packet(pk)

roangel's avatar
roangel committed
243
244
245
246
247
248
    # 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)

249
    def crazyRadioCommandCallback(self, msg):
250
        """Callback to tell CrazyRadio to reconnect"""
251
        print "crazyRadio command received %s" % msg.data
252

253
        if msg.data == CMD_RECONNECT:            # reconnect, check _status first and then do whatever needs to be done
254
            print "entered reconnect"
255
256
            print "_status: %s" % self._status
            if self.get_status() == DISCONNECTED:
257
258
                print "entered disconnected"
                self.connect()
259
260
            if self.get_status() == CONNECTED:
                self.status_pub.publish(CONNECTED)
261

262
263
264
265
266
267
268
        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)

269
270
def controlCommandCallback(data):
    """Callback for controller actions"""
271
    #rospy.loginfo("controller callback : %s, %s, %s", data.roll, data.pitch, data.yaw)
bucyril's avatar
bucyril committed
272

273
    #cmd1..4 must not be 0, as crazyflie onboard controller resets!
274
    #pitch and yaw are inverted on crazyflie controller
roangel's avatar
roangel committed
275
276
277
278
279
    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)
280

roangel's avatar
roangel committed
281
282
283
    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)
284
285
286



bucyril's avatar
bucyril committed
287
288

if __name__ == '__main__':
roangel's avatar
roangel committed
289
290
291
    global node_name
    node_name = "CrazyRadio"
    rospy.init_node(node_name, anonymous=True)
292
    # Initialize the low-level drivers (don't list the debug drivers)
bucyril's avatar
bucyril committed
293
294
    cflib.crtp.init_drivers(enable_debug_driver=False)

295
    #wait until address parameter is set by PPSClient
296
    while not rospy.has_param("~crazyFlieAddress"):
bucyril's avatar
bucyril committed
297
298
        time.sleep(0.05)

299
    #use this following two lines to connect without data from CentralManager
roangel's avatar
roangel committed
300
301
302
    # radio_address = "radio://0/72/2M"
    # rospy.loginfo("manual address loaded")

303
    global cfbattery_pub
304
    cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
305

306
    global cf_client
307

308
    cf_client = PPSRadioClient()
309
    rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
310

311
    time.sleep(1.0)
312

313
    rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
314

315
316
    rospy.spin()
    rospy.loginfo("Turning off crazyflie")
317

roangel's avatar
roangel committed
318
    cf_client._send_to_commander_motor(0, 0, 0, 0)
319
320
    # change state to motors OFF
    cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
321
322
    #wait for client to send its commands
    time.sleep(1.0)
323

324

roangel's avatar
roangel committed
325
326
327
    bag.close()
    rospy.loginfo("bag closed")

328
329
    cf_client._cf.close_link()
    rospy.loginfo("Link closed")