/
multiranger_wall_following.py
130 lines (106 loc) · 5.1 KB
/
multiranger_wall_following.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2023 Bitcraze AB
#
# Crazyflie Python Library
#
# 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 2
# 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 <https://www.gnu.org/licenses/>.
"""
Example script that makes the Crazyflie follow a wall
This examples uses the Flow and Multi-ranger decks to measure distances
in all directions and do wall following. Straight walls with corners
are advised to have in the test environment.
This is a python port of c-based app layer example from the Crazyflie-firmware
found here https://github.com/bitcraze/crazyflie-firmware/tree/master/examples/
demos/app_wall_following_demo
For the example to run the following hardware is needed:
* Crazyflie 2.0
* Crazyradio PA
* Flow deck
* Multiranger deck
"""
import logging
import time
from math import degrees
from math import radians
from wall_following.wall_following import WallFollowing
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.positioning.motion_commander import MotionCommander
from cflib.utils import uri_helper
from cflib.utils.multiranger import Multiranger
URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
def handle_range_measurement(range):
if range is None:
range = 999
return range
if __name__ == '__main__':
# Initialize the low-level drivers
cflib.crtp.init_drivers()
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
keep_flying = True
wall_following = WallFollowing(
angle_value_buffer=0.1, reference_distance_from_wall=0.3,
max_forward_speed=0.3, init_state=WallFollowing.StateWallFollowing.FORWARD)
# Setup logging to get the yaw data
lg_stab = LogConfig(name='Stabilizer', period_in_ms=100)
lg_stab.add_variable('stabilizer.yaw', 'float')
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(URI, cf=cf) as scf:
with MotionCommander(scf) as motion_commander:
with Multiranger(scf) as multiranger:
with SyncLogger(scf, lg_stab) as logger:
while keep_flying:
# initialize variables
velocity_x = 0.0
velocity_y = 0.0
yaw_rate = 0.0
state_wf = WallFollowing.StateWallFollowing.HOVER
# Get Yaw
log_entry = logger.next()
data = log_entry[1]
actual_yaw = data['stabilizer.yaw']
actual_yaw_rad = radians(actual_yaw)
# get front range in meters
front_range = handle_range_measurement(multiranger.front)
top_range = handle_range_measurement(multiranger.up)
left_range = handle_range_measurement(multiranger.left)
right_range = handle_range_measurement(multiranger.right)
# choose here the direction that you want the wall following to turn to
wall_following_direction = WallFollowing.WallFollowingDirection.RIGHT
side_range = left_range
# get velocity commands and current state from wall following state machine
velocity_x, velocity_y, yaw_rate, state_wf = wall_following.wall_follower(
front_range, side_range, actual_yaw_rad, wall_following_direction, time.time())
print('velocity_x', velocity_x, 'velocity_y', velocity_y,
'yaw_rate', yaw_rate, 'state_wf', state_wf)
# convert yaw_rate from rad to deg
# the negative sign is because of this ticket:
# https://github.com/bitcraze/crazyflie-lib-python/issues/389
yaw_rate_deg = -1 * degrees(yaw_rate)
motion_commander.start_linear_motion(
velocity_x, velocity_y, 0, rate_yaw=yaw_rate_deg)
# if top_range is activated, stop the demo
if top_range < 0.2:
keep_flying = False