diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index cbcd2c338..710f9b6c1 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -39,8 +39,9 @@ * Multiranger deck """ import logging -import sys import time +from math import degrees +from math import radians from wall_following.wall_following import WallFollowing @@ -55,21 +56,26 @@ URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -if len(sys.argv) > 1: - URI = sys.argv[1] -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - -wall_following = WallFollowing( - angle_value_buffer=0.1, ref_distance_from_wall=0.3, - max_forward_speed=0.3, init_state=WallFollowing.StateWF.FORWARD) +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') @@ -80,42 +86,33 @@ with Multiranger(scf) as multiranger: with SyncLogger(scf, lg_stab) as logger: - keep_flying = True while keep_flying: # initialize variables velocity_x = 0.0 velocity_y = 0.0 yaw_rate = 0.0 - state_wf = WallFollowing.StateWF.HOVER + state_wf = WallFollowing.StateWallFollowing.HOVER # Get Yaw log_entry = logger.next() data = log_entry[1] actual_yaw = data['stabilizer.yaw'] - actual_yaw_rad = actual_yaw * 3.1415 / 180 - - # get front range in milimeters - front_range = multiranger.front - if front_range is None: - front_range = 999 + actual_yaw_rad = radians(actual_yaw) - # get top range in milimeters - top_range = multiranger.up - if top_range is None: - top_range = 999 + # 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 - # direction = -1 turning right and follow wall with left-range - # direction = 1 turning left and follow wall with right-range - direction = -1 - side_range = multiranger.left # Get range in milimeters - if side_range is None: - side_range = 999 + 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, -1, time.time()) + 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) @@ -123,11 +120,11 @@ # 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*yaw_rate * 180 / 3.1415 + yaw_rate_deg = -1 * degrees(yaw_rate) motion_commander.start_linear_motion( velocity_x, velocity_y, 0, rate_yaw=yaw_rate_deg) - # if spacebar is pressed, stop the demo + # if top_range is activated, stop the demo if top_range < 0.2: keep_flying = False diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py index 28bd26695..fd91d2e39 100644 --- a/examples/multiranger/wall_following/wall_following.py +++ b/examples/multiranger/wall_following/wall_following.py @@ -35,7 +35,7 @@ class WallFollowing(): - class StateWF(Enum): + class StateWallFollowing(Enum): FORWARD = 1 HOVER = 2 TURN_TO_FIND_WALL = 3 @@ -45,10 +45,14 @@ class StateWF(Enum): ROTATE_IN_CORNER = 7 FIND_CORNER = 8 - def __init__(self, ref_distance_from_wall=0.0, + class WallFollowingDirection(Enum): + LEFT = 1 + RIGHT = -1 + + def __init__(self, reference_distance_from_wall=0.0, max_forward_speed=0.2, max_turn_rate=0.5, - direction=1.0, + wall_following_direction=WallFollowingDirection.LEFT, first_run=False, prev_heading=0.0, wall_angle=0.0, @@ -56,15 +60,43 @@ def __init__(self, ref_distance_from_wall=0.0, state_start_time=0.0, ranger_value_buffer=0.2, angle_value_buffer=0.1, - ranger_threshold_lost=0.3, + range_lost_threshold=0.3, in_corner_angle=0.8, wait_for_measurement_seconds=1.0, - init_state=StateWF.FORWARD): + init_state=StateWallFollowing.FORWARD): + """ + __init__ function for the WallFollowing class + + reference_distance_from_wall is the distance from the wall that the Crazyflie + should try to keep + max_forward_speed is the maximum speed the Crazyflie should fly forward + max_turn_rate is the maximum turn rate the Crazyflie should turn with + wall_following_direction is the direction the Crazyflie should follow the wall + (WallFollowingDirection Enum) + first_run is a boolean that is True if the Crazyflie is in the first run of the + wall following demo + prev_heading is the heading of the Crazyflie in the previous state (in rad) + wall_angle is the angle of the wall in the previous state (in rad) + around_corner_back_track is a boolean that is True if the Crazyflie is in the + around corner state and should back track + state_start_time is the time when the Crazyflie entered the current state (in s) + ranger_value_buffer is the buffer value for the ranger measurements (in m) + angle_value_buffer is the buffer value for the angle measurements (in rad) + range_lost_threshold is the threshold for when the Crazyflie should stop + following the wall (in m) + in_corner_angle is the angle the Crazyflie should turn when it is in the corner (in rad) + wait_for_measurement_seconds is the time the Crazyflie should wait for a + measurement before it starts the wall following demo (in s) + init_state is the initial state of the Crazyflie (StateWallFollowing Enum) + self.state is a shared state variable that is used to keep track of the current + state of the Crazyflie's wall following + self.time_now is a shared state variable that is used to keep track of the current (in s) + """ - self.ref_distance_from_wall = ref_distance_from_wall + self.reference_distance_from_wall = reference_distance_from_wall self.max_forward_speed = max_forward_speed self.max_turn_rate = max_turn_rate - self.direction = direction + self.wall_following_direction_value = float(wall_following_direction.value) self.first_run = first_run self.prev_heading = prev_heading self.wall_angle = wall_angle @@ -72,15 +104,18 @@ def __init__(self, ref_distance_from_wall=0.0, self.state_start_time = state_start_time self.ranger_value_buffer = ranger_value_buffer self.angle_value_buffer = angle_value_buffer - self.ranger_threshold_lost = ranger_threshold_lost + self.range_threshold_lost = range_lost_threshold self.in_corner_angle = in_corner_angle self.wait_for_measurement_seconds = wait_for_measurement_seconds self.first_run = True - self.state_wf = init_state + self.state = init_state self.time_now = 0.0 + self.speed_redux_corner = 3.0 + self.speed_redux_straight = 2.0 - def logic_is_close_to(self, real_value, checked_value, margin): + # Helper function + def value_is_close_to(self, real_value, checked_value, margin): if real_value > checked_value - margin and real_value < checked_value + margin: return True else: @@ -94,68 +129,127 @@ def wrap_to_pi(self, number): else: return number - def command_turn(self, ref_rate): - cmd_vel_x = 0.0 - cmd_ang_w = self.direction * ref_rate - return cmd_vel_x, cmd_ang_w + # Command functions + def command_turn(self, reference_rate): + """ + Command the Crazyflie to turn around its yaw axis + + reference_rate and rate_yaw is defined in rad/s + velocity_x is defined in m/s + """ + velocity_x = 0.0 + rate_yaw = self.wall_following_direction_value * reference_rate + return velocity_x, rate_yaw + + def command_align_corner(self, reference_rate, side_range, wanted_distance_from_corner): + """ + Command the Crazyflie to align itself to the outer corner + and make sure it's at a certain distance from it - def command_align_corner(self, ref_rate, range, wanted_distance_from_corner): - if range > wanted_distance_from_corner + 0.3: - cmd_ang_w = self.direction * ref_rate - cmd_vel_y = 0 + side_range and wanted_distance_from_corner is defined in m + reference_rate and rate_yaw is defined in rad/s + velocity_x is defined in m/s + """ + if side_range > wanted_distance_from_corner + self.range_threshold_lost: + rate_yaw = self.wall_following_direction_value * reference_rate + velocity_y = 0.0 else: - if range > wanted_distance_from_corner: - cmd_vel_y = self.direction * (-1.0 * self.max_forward_speed / 3.0) + if side_range > wanted_distance_from_corner: + velocity_y = self.wall_following_direction_value * \ + (-1.0 * self.max_forward_speed / self.speed_redux_corner) else: - cmd_vel_y = self.direction * (self.max_forward_speed / 3.0) - cmd_ang_w = 0 - return cmd_vel_y, cmd_ang_w + velocity_y = self.wall_following_direction_value * (self.max_forward_speed / self.speed_redux_corner) + rate_yaw = 0.0 + return velocity_y, rate_yaw def command_hover(self): - cmd_vel_x = 0.0 - cmd_vel_y = 0.0 - cmd_ang_w = 0.0 - return cmd_vel_x, cmd_vel_y, cmd_ang_w - - def command_forward_along_wall(self, range): - cmd_vel_x = self.max_forward_speed - cmd_vel_y = 0 - check_distance_wall = self.logic_is_close_to(self.ref_distance_from_wall, range, 0.1) + """ + Command the Crazyflie to hover in place + """ + velocity_x = 0.0 + velocity_y = 0.0 + rate_yaw = 0.0 + return velocity_x, velocity_y, rate_yaw + + def command_forward_along_wall(self, side_range): + """ + Command the Crazyflie to fly forward along the wall + while controlling it's distance to it + + side_range is defined in m + velocity_x and velocity_y is defined in m/s + """ + velocity_x = self.max_forward_speed + velocity_y = 0.0 + check_distance_wall = self.value_is_close_to( + self.reference_distance_from_wall, side_range, self.ranger_value_buffer) if not check_distance_wall: - if range > self.ref_distance_from_wall: - cmd_vel_y = self.direction * (-1.0 * self.max_forward_speed / 2.0) + if side_range > self.reference_distance_from_wall: + velocity_y = self.wall_following_direction_value * \ + (-1.0 * self.max_forward_speed / self.speed_redux_straight) else: - cmd_vel_y = self.direction * (self.max_forward_speed / 2.0) - return cmd_vel_x, cmd_vel_y - - def command_turn_around_corner_and_adjust(self, radius, range): - cmd_vel_x = self.max_forward_speed - cmd_ang_w = self.direction * (-1 * cmd_vel_x / radius) - cmd_vel_y = 0.0 - check_distance_wall = self.logic_is_close_to(self.ref_distance_from_wall, range, 0.1) + velocity_y = self.wall_following_direction_value * (self.max_forward_speed / self.speed_redux_straight) + return velocity_x, velocity_y + + def command_turn_around_corner_and_adjust(self, radius, side_range): + """ + Command the Crazyflie to turn around the corner + and adjust it's distance to the corner + + radius is defined in m + side_range is defined in m + velocity_x and velocity_y is defined in m/s + """ + velocity_x = self.max_forward_speed + rate_yaw = self.wall_following_direction_value * (-1 * velocity_x / radius) + velocity_y = 0.0 + check_distance_wall = self.value_is_close_to( + self.reference_distance_from_wall, side_range, self.ranger_value_buffer) if not check_distance_wall: - if range > self.ref_distance_from_wall: - cmd_vel_y = self.direction * (-1.0 * self.max_forward_speed / 3.0) + if side_range > self.reference_distance_from_wall: + velocity_y = self.wall_following_direction_value * \ + (-1.0 * self.max_forward_speed / self.speed_redux_corner) else: - cmd_vel_y = self.direction * (self.max_forward_speed / 3.0) - return cmd_vel_x, cmd_vel_y, cmd_ang_w + velocity_y = self.wall_following_direction_value * (self.max_forward_speed / self.speed_redux_corner) + return velocity_x, velocity_y, rate_yaw - def command_turn_and_adjust(self, rate, range): - cmd_ang_w = self.direction * rate - cmd_vel_y = 0.0 - return cmd_vel_y, cmd_ang_w + # state machine helper functions + def state_transition(self, new_state): + """ + Transition to a new state and reset the state timer - def transition(self, new_state): + new_state is defined in the StateWallFollowing enum + """ self.state_start_time = self.time_now return new_state - def adjust_distance_wall(self, distance_wall_new): - self.ref_distance_from_wall = distance_wall_new + def adjust_reference_distance_wall(self, reference_distance_wall_new): + """ + Adjust the reference distance to the wall + """ + self.reference_distance_from_wall = reference_distance_wall_new + # Wall following State machine def wall_follower(self, front_range, side_range, current_heading, - direction_turn, time_outer_loop): + wall_following_direction, time_outer_loop): + """ + wall_follower is the main function of the wall following state machine. + It takes the current range measurements of the front range and side range + sensors, the current heading of the Crazyflie, the wall following direction + and the current time of the outer loop (the real time or the simulation time) + as input, and handles the state transitions and commands the Crazyflie to + to do the wall following. + + front_range and side_range is defined in m + current_heading is defined in rad + wall_following_direction is defined as WallFollowingDirection enum + time_outer_loop is defined in seconds (double) + command_velocity_x, command_velocity_ y is defined in m/s + command_rate_yaw is defined in rad/s + self.state is defined as StateWallFollowing enum + """ - self.direction = direction_turn + self.wall_following_direction_value = float(wall_following_direction.value) self.time_now = time_outer_loop if self.first_run: @@ -163,90 +257,91 @@ def wall_follower(self, front_range, side_range, current_heading, self.around_corner_back_track = False self.first_run = False - # Handle state transitions - match self.state_wf: - case self.StateWF.FORWARD: - if front_range < self.ref_distance_from_wall + self.ranger_value_buffer: - self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) - case self.StateWF.HOVER: + # -------------- 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.StateWF.TURN_TO_FIND_WALL: - side_range_check = side_range < (self.ref_distance_from_wall / - math.cos(0.78) + self.ranger_value_buffer) - front_range_check = front_range < (self.ref_distance_from_wall / - math.cos(0.78) + self.ranger_value_buffer) + 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 - self.wall_angle = self.direction * \ - (1.57 - math.atan(front_range / side_range) + self.angle_value_buffer) - self.state_wf = self.transition(self.StateWF.TURN_TO_ALIGN_TO_WALL) - # If went too far in heading - if side_range < self.ref_distance_from_wall + self.ranger_value_buffer and \ - front_range > self.ref_distance_from_wall + self.ranger_threshold_lost: + # 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_wf = self.transition(self.StateWF.FIND_CORNER) - case self.StateWF.TURN_TO_ALIGN_TO_WALL: - align_wall_check = self.logic_is_close_to( + 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_wf = self.transition(self.StateWF.FORWARD_ALONG_WALL) - case self.StateWF.FORWARD_ALONG_WALL: + 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.ref_distance_from_wall + self.ranger_threshold_lost: - self.state_wf = self.transition(self.StateWF.FIND_CORNER) + 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.ref_distance_from_wall + self.ranger_value_buffer: + if front_range < self.reference_distance_from_wall + self.ranger_value_buffer: self.prev_heading = current_heading - self.state_wf = self.transition(self.StateWF.ROTATE_IN_CORNER) - case self.StateWF.ROTATE_AROUND_WALL: - if front_range < self.ref_distance_from_wall + self.ranger_value_buffer: - self.state_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) - - case self.StateWF.ROTATE_IN_CORNER: - check_heading_corner = self.logic_is_close_to( + 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_wf = self.transition(self.StateWF.TURN_TO_FIND_WALL) - case self.StateWF.FIND_CORNER: - if side_range <= self.ref_distance_from_wall: - self.state_wf = self.transition(self.StateWF.ROTATE_AROUND_WALL) + 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_wf = self.transition(self.StateWF.HOVER) - - # Handle state actions - cmd_vel_x_temp = 0.0 - cmd_vel_y_temp = 0.0 - cmd_ang_w_temp = 0.0 - - match self.state_wf: - case self.StateWF.FORWARD: - cmd_vel_x_temp = self.max_forward_speed - cmd_vel_y_temp = 0.0 - cmd_ang_w_temp = 0.0 - case self.StateWF.HOVER: - cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_hover() - case self.StateWF.TURN_TO_FIND_WALL: - cmd_vel_x_temp, cmd_ang_w_temp = self.command_turn(self.max_turn_rate) - cmd_vel_y_temp = 0.0 - case self.StateWF.TURN_TO_ALIGN_TO_WALL: + 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: + command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover() + case 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 + case self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL: if self.time_now - self.state_start_time < self.wait_for_measurement_seconds: - cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_hover() + command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover() else: - cmd_vel_x_temp, cmd_ang_w_temp = self.command_turn(self.max_turn_rate) - cmd_vel_y_temp = 0.0 - case self.StateWF.FORWARD_ALONG_WALL: - cmd_vel_x_temp, cmd_vel_y_temp = self.command_forward_along_wall(side_range) - cmd_ang_w_temp = 0.0 - case self.StateWF.ROTATE_AROUND_WALL: + 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.ref_distance_from_wall + self.ranger_threshold_lost: + 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: @@ -254,32 +349,33 @@ def wall_follower(self, front_range, side_range, current_heading, # turn and adjust distance to corner from that point if self.around_corner_back_track: # rotate back if it already went into one direction - cmd_vel_y_temp, cmd_ang_w_temp = self.command_turn_and_adjust( - -1 * self.max_turn_rate, side_range) - cmd_vel_x_temp = 0.0 + command_velocity_y_temp, command_angle_rate_temp = self.command_turn( + -1 * self.max_turn_rate) + command_velocity_x_temp = 0.0 else: - cmd_vel_y_temp, cmd_ang_w_temp = self.command_turn_and_adjust(self.max_turn_rate, side_range) - cmd_vel_x_temp = 0.0 + 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 - cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = \ + command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = \ self.command_turn_around_corner_and_adjust( - self.ref_distance_from_wall, side_range) - case self.StateWF.ROTATE_IN_CORNER: - cmd_vel_x_temp, cmd_ang_w_temp = self.command_turn(self.max_turn_rate) - cmd_vel_y_temp = 0.0 - case self.StateWF.FIND_CORNER: - cmd_vel_y_temp, cmd_ang_w_temp = self.command_align_corner( - -1 * self.max_turn_rate, side_range, self.ref_distance_from_wall) - cmd_vel_x_temp = 0.0 + 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! - cmd_vel_x_temp, cmd_vel_y_temp, cmd_ang_w_temp = self.command_hover() + command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover() - cmd_vel_x = cmd_vel_x_temp - cmd_vel_y = cmd_vel_y_temp - cmd_ang_w = cmd_ang_w_temp + command_velocity_x = command_velocity_x_temp + command_velocity_y = command_velocity_y_temp + command_yaw_rate = command_angle_rate_temp - return cmd_vel_x, cmd_vel_y, cmd_ang_w, self.state_wf + return command_velocity_x, command_velocity_y, command_yaw_rate, self.state