From c2613c15037d9dff04aa25dda3061bbbe4fdefeb Mon Sep 17 00:00:00 2001 From: Gabryel Reyes Date: Fri, 2 Aug 2024 09:56:14 +0200 Subject: [PATCH] Separated obstacle into own class --- .../platoon_supervisor/platoon_supervisor.py | 138 +++++++++++------- 1 file changed, 83 insertions(+), 55 deletions(-) diff --git a/webots/controllers/platoon_supervisor/platoon_supervisor.py b/webots/controllers/platoon_supervisor/platoon_supervisor.py index 62d213d1..b048ff15 100644 --- a/webots/controllers/platoon_supervisor/platoon_supervisor.py +++ b/webots/controllers/platoon_supervisor/platoon_supervisor.py @@ -1,21 +1,77 @@ -""" Platoon Supervisor controller for Webots. -Moves the obstacle up and down when the space key is pressed. """ +""" Platoon Supervisor controller for Webots. """ import sys -from controller import Supervisor # pylint: disable=import-error +from controller import Supervisor, Keyboard, Node, Field # pylint: disable=import-error -DEBOUNCE_TIME = 0.2 # Debounce time for the spacebar in seconds -OBSTACLE_NAME = "OBSTACLE" # Name of the obstacle node -TRANSLATION_FIELD = "translation" # Field name for the translation -RAISED_HEIGHT = 0.4 # Active height in meters -LOWERED_HEIGHT = 0.06 # Rest height in meters - -# Create the Supervisor instance. -supervisor = Supervisor() +class Obstacle: + """ + Class to represent the obstacle. + """ -# Get the keyboard device -keyboard = supervisor.getKeyboard() + DEBOUNCE_TIME = 0.2 # Debounce time for the spacebar in seconds + OBSTACLE_NAME = "OBSTACLE" # Name of the obstacle node + TRANSLATION_FIELD = "translation" # Field name for the translation + RAISED_HEIGHT = 0.4 # Active height in meters + LOWERED_HEIGHT = 0.06 # Rest height in meters + + def __init__(self) -> None: + """ Initialize the obstacle. """ + self.obstacle_node: Node = None + self.translation_field: Field = None + self.is_obstacle_lowered: bool = False + self.last_time: float = 0 + + def setup(self, supervisor: Supervisor) -> bool: + """ Setup the obstacle. """ + is_successful: bool = False + + # Get the obstacle node + self.obstacle_node = supervisor.getFromDef(self.OBSTACLE_NAME) + + if self.obstacle_node is not None: + # Get the translation field of the obstacle node + self.translation_field = self.obstacle_node.getField( + self.TRANSLATION_FIELD) + + if self.translation_field is None: + print( + f"Translation field {self.TRANSLATION_FIELD} \ + of {self.OBSTACLE_NAME} not found.") + else: + # Get current time for debouncing + self.last_time = supervisor.getTime() + curr_pos = self.translation_field.getSFVec3f() + self.translation_field.setSFVec3f([curr_pos[0], + curr_pos[1], + self.RAISED_HEIGHT]) + is_successful = True + + return is_successful + + def process(self, supervisor: Supervisor, keyboard: Keyboard) -> None: + """ Process the obstacle. """ + + if keyboard.getKey() == ord(' '): # If space key is pressed + if (supervisor.getTime() - self.last_time) > self.DEBOUNCE_TIME: # Debounce space key + # Update last time + self.last_time = supervisor.getTime() + + # Get the current position of the obstacle + curr_pos = self.translation_field.getSFVec3f() + + # Move the obstacle + if self.is_obstacle_lowered is True: # Raise the obstacle + print("Obstacle raised") + self.translation_field.setSFVec3f( + [curr_pos[0], curr_pos[1], self.RAISED_HEIGHT]) + else: # Lower the obstacle + print("Obstacle lowered") + self.translation_field.setSFVec3f( + [curr_pos[0], curr_pos[1], self.LOWERED_HEIGHT]) + + # Toggle the obstacle state + self.is_obstacle_lowered = not self.is_obstacle_lowered def main_loop(): @@ -28,22 +84,19 @@ def main_loop(): status = 0 - # Get the obstacle node - obstacle_node = supervisor.getFromDef(OBSTACLE_NAME) + # Create the Supervisor instance. + supervisor = Supervisor() - if obstacle_node is None: + # Get the keyboard device + keyboard = supervisor.getKeyboard() - print(f"Robot DEF {OBSTACLE_NAME} not found.") - status = -1 + # Create the obstacle instance + obstacle = Obstacle() - else: - # Get the translation field of the obstacle node - translation_field = obstacle_node.getField(TRANSLATION_FIELD) - - if translation_field is None: - print( - f"Translation field {TRANSLATION_FIELD} of {OBSTACLE_NAME} not found.") - status = -1 + # Setup the obstacle + if obstacle.setup(supervisor) is False: + # No obstacle found. + obstacle = None if status == 0: # Get the time step of the current world. @@ -52,37 +105,12 @@ def main_loop(): # Enable the keyboard device keyboard.enable(timestep) - # Get current time for debouncing - last_time = supervisor.getTime() - - # Set the initial state to raised obstacle - is_obstacle_lowered = False - curr_pos = translation_field.getSFVec3f() - translation_field.setSFVec3f( - [curr_pos[0], curr_pos[1], RAISED_HEIGHT]) - while supervisor.step(timestep) != -1: - if keyboard.getKey() == ord(' '): # If space key is pressed - if (supervisor.getTime() - last_time) > DEBOUNCE_TIME: # Debounce space key - - # Update last time - last_time = supervisor.getTime() - - # Get the current position of the obstacle - curr_pos = translation_field.getSFVec3f() - - # Move the obstacle - if is_obstacle_lowered is True: # Raise the obstacle - translation_field.setSFVec3f( - [curr_pos[0], curr_pos[1], RAISED_HEIGHT]) - else: # Lower the obstacle - translation_field.setSFVec3f( - [curr_pos[0], curr_pos[1], LOWERED_HEIGHT]) - - # Toggle the obstacle state - is_obstacle_lowered = not is_obstacle_lowered + if obstacle is not None: + obstacle.process(supervisor, keyboard) return status -sys.exit(main_loop()) +if __name__ == "__main__": + sys.exit(main_loop())