Ten followers

start_platoon.bat

@@ -54,7 +54,7 @@ xcopy /s /y %TEMP_PLATOON_LEADER_PATH%\ %TEMP_PLATOON_LEADER_NEW_PATH%\
 
 rem Copy follower folder according to a new folder with the robot's name.
 rem The copy ensures that a existing settings.json will be kept.
-for /L %%i in (1, 1, 2) do (
+for /L %%i in (1, 1, 10) do (
     xcopy /s /y %TEMP_PLATOON_FOLLOWER_PATH%\ %TEMP_PLATOON_FOLLOWER_NEW_PATH%%%i\
 )
 
@@ -67,7 +67,7 @@ echo Start convoy leader.
 start "Convoy Leader" ""%WEBOTS_CONTROLLER%"" --robot-name=%CONVOY_LEADER_ROBOT_NAME% --stdout-redirect %TEMP_PLATOON_LEADER_NEW_PATH%\%PROGRAM_NAME% -n %CONVOY_LEADER_ROBOT_NAME% -c --serialRxCh=%CONVOY_LEADER_RX_CHANNEL% --serialTxCh=%CONVOY_LEADER_TX_CHANNEL% -v"
 
 rem Start the followers
-for /L %%i in (1, 1, 2) do (
+for /L %%i in (1, 1, 10) do (
     echo Start convoy follower %%i.
     call :StartFollower %%i
 )

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())

webots/worlds/zumo_with_com_system/BigPlatoonTrack.wbt

@@ -0,0 +1,167 @@
+#VRML_SIM R2023b utf8
+
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
+EXTERNPROTO "../../protos/Zumo32U4.proto"
+EXTERNPROTO "../../protos/ZumoComSystem.proto"
+EXTERNPROTO "../../protos/PlatoonTrack.proto"
+EXTERNPROTO "../../protos/Supervisor.proto"
+EXTERNPROTO "../../protos/Obstacle.proto"
+
+WorldInfo {
+  info [
+    "Platoon Track for platooning using ConvoyLeader and RemoteControl applications."
+  ]
+  title "Platoon Track"
+  basicTimeStep 8
+  contactProperties [
+    ContactProperties {
+      material1 "rubber"
+      material2 "cardboard"
+      coulombFriction [
+        0.65
+      ]
+    }
+  ]
+}
+Viewpoint {
+  orientation -0.7176343920698073 0.00028570577229950487 0.6964199865675955 3.1410420741084026
+  position -3.3187472129446247 -0.8391292754142821 2.997080235514728
+}
+TexturedBackground {
+}
+TexturedBackgroundLight {
+}
+PlatoonTrack {
+  contactMaterial "cardboard"
+}
+Zumo32U4 {
+  translation -3.15 -1.05 0.013994298332013683
+  rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731
+  name "leader"
+  contactMaterial "rubber"
+  zumoComSystemSlot ZumoComSystem {
+    name "ZumoComSystemLeader"
+    performanceMode TRUE
+  }
+  performanceMode TRUE
+}
+Zumo32U4 {
+  translation -3.15 -1.2 0.013994298332013683
+  rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731
+  name "follower_1"
+  contactMaterial "rubber"
+  zumoComSystemSlot ZumoComSystem {
+    name "ZumoComSystemFollower1"
+    performanceMode TRUE
+  }
+  performanceMode TRUE
+}
+Zumo32U4 {
+  translation -3.15 -1.35 0.013994298332013683
+  rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731
+  name "follower_2"
+  contactMaterial "rubber"
+  zumoComSystemSlot ZumoComSystem {
+    name "ZumoComSystemFollower2"
+    performanceMode TRUE
+  }
+  performanceMode TRUE
+}
+Zumo32U4 {
+  translation -3.15 -1.5 0.013994298332013683
+  rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731
+  name "follower_3"
+  contactMaterial "rubber"
+  zumoComSystemSlot ZumoComSystem {
+    name "ZumoComSystemFollower3"
+    performanceMode TRUE
+  }
+  performanceMode TRUE
+}
+Zumo32U4 {
+  translation -3.15 -1.65 0.013994298332013683
+  rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731
+  name "follower_4"
+  contactMaterial "rubber"
+  zumoComSystemSlot ZumoComSystem {
+    name "ZumoComSystemFollower4"
+    performanceMode TRUE
+  }
+  performanceMode TRUE
+}
+Zumo32U4 {
+  translation -3.15 -1.8 0.013994298332013683
+  rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731
+  name "follower_5"
+  contactMaterial "rubber"
+  zumoComSystemSlot ZumoComSystem {
+    name "ZumoComSystemFollower5"
+    performanceMode TRUE
+  }
+  performanceMode TRUE
+}
+Zumo32U4 {
+  translation -3.15 -1.95 0.013994298332013683
+  rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731
+  name "follower_6"
+  contactMaterial "rubber"
+  zumoComSystemSlot ZumoComSystem {
+    name "ZumoComSystemFollower6"
+    performanceMode TRUE
+  }
+  performanceMode TRUE
+}
+Zumo32U4 {
+  translation -3.15 -2.1 0.013994298332013683
+  rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731
+  name "follower_7"
+  contactMaterial "rubber"
+  zumoComSystemSlot ZumoComSystem {
+    name "ZumoComSystemFollower7"
+    performanceMode TRUE
+  }
+  performanceMode TRUE
+}
+Zumo32U4 {
+  translation -3.15 -2.25 0.013994298332013683
+  rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731
+  name "follower_8"
+  contactMaterial "rubber"
+  zumoComSystemSlot ZumoComSystem {
+    name "ZumoComSystemFollower8"
+    performanceMode TRUE
+  }
+  performanceMode TRUE
+}
+Zumo32U4 {
+  translation -3.15 -2.4 0.013994298332013683
+  rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731
+  name "follower_9"
+  contactMaterial "rubber"
+  zumoComSystemSlot ZumoComSystem {
+    name "ZumoComSystemFollower9"
+    performanceMode TRUE
+  }
+  performanceMode TRUE
+}
+Zumo32U4 {
+  translation -3.15 -2.55 0.013994298332013683
+  rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731
+  name "follower_10"
+  contactMaterial "rubber"
+  zumoComSystemSlot ZumoComSystem {
+    name "ZumoComSystemFollower10"
+    performanceMode TRUE
+  }
+  performanceMode TRUE
+}
+Supervisor {
+  name "PlatoonSupervisor"
+  controller "platoon_supervisor"
+  supervisor TRUE
+}
+DEF OBSTACLE Obstacle {
+  translation -3.28 2 0.4
+  name "Wall"
+}