Merge pull request #147 from BlueAndi/feature/RL

Line Following with Agent-Based Decision Making

.github/workflows/main.yml

@@ -198,7 +198,7 @@ jobs:
     needs: intro
     strategy:
       matrix:
-        python-version: ["3.9", "3.10", "3.11"]
+        python-version: ["3.9", "3.10"]
 
     # Steps represent a sequence of tasks that will be executed as part of the job.
     steps:
@@ -209,11 +209,11 @@ jobs:
         uses: actions/setup-python@v5
         with:
           python-version: ${{ matrix.python-version }}
+          cache: 'pip'
+          cache-dependency-path: .github/workflows/requirements.txt
 
       - name: Install dependencies
-        run: |
-          pip install pylint "git+https://github.com/gabryelreyes/SerialMuxProt.git#egg=SerialMuxProt&subdirectory=python/SerialMuxProt"
+        run: pip install -r .github/workflows/requirements.txt
 
       - name: Analysing the code with pylint
-        run: |
-          pylint ./webots/controllers/*/*.py
+        run: pylint ./webots/controllers/*/*.py

.github/workflows/requirements.txt

@@ -0,0 +1,10 @@
+platformio==6.1.15
+numpy==1.26.4
+tensorflow==2.10.0
+tensorflow-probability==0.15.0
+keras==2.10.0
+matplotlib==3.9.0
+pandas==2.2.2
+pytest==8.3.2
+pylint==3.2.7
+git+https://github.com/gabryelreyes/SerialMuxProt.git#egg=SerialMuxProt&subdirectory=python/SerialMuxProt

lib/APPReinforcementLearning/src/App.cpp

@@ -42,6 +42,7 @@
 #include <Odometry.h>
 #include "ReadyState.h"
 #include <Logging.h>
+#include "TrainingState.h"
 
 /******************************************************************************
  * Compiler Switches
@@ -95,6 +96,7 @@ void App::loop()
 {
     Board::getInstance().process();
     Speedometer::getInstance().process();
+    bool  isDataSent = true;
 
     if (true == m_controlInterval.isTimeout())
     {
@@ -123,19 +125,34 @@ void App::loop()
             payload = {SMPChannelPayload::Status::DONE};
         }
 
-        /* Ignoring return value, as error handling is not available. */
-        (void)m_smpServer.sendData(m_serialMuxProtChannelIdStatus, &payload, sizeof(payload));
+        if (false == m_smpServer.sendData(m_serialMuxProtChannelIdStatus, &payload, sizeof(payload)))
+        {
+            /* Failed to send Status data to the Supervisor. Go to error state. */
+            ErrorState::getInstance().setErrorMsg("SD_SF");
+            m_systemStateMachine.setState(&ErrorState::getInstance());
+        }
+        else
+        {
+            m_statusTimer.restart();
+        }
 
-        m_statusTimer.restart();
+
     }
 
     /* Send periodically line sensor data. */
     if (true == m_sendLineSensorsDataInterval.isTimeout() &&
         (&DrivingState::getInstance() == m_systemStateMachine.getState()))
     {
-        sendLineSensorsData();
-
-        m_sendLineSensorsDataInterval.restart();
+        if (false == sendLineSensorsData())
+        {
+            /* Failed to send Sensor data to the Supervisor. Go to error state. */
+            ErrorState::getInstance().setErrorMsg("SEND_SF");
+            m_systemStateMachine.setState(&ErrorState::getInstance());
+        }
+        else
+        {
+            m_sendLineSensorsDataInterval.restart();
+        }
     }
 
     /* Send Mode selected to The Supervisor. */
@@ -148,10 +165,16 @@ void App::loop()
             SMPChannelPayload::Mode payload =
                 (1 == mode_options) ? SMPChannelPayload::Mode::DRIVING_MODE : SMPChannelPayload::Mode::TRAINING_MODE;
 
-            /* Ignoring return value, as error handling is not available. */
-            (void)m_smpServer.sendData(m_serialMuxProtChannelIdMode, &payload, sizeof(payload));
-
-            m_modeSelectionSent = true;
+            if(false == m_smpServer.sendData(m_serialMuxProtChannelIdMode, &payload, sizeof(payload)))
+            {
+                /* Failed to send Mode data to the Supervisor. Go to error state. */
+                ErrorState::getInstance().setErrorMsg("MD_SF");
+                m_systemStateMachine.setState(&ErrorState::getInstance());
+            }
+            else
+            {
+                m_modeSelectionSent = true;
+            }
         }
     }
 
@@ -174,6 +197,13 @@ void App::handleRemoteCommand(const Command& cmd)
         m_modeSelectionSent = false;
         break;
 
+    case SMPChannelPayload::CmdId::CMD_ID_SET_TRAINING_STATE:
+        Odometry::getInstance().clearPosition();
+        Odometry::getInstance().clearMileage();
+
+        m_systemStateMachine.setState(&TrainingState::getInstance());
+        break;
+
     default:
         /* Nothing to do. */
         break;
@@ -188,12 +218,13 @@ void App::handleRemoteCommand(const Command& cmd)
  * Private Methods
  *****************************************************************************/
 
-void App::sendLineSensorsData() const
+bool App::sendLineSensorsData() const 
 {
     ILineSensors&   lineSensors      = Board::getInstance().getLineSensors();
     uint8_t         maxLineSensors   = lineSensors.getNumLineSensors();
     const uint16_t* lineSensorValues = lineSensors.getSensorValues();
     uint8_t         lineSensorIdx    = 0U;
+    bool            isPayloadSent    = true;
     LineSensorData  payload;
 
     if (LINE_SENSOR_CHANNEL_DLC == (maxLineSensors * sizeof(uint16_t)))
@@ -206,8 +237,9 @@ void App::sendLineSensorsData() const
         }
     }
 
-    /* Ignoring return value, as error handling is not available. */
-    (void)m_smpServer.sendData(m_serialMuxProtChannelIdLineSensors, &payload, sizeof(payload));
+    isPayloadSent = m_smpServer.sendData(m_serialMuxProtChannelIdLineSensors, &payload, sizeof(payload));
+
+    return isPayloadSent;
 }
 
 bool App::setupSerialMuxProt()

lib/APPReinforcementLearning/src/App.h

@@ -150,8 +150,10 @@ class App
 
     /**
      * Send line sensors data via SerialMuxProt.
+     * 
+     * @return true if data has been sent, false otherwise.
      */
-    void sendLineSensorsData() const;
+    bool sendLineSensorsData() const;
 
     /**
      * Copy construction of an instance.

lib/APPReinforcementLearning/src/DrivingState.cpp

@@ -79,9 +79,15 @@ void DrivingState::exit()
     m_observationTimer.stop();
 }
 
-void DrivingState::setTargetSpeeds(int16_t leftMotor, int16_t rightMotor)
+void DrivingState::setTargetSpeeds(int16_t leftSpeed, int16_t rightSpeed)
 {
-    DifferentialDrive::getInstance().setLinearSpeed(leftMotor, rightMotor);
+    DifferentialDrive& diffDrive       = DifferentialDrive::getInstance();
+    const int16_t      MAX_MOTOR_SPEED = diffDrive.getMaxMotorSpeed();
+    const int16_t      MIN_MOTOR_SPEED = 0;
+    leftSpeed                          = constrain(leftSpeed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED);
+    rightSpeed                         = constrain(rightSpeed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED);
+
+    DifferentialDrive::getInstance().setLinearSpeed(leftSpeed, rightSpeed);
 }
 
 bool DrivingState::isAbortRequired()

lib/APPReinforcementLearning/src/ReadyState.cpp

@@ -59,13 +59,6 @@
  * Local Variables
  *****************************************************************************/
 
-const uint16_t ReadyState::SENSOR_VALUE_MAX = Board::getInstance().getLineSensors().getSensorValueMax();
-
-/* Initialize the required sensor IDs to be generic. */
-const uint8_t ReadyState::SENSOR_ID_MOST_LEFT  = 0U;
-const uint8_t ReadyState::SENSOR_ID_MIDDLE     = Board::getInstance().getLineSensors().getNumLineSensors() / 2U;
-const uint8_t ReadyState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineSensors().getNumLineSensors() - 1U;
-
 /******************************************************************************
  * Public Methods
  *****************************************************************************/
@@ -87,7 +80,8 @@ void ReadyState::entry()
         display.print(m_lapTime);
         display.print("ms");
     }
-    m_modeTimeoutTimer.start(mode_selected_period);
+    m_stateTransitionTimer.start(STATE_TRANSITION_PERIOD);
+    m_modeTimeoutTimer.start(MODE_SELECTED_PERIOD);
     m_mode                        = IDLE;
     m_isLastStartStopLineDetected = false;
     m_isButtonAPressed            = false;
@@ -99,11 +93,6 @@ void ReadyState::process(StateMachine& sm)
     IButton& buttonA = Board::getInstance().getButtonA();
     IButton& buttonB = Board::getInstance().getButtonB();
 
-    /* Get each sensor value. */
-    ILineSensors&   lineSensors      = Board::getInstance().getLineSensors();
-    uint8_t         maxLineSensors   = lineSensors.getNumLineSensors();
-    const uint16_t* lineSensorValues = lineSensors.getSensorValues();
-
     /* Shall the driving mode be released? */
     if (true == Util::isButtonTriggered(buttonA, m_isButtonAPressed))
     {
@@ -132,16 +121,11 @@ void ReadyState::process(StateMachine& sm)
         ;
     }
 
-    /**Drive forward until START LINE is crossed */
-    driveUntilStartLineisCrossed();
-
-    if (false == (isStartStopLineDetected(lineSensorValues, maxLineSensors)) && (true == m_isLastStartStopLineDetected))
+    /* Set DrivingState */
+    if (true == m_stateTransitionTimer.isTimeout())
     {
         sm.setState(&DrivingState::getInstance());
-    }
-    else
-    {
-        m_isLastStartStopLineDetected = isStartStopLineDetected(lineSensorValues, maxLineSensors);
+        m_stateTransitionTimer.restart();
     }
 }
 
@@ -156,7 +140,7 @@ void ReadyState::setLapTime(uint32_t lapTime)
     m_lapTime            = lapTime;
 }
 
-uint8_t ReadyState::getSelectedMode()
+ReadyState::Mode ReadyState::getSelectedMode()
 {
     return m_mode;
 }
@@ -174,45 +158,13 @@ ReadyState::ReadyState() :
     m_isButtonAPressed(false),
     m_isButtonBPressed(false),
     m_modeTimeoutTimer(),
+    m_stateTransitionTimer(),
     m_lapTime(0),
     m_isLastStartStopLineDetected(false),
     m_mode(IDLE)
 {
 }
 
-/**Drive forward until START LINE is crossed */
-void ReadyState::driveUntilStartLineisCrossed()
-{
-    DifferentialDrive& diffDrive  = DifferentialDrive::getInstance();
-    int16_t            top_speed  = 2000;           /* Set a top speed of 2000 */
-    int16_t            leftMotor  = top_speed / 2U; /* Drive at half speed */
-    int16_t            rightMotor = top_speed / 2U; /* Drive at half speed */
-    diffDrive.setLinearSpeed(leftMotor, rightMotor);
-}
-
-bool ReadyState::isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const
-{
-    bool           isDetected  = false;
-    const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */
-    const uint32_t LINE_MAX_70 = (SENSOR_VALUE_MAX * 7U) / 10U; /* 70 % of max. value */
-
-    /*
-     * ===     =     ===
-     *   +   + + +   +
-     *   L     M     R
-     */
-    if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_LEFT]) &&
-        (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE - 1U]) &&
-        (LINE_MAX_70 <= lineSensorValues[SENSOR_ID_MIDDLE]) &&
-        (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE + 1U]) &&
-        (LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_RIGHT]))
-    {
-        isDetected = true;
-    }
-
-    return isDetected;
-}
-
 /******************************************************************************
  * External Functions
  *****************************************************************************/

lib/APPReinforcementLearning/src/ReadyState.h

@@ -59,6 +59,16 @@
 class ReadyState : public IState
 {
 public:
+    /**
+     * The mode that can be selected.
+     */
+    enum Mode: uint8_t
+    {
+        IDLE = 0,     /**< No mode has been selected*/
+        DRIVING_MODE, /**< Driving mode Selected. */
+        TRAINING_MODE /**< Training mode Selected. */
+    };
+
     /**
      * Get state instance.
      *
@@ -100,24 +110,18 @@ class ReadyState : public IState
     /**
      *  Set the selected mode.
      *
-     * @return It returns 1 if DrivingMode is selected or 2 if TrainingMode is selected.
+     * @return It returns the selected Mode.
      */
-    uint8_t getSelectedMode();
+    Mode getSelectedMode();
 
 protected:
 private:
-    /**
-     * The mode that can be selected.
-     */
-    enum Mode
-    {
-        IDLE = 0,     /**< No mode has been selected*/
-        DRIVING_MODE, /**< Driving mode Selected. */
-        TRAINING_MODE /**< Training mode Selected. */
-    };
 
     /** Duration of the selected mode in ms. This is the maximum time to select a mode. */
-    static const uint32_t mode_selected_period = 200U;
+    static const uint32_t MODE_SELECTED_PERIOD = 2000U;
+
+    /** Duration to handle State changes in ms. */
+    static const uint32_t STATE_TRANSITION_PERIOD = 2100U;
 
     /**
      * The line sensor threshold (normalized) used to detect the track.
@@ -153,7 +157,7 @@ class ReadyState : public IState
     uint32_t    m_lapTime;                     /**< Lap time in ms of the last successful driven round. */
     bool        m_isButtonAPressed;            /**< Is the button A pressed (last time)? */
     bool        m_isButtonBPressed;            /**< Is the button B pressed (last time)? */
-
+    SimpleTimer m_stateTransitionTimer;        /**< Timer to handle State changes */
     /**
      * Default constructor.
      */
@@ -183,22 +187,6 @@ class ReadyState : public IState
      * @returns Reference to ErrorState instance.
      */
     ReadyState& operator=(const ReadyState& state);
-
-    /*
-     * The robot moves from its current position
-     * until it crosses and leaves the start line.
-     */
-    void driveUntilStartLineisCrossed();
-
-    /**
-     * Is the start/stop line detected?
-     *
-     * @param[in] lineSensorValues  The line sensor values as array.
-     * @param[in] length            The number of line sensor values.
-     *
-     * @return If start/stop line detected, it will return true otherwise false.
-     */
-    bool isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const;
 };
 
 /******************************************************************************

lib/APPReinforcementLearning/src/SerialMuxchannels.h

@@ -89,8 +89,9 @@ namespace SMPChannelPayload
     /** Remote control commands. */
     typedef enum : uint8_t
     {
-        CMD_ID_IDLE = 0,       /**< Nothing to do. */
-        CMD_ID_SET_READY_STATE /**< Set Ready State. */
+        CMD_ID_IDLE = 0,          /**< Nothing to do. */
+        CMD_ID_SET_READY_STATE,   /**< Set Ready State. */
+        CMD_ID_SET_TRAINING_STATE /**< Set Training State */
 
     } CmdId; /**< Command ID */