Implementation of data exchange mechanisms between robot and supervisor using Serial Webots drivers

.github/workflows/main.yml

@@ -212,7 +212,7 @@ jobs:
 
       - name: Install dependencies
         run: |
-          pip install pylint
+          pip install pylint "git+https://github.com/gabryelreyes/SerialMuxProt.git#egg=SerialMuxProt&subdirectory=python/SerialMuxProt"
 
       - name: Analysing the code with pylint
         run: |

README.md

@@ -13,7 +13,7 @@ Several kind of exclusive applications are available:
 * Line Follower - Just a line follower, using a PID controller.
 * Remote Control - The robot is remote controlled by e.g. the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy follower role.
 * Sensor Fusion - The robot provides odometry and inertial data to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip), which calculates the sensor fusion based location information.
-
+* Line Follower with Reinforcement Learning - A line follower with reinforcement learning uses a learning agent that uses rewards and punishments to steer optimally in order to follow a line autonomously.
 ## Table of content
 
 * [The robot](#the-robot)
@@ -158,6 +158,8 @@ Example for the **LineFollowerTarget** application:
 | LineFollowerSimple | Just a simple line follower, using a PID controller. | Yes | No | ./webots/worlds/ETrack.wbt  ./webots/worlds/LargeTrack.wbt  ./webots/worlds/LineFollowerTrack.wbt |
 | RemoteControl | The robot is remote controlled by e.g. the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy follower role. | No | Yes | ./webots/world/zumo_with_com_system/* |
 | SensorFusion | The robot provides odometry and inertial data to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip), which calculates the sensor fusion based location information. | No | Yes | ./webots/worlds/zumo_with_com_system/LineFollowerTrack.wbt |
+| ReinforcementLearning | A line follower with reinforcement learning uses a learning agent that uses rewards and punishments to steer optimally in order to follow a line autonomously. 
+| Yes | No | ./webots/worlds/RL_LineFollower.wbt |
 | Test | Only for testing purposes on native environment. | Yes | No | N/A |
 
 # Documentation

lib/APPReinforcementLearning/src/App.cpp

@@ -26,7 +26,7 @@
 *******************************************************************************/
 /**
  * @brief  LineFollower application with Reinforcement Learning
- * @author Akram Bziouech 
+ * @author Akram Bziouech
  */
 
 /******************************************************************************
@@ -55,7 +55,6 @@
  * Types and classes
  *****************************************************************************/
 
-
 /******************************************************************************
  * Prototypes
  *****************************************************************************/
@@ -75,15 +74,15 @@ void App::setup()
 {
     Serial.begin(SERIAL_BAUDRATE);
     /* Initialize HAL */
-    Board::getInstance().init(); 
+    Board::getInstance().init();
     Logging::disable();
 
     if (false == setupSerialMuxProt())
     {
         ErrorState::getInstance().setErrorMsg("SMP=0");
         m_systemStateMachine.setState(&ErrorState::getInstance());
     }
-    else 
+    else
     {
         m_statusTimer.start(SEND_STATUS_TIMER_INTERVAL);
         m_sendLineSensorsDataInterval.start(SEND_LINE_SENSORS_DATA_PERIOD);
@@ -113,8 +112,9 @@ void App::loop()
 
         m_controlInterval.restart();
     }
-
-    if ((true == m_statusTimer.isTimeout()) && (true == m_smpServer.isSynced())&& (&DrivingState::getInstance() == m_systemStateMachine.getState()))
+
+    if ((true == m_statusTimer.isTimeout()) && (true == m_smpServer.isSynced()) &&
+        (&DrivingState::getInstance() == m_systemStateMachine.getState()))
     {
         Status payload = {SMPChannelPayload::Status::NOT_DONE};
 
@@ -128,9 +128,10 @@ void App::loop()
 
         m_statusTimer.restart();
     }
-    
+
     /* Send periodically line sensor data. */
-    if (true == m_sendLineSensorsDataInterval.isTimeout() && (&DrivingState::getInstance() == m_systemStateMachine.getState()) )
+    if (true == m_sendLineSensorsDataInterval.isTimeout() &&
+        (&DrivingState::getInstance() == m_systemStateMachine.getState()))
     {
         sendLineSensorsData();
 
@@ -139,12 +140,13 @@ void App::loop()
 
     /* Send Mode selected to The Supervisor. */
     if (&ReadyState::getInstance() == m_systemStateMachine.getState() && (!m_modeSelectionSent))
-    {    
+    {
         uint8_t mode_options = ReadyState::getInstance().setSelectedMode();
 
-        if(mode_options > 0)
+        if (mode_options > 0)
         {
-            SMPChannelPayload::Mode payload = (mode_options == 1) ? SMPChannelPayload::Mode::DRIVING_MODE : SMPChannelPayload::Mode::TRAINING_MODE;
+            SMPChannelPayload::Mode payload =
+                (mode_options == 1) ? 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));
@@ -153,7 +155,6 @@ void App::loop()
         }
     }
 
-
     m_smpServer.process(millis());
 
     m_systemStateMachine.process();
@@ -215,16 +216,15 @@ bool App::setupSerialMuxProt()
 
     /* Channel subscription. */
     m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedSetpointsChannelCallback);
-    m_smpServer.subscribeToChannel(COMMAND_CHANNEL_NAME,App_cmdChannelCallback);
+    m_smpServer.subscribeToChannel(COMMAND_CHANNEL_NAME, App_cmdChannelCallback);
 
     /* Channel creation. */
-    m_serialMuxProtChannelIdStatus = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC);
-    m_serialMuxProtChannelIdLineSensors = 
-        m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC);
-    m_serialMuxProtChannelIdMode = m_smpServer.createChannel(MODE_CHANNEL_NAME, MODE_CHANNEL_DLC);
+    m_serialMuxProtChannelIdStatus      = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC);
+    m_serialMuxProtChannelIdLineSensors = m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC);
+    m_serialMuxProtChannelIdMode        = m_smpServer.createChannel(MODE_CHANNEL_NAME, MODE_CHANNEL_DLC);
 
     /* Channels succesfully created? */
-    if ((0U != m_serialMuxProtChannelIdStatus) && (0U != m_serialMuxProtChannelIdLineSensors) && 
+    if ((0U != m_serialMuxProtChannelIdStatus) && (0U != m_serialMuxProtChannelIdLineSensors) &&
         (0U != m_serialMuxProtChannelIdMode))
     {
         isSuccessful = true;

lib/APPReinforcementLearning/src/App.h

@@ -57,14 +57,14 @@
  * Types and Classes
  *****************************************************************************/
 
-/** The line follower application. */
+/** The line follower with reinforcement learning application. */
 class App
 {
 public:
     /**
-     * Construct the line follower application.
+     * Construct the line follower with reinforcement learning application.
      */
-    App() : 
+    App() :
         m_systemStateMachine(),
         m_controlInterval(),
         m_serialMuxProtChannelIdStatus(0U),
@@ -78,7 +78,7 @@ class App
     }
 
     /**
-     * Destroy the line follower application.
+     * Destroy the line follower with reinforcement learning application.
      */
     ~App()
     {
@@ -138,16 +138,16 @@ class App
     /** SerialMuxProt Server Instance. */
     SMPServer m_smpServer;
 
-    /* Ensue that the mode is only sent once*/
-    bool m_modeSelectionSent; 
+    /** Ensue that the mode is only sent once */
+    bool m_modeSelectionSent;
 
     /**
      * Setup the SerialMuxProt channels.
      *
      * @return If successful returns true, otherwise false.
      */
     bool setupSerialMuxProt();
-    
+
     /**
      * Send line sensors data via SerialMuxProt.
      */

lib/APPReinforcementLearning/src/DrivingState.cpp

@@ -26,7 +26,7 @@
 *******************************************************************************/
 /**
  * @brief  Driving state
- * @author Akram Bziouech 
+ * @author Akram Bziouech
  */
 
 /******************************************************************************

lib/APPReinforcementLearning/src/DrivingState.h

@@ -26,7 +26,7 @@
 *******************************************************************************/
 /**
  * @brief  Driving state
- * @author @author Akram Bziouech
+ * @author Akram Bziouech
  *
  * @addtogroup Application
  *
@@ -102,14 +102,14 @@ class DrivingState : public IState
      * Check the abort conditions while driving the challenge.
      *
      * @return If abort is required, it will return true otherwise false.
-    */
+     */
     bool isAbortRequired();
 
 protected:
 private:
     /** Observation duration in ms. This is the max. time within the robot must be finished its drive. */
     static const uint32_t OBSERVATION_DURATION = 3000000;
-    
+
     SimpleTimer m_observationTimer; /**< Observation timer to observe the max. time per challenge. */
 
     /**

lib/APPReinforcementLearning/src/ErrorState.cpp

@@ -65,8 +65,10 @@
 
 void ErrorState::entry()
 {
-    IDisplay& display = Board::getInstance().getDisplay();
-
+    IBoard&   board   = Board::getInstance();
+    IDisplay& display = board.getDisplay();
+
+    /* Stop the motors in any case! */
     DifferentialDrive::getInstance().disable();
 
     display.clear();
@@ -81,7 +83,6 @@ void ErrorState::entry()
     {
         display.print(m_errorMsg);
     }
-
 }
 
 void ErrorState::process(StateMachine& sm)

lib/APPReinforcementLearning/src/IBoard.h

@@ -27,7 +27,7 @@
 /**
  * @brief  Board interface, which abstracts the physical board
  * @author Andreas Merkle <[email protected]>
- * 
+ *
  * @addtogroup HALInterfaces
  *
  * @{
@@ -71,7 +71,6 @@
 class IBoard
 {
 public:
-
     /**
      * Destroys the board interface.
      */
@@ -121,7 +120,7 @@ class IBoard
 
     /**
      * Get encoders driver.
-     * 
+     *
      * @return Encoders driver.
      */
     virtual IEncoders& getEncoders() = 0;
@@ -163,7 +162,7 @@ class IBoard
 
     /**
      * Get the settings.
-     * 
+     *
      * @return Settings
      */
     virtual ISettings& getSettings() = 0;
@@ -183,9 +182,8 @@ class IBoard
      * Process actuators and sensors.
      */
     virtual void process() = 0;
-
-protected:
 
+protected:
     /**
      * Constructs the board interface.
      */
@@ -194,7 +192,6 @@ class IBoard
     }
 
 private:
-
 };
 
 /******************************************************************************

lib/APPReinforcementLearning/src/ReadyState.cpp

@@ -26,7 +26,7 @@
 *******************************************************************************/
 /**
  * @brief  Ready state
- * @author Akram Bziouech 
+ * @author Akram Bziouech
  */
 
 /******************************************************************************
@@ -72,7 +72,7 @@ const uint8_t ReadyState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineSen
 
 void ReadyState::entry()
 {
-    IDisplay&  display   = Board::getInstance().getDisplay();
+    IDisplay& display = Board::getInstance().getDisplay();
     display.clear();
     display.print("A: TMD");
     display.gotoXY(0, 1);
@@ -82,17 +82,16 @@ void ReadyState::entry()
     diffDrive.setLinearSpeed(0, 0);
 
     if (true == m_isLapTimeAvailable)
-    {   
+    {
         display.gotoXY(0, 2);
         display.print(m_lapTime);
         display.print("ms");
     }
     m_modeTimeoutTimer.start(mode_selected_period);
-    m_mode = IDLE;
+    m_mode                        = IDLE;
     m_isLastStartStopLineDetected = false;
-    m_isButtonAPressed = false;
-    m_isButtonBPressed = false;
-
+    m_isButtonAPressed            = false;
+    m_isButtonBPressed            = false;
 }
 
 void ReadyState::process(StateMachine& sm)
@@ -108,12 +107,12 @@ void ReadyState::process(StateMachine& sm)
     /* Shall the driving mode be released? */
     if (true == Util::isButtonTriggered(buttonA, m_isButtonAPressed))
     {
-        m_mode = DRIVING_MODE; 
+        m_mode = DRIVING_MODE;
     }
     /* Shall the Training mode be released? */
     else if (true == Util::isButtonTriggered(buttonB, m_isButtonBPressed))
-    {   
-        m_mode = TRAINING_MODE;  
+    {
+        m_mode = TRAINING_MODE;
     }
     else if (true == m_modeTimeoutTimer.isTimeout() && (m_mode == IDLE))
     {
@@ -163,24 +162,24 @@ uint8_t ReadyState::setSelectedMode()
  * Private Methods
  *****************************************************************************/
 
-ReadyState::ReadyState() : 
-    m_isLapTimeAvailable(false), 
+ReadyState::ReadyState() :
+    m_isLapTimeAvailable(false),
     m_isButtonAPressed(false),
     m_isButtonBPressed(false),
     m_modeTimeoutTimer(),
     m_lapTime(0),
     m_isLastStartStopLineDetected(false),
-    m_mode(IDLE)   
+    m_mode(IDLE)
 {
 }
 
 /**Drive forward until START LINE is crossed */
-void ReadyState :: DriveUntilStartLineisCrossed()
+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 */
+    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);
 }