Skip to content

remove switch case and update explanation #399

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
knmcguire merged 2 commits into from
May 8, 2023
Merged

remove switch case and update explanation #399

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c8f2be7
remove switch case and update explanation
knmcguire May 5, 2023
f481768
import order
knmcguire May 5, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
226 changes: 114 additions & 112 deletions examples/multiranger/wall_following/wall_following.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,12 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
# ........... ____ _ __
# | ,-^-, | / __ )(_) /_______________ _____ ___
# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# | / ,..´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# +....... /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# GNU general public license v3.0
#
# Copyright (C) 2023 Bitcraze AB
#
Expand All @@ -23,12 +24,15 @@
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
"""
Helper class for the wall following demo
file: wall_following.py

Class for the wall following demo

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

Author: Kimberly McGuire (Bitcraze AB)
"""
import math
from enum import Enum
Expand Down Expand Up @@ -258,121 +262,119 @@ def wall_follower(self, front_range, side_range, current_heading,
self.first_run = False

# -------------- Handle state transitions ---------------- #
match self.state:
case self.StateWallFollowing.FORWARD:
if front_range < self.reference_distance_from_wall + self.ranger_value_buffer:
self.state = self.state_transition(self.StateWallFollowing.TURN_TO_FIND_WALL)
case self.StateWallFollowing.HOVER:
print('hover')
case self.StateWallFollowing.TURN_TO_FIND_WALL:
# Turn until 45 degrees from wall such that the front and side range sensors
# can detect the wall
side_range_check = side_range < (self.reference_distance_from_wall /
math.cos(math.pi/4) + self.ranger_value_buffer)
front_range_check = front_range < (self.reference_distance_from_wall /
math.cos(math.pi/4) + self.ranger_value_buffer)
if side_range_check and front_range_check:
self.prev_heading = current_heading
# Calculate the angle to the wall
self.wall_angle = self.wall_following_direction_value * \
(math.pi/2 - math.atan(front_range / side_range) + self.angle_value_buffer)
self.state = self.state_transition(self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL)
# If went too far in heading and lost the wall, go to find corner.
if side_range < self.reference_distance_from_wall + self.ranger_value_buffer and \
front_range > self.reference_distance_from_wall + self.range_threshold_lost:
self.around_corner_back_track = False
self.prev_heading = current_heading
self.state = self.state_transition(self.StateWallFollowing.FIND_CORNER)
case self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL:
align_wall_check = self.value_is_close_to(
self.wrap_to_pi(current_heading - self.prev_heading), self.wall_angle, self.angle_value_buffer)
if align_wall_check:
self.state = self.state_transition(self.StateWallFollowing.FORWARD_ALONG_WALL)
case self.StateWallFollowing.FORWARD_ALONG_WALL:
# If side range is out of reach,
# end of the wall is reached
if side_range > self.reference_distance_from_wall + self.range_threshold_lost:
self.state = self.state_transition(self.StateWallFollowing.FIND_CORNER)
# If front range is small
# then corner is reached
if front_range < self.reference_distance_from_wall + self.ranger_value_buffer:
self.prev_heading = current_heading
self.state = self.state_transition(self.StateWallFollowing.ROTATE_IN_CORNER)
case self.StateWallFollowing.ROTATE_AROUND_WALL:
if front_range < self.reference_distance_from_wall + self.ranger_value_buffer:
self.state = self.state_transition(self.StateWallFollowing.TURN_TO_FIND_WALL)
case self.StateWallFollowing.ROTATE_IN_CORNER:
check_heading_corner = self.value_is_close_to(
math.fabs(self.wrap_to_pi(current_heading-self.prev_heading)),
self.in_corner_angle, self.angle_value_buffer)
if check_heading_corner:
self.state = self.state_transition(self.StateWallFollowing.TURN_TO_FIND_WALL)
case self.StateWallFollowing.FIND_CORNER:
if side_range <= self.reference_distance_from_wall:
self.state = self.state_transition(self.StateWallFollowing.ROTATE_AROUND_WALL)
case _:
self.state = self.state_transition(self.StateWallFollowing.HOVER)
if self.state == self.StateWallFollowing.FORWARD:
if front_range < self.reference_distance_from_wall + self.ranger_value_buffer:
self.state = self.state_transition(self.StateWallFollowing.TURN_TO_FIND_WALL)
elif self.state == self.StateWallFollowing.HOVER:
print('hover')
elif self.state == self.StateWallFollowing.TURN_TO_FIND_WALL:
# Turn until 45 degrees from wall such that the front and side range sensors
# can detect the wall
side_range_check = side_range < (self.reference_distance_from_wall /
math.cos(math.pi/4) + self.ranger_value_buffer)
front_range_check = front_range < (self.reference_distance_from_wall /
math.cos(math.pi/4) + self.ranger_value_buffer)
if side_range_check and front_range_check:
self.prev_heading = current_heading
# Calculate the angle to the wall
self.wall_angle = self.wall_following_direction_value * \
(math.pi/2 - math.atan(front_range / side_range) + self.angle_value_buffer)
self.state = self.state_transition(self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL)
# If went too far in heading and lost the wall, go to find corner.
if side_range < self.reference_distance_from_wall + self.ranger_value_buffer and \
front_range > self.reference_distance_from_wall + self.range_threshold_lost:
self.around_corner_back_track = False
self.prev_heading = current_heading
self.state = self.state_transition(self.StateWallFollowing.FIND_CORNER)
elif self.state == self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL:
align_wall_check = self.value_is_close_to(
self.wrap_to_pi(current_heading - self.prev_heading), self.wall_angle, self.angle_value_buffer)
if align_wall_check:
self.state = self.state_transition(self.StateWallFollowing.FORWARD_ALONG_WALL)
elif self.state == self.StateWallFollowing.FORWARD_ALONG_WALL:
# If side range is out of reach,
# end of the wall is reached
if side_range > self.reference_distance_from_wall + self.range_threshold_lost:
self.state = self.state_transition(self.StateWallFollowing.FIND_CORNER)
# If front range is small
# then corner is reached
if front_range < self.reference_distance_from_wall + self.ranger_value_buffer:
self.prev_heading = current_heading
self.state = self.state_transition(self.StateWallFollowing.ROTATE_IN_CORNER)
elif self.state == self.StateWallFollowing.ROTATE_AROUND_WALL:
if front_range < self.reference_distance_from_wall + self.ranger_value_buffer:
self.state = self.state_transition(self.StateWallFollowing.TURN_TO_FIND_WALL)
elif self.state == self.StateWallFollowing.ROTATE_IN_CORNER:
check_heading_corner = self.value_is_close_to(
math.fabs(self.wrap_to_pi(current_heading-self.prev_heading)),
self.in_corner_angle, self.angle_value_buffer)
if check_heading_corner:
self.state = self.state_transition(self.StateWallFollowing.TURN_TO_FIND_WALL)
elif self.state == self.StateWallFollowing.FIND_CORNER:
if side_range <= self.reference_distance_from_wall:
self.state = self.state_transition(self.StateWallFollowing.ROTATE_AROUND_WALL)
else:
self.state = self.state_transition(self.StateWallFollowing.HOVER)

# -------------- Handle state actions ---------------- #
command_velocity_x_temp = 0.0
command_velocity_y_temp = 0.0
command_angle_rate_temp = 0.0

match self.state:
case self.StateWallFollowing.FORWARD:
command_velocity_x_temp = self.max_forward_speed
command_velocity_y_temp = 0.0
command_angle_rate_temp = 0.0
case self.StateWallFollowing.HOVER:
if self.state == self.StateWallFollowing.FORWARD:
command_velocity_x_temp = self.max_forward_speed
command_velocity_y_temp = 0.0
command_angle_rate_temp = 0.0
elif self.state == self.StateWallFollowing.HOVER:
command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover()
elif self.state == self.StateWallFollowing.TURN_TO_FIND_WALL:
command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate)
command_velocity_y_temp = 0.0
elif self.state == self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL:
if self.time_now - self.state_start_time < self.wait_for_measurement_seconds:
command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover()
case self.StateWallFollowing.TURN_TO_FIND_WALL:
else:
command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate)
command_velocity_y_temp = 0.0
case self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL:
if self.time_now - self.state_start_time < self.wait_for_measurement_seconds:
command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover()
else:
command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate)
command_velocity_y_temp = 0.0
case self.StateWallFollowing.FORWARD_ALONG_WALL:
command_velocity_x_temp, command_velocity_y_temp = self.command_forward_along_wall(side_range)
command_angle_rate_temp = 0.0
case self.StateWallFollowing.ROTATE_AROUND_WALL:
# If first time around corner
# first try to find the wall again
# if side range is larger than preffered distance from wall
if side_range > self.reference_distance_from_wall + self.range_threshold_lost:
# check if scanning already occured
if self.wrap_to_pi(math.fabs(current_heading - self.prev_heading)) > \
self.in_corner_angle:
self.around_corner_back_track = True
# turn and adjust distance to corner from that point
if self.around_corner_back_track:
# rotate back if it already went into one direction
command_velocity_y_temp, command_angle_rate_temp = self.command_turn(
-1 * self.max_turn_rate)
command_velocity_x_temp = 0.0
else:
command_velocity_y_temp, command_angle_rate_temp = self.command_turn(
self.max_turn_rate)
command_velocity_x_temp = 0.0
elif self.state == self.StateWallFollowing.FORWARD_ALONG_WALL:
command_velocity_x_temp, command_velocity_y_temp = self.command_forward_along_wall(side_range)
command_angle_rate_temp = 0.0
elif self.state == self.StateWallFollowing.ROTATE_AROUND_WALL:
# If first time around corner
# first try to find the wall again
# if side range is larger than preffered distance from wall
if side_range > self.reference_distance_from_wall + self.range_threshold_lost:
# check if scanning already occured
if self.wrap_to_pi(math.fabs(current_heading - self.prev_heading)) > \
self.in_corner_angle:
self.around_corner_back_track = True
# turn and adjust distance to corner from that point
if self.around_corner_back_track:
# rotate back if it already went into one direction
command_velocity_y_temp, command_angle_rate_temp = self.command_turn(
-1 * self.max_turn_rate)
command_velocity_x_temp = 0.0
else:
# continue to turn around corner
self.prev_heading = current_heading
self.around_corner_back_track = False
command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = \
self.command_turn_around_corner_and_adjust(
self.reference_distance_from_wall, side_range)
case self.StateWallFollowing.ROTATE_IN_CORNER:
command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate)
command_velocity_y_temp = 0.0
case self.StateWallFollowing.FIND_CORNER:
command_velocity_y_temp, command_angle_rate_temp = self.command_align_corner(
-1 * self.max_turn_rate, side_range, self.reference_distance_from_wall)
command_velocity_x_temp = 0.0
case _:
# state does not exist, so hover!
command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover()
command_velocity_y_temp, command_angle_rate_temp = self.command_turn(
self.max_turn_rate)
command_velocity_x_temp = 0.0
else:
# continue to turn around corner
self.prev_heading = current_heading
self.around_corner_back_track = False
command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = \
self.command_turn_around_corner_and_adjust(
self.reference_distance_from_wall, side_range)
elif self.state == self.StateWallFollowing.ROTATE_IN_CORNER:
command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate)
command_velocity_y_temp = 0.0
elif self.state == self.StateWallFollowing.FIND_CORNER:
command_velocity_y_temp, command_angle_rate_temp = self.command_align_corner(
-1 * self.max_turn_rate, side_range, self.reference_distance_from_wall)
command_velocity_x_temp = 0.0
else:
# state does not exist, so hover!
command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover()

command_velocity_x = command_velocity_x_temp
command_velocity_y = command_velocity_y_temp
Expand Down