From 6c42756a98927a7ad59a5a6006e1fe5697a6eeb4 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 17 Jan 2024 10:02:25 +0100 Subject: [PATCH 01/36] Separated setup blocks into methods for better readability. --- lib/ConvoyLeader/src/App.cpp | 190 ++++++++++++++++++++--------------- lib/ConvoyLeader/src/App.h | 20 +++- 2 files changed, 128 insertions(+), 82 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index 538c03b1..f8192db5 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -153,88 +153,26 @@ void App::setup() { LOG_FATAL("Network configuration could not be set."); } + else if (false == setupMqttClient()) + { + LOG_FATAL("Failed to setup MQTT client."); + } + else if (PLATOON_LEADER_ID != settings.getPlatoonVehicleId()) + { + /* Correct config.json file loaded? */ + LOG_FATAL("Platoon Vehicle ID must be 0 for the leader."); + } + else if (false == m_v2vClient.init(settings.getPlatoonPlatoonId(), settings.getPlatoonVehicleId())) + { + LOG_FATAL("Failed to initialize V2V client."); + } + else if (false == setupSerialMuxProt()) + { + LOG_FATAL("Failed to setup SerialMuxProt."); + } else { - /* Setup MQTT Server, Birth and Will messages. */ - StaticJsonDocument birthDoc; - char birthMsgArray[JSON_BIRTHMESSAGE_MAX_SIZE]; - String birthMessage; - - birthDoc["name"] = settings.getRobotName().c_str(); - (void)serializeJson(birthDoc, birthMsgArray); - birthMessage = birthMsgArray; - - if (false == m_mqttClient.init()) - { - LOG_FATAL("Failed to initialize MQTT client."); - } - else - { - MqttSettings mqttSettings = {settings.getRobotName(), - settings.getMqttBrokerAddress(), - settings.getMqttPort(), - TOPIC_NAME_BIRTH, - birthMessage, - TOPIC_NAME_WILL, - birthMessage, - true}; - - if (false == m_mqttClient.setConfig(mqttSettings)) - { - LOG_FATAL("MQTT configuration could not be set."); - } - else if (PLATOON_LEADER_ID != settings.getPlatoonVehicleId()) - { - /* Correct config.json file loaded? */ - LOG_FATAL("Platoon Vehicle ID must be 0 for the leader."); - } - else if (false == m_v2vClient.init(settings.getPlatoonPlatoonId(), settings.getPlatoonVehicleId())) - { - LOG_FATAL("Failed to initialize V2V client."); - } - else - { - /* Setup SerialMuxProt Channels */ - m_smpServer.subscribeToChannel(CURRENT_VEHICLE_DATA_CHANNEL_DLC_CHANNEL_NAME, - App_currentVehicleChannelCallback); - m_serialMuxProtChannelIdMotorSpeedSetpoints = - m_smpServer.createChannel(SPEED_SETPOINT_CHANNEL_NAME, SPEED_SETPOINT_CHANNEL_DLC); - - if (0U == m_serialMuxProtChannelIdMotorSpeedSetpoints) - { - LOG_FATAL("Could not create SerialMuxProt Channel: %s.", SPEED_SETPOINT_CHANNEL_NAME); - } - else - { - - ProcessingChainFactory& processingChainFactory = ProcessingChainFactory::getInstance(); - - processingChainFactory.registerLongitudinalControllerCreateFunc(LongitudinalController::create); - processingChainFactory.registerLongitudinalSafetyPolicyCreateFunc( - LongitudinalSafetyPolicy::create); - processingChainFactory.registerLateralControllerCreateFunc(LateralController::create); - processingChainFactory.registerLateralSafetyPolicyCreateFunc(LateralSafetyPolicy::create); - - PlatoonController::InputWaypointCallback lambdaInputWaypointCallback = - [this](Waypoint& waypoint) { return this->inputWaypointCallback(waypoint); }; - PlatoonController::OutputWaypointCallback lambdaOutputWaypointCallback = - [this](const Waypoint& waypoint) { return this->outputWaypointCallback(waypoint); }; - PlatoonController::MotorSetpointCallback lambdaMotorSetpointCallback = - [this](const int16_t left, const int16_t right) - { return this->motorSetpointCallback(left, right); }; - - if (false == m_platoonController.init(lambdaInputWaypointCallback, lambdaOutputWaypointCallback, - lambdaMotorSetpointCallback)) - { - LOG_FATAL("Could not initialize Platoon Controller."); - } - else - { - isSuccessful = true; - } - } - } - } + isSuccessful = setupPlatoonController(); } } @@ -326,6 +264,98 @@ void App::fatalErrorHandler() } } +bool App::setupMqttClient() +{ + /* Setup MQTT Server, Birth and Will messages. */ + bool isSuccessful = false; + SettingsHandler& settings = SettingsHandler::getInstance(); + StaticJsonDocument birthDoc; + char birthMsgArray[JSON_BIRTHMESSAGE_MAX_SIZE]; + String birthMessage; + + birthDoc["name"] = settings.getRobotName(); + (void)serializeJson(birthDoc, birthMsgArray); + birthMessage = birthMsgArray; + + MqttSettings mqttSettings = {settings.getRobotName(), + settings.getMqttBrokerAddress(), + settings.getMqttPort(), + TOPIC_NAME_BIRTH, + birthMessage, + TOPIC_NAME_WILL, + birthMessage, + true}; + + if (false == m_mqttClient.init()) + { + LOG_FATAL("Failed to initialize MQTT client."); + } + else if (false == m_mqttClient.setConfig(mqttSettings)) + { + LOG_FATAL("MQTT configuration could not be set."); + } + else + { + isSuccessful = true; + } + + return isSuccessful; +} + +bool App::setupSerialMuxProt() +{ + bool isSuccessful = false; + + /* Channel subscription. */ + m_smpServer.subscribeToChannel(CURRENT_VEHICLE_DATA_CHANNEL_DLC_CHANNEL_NAME, App_currentVehicleChannelCallback); + + /* Channel creation. */ + m_serialMuxProtChannelIdMotorSpeedSetpoints = + m_smpServer.createChannel(SPEED_SETPOINT_CHANNEL_NAME, SPEED_SETPOINT_CHANNEL_DLC); + + if (0U == m_serialMuxProtChannelIdMotorSpeedSetpoints) + { + LOG_FATAL("Could not create SerialMuxProt Channel: %s.", SPEED_SETPOINT_CHANNEL_NAME); + } + else + { + isSuccessful = true; + } + + return isSuccessful; +} + +bool App::setupPlatoonController() +{ + bool isSuccessful = false; + + ProcessingChainFactory& processingChainFactory = ProcessingChainFactory::getInstance(); + + PlatoonController::InputWaypointCallback lambdaInputWaypointCallback = [this](Waypoint& waypoint) + { return this->inputWaypointCallback(waypoint); }; + PlatoonController::OutputWaypointCallback lambdaOutputWaypointCallback = [this](const Waypoint& waypoint) + { return this->outputWaypointCallback(waypoint); }; + PlatoonController::MotorSetpointCallback lambdaMotorSetpointCallback = + [this](const int16_t left, const int16_t right) { return this->motorSetpointCallback(left, right); }; + + processingChainFactory.registerLongitudinalControllerCreateFunc(LongitudinalController::create); + processingChainFactory.registerLongitudinalSafetyPolicyCreateFunc(LongitudinalSafetyPolicy::create); + processingChainFactory.registerLateralControllerCreateFunc(LateralController::create); + processingChainFactory.registerLateralSafetyPolicyCreateFunc(LateralSafetyPolicy::create); + + if (false == m_platoonController.init(lambdaInputWaypointCallback, lambdaOutputWaypointCallback, + lambdaMotorSetpointCallback)) + { + LOG_FATAL("Could not initialize Platoon Controller."); + } + else + { + isSuccessful = true; + } + + return isSuccessful; +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index 0d0f8770..23769e75 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -172,9 +172,25 @@ class App void fatalErrorHandler(); /** - * Send speed setpoints using SerialMuxProt. + * Setup the MQTT client. + * + * @return If successful returns true, otherwise false. + */ + bool setupMqttClient(); + + /** + * Setup the SerialMuxProt channels. + * + * @return If successful returns true, otherwise false. + */ + bool setupSerialMuxProt(); + + /** + * Setup the platoon controller. + * + * @return If successful returns true, otherwise false. */ - void sendSpeedSetpoints(); + bool setupPlatoonController(); private: /* Not allowed. */ From 242099e08d2e9829db723d5d236075b46e89f9b2 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 17 Jan 2024 14:20:12 +0100 Subject: [PATCH 02/36] Removed all unnecessary components --- lib/ConvoyLeader/src/App.cpp | 71 +---------- lib/ConvoyLeader/src/App.h | 45 ------- lib/ConvoyLeader/src/LateralController.cpp | 100 --------------- lib/ConvoyLeader/src/LateralController.h | 106 ---------------- lib/ConvoyLeader/src/LateralSafetyPolicy.cpp | 95 -------------- lib/ConvoyLeader/src/LateralSafetyPolicy.h | 99 --------------- .../src/LongitudinalController.cpp | 106 ---------------- lib/ConvoyLeader/src/LongitudinalController.h | 117 ------------------ .../src/LongitudinalSafetyPolicy.cpp | 100 --------------- .../src/LongitudinalSafetyPolicy.h | 106 ---------------- 10 files changed, 2 insertions(+), 943 deletions(-) delete mode 100644 lib/ConvoyLeader/src/LateralController.cpp delete mode 100644 lib/ConvoyLeader/src/LateralController.h delete mode 100644 lib/ConvoyLeader/src/LateralSafetyPolicy.cpp delete mode 100644 lib/ConvoyLeader/src/LateralSafetyPolicy.h delete mode 100644 lib/ConvoyLeader/src/LongitudinalController.cpp delete mode 100644 lib/ConvoyLeader/src/LongitudinalController.h delete mode 100644 lib/ConvoyLeader/src/LongitudinalSafetyPolicy.cpp delete mode 100644 lib/ConvoyLeader/src/LongitudinalSafetyPolicy.h diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index f8192db5..f4d8fdb2 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -40,11 +40,6 @@ #include #include #include -#include -#include "LongitudinalController.h" -#include "LongitudinalSafetyPolicy.h" -#include "LateralController.h" -#include "LateralSafetyPolicy.h" /****************************************************************************** * Compiler Switches @@ -172,7 +167,7 @@ void App::setup() } else { - isSuccessful = setupPlatoonController(); + isSuccessful = true; } } @@ -207,42 +202,11 @@ void App::loop() /* Process V2V Communication */ m_v2vClient.process(); - - /* Process Platoon Controller */ - m_platoonController.process(m_v2vClient.getWaypointQueueSize()); } void App::currentVehicleChannelCallback(const VehicleData& vehicleData) { - Waypoint vehicleDataAsWaypoint; - - vehicleDataAsWaypoint.xPos = vehicleData.xPos; - vehicleDataAsWaypoint.yPos = vehicleData.yPos; - vehicleDataAsWaypoint.orientation = vehicleData.orientation; - vehicleDataAsWaypoint.left = vehicleData.left; - vehicleDataAsWaypoint.right = vehicleData.right; - vehicleDataAsWaypoint.center = vehicleData.center; - - m_platoonController.setLatestVehicleData(vehicleDataAsWaypoint); -} - -bool App::inputWaypointCallback(Waypoint& waypoint) -{ - return m_v2vClient.getNextWaypoint(waypoint); -} - -bool App::outputWaypointCallback(const Waypoint& waypoint) -{ - return m_v2vClient.sendWaypoint(waypoint); -} - -bool App::motorSetpointCallback(const int16_t left, const int16_t right) -{ - SpeedData payload; - payload.left = left; - payload.right = right; - - return m_smpServer.sendData(m_serialMuxProtChannelIdMotorSpeedSetpoints, &payload, sizeof(payload)); + UTIL_NOT_USED(vehicleData); } /****************************************************************************** @@ -325,37 +289,6 @@ bool App::setupSerialMuxProt() return isSuccessful; } -bool App::setupPlatoonController() -{ - bool isSuccessful = false; - - ProcessingChainFactory& processingChainFactory = ProcessingChainFactory::getInstance(); - - PlatoonController::InputWaypointCallback lambdaInputWaypointCallback = [this](Waypoint& waypoint) - { return this->inputWaypointCallback(waypoint); }; - PlatoonController::OutputWaypointCallback lambdaOutputWaypointCallback = [this](const Waypoint& waypoint) - { return this->outputWaypointCallback(waypoint); }; - PlatoonController::MotorSetpointCallback lambdaMotorSetpointCallback = - [this](const int16_t left, const int16_t right) { return this->motorSetpointCallback(left, right); }; - - processingChainFactory.registerLongitudinalControllerCreateFunc(LongitudinalController::create); - processingChainFactory.registerLongitudinalSafetyPolicyCreateFunc(LongitudinalSafetyPolicy::create); - processingChainFactory.registerLateralControllerCreateFunc(LateralController::create); - processingChainFactory.registerLateralSafetyPolicyCreateFunc(LateralSafetyPolicy::create); - - if (false == m_platoonController.init(lambdaInputWaypointCallback, lambdaOutputWaypointCallback, - lambdaMotorSetpointCallback)) - { - LOG_FATAL("Could not initialize Platoon Controller."); - } - else - { - isSuccessful = true; - } - - return isSuccessful; -} - /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index 23769e75..ead3b9fe 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -48,7 +48,6 @@ #include #include #include "SerialMuxChannels.h" -#include #include /****************************************************************************** @@ -70,7 +69,6 @@ class App m_smpServer(Board::getInstance().getDevice().getStream(), this), m_serialMuxProtChannelIdMotorSpeedSetpoints(0U), m_mqttClient(), - m_platoonController(), m_v2vClient(m_mqttClient) { } @@ -99,37 +97,6 @@ class App */ void currentVehicleChannelCallback(const VehicleData& vehicleData); - /** - * Input waypoint callback. - * Called in order to get the next waypoint into the platoon controller. - * - * @param[out] waypoint Next waypoint. - * - * @return If a waypoint is available, it returns true. Otherwise, false. - */ - bool inputWaypointCallback(Waypoint& waypoint); - - /** - * Output waypoint callback. - * Called in order to send the last waypoint to the next platoon participant. - * - * @param[in] waypoint Last waypoint. - * - * @return If the waypoint was sent successfully, returns true. Otherwise, false. - */ - bool outputWaypointCallback(const Waypoint& waypoint); - - /** - * Motor setpoint callback. - * Called in order to send the motor speeds using SerialMuxProt to the robot. - * - * @param[in] left Left motor speed [steps/s]. - * @param[in] right Right motor speed [steps/s]. - * - * @return If the motor speeds were sent successfully, returns true. Otherwise, false. - */ - bool motorSetpointCallback(const int16_t left, const int16_t right); - private: /** Minimum battery level in percent. */ static const uint8_t MIN_BATTERY_LEVEL = 10U; @@ -160,11 +127,6 @@ class App */ V2VClient m_v2vClient; - /** - * Platoon controller instance. - */ - PlatoonController m_platoonController; - private: /** * Handler of fatal errors in the Application. @@ -185,13 +147,6 @@ class App */ bool setupSerialMuxProt(); - /** - * Setup the platoon controller. - * - * @return If successful returns true, otherwise false. - */ - bool setupPlatoonController(); - private: /* Not allowed. */ App(const App& app); /**< Copy construction of an instance. */ diff --git a/lib/ConvoyLeader/src/LateralController.cpp b/lib/ConvoyLeader/src/LateralController.cpp deleted file mode 100644 index 039d1a47..00000000 --- a/lib/ConvoyLeader/src/LateralController.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Concrete Lateral Controller - * @author Gabryel Reyes - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include "LateralController.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -LateralController::LateralController() : ILateralController(), m_headingFinder() -{ -} - -LateralController::~LateralController() -{ -} - -bool LateralController::calculateLateralMovement(const Waypoint& currentWaypoint, const Waypoint& targetWaypoint, - const int16_t centerSpeedSetpoint, int16_t& leftMotorSpeedSetpoint, - int16_t& rightMotorSpeedSetpoint) -{ - bool isSuccessful = true; - - m_headingFinder.setOdometryData(currentWaypoint.xPos, currentWaypoint.yPos, currentWaypoint.orientation); - m_headingFinder.setMotorSpeedData(centerSpeedSetpoint, centerSpeedSetpoint); - m_headingFinder.setTargetHeading(targetWaypoint.xPos, targetWaypoint.yPos); - - m_headingFinder.process(leftMotorSpeedSetpoint, rightMotorSpeedSetpoint); - - return isSuccessful; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/ConvoyLeader/src/LateralController.h b/lib/ConvoyLeader/src/LateralController.h deleted file mode 100644 index feba9f4c..00000000 --- a/lib/ConvoyLeader/src/LateralController.h +++ /dev/null @@ -1,106 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Concrete LateralController. - * @author Gabryel Reyes - * - * @addtogroup Application - * - * @{ - */ -#ifndef LATERAL_CONTROLLER_H -#define LATERAL_CONTROLLER_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Concrete LateralController. */ -class LateralController : public ILateralController -{ -public: - /** - * Lateral Controller constructor. - */ - LateralController(); - - /** - * Lateral Controller destructor. - */ - virtual ~LateralController(); - - /** - * Creates a LateralController instance, for registering in the ProcessingChainFactory. - * - * @return If successful, returns a pointer to the LateralController instance. Otherwise nullptr. - */ - static ILateralController* create() - { - return new (std::nothrow) LateralController(); - } - - /** - * Calculates the motor speeds for the next step. - * - * @param[in] currentWaypoint Current waypoint where the vehicle is found. - * @param[in] targetWaypoint Target waypoint to drive to. - * @param[in] centerSpeedSetpoint Center speed setpoint [steps/s] calculated by longitudinal controller. - * @param[out] leftMotorSpeedSetpoint Left motor speed setpoint [steps/s]. - * @param[out] rightMotorSpeedSetpoint Right motor speed setpoint [steps/s]. - * - * @return If successful, returns true otherwise false. - */ - bool calculateLateralMovement(const Waypoint& currentWaypoint, const Waypoint& targetWaypoint, - const int16_t centerSpeedSetpoint, int16_t& leftMotorSpeedSetpoint, - int16_t& rightMotorSpeedSetpoint) final; - -private: - /** Heading finder. */ - HeadingFinder m_headingFinder; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LATERAL_CONTROLLER_H */ -/** @} */ diff --git a/lib/ConvoyLeader/src/LateralSafetyPolicy.cpp b/lib/ConvoyLeader/src/LateralSafetyPolicy.cpp deleted file mode 100644 index cd58a457..00000000 --- a/lib/ConvoyLeader/src/LateralSafetyPolicy.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Concrete LateralSafetyPolicy - * @author Gabryel Reyes - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include "LateralSafetyPolicy.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -LateralSafetyPolicy::LateralSafetyPolicy() -{ -} - -LateralSafetyPolicy::~LateralSafetyPolicy() -{ -} - -bool LateralSafetyPolicy::check(int16_t& leftMotorSpeedSetpoint, int16_t& rightMotorSpeedSetpoint) -{ - bool isSuccessful = true; - - UTIL_NOT_USED(leftMotorSpeedSetpoint); - UTIL_NOT_USED(rightMotorSpeedSetpoint); - - return isSuccessful; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/ConvoyLeader/src/LateralSafetyPolicy.h b/lib/ConvoyLeader/src/LateralSafetyPolicy.h deleted file mode 100644 index 899d0667..00000000 --- a/lib/ConvoyLeader/src/LateralSafetyPolicy.h +++ /dev/null @@ -1,99 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Concrete LongitudinalSafetyPolicy. - * @author Gabryel Reyes - * - * @addtogroup Application - * - * @{ - */ -#ifndef LATERAL_SAFETY_POLICY_H -#define LATERAL_SAFETY_POLICY_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Concrete Lateral Safety Policy. */ -class LateralSafetyPolicy : public ILateralSafetyPolicy -{ -public: - /** - * Lateral Safety Policy constructor. - */ - LateralSafetyPolicy(); - - /** - * Lateral Safety Policy destructor. - */ - virtual ~LateralSafetyPolicy(); - - /** - * Creates a LateralSafetyPolicy instance, for registering in the ProcessingChainFactory. - * - * @return If successful, returns a pointer to the LateralSafetyPolicy instance. Otherwise nullptr. - */ - static ILateralSafetyPolicy* create() - { - return new (std::nothrow) LateralSafetyPolicy(); - } - - /** - * Checks if the lateral safety policy is satisfied by the calculated motor speeds. - * If not, the motor speeds are adjusted accordingly. - * - * @param[in,out] leftMotorSpeedSetpoint Left motor speed [steps/s]. - * @param[in,out] rightMotorSpeedSetpoint Right motor speed [steps/s]. - * - * @return True is satisfied, otherwise false. - */ - bool check(int16_t& leftMotorSpeedSetpoint, int16_t& rightMotorSpeedSetpoint) final; - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LATERAL_SAFETY_POLICY_H */ -/** @} */ diff --git a/lib/ConvoyLeader/src/LongitudinalController.cpp b/lib/ConvoyLeader/src/LongitudinalController.cpp deleted file mode 100644 index 086d91cf..00000000 --- a/lib/ConvoyLeader/src/LongitudinalController.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Concrete Longitudinal Controller. - * @author Gabryel Reyes - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include "LongitudinalController.h" -#include "PlatoonUtils.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -LongitudinalController::LongitudinalController() : ILongitudinalController() -{ -} - -LongitudinalController::~LongitudinalController() -{ -} - -bool LongitudinalController::calculateLongitudinalMovement(const Waypoint& currentWaypoint, - const Waypoint& targetWaypoint, int16_t& centerSpeedSetpoint) -{ - bool isSuccessful = true; - int32_t distance = PlatoonUtils::calculateAbsoluteDistance(targetWaypoint, currentWaypoint); - - if (0 == distance) - { - /* Target reached. */ - centerSpeedSetpoint = 0; - } - else - { - /* Calculate speed using ramp factor. */ - centerSpeedSetpoint = constrain(((distance * RAMP_FACTOR) + OFFSET_SPEED), -MAX_MOTOR_SPEED, MAX_MOTOR_SPEED); - } - - return isSuccessful; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/ConvoyLeader/src/LongitudinalController.h b/lib/ConvoyLeader/src/LongitudinalController.h deleted file mode 100644 index dbb1f961..00000000 --- a/lib/ConvoyLeader/src/LongitudinalController.h +++ /dev/null @@ -1,117 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Concrete Longitudinal Controller. - * @author Gabryel Reyes - * - * @addtogroup Application - * - * @{ - */ -#ifndef LONGITUDINAL_CONTROLLER_H -#define LONGITUDINAL_CONTROLLER_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Concrete Longitudinal Controller */ -class LongitudinalController : public ILongitudinalController -{ -public: - /** - * Longitudinal Controller constructor. - */ - LongitudinalController(); - - /** - * Longitudinal Controller destructor. - */ - virtual ~LongitudinalController(); - - /** - * Creates a LongitudinalController instance, for registering in the ProcessingChainFactory. - * - * @return If successful, returns a pointer to the LongitudinalController instance. Otherwise nullptr. - */ - static ILongitudinalController* create() - { - return new (std::nothrow) LongitudinalController(); - } - - /** - * Calculates the motor speeds for the next step. - * - * @param[in] currentWaypoint Current waypoint where the vehicle is found. - * @param[in] targetWaypoint Target waypoint to drive to. - * @param[out] centerSpeedSetpoint Center speed setpoint [steps/s]. - * - * @return If successful, returns true otherwise false. - */ - bool calculateLongitudinalMovement(const Waypoint& currentWaypoint, const Waypoint& targetWaypoint, - int16_t& centerSpeedSetpoint) final; - -private: - /** - * Maximum motor speed in encoder steps/s - * Speed determined experimentally using the motor calibration of the RadonUlzer. - */ - static const int16_t MAX_MOTOR_SPEED = 2400; - - /** Minimum distance to drive with max motor speed to in mm.*/ - static const int16_t MIN_DISTANCE_TO_MAX_SPEED = 400; - - /** - * Offset speed in encoder steps/s - * Used to being too slow when approaching the target waypoint. - */ - static const int16_t OFFSET_SPEED = 500; - - /** Ramp factor. */ - static const int16_t RAMP_FACTOR = MAX_MOTOR_SPEED / MIN_DISTANCE_TO_MAX_SPEED; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LONGITUDINAL_CONTROLLER_H */ -/** @} */ diff --git a/lib/ConvoyLeader/src/LongitudinalSafetyPolicy.cpp b/lib/ConvoyLeader/src/LongitudinalSafetyPolicy.cpp deleted file mode 100644 index 44ffa1a4..00000000 --- a/lib/ConvoyLeader/src/LongitudinalSafetyPolicy.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Concrete Longitudinal Safety Policy. - * @author Gabryel Reyes - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include "LongitudinalSafetyPolicy.h" -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -LongitudinalSafetyPolicy::LongitudinalSafetyPolicy() -{ -} - -LongitudinalSafetyPolicy::~LongitudinalSafetyPolicy() -{ -} - -bool LongitudinalSafetyPolicy::check(int16_t& centerSpeedSetpoint) -{ - bool isSuccessful = true; - int16_t constraintSpeed = constrain(centerSpeedSetpoint, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED); - - if (constraintSpeed != centerSpeedSetpoint) - { - centerSpeedSetpoint = constraintSpeed; - LOG_WARNING("Speed setpoint constrained to %d", centerSpeedSetpoint); - } - - return isSuccessful; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/ConvoyLeader/src/LongitudinalSafetyPolicy.h b/lib/ConvoyLeader/src/LongitudinalSafetyPolicy.h deleted file mode 100644 index d98714d9..00000000 --- a/lib/ConvoyLeader/src/LongitudinalSafetyPolicy.h +++ /dev/null @@ -1,106 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Concrete Longitudinal Safety Policy. - * @author Gabryel Reyes - * - * @addtogroup Application - * - * @{ - */ -#ifndef LONGITUDINAL_SAFETY_POLICY_H -#define LONGITUDINAL_SAFETY_POLICY_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Concrete Longitudinal Safety Policy. */ -class LongitudinalSafetyPolicy : public ILongitudinalSafetyPolicy -{ -public: - /** - * Longitudinal Safety Policy constructor. - */ - LongitudinalSafetyPolicy(); - - /** - * Longitudinal Safety Policy destructor. - */ - virtual ~LongitudinalSafetyPolicy(); - - /** - * Creates a LongitudinalSafetyPolicy instance, for registering in the ProcessingChainFactory. - * - * @return If successful, returns a pointer to the LongitudinalSafetyPolicy instance. Otherwise nullptr. - */ - static ILongitudinalSafetyPolicy* create() - { - return new (std::nothrow) LongitudinalSafetyPolicy(); - } - - /** - * Checks if the longitudinal safety policy is satisfied by the calculated center speed. - * If not, the center speed is adjusted accordingly. - * - * @param[in,out] centerSpeedSetpoint Center speed [steps/s]. - * - * @return True is satisfied, otherwise false. - */ - bool check(int16_t& centerSpeedSetpoint) final; - -private: - /** - * Maximum motor speed in encoder steps/s - * Speed determined experimentally using the motor calibration of the RadonUlzer. - */ - static const int16_t MAX_MOTOR_SPEED = 2400; - - /** Minimum motor speed in encoder steps/s */ - static const int16_t MIN_MOTOR_SPEED = 0; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LONGITUDINAL_SAFETY_POLICY_H */ -/** @} */ From 78d2144a54cd530b67cb532158950b7219dd9760 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 17 Jan 2024 14:37:05 +0100 Subject: [PATCH 03/36] Created missing SMP channels --- lib/ConvoyLeader/src/App.cpp | 48 ++++++++++--- lib/ConvoyLeader/src/App.h | 14 +++- lib/ConvoyLeader/src/RemoteControl.h | 85 ++++++++++++++++++++++++ lib/ConvoyLeader/src/SerialMuxChannels.h | 77 ++++++++++++++------- 4 files changed, 188 insertions(+), 36 deletions(-) create mode 100644 lib/ConvoyLeader/src/RemoteControl.h diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index f4d8fdb2..aa8ad575 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -33,6 +33,7 @@ * Includes *****************************************************************************/ #include "App.h" +#include "RemoteControl.h" #include #include #include @@ -61,6 +62,7 @@ * Prototypes *****************************************************************************/ +static void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); static void App_currentVehicleChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); /****************************************************************************** @@ -271,17 +273,18 @@ bool App::setupSerialMuxProt() bool isSuccessful = false; /* Channel subscription. */ - m_smpServer.subscribeToChannel(CURRENT_VEHICLE_DATA_CHANNEL_DLC_CHANNEL_NAME, App_currentVehicleChannelCallback); + m_smpServer.subscribeToChannel(COMMAND_RESPONSE_CHANNEL_NAME, App_cmdRspChannelCallback); + m_smpServer.subscribeToChannel(CURRENT_VEHICLE_DATA_CHANNEL_NAME, App_currentVehicleChannelCallback); /* Channel creation. */ - m_serialMuxProtChannelIdMotorSpeedSetpoints = + m_serialMuxProtChannelIdRemoteCtrl = m_smpServer.createChannel(COMMAND_CHANNEL_NAME, COMMAND_CHANNEL_DLC); + m_serialMuxProtChannelIdMotorSpeeds = m_smpServer.createChannel(SPEED_SETPOINT_CHANNEL_NAME, SPEED_SETPOINT_CHANNEL_DLC); + m_serialMuxProtChannelInitialVehicleData = + m_smpServer.createChannel(INITIAL_VEHICLE_DATA_CHANNEL_NAME, INITIAL_VEHICLE_DATA_CHANNEL_DLC); - if (0U == m_serialMuxProtChannelIdMotorSpeedSetpoints) - { - LOG_FATAL("Could not create SerialMuxProt Channel: %s.", SPEED_SETPOINT_CHANNEL_NAME); - } - else + if ((0U != m_serialMuxProtChannelIdRemoteCtrl) && (0U != m_serialMuxProtChannelIdMotorSpeeds) && + (0U != m_serialMuxProtChannelInitialVehicleData)) { isSuccessful = true; } @@ -297,6 +300,33 @@ bool App::setupSerialMuxProt() * Local Functions *****************************************************************************/ +/** + * Receives remote control command responses over SerialMuxProt channel. + * + * @param[in] payload Command id + * @param[in] payloadSize Size of command id + * @param[in] userData User data + */ +void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) +{ + UTIL_NOT_USED(userData); + if ((nullptr != payload) && (COMMAND_RESPONSE_CHANNEL_DLC == payloadSize)) + { + const CommandResponse* cmdRsp = reinterpret_cast(payload); + LOG_DEBUG("CMD_RSP: ID: 0x%02X , RSP: 0x%02X", cmdRsp->commandId, cmdRsp->responseId); + + if (RemoteControl::CMD_ID_GET_MAX_SPEED == cmdRsp->commandId) + { + LOG_DEBUG("Max Speed: %d", cmdRsp->maxMotorSpeed); + } + } + else + { + LOG_WARNING("CMD_RSP: Invalid payload size. Expected: %u Received: %u", COMMAND_RESPONSE_CHANNEL_DLC, + payloadSize); + } +} + /** * Receives current position and heading of the robot over SerialMuxProt channel. * @@ -314,7 +344,7 @@ void App_currentVehicleChannelCallback(const uint8_t* payload, const uint8_t pay } else { - LOG_WARNING("%s: Invalid payload size. Expected: %u Received: %u", - CURRENT_VEHICLE_DATA_CHANNEL_DLC_CHANNEL_NAME, CURRENT_VEHICLE_DATA_CHANNEL_DLC, payloadSize); + LOG_WARNING("%s: Invalid payload size. Expected: %u Received: %u", CURRENT_VEHICLE_DATA_CHANNEL_NAME, + CURRENT_VEHICLE_DATA_CHANNEL_DLC, payloadSize); } } diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index ead3b9fe..2f8b200b 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -66,8 +66,10 @@ class App * Construct the convoy leader application. */ App() : + m_serialMuxProtChannelIdRemoteCtrl(0U), + m_serialMuxProtChannelIdMotorSpeeds(0U), + m_serialMuxProtChannelInitialVehicleData(0U), m_smpServer(Board::getInstance().getDevice().getStream(), this), - m_serialMuxProtChannelIdMotorSpeedSetpoints(0U), m_mqttClient(), m_v2vClient(m_mqttClient) { @@ -107,8 +109,14 @@ class App /** MQTT topic name for will messages. */ static const char* TOPIC_NAME_WILL; - /** SerialMuxProt Channel id sending sending motor speed setpoints. */ - uint8_t m_serialMuxProtChannelIdMotorSpeedSetpoints; + /** SerialMuxProt Channel id for sending remote control commands. */ + uint8_t m_serialMuxProtChannelIdRemoteCtrl; + + /** SerialMuxProt Channel id for sending motor speeds. */ + uint8_t m_serialMuxProtChannelIdMotorSpeeds; + + /** SerialMuxProt Channel id for sending initial position data. */ + uint8_t m_serialMuxProtChannelInitialVehicleData; /** * SerialMuxProt Server Instance diff --git a/lib/ConvoyLeader/src/RemoteControl.h b/lib/ConvoyLeader/src/RemoteControl.h new file mode 100644 index 00000000..3cdc78ae --- /dev/null +++ b/lib/ConvoyLeader/src/RemoteControl.h @@ -0,0 +1,85 @@ +/* MIT License + * + * Copyright (c) 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief RemoteControl common constants. + * @author Gabryel Reyes + * + * @addtogroup App + * + * @{ + */ +#ifndef REMOTE_CONTROL_H +#define REMOTE_CONTROL_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** RemoteControl application constants */ +namespace RemoteControl +{ + /** Remote control commands. */ + typedef enum : uint8_t + { + CMD_ID_IDLE = 0, /**< Nothing to do. */ + CMD_ID_START_LINE_SENSOR_CALIB, /**< Start line sensor calibration. */ + CMD_ID_START_MOTOR_SPEED_CALIB, /**< Start motor speed calibration. */ + CMD_ID_REINIT_BOARD, /**< Re-initialize the board. Required for webots simulation. */ + CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ + + } CmdId; + + /** Remote control command responses. */ + typedef enum : uint8_t + { + RSP_ID_OK = 0, /**< Command successful executed. */ + RSP_ID_PENDING, /**< Command is pending. */ + RSP_ID_ERROR /**< Command failed. */ + + } RspId; +} /* namespace RemoteControl */ + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* REMOTE_CONTROL_H */ +/** @} */ diff --git a/lib/ConvoyLeader/src/SerialMuxChannels.h b/lib/ConvoyLeader/src/SerialMuxChannels.h index b8437e23..13884ccd 100644 --- a/lib/ConvoyLeader/src/SerialMuxChannels.h +++ b/lib/ConvoyLeader/src/SerialMuxChannels.h @@ -25,19 +25,12 @@ DESCRIPTION *******************************************************************************/ /** - * @brief Channel structure definition for the SerialMuxProt. - * @author Gabryel Reyes - * - * @addtogroup Application - * - * @{ + * @brief Channel structure definition for the SerialMuxProt. + * @author Gabryel Reyes */ -#ifndef SERIAL_MUX_CHANNELS_H -#define SERIAL_MUX_CHANNELS_H -/****************************************************************************** - * Compile Switches - *****************************************************************************/ +#ifndef SERIAL_MUX_CHANNELS_H_ +#define SERIAL_MUX_CHANNELS_H_ /****************************************************************************** * Includes @@ -52,11 +45,17 @@ /** Maximum number of SerialMuxProt Channels. */ #define MAX_CHANNELS (10U) -/** Name of Channel to send Current Vehicle Data to. */ -#define CURRENT_VEHICLE_DATA_CHANNEL_DLC_CHANNEL_NAME "CURR_DATA" +/** Name of Channel to send Commands to. */ +#define COMMAND_CHANNEL_NAME "CMD" -/** DLC of Current Vehicle Data Channel */ -#define CURRENT_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) +/** DLC of Command Channel. */ +#define COMMAND_CHANNEL_DLC (sizeof(Command)) + +/** Name of Channel to receive Command Responses from. */ +#define COMMAND_RESPONSE_CHANNEL_NAME "CMD_RSP" + +/** DLC of Command Response Channel. */ +#define COMMAND_RESPONSE_CHANNEL_DLC (sizeof(CommandResponse)) /** Name of Channel to send Motor Speed Setpoints to. */ #define SPEED_SETPOINT_CHANNEL_NAME "SPEED_SET" @@ -64,10 +63,48 @@ /** DLC of Speedometer Channel */ #define SPEED_SETPOINT_CHANNEL_DLC (sizeof(SpeedData)) +/** Name of Channel to send Current Vehicle Data to. */ +#define CURRENT_VEHICLE_DATA_CHANNEL_NAME "CURR_DATA" + +/** DLC of Current Vehicle Data Channel */ +#define CURRENT_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) + +/** Name of Channel to send Initial Vehicle Data to. */ +#define INITIAL_VEHICLE_DATA_CHANNEL_NAME "INIT_DATA" + +/** DLC of Initial Vehicle Data Channel */ +#define INITIAL_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) + /****************************************************************************** * Types and Classes *****************************************************************************/ +/** Struct of the "Command" channel payload. */ +typedef struct _Command +{ + uint8_t commandId; /**< Command ID */ +} __attribute__((packed)) Command; + +/** Struct of the "Command Response" channel payload. */ +typedef struct _CommandResponse +{ + uint8_t commandId; /**< Command ID */ + uint8_t responseId; /**< Response to the command */ + + /** Response Payload. */ + union + { + int16_t maxMotorSpeed; /**< Max speed [steps/s]. */ + }; +} __attribute__((packed)) CommandResponse; + +/** Struct of the "Speed" channel payload. */ +typedef struct _SpeedData +{ + int16_t left; /**< Left motor speed [steps/s] */ + int16_t right; /**< Right motor speed [steps/s] */ +} __attribute__((packed)) SpeedData; + /** Struct of the "Current Vehicle Data" channel payload. */ typedef struct _VehicleData { @@ -79,16 +116,8 @@ typedef struct _VehicleData int16_t center; /**< Center speed [steps/s]. */ } __attribute__((packed)) VehicleData; -/** Struct of the "Speed" channel payload. */ -typedef struct _SpeedData -{ - int16_t left; /**< Left motor speed [steps/s]. */ - int16_t right; /**< Right motor speed [steps/s]. */ -} __attribute__((packed)) SpeedData; - /****************************************************************************** * Functions *****************************************************************************/ -#endif /* SERIAL_MUX_CHANNELS_H */ -/** @} */ +#endif /* SERIAL_MUX_CHANNELS_H_ */ \ No newline at end of file From 244f37c1520a3de0f8cefb272cc24653b110d08d Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 15 Jan 2024 12:01:51 +0100 Subject: [PATCH 04/36] Added configurations for platoon participants --- data/config/follower_1.json | 28 ++++++++++++++++++++++++++++ data/config/follower_2.json | 28 ++++++++++++++++++++++++++++ data/config/leader.json | 28 ++++++++++++++++++++++++++++ 3 files changed, 84 insertions(+) create mode 100644 data/config/follower_1.json create mode 100644 data/config/follower_2.json create mode 100644 data/config/leader.json diff --git a/data/config/follower_1.json b/data/config/follower_1.json new file mode 100644 index 00000000..a366d422 --- /dev/null +++ b/data/config/follower_1.json @@ -0,0 +1,28 @@ +{ + "robotName": "follower_1", + "wifi": { + "ssid": "TheForce", + "pswd": "hanshotfirst" + }, + "mqtt": { + "host": "localhost", + "port": 1883 + }, + "ap": { + "ssid": "DCS_AP", + "pswd": "hanshotfirst" + }, + "webServer": { + "user": "admin", + "pswd": "admin" + }, + "platoon": { + "platoonId": "0", + "vehicleId": "1" + }, + "initialPosition": { + "x": "0", + "y": "0", + "heading": "0" + } +} \ No newline at end of file diff --git a/data/config/follower_2.json b/data/config/follower_2.json new file mode 100644 index 00000000..88817bbe --- /dev/null +++ b/data/config/follower_2.json @@ -0,0 +1,28 @@ +{ + "robotName": "follower_2", + "wifi": { + "ssid": "TheForce", + "pswd": "hanshotfirst" + }, + "mqtt": { + "host": "localhost", + "port": 1883 + }, + "ap": { + "ssid": "DCS_AP", + "pswd": "hanshotfirst" + }, + "webServer": { + "user": "admin", + "pswd": "admin" + }, + "platoon": { + "platoonId": "0", + "vehicleId": "2" + }, + "initialPosition": { + "x": "0", + "y": "0", + "heading": "0" + } +} \ No newline at end of file diff --git a/data/config/leader.json b/data/config/leader.json new file mode 100644 index 00000000..54ffdbe4 --- /dev/null +++ b/data/config/leader.json @@ -0,0 +1,28 @@ +{ + "robotName": "leader", + "wifi": { + "ssid": "TheForce", + "pswd": "hanshotfirst" + }, + "mqtt": { + "host": "localhost", + "port": 1883 + }, + "ap": { + "ssid": "DCS_AP", + "pswd": "hanshotfirst" + }, + "webServer": { + "user": "admin", + "pswd": "admin" + }, + "platoon": { + "platoonId": "0", + "vehicleId": "0" + }, + "initialPosition": { + "x": "0", + "y": "0", + "heading": "0" + } +} \ No newline at end of file From 3fb85ba434f727509ad29a65dcfbd199c856b438 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 17 Jan 2024 15:02:51 +0100 Subject: [PATCH 05/36] Sending initial data --- lib/ConvoyLeader/src/App.cpp | 19 ++++++++++++++++++- lib/ConvoyLeader/src/App.h | 8 +++++++- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index aa8ad575..d0aea266 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -204,11 +204,28 @@ void App::loop() /* Process V2V Communication */ m_v2vClient.process(); + + if (false == m_initialDataSent) + { + SettingsHandler& settings = SettingsHandler::getInstance(); + VehicleData initialVehicleData; + initialVehicleData.xPos = settings.getInitialXPosition(); + initialVehicleData.yPos = settings.getInitialYPosition(); + initialVehicleData.orientation = settings.getInitialHeading(); + + if (true == m_smpServer.sendData(m_serialMuxProtChannelInitialVehicleData, &initialVehicleData, + sizeof(initialVehicleData))) + { + LOG_DEBUG("Initial vehicle data sent."); + m_initialDataSent = true; + } + } } void App::currentVehicleChannelCallback(const VehicleData& vehicleData) { - UTIL_NOT_USED(vehicleData); + LOG_DEBUG("X: %d Y: %d Heading: %d Left: %d Right: %d Center: %d", vehicleData.xPos, vehicleData.yPos, + vehicleData.orientation, vehicleData.left, vehicleData.right, vehicleData.center); } /****************************************************************************** diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index 2f8b200b..b902e3ab 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -71,7 +71,8 @@ class App m_serialMuxProtChannelInitialVehicleData(0U), m_smpServer(Board::getInstance().getDevice().getStream(), this), m_mqttClient(), - m_v2vClient(m_mqttClient) + m_v2vClient(m_mqttClient), + m_initialDataSent(false) { } @@ -135,6 +136,11 @@ class App */ V2VClient m_v2vClient; + /** + * Flag for setting initial data through SMP. + */ + bool m_initialDataSent; + private: /** * Handler of fatal errors in the Application. From 75ea40b1a87bbaeb226ec559d0bc444b3952b950 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 17 Jan 2024 17:04:02 +0100 Subject: [PATCH 06/36] Added longitudinal controller --- lib/ConvoyLeader/src/App.cpp | 72 ++++++- lib/ConvoyLeader/src/App.h | 34 +++- .../src/LongitudinalController.cpp | 142 +++++++++++++ lib/ConvoyLeader/src/LongitudinalController.h | 186 ++++++++++++++++++ lib/ConvoyLeader/src/RemoteControl.h | 3 +- lib/ConvoyLeader/src/SerialMuxChannels.h | 5 +- 6 files changed, 435 insertions(+), 7 deletions(-) create mode 100644 lib/ConvoyLeader/src/LongitudinalController.cpp create mode 100644 lib/ConvoyLeader/src/LongitudinalController.h diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index d0aea266..e406c17d 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -81,6 +81,9 @@ const char* App::TOPIC_NAME_BIRTH = "birth"; /* MQTT topic name for will messages. */ const char* App::TOPIC_NAME_WILL = "will"; +/* MQTT topic name for release messages. */ +const char* App::TOPIC_NAME_RELEASE = "release"; + /** Buffer size for JSON serialization of birth / will message */ static const uint32_t JSON_BIRTHMESSAGE_MAX_SIZE = 64U; @@ -169,6 +172,11 @@ void App::setup() } else { + LongitudinalController::MotorSetpointCallback motorSetpointCallback = [this](const int16_t& topCenterSpeed) + { return this->motorSetpointCallback(topCenterSpeed); }; + + m_longitudinalController.setMotorSetpointCallback(motorSetpointCallback); + isSuccessful = true; } } @@ -205,6 +213,9 @@ void App::loop() /* Process V2V Communication */ m_v2vClient.process(); + /* Process Longitudinal Controller */ + m_longitudinalController.process(); + if (false == m_initialDataSent) { SettingsHandler& settings = SettingsHandler::getInstance(); @@ -220,12 +231,64 @@ void App::loop() m_initialDataSent = true; } } + + if (LongitudinalController::STATE_STARTUP == m_longitudinalController.getState()) + { + Command payload; + payload.commandId = RemoteControl::CMD_ID_GET_MAX_SPEED; + + (void)m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &payload, sizeof(payload)); + } } void App::currentVehicleChannelCallback(const VehicleData& vehicleData) { LOG_DEBUG("X: %d Y: %d Heading: %d Left: %d Right: %d Center: %d", vehicleData.xPos, vehicleData.yPos, vehicleData.orientation, vehicleData.left, vehicleData.right, vehicleData.center); + + Waypoint vehicleDataAsWaypoint; + + vehicleDataAsWaypoint.xPos = vehicleData.xPos; + vehicleDataAsWaypoint.yPos = vehicleData.yPos; + vehicleDataAsWaypoint.orientation = vehicleData.orientation; + vehicleDataAsWaypoint.left = vehicleData.left; + vehicleDataAsWaypoint.right = vehicleData.right; + vehicleDataAsWaypoint.center = vehicleData.center; + + m_longitudinalController.calculateTopMotorSpeed(vehicleDataAsWaypoint); +} + +bool App::motorSetpointCallback(const int16_t topCenterSpeed) +{ + SpeedData payload; + payload.center = topCenterSpeed; + + return m_smpServer.sendData(m_serialMuxProtChannelIdMotorSpeeds, &payload, sizeof(payload)); +} + +void App::release() +{ + Command payload; + payload.commandId = RemoteControl::CMD_ID_START_DRIVING; + + /* Release robot. */ + if (false == m_longitudinalController.release()) + { + LOG_WARNING("Robot could not be released."); + } + else if (false == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &payload, sizeof(payload))) + { + LOG_WARNING("Failed to send release command to RU."); + } + else + { + LOG_INFO("Robot released."); + } +} + +void App::setMaxMotorSpeed(const int16_t maxMotorSpeed) +{ + m_longitudinalController.setMaxMotorSpeed(maxMotorSpeed); } /****************************************************************************** @@ -279,6 +342,8 @@ bool App::setupMqttClient() } else { + m_mqttClient.subscribe(TOPIC_NAME_RELEASE, true, [this](const String& payload) { this->release(); }); + isSuccessful = true; } @@ -326,15 +391,16 @@ bool App::setupSerialMuxProt() */ void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) { - UTIL_NOT_USED(userData); - if ((nullptr != payload) && (COMMAND_RESPONSE_CHANNEL_DLC == payloadSize)) + if ((nullptr != payload) && (COMMAND_RESPONSE_CHANNEL_DLC == payloadSize) && (nullptr != userData)) { - const CommandResponse* cmdRsp = reinterpret_cast(payload); + App* application = reinterpret_cast(userData); + const CommandResponse* cmdRsp = reinterpret_cast(payload); LOG_DEBUG("CMD_RSP: ID: 0x%02X , RSP: 0x%02X", cmdRsp->commandId, cmdRsp->responseId); if (RemoteControl::CMD_ID_GET_MAX_SPEED == cmdRsp->commandId) { LOG_DEBUG("Max Speed: %d", cmdRsp->maxMotorSpeed); + application->setMaxMotorSpeed(cmdRsp->maxMotorSpeed); } } else diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index b902e3ab..5a472e36 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -49,6 +49,7 @@ #include #include "SerialMuxChannels.h" #include +#include "LongitudinalController.h" /****************************************************************************** * Macros @@ -72,7 +73,8 @@ class App m_smpServer(Board::getInstance().getDevice().getStream(), this), m_mqttClient(), m_v2vClient(m_mqttClient), - m_initialDataSent(false) + m_initialDataSent(false), + m_longitudinalController() { } @@ -100,6 +102,28 @@ class App */ void currentVehicleChannelCallback(const VehicleData& vehicleData); + /** + * Motor setpoint callback. + * Called in order to send the motor speeds using SerialMuxProt to the robot. + * + * @param[in] topCenterSpeed Center motor speed [steps/s]. + * + * @return If the motor speed was sent successfully, returns true. Otherwise, false. + */ + bool motorSetpointCallback(const int16_t topCenterSpeed); + + /** + * Release robot and start driving. + */ + void release(); + + /** + * Set max motor speed. + * + * @param[in] maxMotorSpeed Max motor speed [steps/s]. + */ + void setMaxMotorSpeed(const int16_t maxMotorSpeed); + private: /** Minimum battery level in percent. */ static const uint8_t MIN_BATTERY_LEVEL = 10U; @@ -110,6 +134,9 @@ class App /** MQTT topic name for will messages. */ static const char* TOPIC_NAME_WILL; + /** MQTT topic name for release messages. */ + static const char* TOPIC_NAME_RELEASE; + /** SerialMuxProt Channel id for sending remote control commands. */ uint8_t m_serialMuxProtChannelIdRemoteCtrl; @@ -141,6 +168,11 @@ class App */ bool m_initialDataSent; + /** + * Longitudinal controller. + */ + LongitudinalController m_longitudinalController; + private: /** * Handler of fatal errors in the Application. diff --git a/lib/ConvoyLeader/src/LongitudinalController.cpp b/lib/ConvoyLeader/src/LongitudinalController.cpp new file mode 100644 index 00000000..00e837ca --- /dev/null +++ b/lib/ConvoyLeader/src/LongitudinalController.cpp @@ -0,0 +1,142 @@ +/* MIT License + * + * Copyright (c) 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Leader Longitudinal Controller. + * @author Gabryel Reyes + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include "LongitudinalController.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +LongitudinalController::LongitudinalController() : + m_maxMotorSpeed(0), + m_state(STATE_STARTUP), + m_lastFollowerFeedback(), + m_motorSetpointCallback(nullptr) +{ +} + +LongitudinalController::~LongitudinalController() +{ +} + +void LongitudinalController::process() +{ + switch (m_state) + { + case STATE_STARTUP: + /* Max speed must be positive. */ + if (0 < m_maxMotorSpeed) + { + m_state = STATE_READY; + } + + break; + + case STATE_READY: + /* Wait for external release. */ + break; + + case STATE_DRIVING: + /* Allow top motor speed calculation. */ + break; + + case STATE_SAFE: + /* Stop the vehicle. Sent continously to prevent overwriting by other modules. */ + if (nullptr != m_motorSetpointCallback) + { + m_motorSetpointCallback(0); + } + + break; + + default: + /* Should never happen. */ + m_state = STATE_SAFE; + break; + } +} + +void LongitudinalController::calculateTopMotorSpeed(const Waypoint& currentVehicleData) +{ + if (STATE_DRIVING == m_state) + { + /* TODO: Check follower feedback. Calculate platoon length and react accordingly. */ + + /* TODO: Calculate top motor speed. */ + + /* Send top motor speed. */ + if (nullptr != m_motorSetpointCallback) + { + m_motorSetpointCallback(m_maxMotorSpeed); + } + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/ConvoyLeader/src/LongitudinalController.h b/lib/ConvoyLeader/src/LongitudinalController.h new file mode 100644 index 00000000..6044a664 --- /dev/null +++ b/lib/ConvoyLeader/src/LongitudinalController.h @@ -0,0 +1,186 @@ +/* MIT License + * + * Copyright (c) 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Leader Longitudinal Controller. + * @author Gabryel Reyes + * + * @addtogroup Application + * + * @{ + */ +#ifndef LONGITUDINAL_CONTROLLER_H +#define LONGITUDINAL_CONTROLLER_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Leader Longitudinal Controller. */ +class LongitudinalController +{ +public: + /** + * Motor setpoint callback. + * Called in order to set the motor speeds. + */ + typedef std::function MotorSetpointCallback; + + /** Controller states. */ + enum STATE : uint8_t + { + STATE_STARTUP = 0, /**< Startup state. */ + STATE_READY, /**< Ready state. */ + STATE_DRIVING, /**< Driving state. */ + STATE_SAFE, /**< Safe state. */ + }; + + /** + * LongitudinalController constructor. + */ + LongitudinalController(); + + /** + * Default destructor. + */ + ~LongitudinalController(); + + /** + * Process the longitudinal controller. + */ + void process(); + + /** + * Set the maximum motor speed of the robot. + * + * @param[in] maxMotorSpeed Maximum motor speed. + */ + void setMaxMotorSpeed(int16_t maxMotorSpeed) + { + m_maxMotorSpeed = maxMotorSpeed; + } + + /** + * Release robot for driving state, only if in ready state. + * + * @return If the robot was released, returns true. Otherwise false. + */ + bool release() + { + bool isReleased = false; + + if (STATE_READY == m_state) + { + m_state = STATE_DRIVING; + isReleased = true; + } + + return isReleased; + } + + /** + * Set incoming feedback from last follower. + * + * @param[in] feedback Feedback from last follower. + */ + void setLastFollowerFeedback(const Waypoint& feedback) + { + m_lastFollowerFeedback = feedback; + } + + /** + * Get current state. + * + * @return Current state. + */ + STATE getState() const + { + return m_state; + } + + /** + * Set safe state. This will stop the robot. + */ + void setSafeState() + { + m_state = STATE_SAFE; + } + + /** + * Set motor setpoint callback. + * + * @param[in] motorSetpointCallback Motor setpoint callback. + */ + void setMotorSetpointCallback(const MotorSetpointCallback& motorSetpointCallback) + { + m_motorSetpointCallback = motorSetpointCallback; + } + + /** + * Calculate the top motor speed and send it to the robot. + * + * @param[in] currentVehicleData Current vehicle data. + */ + void calculateTopMotorSpeed(const Waypoint& currentVehicleData); + +private: + /** Maximum motor speed. */ + int16_t m_maxMotorSpeed; + + /** Current state. */ + STATE m_state; + + /** Feedback from last follower. */ + Waypoint m_lastFollowerFeedback; + + /** + * Motor setpoint callback. + */ + MotorSetpointCallback m_motorSetpointCallback; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LONGITUDINAL_CONTROLLER_H */ +/** @} */ diff --git a/lib/ConvoyLeader/src/RemoteControl.h b/lib/ConvoyLeader/src/RemoteControl.h index 3cdc78ae..077592c2 100644 --- a/lib/ConvoyLeader/src/RemoteControl.h +++ b/lib/ConvoyLeader/src/RemoteControl.h @@ -28,7 +28,7 @@ * @brief RemoteControl common constants. * @author Gabryel Reyes * - * @addtogroup App + * @addtogroup Application * * @{ */ @@ -64,6 +64,7 @@ namespace RemoteControl CMD_ID_START_MOTOR_SPEED_CALIB, /**< Start motor speed calibration. */ CMD_ID_REINIT_BOARD, /**< Re-initialize the board. Required for webots simulation. */ CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ + CMD_ID_START_DRIVING, /**< Start driving. */ } CmdId; diff --git a/lib/ConvoyLeader/src/SerialMuxChannels.h b/lib/ConvoyLeader/src/SerialMuxChannels.h index 13884ccd..544d649b 100644 --- a/lib/ConvoyLeader/src/SerialMuxChannels.h +++ b/lib/ConvoyLeader/src/SerialMuxChannels.h @@ -101,8 +101,9 @@ typedef struct _CommandResponse /** Struct of the "Speed" channel payload. */ typedef struct _SpeedData { - int16_t left; /**< Left motor speed [steps/s] */ - int16_t right; /**< Right motor speed [steps/s] */ + int16_t left; /**< Left motor speed [steps/s] */ + int16_t right; /**< Right motor speed [steps/s] */ + int16_t center; /**< Center motor speed [steps/s] */ } __attribute__((packed)) SpeedData; /** Struct of the "Current Vehicle Data" channel payload. */ From fc8acc2820de98f897b0077e93288776cb4b73cf Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 17 Jan 2024 17:11:30 +0100 Subject: [PATCH 07/36] Added starting heading according to LineFollowerTrack --- data/config/leader.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/data/config/leader.json b/data/config/leader.json index 54ffdbe4..8940ff06 100644 --- a/data/config/leader.json +++ b/data/config/leader.json @@ -23,6 +23,6 @@ "initialPosition": { "x": "0", "y": "0", - "heading": "0" + "heading": "1588" } } \ No newline at end of file From 564343eecec3798dbf245b65075c0419cd33b705 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 18 Jan 2024 09:57:26 +0100 Subject: [PATCH 08/36] Added more constructors and utilities to Waypoint class --- lib/PlatoonService/src/V2VClient.cpp | 74 ++----------- lib/PlatoonService/src/Waypoint.cpp | 151 +++++++++++++++++++++++++++ lib/PlatoonService/src/Waypoint.h | 50 ++++++++- 3 files changed, 207 insertions(+), 68 deletions(-) create mode 100644 lib/PlatoonService/src/Waypoint.cpp diff --git a/lib/PlatoonService/src/V2VClient.cpp b/lib/PlatoonService/src/V2VClient.cpp index 8e31f5e3..0af2e955 100644 --- a/lib/PlatoonService/src/V2VClient.cpp +++ b/lib/PlatoonService/src/V2VClient.cpp @@ -33,9 +33,7 @@ * Includes *****************************************************************************/ #include "V2VClient.h" -#include #include -#include /****************************************************************************** * Compiler Switches @@ -57,18 +55,6 @@ * Local Variables *****************************************************************************/ -/* MQTT topic name for birth messages. */ -const char* V2VClient::TOPIC_NAME_BIRTH = "birth"; - -/* MQTT topic name for will messages. */ -const char* V2VClient::TOPIC_NAME_WILL = "will"; - -/** Default size of the JSON Document for parsing. */ -static const uint32_t JSON_DOC_DEFAULT_SIZE = 1024U; - -/** Platoon leader vehicle ID. */ -static const uint8_t PLATOON_LEADER_ID = 0U; - /****************************************************************************** * Public Methods *****************************************************************************/ @@ -163,24 +149,14 @@ void V2VClient::process() bool V2VClient::sendWaypoint(const Waypoint& waypoint) { - bool isSuccessful = false; - StaticJsonDocument jsonPayload; - - jsonPayload["X"] = waypoint.xPos; /**< X position [mm]. */ - jsonPayload["Y"] = waypoint.yPos; /**< Y position [mm]. */ - jsonPayload["Orientation"] = waypoint.orientation; /**< Orientation [mrad]. */ - jsonPayload["Left"] = waypoint.left; /**< Left motor speed [steps/s]. */ - jsonPayload["Right"] = waypoint.right; /**< Right motor speed [steps/s]. */ - jsonPayload["Center"] = waypoint.center; /**< Center speed [steps/s]. */ - - size_t jsonBufferSize = measureJson(jsonPayload) + 1U; - char jsonBuffer[jsonBufferSize]; + bool isSuccessful = false; + String payload = waypoint.serialize(); - if ((jsonBufferSize - 1U) != serializeJson(jsonPayload, jsonBuffer, jsonBufferSize)) + if (true == payload.isEmpty()) { - LOG_ERROR("JSON serialization failed."); + LOG_DEBUG("Failed to serialize waypoint."); } - else if (false == m_mqttClient.publish(m_outputTopic, false, String(jsonBuffer))) + else if (false == m_mqttClient.publish(m_outputTopic, false, payload)) { LOG_ERROR("Failed to publish MQTT message to %s.", m_outputTopic); } @@ -229,47 +205,15 @@ size_t V2VClient::getWaypointQueueSize() const void V2VClient::targetWaypointTopicCallback(const String& payload) { - StaticJsonDocument jsonPayload; - DeserializationError error = deserializeJson(jsonPayload, payload.c_str()); + Waypoint* waypoint = Waypoint::deserialize(payload); - if (error != DeserializationError::Ok) + if (nullptr == waypoint) { - LOG_ERROR("JSON Deserialization Error %d.", error); + LOG_ERROR("Failed to deserialize received waypoint."); } else { - JsonVariant jsonXPos = jsonPayload["X"]; /**< X position [mm]. */ - JsonVariant jsonYPos = jsonPayload["Y"]; /**< Y position [mm]. */ - JsonVariant jsonOrientation = jsonPayload["Orientation"]; /**< Orientation [mrad]. */ - JsonVariant jsonLeft = jsonPayload["Left"]; /**< Left motor speed [steps/s]. */ - JsonVariant jsonRight = jsonPayload["Right"]; /**< Right motor speed [steps/s]. */ - JsonVariant jsonCenter = jsonPayload["Center"]; /**< Center speed [steps/s]. */ - - if ((false == jsonXPos.isNull()) && (false == jsonYPos.isNull()) && (false == jsonOrientation.isNull()) && - (false == jsonLeft.isNull()) && (false == jsonRight.isNull()) && (false == jsonCenter.isNull())) - { - Waypoint* waypoint = new (std::nothrow) Waypoint(); - - if (nullptr != waypoint) - { - waypoint->xPos = jsonXPos.as(); - waypoint->yPos = jsonYPos.as(); - waypoint->orientation = jsonOrientation.as(); - waypoint->left = jsonLeft.as(); - waypoint->right = jsonRight.as(); - waypoint->center = jsonCenter.as(); - - m_waypointQueue.push(waypoint); - } - else - { - LOG_ERROR("Failed to allocate memory for received waypoint."); - } - } - else - { - LOG_WARNING("Received invalid waypoint."); - } + m_waypointQueue.push(waypoint); } } diff --git a/lib/PlatoonService/src/Waypoint.cpp b/lib/PlatoonService/src/Waypoint.cpp new file mode 100644 index 00000000..b3160cdb --- /dev/null +++ b/lib/PlatoonService/src/Waypoint.cpp @@ -0,0 +1,151 @@ +/* MIT License + * + * Copyright (c) 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Definition of a Waypoint. + * @author Gabryel Reyes + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include "Waypoint.h" +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** Default size of the JSON Document for parsing. */ +static const uint32_t JSON_DOC_DEFAULT_SIZE = 1024U; + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +Waypoint* Waypoint::deserialize(const String& serializedWaypoint) +{ + Waypoint* waypoint = nullptr; + StaticJsonDocument jsonPayload; + DeserializationError error = deserializeJson(jsonPayload, serializedWaypoint.c_str()); + + if (error != DeserializationError::Ok) + { + LOG_ERROR("JSON Deserialization Error %d.", error); + } + else + { + JsonVariant jsonXPos = jsonPayload["X"]; /**< X position [mm]. */ + JsonVariant jsonYPos = jsonPayload["Y"]; /**< Y position [mm]. */ + JsonVariant jsonOrientation = jsonPayload["Orientation"]; /**< Orientation [mrad]. */ + JsonVariant jsonLeft = jsonPayload["Left"]; /**< Left motor speed [steps/s]. */ + JsonVariant jsonRight = jsonPayload["Right"]; /**< Right motor speed [steps/s]. */ + JsonVariant jsonCenter = jsonPayload["Center"]; /**< Center speed [steps/s]. */ + + if ((false == jsonXPos.isNull()) && (false == jsonYPos.isNull()) && (false == jsonOrientation.isNull()) && + (false == jsonLeft.isNull()) && (false == jsonRight.isNull()) && (false == jsonCenter.isNull())) + { + + int32_t xPos = jsonXPos.as(); + int32_t yPos = jsonYPos.as(); + int32_t orientation = jsonOrientation.as(); + int16_t left = jsonLeft.as(); + int16_t right = jsonRight.as(); + int16_t center = jsonCenter.as(); + + waypoint = new (std::nothrow) Waypoint(xPos, yPos, orientation, left, right, center); + } + } + + return waypoint; +} + +String Waypoint::serialize() const +{ + String serializedWaypoint; + StaticJsonDocument jsonPayload; + + jsonPayload["X"] = xPos; /**< X position [mm]. */ + jsonPayload["Y"] = yPos; /**< Y position [mm]. */ + jsonPayload["Orientation"] = orientation; /**< Orientation [mrad]. */ + jsonPayload["Left"] = left; /**< Left motor speed [steps/s]. */ + jsonPayload["Right"] = right; /**< Right motor speed [steps/s]. */ + jsonPayload["Center"] = center; /**< Center speed [steps/s]. */ + + size_t jsonBufferSize = measureJson(jsonPayload) + 1U; + char jsonBuffer[jsonBufferSize]; + + if ((jsonBufferSize - 1U) != serializeJson(jsonPayload, jsonBuffer, jsonBufferSize)) + { + LOG_ERROR("JSON serialization failed."); + } + else + { + serializedWaypoint = jsonBuffer; + } + + return serializedWaypoint; +} + +void Waypoint::debugPrint() const +{ + LOG_DEBUG("X: %d, Y: %d, Orientation: %d, Left: %d, Right: %d, Center: %d", xPos, yPos, orientation, left, right, + center); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/PlatoonService/src/Waypoint.h b/lib/PlatoonService/src/Waypoint.h index ed996db8..5f84821a 100644 --- a/lib/PlatoonService/src/Waypoint.h +++ b/lib/PlatoonService/src/Waypoint.h @@ -44,6 +44,7 @@ *****************************************************************************/ #include +#include /****************************************************************************** * Macros @@ -57,8 +58,9 @@ * Waypoint structure definition. * Defines the position of a waypoint in the map and the speed at which is to be reached. */ -typedef struct _Waypoint +class __attribute__((packed)) Waypoint { +public: int32_t xPos; /**< X position [mm]. */ int32_t yPos; /**< Y position [mm]. */ int32_t orientation; /**< Orientation [mrad]. */ @@ -69,11 +71,53 @@ typedef struct _Waypoint /** * Default constructor. */ - _Waypoint() : xPos(0), yPos(0), orientation(0), left(0), right(0), center(0) + Waypoint() : Waypoint(0, 0, 0, 0, 0, 0) { } -} __attribute__((packed)) Waypoint; + /** + * Constructor + * + * @param[in] xPos X position [mm]. + * @param[in] yPos Y position [mm]. + * @param[in] orientation Orientation [mrad]. + * @param[in] left Left motor speed [steps/s]. + * @param[in] right Right motor speed [steps/s]. + * @param[in] center Center speed [steps/s]. + */ + Waypoint(int32_t xPos, int32_t yPos, int32_t orientation, int16_t left, int16_t right, int16_t center) : + xPos(xPos), + yPos(yPos), + orientation(orientation), + left(left), + right(right), + center(center) + { + } + + /** + * Deserialize a waypoint. + * The waypoint is created on the heap and must be deleted by the caller. + * + * @param[in] serializedWaypoint Serialized waypoint. + * + * @return Pointer to a waypoint object. In case of an error, it returns nullptr. + */ + static Waypoint* deserialize(const String& serializedWaypoint); + + /** + * Serialize the waypoint. + * + * @return Serialized waypoint. Returns an empty string in case of an error. + */ + String serialize() const; + + /** + * Print waypoint data to the serial console. + * Uses the LOG_DEBUG macro, so it can be deactivated. + */ + void debugPrint() const; +}; /****************************************************************************** * Functions From e4002a6c61bd5be5a74970076a2aad36f9b38ced Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 18 Jan 2024 09:59:04 +0100 Subject: [PATCH 09/36] Removed unnecessary members and includes --- lib/PlatoonService/src/V2VClient.h | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/lib/PlatoonService/src/V2VClient.h b/lib/PlatoonService/src/V2VClient.h index e0a16fe2..76d07274 100644 --- a/lib/PlatoonService/src/V2VClient.h +++ b/lib/PlatoonService/src/V2VClient.h @@ -58,6 +58,9 @@ class V2VClient { public: + /** Platoon leader vehicle ID. */ + static const uint8_t PLATOON_LEADER_ID = 0U; + /** * Constructs a V2V client. * @@ -111,12 +114,6 @@ class V2VClient size_t getWaypointQueueSize() const; private: - /** MQTT topic name for birth messages. */ - static const char* TOPIC_NAME_BIRTH; - - /** MQTT topic name for will messages. */ - static const char* TOPIC_NAME_WILL; - /** Max topic length */ static const uint8_t MAX_TOPIC_LENGTH = 64U; From f6cc62dc6dcb75ecbc87814e09389b280f288abf Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 18 Jan 2024 10:00:23 +0100 Subject: [PATCH 10/36] Updated application --- lib/ConvoyLeader/src/App.cpp | 40 +++++++++++-------- lib/ConvoyLeader/src/App.h | 7 ++++ .../src/LongitudinalController.cpp | 7 +++- 3 files changed, 35 insertions(+), 19 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index e406c17d..42f4bb6b 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -87,9 +87,6 @@ const char* App::TOPIC_NAME_RELEASE = "release"; /** Buffer size for JSON serialization of birth / will message */ static const uint32_t JSON_BIRTHMESSAGE_MAX_SIZE = 64U; -/** Platoon leader vehicle ID. */ -static const uint8_t PLATOON_LEADER_ID = 0U; - /****************************************************************************** * Public Methods *****************************************************************************/ @@ -157,7 +154,7 @@ void App::setup() { LOG_FATAL("Failed to setup MQTT client."); } - else if (PLATOON_LEADER_ID != settings.getPlatoonVehicleId()) + else if (V2VClient::PLATOON_LEADER_ID != settings.getPlatoonVehicleId()) { /* Correct config.json file loaded? */ LOG_FATAL("Platoon Vehicle ID must be 0 for the leader."); @@ -243,17 +240,10 @@ void App::loop() void App::currentVehicleChannelCallback(const VehicleData& vehicleData) { - LOG_DEBUG("X: %d Y: %d Heading: %d Left: %d Right: %d Center: %d", vehicleData.xPos, vehicleData.yPos, - vehicleData.orientation, vehicleData.left, vehicleData.right, vehicleData.center); - - Waypoint vehicleDataAsWaypoint; + Waypoint vehicleDataAsWaypoint(vehicleData.xPos, vehicleData.yPos, vehicleData.orientation, vehicleData.left, + vehicleData.right, vehicleData.center); - vehicleDataAsWaypoint.xPos = vehicleData.xPos; - vehicleDataAsWaypoint.yPos = vehicleData.yPos; - vehicleDataAsWaypoint.orientation = vehicleData.orientation; - vehicleDataAsWaypoint.left = vehicleData.left; - vehicleDataAsWaypoint.right = vehicleData.right; - vehicleDataAsWaypoint.center = vehicleData.center; + vehicleDataAsWaypoint.debugPrint(); m_longitudinalController.calculateTopMotorSpeed(vehicleDataAsWaypoint); } @@ -291,6 +281,11 @@ void App::setMaxMotorSpeed(const int16_t maxMotorSpeed) m_longitudinalController.setMaxMotorSpeed(maxMotorSpeed); } +void App::setLastFollowerFeedback(const Waypoint& feedback) +{ + m_longitudinalController.setLastFollowerFeedback(feedback); +} + /****************************************************************************** * Protected Methods *****************************************************************************/ @@ -342,9 +337,20 @@ bool App::setupMqttClient() } else { - m_mqttClient.subscribe(TOPIC_NAME_RELEASE, true, [this](const String& payload) { this->release(); }); - - isSuccessful = true; + isSuccessful = + m_mqttClient.subscribe(TOPIC_NAME_RELEASE, true, [this](const String& payload) { this->release(); }); + + isSuccessful &= m_mqttClient.subscribe("platoons/0/vehicles/0/feedback", false, + [this](const String& payload) + { + Waypoint* waypoint = Waypoint::deserialize(payload); + + if (nullptr != waypoint) + { + this->setLastFollowerFeedback(*waypoint); + delete waypoint; + } + }); } return isSuccessful; diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index 5a472e36..4ee9a760 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -124,6 +124,13 @@ class App */ void setMaxMotorSpeed(const int16_t maxMotorSpeed); + /** + * Set incoming feedback from last follower. + * + * @param[in] feedback Feedback from last follower. + */ + void setLastFollowerFeedback(const Waypoint& feedback); + private: /** Minimum battery level in percent. */ static const uint8_t MIN_BATTERY_LEVEL = 10U; diff --git a/lib/ConvoyLeader/src/LongitudinalController.cpp b/lib/ConvoyLeader/src/LongitudinalController.cpp index 00e837ca..b353d7d8 100644 --- a/lib/ConvoyLeader/src/LongitudinalController.cpp +++ b/lib/ConvoyLeader/src/LongitudinalController.cpp @@ -113,14 +113,17 @@ void LongitudinalController::calculateTopMotorSpeed(const Waypoint& currentVehic { if (STATE_DRIVING == m_state) { + int16_t topMotorSpeed = 0; + /* TODO: Check follower feedback. Calculate platoon length and react accordingly. */ - /* TODO: Calculate top motor speed. */ + /* Calculate top motor speed. */ + topMotorSpeed = m_maxMotorSpeed; /* Send top motor speed. */ if (nullptr != m_motorSetpointCallback) { - m_motorSetpointCallback(m_maxMotorSpeed); + m_motorSetpointCallback(topMotorSpeed); } } } From 7635f7dc971f90f485c8eb994cf50161d6d78643 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 18 Jan 2024 10:18:30 +0100 Subject: [PATCH 11/36] Removed unncessary packed attribute --- lib/PlatoonService/src/Waypoint.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/PlatoonService/src/Waypoint.h b/lib/PlatoonService/src/Waypoint.h index 5f84821a..8fd52fe9 100644 --- a/lib/PlatoonService/src/Waypoint.h +++ b/lib/PlatoonService/src/Waypoint.h @@ -58,7 +58,7 @@ * Waypoint structure definition. * Defines the position of a waypoint in the map and the speed at which is to be reached. */ -class __attribute__((packed)) Waypoint +class Waypoint { public: int32_t xPos; /**< X position [mm]. */ From 88563ccf7730773acdebdb3d9655c5dd2f529c7a Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 18 Jan 2024 10:43:30 +0100 Subject: [PATCH 12/36] Send waypoints to follower --- lib/ConvoyLeader/src/App.cpp | 22 ++++++++++++++++++---- lib/ConvoyLeader/src/App.h | 17 ++++++++++++++++- 2 files changed, 34 insertions(+), 5 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index 42f4bb6b..402df512 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -174,6 +174,8 @@ void App::setup() m_longitudinalController.setMotorSetpointCallback(motorSetpointCallback); + m_sendWaypointTimer.start(SEND_WAYPOINT_TIMER_INTERVAL); + isSuccessful = true; } } @@ -236,16 +238,28 @@ void App::loop() (void)m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &payload, sizeof(payload)); } + + if (true == m_sendWaypointTimer.isTimeout()) + { + if (false == m_v2vClient.sendWaypoint(latestVehicleData)) + { + LOG_WARNING("Waypoint could not be sent."); + } + else + { + m_sendWaypointTimer.restart(); + } + } } void App::currentVehicleChannelCallback(const VehicleData& vehicleData) { - Waypoint vehicleDataAsWaypoint(vehicleData.xPos, vehicleData.yPos, vehicleData.orientation, vehicleData.left, - vehicleData.right, vehicleData.center); + latestVehicleData = Waypoint(vehicleData.xPos, vehicleData.yPos, vehicleData.orientation, vehicleData.left, + vehicleData.right, vehicleData.center); - vehicleDataAsWaypoint.debugPrint(); + latestVehicleData.debugPrint(); - m_longitudinalController.calculateTopMotorSpeed(vehicleDataAsWaypoint); + m_longitudinalController.calculateTopMotorSpeed(latestVehicleData); } bool App::motorSetpointCallback(const int16_t topCenterSpeed) diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index 4ee9a760..796cadb2 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -50,6 +50,7 @@ #include "SerialMuxChannels.h" #include #include "LongitudinalController.h" +#include /****************************************************************************** * Macros @@ -74,7 +75,8 @@ class App m_mqttClient(), m_v2vClient(m_mqttClient), m_initialDataSent(false), - m_longitudinalController() + m_longitudinalController(), + m_sendWaypointTimer() { } @@ -135,6 +137,9 @@ class App /** Minimum battery level in percent. */ static const uint8_t MIN_BATTERY_LEVEL = 10U; + /** Send waypoint timer interval. */ + static const uint32_t SEND_WAYPOINT_TIMER_INTERVAL = 500U; + /** MQTT topic name for birth messages. */ static const char* TOPIC_NAME_BIRTH; @@ -180,6 +185,16 @@ class App */ LongitudinalController m_longitudinalController; + /** + * Latest vehicle data from RU. + */ + Waypoint latestVehicleData; + + /** + * Send waypoint timer. + */ + SimpleTimer m_sendWaypointTimer; + private: /** * Handler of fatal errors in the Application. From 8715c640eec0c85ada633200988c8748f2fab9f5 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Fri, 19 Jan 2024 08:55:23 +0100 Subject: [PATCH 13/36] Fixed debug message --- lib/PlatoonService/src/V2VClient.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/PlatoonService/src/V2VClient.cpp b/lib/PlatoonService/src/V2VClient.cpp index 0af2e955..4a6dc00f 100644 --- a/lib/PlatoonService/src/V2VClient.cpp +++ b/lib/PlatoonService/src/V2VClient.cpp @@ -158,7 +158,7 @@ bool V2VClient::sendWaypoint(const Waypoint& waypoint) } else if (false == m_mqttClient.publish(m_outputTopic, false, payload)) { - LOG_ERROR("Failed to publish MQTT message to %s.", m_outputTopic); + LOG_ERROR("Failed to publish MQTT message to %s.", m_outputTopic.c_str()); } else { From 11ccef504dffdf3a41c2a43414994d4a258f714c Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 22 Jan 2024 15:48:44 +0100 Subject: [PATCH 14/36] Initial position sent using a command and not a dedicated channel --- lib/ConvoyLeader/src/App.cpp | 66 ++++++++++++++---------- lib/ConvoyLeader/src/App.h | 40 ++++++++++---- lib/ConvoyLeader/src/RemoteControl.h | 1 + lib/ConvoyLeader/src/SerialMuxChannels.h | 17 +++--- 4 files changed, 83 insertions(+), 41 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index 402df512..3b69876c 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -175,6 +175,7 @@ void App::setup() m_longitudinalController.setMotorSetpointCallback(motorSetpointCallback); m_sendWaypointTimer.start(SEND_WAYPOINT_TIMER_INTERVAL); + m_initialCommandTimer.start(SEND_WAYPOINT_TIMER_INTERVAL); isSuccessful = true; } @@ -215,33 +216,44 @@ void App::loop() /* Process Longitudinal Controller */ m_longitudinalController.process(); - if (false == m_initialDataSent) + if (true == m_initialCommandTimer.isTimeout()) { - SettingsHandler& settings = SettingsHandler::getInstance(); - VehicleData initialVehicleData; - initialVehicleData.xPos = settings.getInitialXPosition(); - initialVehicleData.yPos = settings.getInitialYPosition(); - initialVehicleData.orientation = settings.getInitialHeading(); - - if (true == m_smpServer.sendData(m_serialMuxProtChannelInitialVehicleData, &initialVehicleData, - sizeof(initialVehicleData))) + if (false == m_receivedMaxMotorSpeed) { - LOG_DEBUG("Initial vehicle data sent."); - m_initialDataSent = true; + Command payload; + payload.commandId = RemoteControl::CMD_ID_GET_MAX_SPEED; + + (void)m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &payload, sizeof(payload)); } - } - if (LongitudinalController::STATE_STARTUP == m_longitudinalController.getState()) - { - Command payload; - payload.commandId = RemoteControl::CMD_ID_GET_MAX_SPEED; + if (false == m_initialPositionSent) + { + SettingsHandler& settings = SettingsHandler::getInstance(); + Command initialVehicleDataCmd; + initialVehicleDataCmd.commandId = RemoteControl::CMD_ID_SET_INIT_POS; + initialVehicleDataCmd.xPos = settings.getInitialXPosition(); + initialVehicleDataCmd.yPos = settings.getInitialYPosition(); + initialVehicleDataCmd.orientation = settings.getInitialHeading(); + + (void)m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &initialVehicleDataCmd, + sizeof(initialVehicleDataCmd)); + } - (void)m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &payload, sizeof(payload)); + if ((false == m_receivedMaxMotorSpeed) || (false == m_initialPositionSent)) + { + /* Something is pending. */ + m_initialCommandTimer.restart(); + } + else + { + /* Nothing is pending anymore. */ + m_initialCommandTimer.stop(); + } } if (true == m_sendWaypointTimer.isTimeout()) { - if (false == m_v2vClient.sendWaypoint(latestVehicleData)) + if (false == m_v2vClient.sendWaypoint(m_latestVehicleData)) { LOG_WARNING("Waypoint could not be sent."); } @@ -254,12 +266,12 @@ void App::loop() void App::currentVehicleChannelCallback(const VehicleData& vehicleData) { - latestVehicleData = Waypoint(vehicleData.xPos, vehicleData.yPos, vehicleData.orientation, vehicleData.left, - vehicleData.right, vehicleData.center); + m_latestVehicleData = Waypoint(vehicleData.xPos, vehicleData.yPos, vehicleData.orientation, vehicleData.left, + vehicleData.right, vehicleData.center); - latestVehicleData.debugPrint(); + // latestVehicleData.debugPrint(); - m_longitudinalController.calculateTopMotorSpeed(latestVehicleData); + m_longitudinalController.calculateTopMotorSpeed(m_latestVehicleData); } bool App::motorSetpointCallback(const int16_t topCenterSpeed) @@ -382,11 +394,8 @@ bool App::setupSerialMuxProt() m_serialMuxProtChannelIdRemoteCtrl = m_smpServer.createChannel(COMMAND_CHANNEL_NAME, COMMAND_CHANNEL_DLC); m_serialMuxProtChannelIdMotorSpeeds = m_smpServer.createChannel(SPEED_SETPOINT_CHANNEL_NAME, SPEED_SETPOINT_CHANNEL_DLC); - m_serialMuxProtChannelInitialVehicleData = - m_smpServer.createChannel(INITIAL_VEHICLE_DATA_CHANNEL_NAME, INITIAL_VEHICLE_DATA_CHANNEL_DLC); - if ((0U != m_serialMuxProtChannelIdRemoteCtrl) && (0U != m_serialMuxProtChannelIdMotorSpeeds) && - (0U != m_serialMuxProtChannelInitialVehicleData)) + if ((0U != m_serialMuxProtChannelIdRemoteCtrl) && (0U != m_serialMuxProtChannelIdMotorSpeeds)) { isSuccessful = true; } @@ -421,6 +430,11 @@ void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize { LOG_DEBUG("Max Speed: %d", cmdRsp->maxMotorSpeed); application->setMaxMotorSpeed(cmdRsp->maxMotorSpeed); + application->notifyMaxMotorSpeedIsReceived(); + } + else if (RemoteControl::CMD_ID_SET_INIT_POS == cmdRsp->commandId) + { + application->notifyInitialPositionIsSet(); } } else diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index 796cadb2..89357d43 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -70,11 +70,10 @@ class App App() : m_serialMuxProtChannelIdRemoteCtrl(0U), m_serialMuxProtChannelIdMotorSpeeds(0U), - m_serialMuxProtChannelInitialVehicleData(0U), m_smpServer(Board::getInstance().getDevice().getStream(), this), m_mqttClient(), m_v2vClient(m_mqttClient), - m_initialDataSent(false), + m_initialPositionSent(false), m_longitudinalController(), m_sendWaypointTimer() { @@ -133,11 +132,27 @@ class App */ void setLastFollowerFeedback(const Waypoint& feedback); + /** + * Notify initial position is set. + */ + void notifyInitialPositionIsSet() + { + m_initialPositionSent = true; + } + + /** + * Notify max motor speed is received. + */ + void notifyMaxMotorSpeedIsReceived() + { + m_receivedMaxMotorSpeed = true; + } + private: /** Minimum battery level in percent. */ static const uint8_t MIN_BATTERY_LEVEL = 10U; - /** Send waypoint timer interval. */ + /** Send waypoint timer interval in ms. */ static const uint32_t SEND_WAYPOINT_TIMER_INTERVAL = 500U; /** MQTT topic name for birth messages. */ @@ -155,9 +170,6 @@ class App /** SerialMuxProt Channel id for sending motor speeds. */ uint8_t m_serialMuxProtChannelIdMotorSpeeds; - /** SerialMuxProt Channel id for sending initial position data. */ - uint8_t m_serialMuxProtChannelInitialVehicleData; - /** * SerialMuxProt Server Instance * @@ -176,9 +188,14 @@ class App V2VClient m_v2vClient; /** - * Flag for setting initial data through SMP. + * Flag for setting initial position through SMP. + */ + bool m_initialPositionSent; + + /** + * Flag for received the max motor speed through SMP. */ - bool m_initialDataSent; + bool m_receivedMaxMotorSpeed; /** * Longitudinal controller. @@ -188,13 +205,18 @@ class App /** * Latest vehicle data from RU. */ - Waypoint latestVehicleData; + Waypoint m_latestVehicleData; /** * Send waypoint timer. */ SimpleTimer m_sendWaypointTimer; + /** + * Timer for sending initial commands. + */ + SimpleTimer m_initialCommandTimer; + private: /** * Handler of fatal errors in the Application. diff --git a/lib/ConvoyLeader/src/RemoteControl.h b/lib/ConvoyLeader/src/RemoteControl.h index 077592c2..8137837e 100644 --- a/lib/ConvoyLeader/src/RemoteControl.h +++ b/lib/ConvoyLeader/src/RemoteControl.h @@ -65,6 +65,7 @@ namespace RemoteControl CMD_ID_REINIT_BOARD, /**< Re-initialize the board. Required for webots simulation. */ CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ CMD_ID_START_DRIVING, /**< Start driving. */ + CMD_ID_SET_INIT_POS, /**< Set initial position. */ } CmdId; diff --git a/lib/ConvoyLeader/src/SerialMuxChannels.h b/lib/ConvoyLeader/src/SerialMuxChannels.h index 544d649b..8abc6709 100644 --- a/lib/ConvoyLeader/src/SerialMuxChannels.h +++ b/lib/ConvoyLeader/src/SerialMuxChannels.h @@ -69,12 +69,6 @@ /** DLC of Current Vehicle Data Channel */ #define CURRENT_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) -/** Name of Channel to send Initial Vehicle Data to. */ -#define INITIAL_VEHICLE_DATA_CHANNEL_NAME "INIT_DATA" - -/** DLC of Initial Vehicle Data Channel */ -#define INITIAL_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) - /****************************************************************************** * Types and Classes *****************************************************************************/ @@ -83,6 +77,17 @@ typedef struct _Command { uint8_t commandId; /**< Command ID */ + + union + { + struct + { + int32_t xPos; /**< X position [mm]. */ + int32_t yPos; /**< Y position [mm]. */ + int32_t orientation; /**< Orientation [mrad]. */ + }; + }; + } __attribute__((packed)) Command; /** Struct of the "Command Response" channel payload. */ From 9b35d9166913b1793954b21c45899b78b3bcce38 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 22 Jan 2024 15:56:18 +0100 Subject: [PATCH 15/36] Added new architecture diagrams --- .../uml/LogicalView/Leader/Leader.puml | 102 ++++++++++++++ .../uml/ProcessView/Leader/AppSequence.puml | 131 ++++++++++++++++++ .../Leader/SerialMuxProtCommunication.puml | 53 +++++++ .../uml/ProcessView/Leader/StateMachine.puml | 43 ++++++ 4 files changed, 329 insertions(+) create mode 100644 doc/architecture/uml/LogicalView/Leader/Leader.puml create mode 100644 doc/architecture/uml/ProcessView/Leader/AppSequence.puml create mode 100644 doc/architecture/uml/ProcessView/Leader/SerialMuxProtCommunication.puml create mode 100644 doc/architecture/uml/ProcessView/Leader/StateMachine.puml diff --git a/doc/architecture/uml/LogicalView/Leader/Leader.puml b/doc/architecture/uml/LogicalView/Leader/Leader.puml new file mode 100644 index 00000000..294d27bc --- /dev/null +++ b/doc/architecture/uml/LogicalView/Leader/Leader.puml @@ -0,0 +1,102 @@ +@startuml DcsClassDiagram + +Title DCS Class Diagram + +class "Application" as app { + + setup() : void + + loop() : void + + release() : void + - m_serialMuxProtServer : SMPServer + - m_systemStateMachine : StateMachine + - m_remoteControlHandler : RemoteControlHandler + - m_safetyMonitor : SafetyMonitor +} + +note right of app + Callbacks for SerialMuxProtServer and MqttClient are defined here. + Uses the setters of each class or state to forward the data. + Uses the getters of each class or state to get the data that shall be sent to the RU or the V2V controller. +end note + +class "V2V Controller" as v2v { + - m_mqttClient : MqttClient + - m_rxWaypointQueue : Queue + + init(MqttConfig) : bool + + process() : bool + + sendWaypoint(const Waypoint&) : bool + + getWaypoint(Waypoint&) : bool +} + +class StateMachine <> { + + setState(state : IState*) : void + + getState() : IState* + + process() : void +} + +class "RemoteCommandResponseHandler" as smpHandler { + + processResponse(const CommandResponse&) : void +} + +note right of smpHandler + This class is responsible for handling + the response from the remote command + sent to RU. + - Notifies StartupState of successful commands. + - Sets max speed in DrivingState. +end note + +class "SafetyMonitor" as safety { + + process(StateMachine& sm) : void + + getPendingHeartbeat(const Hearbeat& hb) : bool + + heartbeatCallback(const Hearbeat& hb) : void +} + +interface IState { + + {abstract} entry() : void + + {abstract} process(sm : StateMachine&) : void + + {abstract} exit() : void +} + +class StartupState <> +{ + + getPendingCommand(Command& cmd) : bool + + notify(Event event) : void +} + +class IdleState <> + +class DrivingState <> { + - m_longitudinalController : LongitudinalController + + setMaxMotorSpeed(int16_t maxMotorSpeed) : void + + calculateTopMotorSpeed(const Waypoint& currentVehicleData): int16_t + + setLastFollowerFeedback(const Waypoint& feedback) : void + + getLatestVehicleData(Waypoint& vehicleData) : bool +} + +protocol "SerialMuxChannels" as smpch { + + typedef SerialMuxProtServer SMPServer + + struct Command + + struct CommandReponse + + struct SpeedData + + struct VehicleData +} + +app *--> v2v +app *--> StateMachine +app *--> smpHandler +app *--> safety +app ..> DrivingState: <> +app ..> StartupState: <> +app ..> smpch: <> + +StateMachine o--> "0..1" IState + +IState <|.. StartupState: <> +IState <|.. IdleState: <> +IState <|.. DrivingState: <> +IState <|.. ErrorState: <> + +smpHandler ..> StartupState: <> +smpHandler ..> DrivingState: <> + +@enduml \ No newline at end of file diff --git a/doc/architecture/uml/ProcessView/Leader/AppSequence.puml b/doc/architecture/uml/ProcessView/Leader/AppSequence.puml new file mode 100644 index 00000000..400e278f --- /dev/null +++ b/doc/architecture/uml/ProcessView/Leader/AppSequence.puml @@ -0,0 +1,131 @@ +@startuml LeaderAppSequenceDiagram + +autoactivate on + +Title Leader App Sequence Diagram + +participant "App" as App +participant "State Machine" as SM +participant "SMP Server" as Smp +participant "V2V Client" as Mqtt +participant "Remote Command Response Handler" as RCRH +participant "Startup State" as StartupState +participant "Driving State" as DrivingState +participant "Safety Monitor" as SafetyMonitor + +entity "RadonUlzer" as RU + +== State Machine: Startup State == + +group Startup Commands [Repeats for each pending command] + App -> SM : process() + SM -> StartupState : process() + return + return + + App -> StartupState : getPendingCommand(Command& cmd) : bool + StartupState -> SM: inCorrectState() + return true + return pending command is valid + + App -> Smp : sendCommand(cmd) : bool + Smp -> RU : send + deactivate + return command sent + + RU -->> Smp : response + + App -> Smp : process() + Smp -> App : CMD_RSP callback + App -> RCRH : handleCommandResponse(rsp) + RCRH -> StartupState : notify(Event event) + return + RCRH -> DrivingState : setMaxSpeed(speed) + return + return + return + return +end group + +App -> SM : process() + SM -> StartupState : process() + StartupState -> SM : setState(IdleState) + return + return +return + +== State Machine: Idle State == + +group Wait for release + App -> Mqtt : process() + Mqtt -> App : cmdCallback(releaseCmd) + App -> Smp : sendCommand(releaseCmd) + Smp -> RU : send + deactivate + return + return + return + + RU -->> Smp : response + + App -> Smp : process() + Smp -> App : CMD_RSP callback + App -> RCRH : handleCommandResponse(OK) + RCRH -> SM : setState(DrivingState) + return + return + return + return +end group + +== State Machine: Driving State == +group Driving + App -> Smp : process() + Smp -> App : currentVehicleDataCallback(CurrentVehicleData data) + App -> DrivingState : calculateTopMotorSpeed(const Waypoint& currentVehicleData): int16_t + DrivingState -> SM : inCorrectState() + return true + return top speed + App -> Smp : sendSpeed(topSpeed) + Smp -> RU : send + deactivate + return + return + return + + App -> Mqtt : process() + Mqtt -> App : lastFollowerFeedbackCallback() + App -> DrivingState : setLastFollowerFeedback(const Waypoint& feedback) + return + return + return + + App -> DrivingState : getLatestVehicleData(Waypoint& vehicleData) + DrivingState -> SM : inCorrectState() + return true + return vehicle data is valid + + App -> Mqtt : sendWaypoint(const Waypoint& vehicleData) + return + +end group + +== Stateless == + +loop + App -> Smp : process() + Smp -> App : Heartbeat callback + App -> SafetyMonitor : heartbeatCallback() + return + return + return + + App -> SafetyMonitor : process() + SafetyMonitor -> SafetyMonitor : checkHeartbeat() + return false + SafetyMonitor -> SM : setState(ErrorState) + return + return +end loop + +@enduml \ No newline at end of file diff --git a/doc/architecture/uml/ProcessView/Leader/SerialMuxProtCommunication.puml b/doc/architecture/uml/ProcessView/Leader/SerialMuxProtCommunication.puml new file mode 100644 index 00000000..75c34ec9 --- /dev/null +++ b/doc/architecture/uml/ProcessView/Leader/SerialMuxProtCommunication.puml @@ -0,0 +1,53 @@ +@startuml CommSequence + +Title Sequence Diagram RU and DCS + +autoactivate on + +participant "RU" as RU +participant "DCS" as DCS + +note across: Possible STATUS = {STARTUP, OK, ERROR} + +group State-Independent Messages + RU --> DCS: STATUS: RU_STATE + DCS --> RU: STATUS: DCS_STATE +end + +group StateLessSetup + RU --> DCS: Subscribe: STATUS + RU --> DCS: Subscribe: CMD + RU --> DCS: Subscribe: SPEED_SETPOINT + DCS --> RU: Subscribe: STATUS + DCS --> RU: Subscribe: CMD_RSP + DCS --> RU: Subscribe: CURRENT_VEHICLE_DATA +end + +group StatupState + DCS -> RU: CMD: GetMaxSpeed + RU -->> DCS: CMD_RSP:OK + maxSpeed + DCS -> RU: CMD: SetInitialPosition + RU -->> DCS: CMD_RSP:OK +end + +group LineSensorsCalibrationState + +end + +group IdleState + DCS -> RU: CMD: Release + RU -->> DCS: CMD_RSP:OK +end + +group DrivingState + RU ->> DCS: CURRENT_VEHICLE_DATA:Data + DCS -> DCS: CalculateSpeed + return + DCS --> RU: SPEED_SETPOINT: Top Center Speed +end + +group ErrorState + +end + +@enduml \ No newline at end of file diff --git a/doc/architecture/uml/ProcessView/Leader/StateMachine.puml b/doc/architecture/uml/ProcessView/Leader/StateMachine.puml new file mode 100644 index 00000000..2b3d0b95 --- /dev/null +++ b/doc/architecture/uml/ProcessView/Leader/StateMachine.puml @@ -0,0 +1,43 @@ +@startuml DcsStateMachine + +Title DroidControlShip ConvoyLeader State Machine + +hide empty description + +state StatelessSetup +state ErrorState +state StartupState +state IdleState +state DrivingState + +[*] --> StatelessSetup: Power up +StatelessSetup -right-> StartupState : [Basic services initialized] +StatelessSetup --> ErrorState: [Basic services init failed] + +StartupState --> IdleState : [Initial data loaded and exchanged] +StartupState --> ErrorState : [Error in initialization] + +IdleState --> DrivingState : [Release command received] +IdleState --> ErrorState : [Lost connection to RU] + +DrivingState --> IdleState : [Stop command received] +DrivingState --> ErrorState : [Error while driving] + +note left of StatelessSetup + Initialization of: + - Logger + - HAL + - Network + - MQTT + - SMP + - Longitudinal Controller +end note + +note right of StartupState + Establish connections: + - Network + - MQTT + - SMP +end note + +@enduml \ No newline at end of file From f1bb06e818492ef0a2d2d9844733fc4452a1d91d Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 23 Jan 2024 11:51:20 +0100 Subject: [PATCH 16/36] Simplified leader app sequence --- .../uml/ProcessView/Leader/AppSequence.puml | 60 +++++++++---------- 1 file changed, 28 insertions(+), 32 deletions(-) diff --git a/doc/architecture/uml/ProcessView/Leader/AppSequence.puml b/doc/architecture/uml/ProcessView/Leader/AppSequence.puml index 400e278f..3b4b220f 100644 --- a/doc/architecture/uml/ProcessView/Leader/AppSequence.puml +++ b/doc/architecture/uml/ProcessView/Leader/AppSequence.puml @@ -18,17 +18,10 @@ entity "RadonUlzer" as RU == State Machine: Startup State == group Startup Commands [Repeats for each pending command] - App -> SM : process() - SM -> StartupState : process() - return - return + App -> StartupState : getPendingCommand + return pendingCommand - App -> StartupState : getPendingCommand(Command& cmd) : bool - StartupState -> SM: inCorrectState() - return true - return pending command is valid - - App -> Smp : sendCommand(cmd) : bool + App -> Smp : sendCommand(pendingCommand) Smp -> RU : send deactivate return command sent @@ -38,28 +31,29 @@ group Startup Commands [Repeats for each pending command] App -> Smp : process() Smp -> App : CMD_RSP callback App -> RCRH : handleCommandResponse(rsp) - RCRH -> StartupState : notify(Event event) - return - RCRH -> DrivingState : setMaxSpeed(speed) - return + alt OK + RCRH -> StartupState : notify(Event event) + return + end + alt max speed received + RCRH -> DrivingState : setMaxSpeed(speed) + return + end + alt error received + RCRH -> SM : setState(ErrorState) + return + end return return return end group -App -> SM : process() - SM -> StartupState : process() - StartupState -> SM : setState(IdleState) - return - return -return - == State Machine: Idle State == group Wait for release App -> Mqtt : process() - Mqtt -> App : cmdCallback(releaseCmd) - App -> Smp : sendCommand(releaseCmd) + Mqtt -> App : release command + App -> Smp : release command Smp -> RU : send deactivate return @@ -70,9 +64,15 @@ group Wait for release App -> Smp : process() Smp -> App : CMD_RSP callback - App -> RCRH : handleCommandResponse(OK) - RCRH -> SM : setState(DrivingState) - return + App -> RCRH : handleCommandResponse(rsp) + alt OK + RCRH -> SM : setState(DrivingState) + return + end + alt error received + RCRH -> SM : setState(ErrorState) + return + end return return return @@ -81,10 +81,8 @@ end group == State Machine: Driving State == group Driving App -> Smp : process() - Smp -> App : currentVehicleDataCallback(CurrentVehicleData data) - App -> DrivingState : calculateTopMotorSpeed(const Waypoint& currentVehicleData): int16_t - DrivingState -> SM : inCorrectState() - return true + Smp -> App : CurrentVehicleData + App -> DrivingState : CurrentVehicleData return top speed App -> Smp : sendSpeed(topSpeed) Smp -> RU : send @@ -101,8 +99,6 @@ group Driving return App -> DrivingState : getLatestVehicleData(Waypoint& vehicleData) - DrivingState -> SM : inCorrectState() - return true return vehicle data is valid App -> Mqtt : sendWaypoint(const Waypoint& vehicleData) From b3883ae37caf3624d818bf01a3f12a17ed310d03 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 23 Jan 2024 12:45:34 +0100 Subject: [PATCH 17/36] Added SMP Channels descriptions --- doc/configuration/SerialMuxChannels.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 doc/configuration/SerialMuxChannels.md diff --git a/doc/configuration/SerialMuxChannels.md b/doc/configuration/SerialMuxChannels.md new file mode 100644 index 00000000..572e9bb6 --- /dev/null +++ b/doc/configuration/SerialMuxChannels.md @@ -0,0 +1,13 @@ +# SerialMuxProtocol Channels + +The following table lists the current SerialMuxProtocol channels being used in the applications, along with their characteristics. +Specific payload implementations can be found in the respective `SerialMuxChannels.h` files for each application + + +| Name | Publisher | Payload | Send-type | Comment | +|:---------:|:---------:|:---------------:|-----------------------|:-------------------------------------------------:| +| CMD | DCS | Command | Triggered / On-Demand | | +| CMD_RSP | RU | CommandResponse | Triggered / On-Demand | Responds to commands from the CMD channel. | +| SPEED_SET | DCS | SpeedData | Periodic | | +| CURR_DATA | RU | VehicleData | Periodic | | +| STATUS | DCS / RU | Status | Periodic | Used as an application heartbeat between systems. | \ No newline at end of file From 83b7f60ca2fb4a08723c514a762ad9878c804e3b Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 23 Jan 2024 13:53:18 +0100 Subject: [PATCH 18/36] Added State Machine components --- lib/Service/src/IState.h | 102 +++++++++++++++++++++++++++ lib/Service/src/StateMachine.cpp | 100 +++++++++++++++++++++++++++ lib/Service/src/StateMachine.h | 114 +++++++++++++++++++++++++++++++ 3 files changed, 316 insertions(+) create mode 100644 lib/Service/src/IState.h create mode 100644 lib/Service/src/StateMachine.cpp create mode 100644 lib/Service/src/StateMachine.h diff --git a/lib/Service/src/IState.h b/lib/Service/src/IState.h new file mode 100644 index 00000000..143dfc08 --- /dev/null +++ b/lib/Service/src/IState.h @@ -0,0 +1,102 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract state interface + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef ISTATE_H +#define ISTATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/* Forward declaration */ +class StateMachine; + +/** State interface */ +class IState +{ +public: + /** + * Default constructor. + */ + IState() + { + } + + /** + * Default destructor. + */ + virtual ~IState() + { + } + + /** + * If the state is entered, this method will called once. + */ + virtual void entry() = 0; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + virtual void process(StateMachine& sm) = 0; + + /** + * If the state is left, this method will be called once. + */ + virtual void exit() = 0; + +protected: +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ISTATE_H */ +/** @} */ diff --git a/lib/Service/src/StateMachine.cpp b/lib/Service/src/StateMachine.cpp new file mode 100644 index 00000000..02117342 --- /dev/null +++ b/lib/Service/src/StateMachine.cpp @@ -0,0 +1,100 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Statemachine + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void StateMachine::process() +{ + /* Change state? */ + if (nullptr != m_nextState) + { + /* Leave current state */ + if (nullptr != m_currentState) + { + m_currentState->exit(); + } + + m_currentState = m_nextState; + m_nextState = nullptr; + + /* Enter new state */ + m_currentState->entry(); + } + + /* Process current state */ + if (nullptr != m_currentState) + { + m_currentState->process(*this); + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/src/StateMachine.h b/lib/Service/src/StateMachine.h new file mode 100644 index 00000000..d2fed5e2 --- /dev/null +++ b/lib/Service/src/StateMachine.h @@ -0,0 +1,114 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Statemachine + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef STATE_MACHINE_H +#define STATE_MACHINE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IState.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system state machine. */ +class StateMachine +{ +public: + /** + * Default constructor. + */ + StateMachine() : m_currentState(nullptr), m_nextState(nullptr) + { + } + + /** + * Default destructor. + */ + ~StateMachine() + { + } + + /** + * Set next state. + * + * @param[in] state Next state. + */ + void setState(IState* state) + { + m_nextState = state; + } + + /** + * Get current state. + * + * @return Current state. + */ + IState* getState() + { + return m_currentState; + } + + /** + * Process state machine. + */ + void process(); + +protected: +private: + IState* m_currentState; /**< Current active state */ + IState* m_nextState; /**< Next state */ + + /* Not allowed. */ + StateMachine(const StateMachine& sm); /**< Copy construction of an instance. */ + StateMachine& operator=(const StateMachine& sm); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* STATE_MACHINE_H */ +/** @} */ From b62887d4a5c378d6a10ca97e557157b63e7e5724 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 23 Jan 2024 13:58:23 +0100 Subject: [PATCH 19/36] Defined SMPServer as typedef of templated Server Reorganized includes --- lib/ConvoyLeader/src/App.h | 8 ++++---- lib/ConvoyLeader/src/SerialMuxChannels.h | 4 ++++ 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index 89357d43..bbfce450 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -46,11 +46,11 @@ #include #include #include -#include -#include "SerialMuxChannels.h" +#include +#include #include +#include "SerialMuxChannels.h" #include "LongitudinalController.h" -#include /****************************************************************************** * Macros @@ -175,7 +175,7 @@ class App * * @tparam tMaxChannels set to MAX_CHANNELS, defined in SerialMuxChannels.h. */ - SerialMuxProtServer m_smpServer; + SMPServer m_smpServer; /** * MQTTClient Instance diff --git a/lib/ConvoyLeader/src/SerialMuxChannels.h b/lib/ConvoyLeader/src/SerialMuxChannels.h index 8abc6709..6e7688b0 100644 --- a/lib/ConvoyLeader/src/SerialMuxChannels.h +++ b/lib/ConvoyLeader/src/SerialMuxChannels.h @@ -37,6 +37,7 @@ *****************************************************************************/ #include +#include /****************************************************************************** * Macros @@ -73,6 +74,9 @@ * Types and Classes *****************************************************************************/ +/** SerialMuxProt Server with fixed template argument. */ +typedef SerialMuxProtServer SMPServer; + /** Struct of the "Command" channel payload. */ typedef struct _Command { From b83f10285ff6a745f661278245c1eaf35989d433 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 23 Jan 2024 14:22:54 +0100 Subject: [PATCH 20/36] Introduced State Machine and startup state --- lib/ConvoyLeader/src/App.cpp | 10 +++ lib/ConvoyLeader/src/App.h | 3 + lib/ConvoyLeader/src/StartupState.cpp | 91 ++++++++++++++++++++ lib/ConvoyLeader/src/StartupState.h | 117 ++++++++++++++++++++++++++ 4 files changed, 221 insertions(+) create mode 100644 lib/ConvoyLeader/src/StartupState.cpp create mode 100644 lib/ConvoyLeader/src/StartupState.h diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index 3b69876c..d536acb0 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -41,6 +41,7 @@ #include #include #include +#include "StartupState.h" /****************************************************************************** * Compiler Switches @@ -169,14 +170,19 @@ void App::setup() } else { + /* Setup longitudinal controller. */ LongitudinalController::MotorSetpointCallback motorSetpointCallback = [this](const int16_t& topCenterSpeed) { return this->motorSetpointCallback(topCenterSpeed); }; m_longitudinalController.setMotorSetpointCallback(motorSetpointCallback); + /* Initialize timers. */ m_sendWaypointTimer.start(SEND_WAYPOINT_TIMER_INTERVAL); m_initialCommandTimer.start(SEND_WAYPOINT_TIMER_INTERVAL); + /* Start with startup state. */ + m_systemStateMachine.setState(&StartupState::getInstance()); + isSuccessful = true; } } @@ -216,6 +222,10 @@ void App::loop() /* Process Longitudinal Controller */ m_longitudinalController.process(); + /* Process System State Machine */ + m_systemStateMachine.process(); + + /* Process periodic tasks. */ if (true == m_initialCommandTimer.isTimeout()) { if (false == m_receivedMaxMotorSpeed) diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index bbfce450..e1447457 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -187,6 +187,9 @@ class App */ V2VClient m_v2vClient; + /** The system state machine. */ + StateMachine m_systemStateMachine; + /** * Flag for setting initial position through SMP. */ diff --git a/lib/ConvoyLeader/src/StartupState.cpp b/lib/ConvoyLeader/src/StartupState.cpp new file mode 100644 index 00000000..66c3f685 --- /dev/null +++ b/lib/ConvoyLeader/src/StartupState.cpp @@ -0,0 +1,91 @@ +/* MIT License + * + * Copyright (c) 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup State. + * @author Gabryel Reyes + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include "StartupState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void StartupState::entry() +{ + /* Nothing to do */ +} + +void StartupState::process(StateMachine& sm) +{ + /* Nothing to do */ +} + +void StartupState::exit() +{ + /* Nothing to do */ +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/ConvoyLeader/src/StartupState.h b/lib/ConvoyLeader/src/StartupState.h new file mode 100644 index 00000000..32516900 --- /dev/null +++ b/lib/ConvoyLeader/src/StartupState.h @@ -0,0 +1,117 @@ +/* MIT License + * + * Copyright (c) 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup State. + * @author Gabryel Reyes + * + * @addtogroup Application + * + * @{ + */ +#ifndef STARTUP_STATE_H +#define STARTUP_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system startup state. */ +class StartupState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static StartupState& getInstance() + { + static StartupState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** + * Default constructor. + */ + StartupState() + { + } + + /** + * Default destructor. + */ + virtual ~StartupState() + { + } + + /* Not allowed. */ + StartupState(const StartupState& state); /**< Copy construction of an instance. */ + StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* STARTUP_STATE_H */ +/** @} */ From e5b4fe7012c4bd5c11e0633dd020048e76bcb298 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 23 Jan 2024 15:32:25 +0100 Subject: [PATCH 21/36] Added Startup State --- lib/ConvoyLeader/src/App.cpp | 37 ++++---------- lib/ConvoyLeader/src/App.h | 27 ----------- lib/ConvoyLeader/src/StartupState.cpp | 70 +++++++++++++++++++++++++-- lib/ConvoyLeader/src/StartupState.h | 34 ++++++++++++- 4 files changed, 109 insertions(+), 59 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index d536acb0..ae5f4f82 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -226,39 +226,20 @@ void App::loop() m_systemStateMachine.process(); /* Process periodic tasks. */ - if (true == m_initialCommandTimer.isTimeout()) + if ((true == m_initialCommandTimer.isTimeout()) && (true == m_smpServer.isSynced())) { - if (false == m_receivedMaxMotorSpeed) - { - Command payload; - payload.commandId = RemoteControl::CMD_ID_GET_MAX_SPEED; - - (void)m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &payload, sizeof(payload)); - } + Command* pendingCommand = StartupState::getInstance().getPendingCommand(); - if (false == m_initialPositionSent) + if (nullptr != pendingCommand) { - SettingsHandler& settings = SettingsHandler::getInstance(); - Command initialVehicleDataCmd; - initialVehicleDataCmd.commandId = RemoteControl::CMD_ID_SET_INIT_POS; - initialVehicleDataCmd.xPos = settings.getInitialXPosition(); - initialVehicleDataCmd.yPos = settings.getInitialYPosition(); - initialVehicleDataCmd.orientation = settings.getInitialHeading(); - - (void)m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &initialVehicleDataCmd, - sizeof(initialVehicleDataCmd)); - } + if (false == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, pendingCommand, sizeof(Command))) + { + LOG_WARNING("Failed to send pending command to RU."); + } - if ((false == m_receivedMaxMotorSpeed) || (false == m_initialPositionSent)) - { /* Something is pending. */ m_initialCommandTimer.restart(); } - else - { - /* Nothing is pending anymore. */ - m_initialCommandTimer.stop(); - } } if (true == m_sendWaypointTimer.isTimeout()) @@ -440,11 +421,11 @@ void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize { LOG_DEBUG("Max Speed: %d", cmdRsp->maxMotorSpeed); application->setMaxMotorSpeed(cmdRsp->maxMotorSpeed); - application->notifyMaxMotorSpeedIsReceived(); + StartupState::getInstance().notifyCommandProcessed(); } else if (RemoteControl::CMD_ID_SET_INIT_POS == cmdRsp->commandId) { - application->notifyInitialPositionIsSet(); + StartupState::getInstance().notifyCommandProcessed(); } } else diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index e1447457..d9dc2e96 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -73,7 +73,6 @@ class App m_smpServer(Board::getInstance().getDevice().getStream(), this), m_mqttClient(), m_v2vClient(m_mqttClient), - m_initialPositionSent(false), m_longitudinalController(), m_sendWaypointTimer() { @@ -132,22 +131,6 @@ class App */ void setLastFollowerFeedback(const Waypoint& feedback); - /** - * Notify initial position is set. - */ - void notifyInitialPositionIsSet() - { - m_initialPositionSent = true; - } - - /** - * Notify max motor speed is received. - */ - void notifyMaxMotorSpeedIsReceived() - { - m_receivedMaxMotorSpeed = true; - } - private: /** Minimum battery level in percent. */ static const uint8_t MIN_BATTERY_LEVEL = 10U; @@ -190,16 +173,6 @@ class App /** The system state machine. */ StateMachine m_systemStateMachine; - /** - * Flag for setting initial position through SMP. - */ - bool m_initialPositionSent; - - /** - * Flag for received the max motor speed through SMP. - */ - bool m_receivedMaxMotorSpeed; - /** * Longitudinal controller. */ diff --git a/lib/ConvoyLeader/src/StartupState.cpp b/lib/ConvoyLeader/src/StartupState.cpp index 66c3f685..fb514bbf 100644 --- a/lib/ConvoyLeader/src/StartupState.cpp +++ b/lib/ConvoyLeader/src/StartupState.cpp @@ -34,6 +34,8 @@ *****************************************************************************/ #include "StartupState.h" +#include "RemoteControl.h" +#include /****************************************************************************** * Compiler Switches @@ -61,17 +63,79 @@ void StartupState::entry() { - /* Nothing to do */ + m_isActive = true; } void StartupState::process(StateMachine& sm) { - /* Nothing to do */ + + SettingsHandler& settings = SettingsHandler::getInstance(); + + switch (m_pendingCommandCounter) + { + case CMD_GET_MAX_SPEED: + if (nullptr == m_pendingCommand) + { + /* Create new pending commmand. */ + m_pendingCommand = new Command{RemoteControl::CMD_ID_GET_MAX_SPEED}; + } + else + { + /* Command is still pending. */ + } + break; + + case CMD_SET_INIT_POS: + if (nullptr == m_pendingCommand) + { + /* Create new pending commmand. */ + m_pendingCommand = new Command; + m_pendingCommand->commandId = RemoteControl::CMD_ID_SET_INIT_POS; + m_pendingCommand->xPos = settings.getInitialXPosition(); + m_pendingCommand->yPos = settings.getInitialYPosition(); + m_pendingCommand->orientation = settings.getInitialHeading(); + } + else + { + /* Command is still pending. */ + } + break; + + case CMD_NONE: + /* All commands processed. Switch to idle state. */ + // sm.setState(&IdleState::getInstance()); + break; + + default: + break; + } } void StartupState::exit() { - /* Nothing to do */ + /* Ensure the pending command is deleted. */ + if (nullptr != m_pendingCommand) + { + delete m_pendingCommand; + m_pendingCommand = nullptr; + } + + m_isActive = false; +} + +Command* StartupState::getPendingCommand() +{ + return (true == m_isActive) ? m_pendingCommand : nullptr; +} + +void StartupState::notifyCommandProcessed() +{ + if (nullptr != m_pendingCommand) + { + delete m_pendingCommand; + m_pendingCommand = nullptr; + m_pendingCommandCounter++; + } } /****************************************************************************** diff --git a/lib/ConvoyLeader/src/StartupState.h b/lib/ConvoyLeader/src/StartupState.h index 32516900..6432f6c8 100644 --- a/lib/ConvoyLeader/src/StartupState.h +++ b/lib/ConvoyLeader/src/StartupState.h @@ -44,6 +44,9 @@ *****************************************************************************/ #include +#include +#include "SerialMuxChannels.h" +#include "SimpleTimer.hpp" /****************************************************************************** * Macros @@ -57,6 +60,14 @@ class StartupState : public IState { public: + /** Commands to send and process in the Startup State. */ + enum StartupCommands : uint8_t + { + CMD_GET_MAX_SPEED = 0, /**< Get maximum motor speed. */ + CMD_SET_INIT_POS, /**< Set initial position. */ + CMD_NONE /**< No pending command. Required to signal the end of the enum. */ + }; + /** * Get state instance. * @@ -88,12 +99,33 @@ class StartupState : public IState */ void exit() final; + /** + * Get pending command. If there is no pending command or the state is not active, it will return nullptr. + * + * @return Pending command or nullptr. + */ + Command* getPendingCommand(); + + /** + * Notify the state, that the pending command is successfully processed by RU. + */ + void notifyCommandProcessed(); + protected: private: + /** Flag: State is active. */ + bool m_isActive; + + /** Pending command. */ + Command* m_pendingCommand; + + /** Pending command counter. */ + uint8_t m_pendingCommandCounter; + /** * Default constructor. */ - StartupState() + StartupState() : IState(), m_isActive(false), m_pendingCommand(nullptr), m_pendingCommandCounter(CMD_GET_MAX_SPEED) { } From 62d7ccec670598a267052d8e4fb77b0ff809fb7a Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 23 Jan 2024 15:36:05 +0100 Subject: [PATCH 22/36] Fixed include order --- lib/ConvoyLeader/src/StartupState.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/ConvoyLeader/src/StartupState.h b/lib/ConvoyLeader/src/StartupState.h index 6432f6c8..2d28f38e 100644 --- a/lib/ConvoyLeader/src/StartupState.h +++ b/lib/ConvoyLeader/src/StartupState.h @@ -45,8 +45,8 @@ #include #include +#include #include "SerialMuxChannels.h" -#include "SimpleTimer.hpp" /****************************************************************************** * Macros From 8215470d74b2e8d8cebe7fd2ffe72fa10d319292 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 23 Jan 2024 15:52:02 +0100 Subject: [PATCH 23/36] Fixed and updated snippets to latest copyright --- .vscode/templates.code-snippets | 4 ++-- lib/ConvoyLeader/src/LongitudinalController.cpp | 2 +- lib/ConvoyLeader/src/LongitudinalController.h | 2 +- lib/ConvoyLeader/src/RemoteControl.h | 2 +- lib/ConvoyLeader/src/StartupState.cpp | 2 +- lib/ConvoyLeader/src/StartupState.h | 2 +- lib/PlatoonService/src/Waypoint.cpp | 2 +- lib/RemoteControl/src/RemoteControl.h | 2 +- 8 files changed, 9 insertions(+), 9 deletions(-) diff --git a/.vscode/templates.code-snippets b/.vscode/templates.code-snippets index 4ad7a03a..8387abf9 100644 --- a/.vscode/templates.code-snippets +++ b/.vscode/templates.code-snippets @@ -61,7 +61,7 @@ "body": [ "/* MIT License", " *", - " * Copyright (c) $CURRENT_YEAR Andreas Merkle ", + " * Copyright (c) 2023 - $CURRENT_YEAR Andreas Merkle ", " *", " * Permission is hereby granted, free of charge, to any person obtaining a copy", " * of this software and associated documentation files (the \"Software\"), to deal", @@ -128,7 +128,7 @@ "body": [ "/* MIT License", " *", - " * Copyright (c) $CURRENT_YEAR Andreas Merkle ", + " * Copyright (c) 2023 - $CURRENT_YEAR Andreas Merkle ", " *", " * Permission is hereby granted, free of charge, to any person obtaining a copy", " * of this software and associated documentation files (the \"Software\"), to deal", diff --git a/lib/ConvoyLeader/src/LongitudinalController.cpp b/lib/ConvoyLeader/src/LongitudinalController.cpp index b353d7d8..3d9f6c47 100644 --- a/lib/ConvoyLeader/src/LongitudinalController.cpp +++ b/lib/ConvoyLeader/src/LongitudinalController.cpp @@ -1,6 +1,6 @@ /* MIT License * - * Copyright (c) 2024 Andreas Merkle + * Copyright (c) 2023 - 2024 Andreas Merkle * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal diff --git a/lib/ConvoyLeader/src/LongitudinalController.h b/lib/ConvoyLeader/src/LongitudinalController.h index 6044a664..bb9c8e57 100644 --- a/lib/ConvoyLeader/src/LongitudinalController.h +++ b/lib/ConvoyLeader/src/LongitudinalController.h @@ -1,6 +1,6 @@ /* MIT License * - * Copyright (c) 2024 Andreas Merkle + * Copyright (c) 2023 - 2024 Andreas Merkle * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal diff --git a/lib/ConvoyLeader/src/RemoteControl.h b/lib/ConvoyLeader/src/RemoteControl.h index 8137837e..376d8ef9 100644 --- a/lib/ConvoyLeader/src/RemoteControl.h +++ b/lib/ConvoyLeader/src/RemoteControl.h @@ -1,6 +1,6 @@ /* MIT License * - * Copyright (c) 2024 Andreas Merkle + * Copyright (c) 2023 - 2024 Andreas Merkle * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal diff --git a/lib/ConvoyLeader/src/StartupState.cpp b/lib/ConvoyLeader/src/StartupState.cpp index fb514bbf..1b78faf5 100644 --- a/lib/ConvoyLeader/src/StartupState.cpp +++ b/lib/ConvoyLeader/src/StartupState.cpp @@ -1,6 +1,6 @@ /* MIT License * - * Copyright (c) 2024 Andreas Merkle + * Copyright (c) 2023 - 2024 Andreas Merkle * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal diff --git a/lib/ConvoyLeader/src/StartupState.h b/lib/ConvoyLeader/src/StartupState.h index 2d28f38e..b3719f70 100644 --- a/lib/ConvoyLeader/src/StartupState.h +++ b/lib/ConvoyLeader/src/StartupState.h @@ -1,6 +1,6 @@ /* MIT License * - * Copyright (c) 2024 Andreas Merkle + * Copyright (c) 2023 - 2024 Andreas Merkle * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal diff --git a/lib/PlatoonService/src/Waypoint.cpp b/lib/PlatoonService/src/Waypoint.cpp index b3160cdb..f2c5d32d 100644 --- a/lib/PlatoonService/src/Waypoint.cpp +++ b/lib/PlatoonService/src/Waypoint.cpp @@ -1,6 +1,6 @@ /* MIT License * - * Copyright (c) 2024 Andreas Merkle + * Copyright (c) 2023 - 2024 Andreas Merkle * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal diff --git a/lib/RemoteControl/src/RemoteControl.h b/lib/RemoteControl/src/RemoteControl.h index 3cdc78ae..b3d3ba1d 100644 --- a/lib/RemoteControl/src/RemoteControl.h +++ b/lib/RemoteControl/src/RemoteControl.h @@ -1,6 +1,6 @@ /* MIT License * - * Copyright (c) 2024 Andreas Merkle + * Copyright (c) 2023 - 2024 Andreas Merkle * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal From f9643d96541a56d2929dba57dd1d2ca60762dc02 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 23 Jan 2024 15:58:40 +0100 Subject: [PATCH 24/36] Processing of periodic tasks separated --- lib/ConvoyLeader/src/App.cpp | 64 +++++++++++++++------------ lib/ConvoyLeader/src/App.h | 5 +++ lib/ConvoyLeader/src/StartupState.cpp | 4 +- 3 files changed, 43 insertions(+), 30 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index ae5f4f82..fdaf6401 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -226,33 +226,7 @@ void App::loop() m_systemStateMachine.process(); /* Process periodic tasks. */ - if ((true == m_initialCommandTimer.isTimeout()) && (true == m_smpServer.isSynced())) - { - Command* pendingCommand = StartupState::getInstance().getPendingCommand(); - - if (nullptr != pendingCommand) - { - if (false == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, pendingCommand, sizeof(Command))) - { - LOG_WARNING("Failed to send pending command to RU."); - } - - /* Something is pending. */ - m_initialCommandTimer.restart(); - } - } - - if (true == m_sendWaypointTimer.isTimeout()) - { - if (false == m_v2vClient.sendWaypoint(m_latestVehicleData)) - { - LOG_WARNING("Waypoint could not be sent."); - } - else - { - m_sendWaypointTimer.restart(); - } - } + processPeriodicTasks(); } void App::currentVehicleChannelCallback(const VehicleData& vehicleData) @@ -394,6 +368,36 @@ bool App::setupSerialMuxProt() return isSuccessful; } +void App::processPeriodicTasks() +{ + if ((true == m_initialCommandTimer.isTimeout()) && (true == m_smpServer.isSynced())) + { + Command* pendingCommand = StartupState::getInstance().getPendingCommand(); + + if (nullptr != pendingCommand) + { + if (false == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, pendingCommand, sizeof(Command))) + { + LOG_WARNING("Failed to send pending command to RU."); + } + + m_initialCommandTimer.restart(); + } + } + + if (true == m_sendWaypointTimer.isTimeout()) + { + if (false == m_v2vClient.sendWaypoint(m_latestVehicleData)) + { + LOG_WARNING("Waypoint could not be sent."); + } + else + { + m_sendWaypointTimer.restart(); + } + } +} + /****************************************************************************** * External Functions *****************************************************************************/ @@ -417,7 +421,11 @@ void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize const CommandResponse* cmdRsp = reinterpret_cast(payload); LOG_DEBUG("CMD_RSP: ID: 0x%02X , RSP: 0x%02X", cmdRsp->commandId, cmdRsp->responseId); - if (RemoteControl::CMD_ID_GET_MAX_SPEED == cmdRsp->commandId) + if (RemoteControl::RSP_ID_ERROR == cmdRsp->responseId) + { + /* Go to error state. */ + } + else if (RemoteControl::CMD_ID_GET_MAX_SPEED == cmdRsp->commandId) { LOG_DEBUG("Max Speed: %d", cmdRsp->maxMotorSpeed); application->setMaxMotorSpeed(cmdRsp->maxMotorSpeed); diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index d9dc2e96..fc2afc96 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -213,6 +213,11 @@ class App */ bool setupSerialMuxProt(); + /** + * Process periodic tasks. + */ + void processPeriodicTasks(); + private: /* Not allowed. */ App(const App& app); /**< Copy construction of an instance. */ diff --git a/lib/ConvoyLeader/src/StartupState.cpp b/lib/ConvoyLeader/src/StartupState.cpp index 1b78faf5..ea3cd6ff 100644 --- a/lib/ConvoyLeader/src/StartupState.cpp +++ b/lib/ConvoyLeader/src/StartupState.cpp @@ -34,6 +34,7 @@ *****************************************************************************/ #include "StartupState.h" +#include "IdleState.h" #include "RemoteControl.h" #include @@ -68,7 +69,6 @@ void StartupState::entry() void StartupState::process(StateMachine& sm) { - SettingsHandler& settings = SettingsHandler::getInstance(); switch (m_pendingCommandCounter) @@ -103,7 +103,7 @@ void StartupState::process(StateMachine& sm) case CMD_NONE: /* All commands processed. Switch to idle state. */ - // sm.setState(&IdleState::getInstance()); + sm.setState(&IdleState::getInstance()); break; default: From 921e4ae27051fb9d464b3d0fc63c9c7bbaca7b04 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 08:05:57 +0100 Subject: [PATCH 25/36] Added missing nothrow --- lib/ConvoyLeader/src/StartupState.cpp | 23 +++++++++++++++++------ lib/ConvoyLeader/src/StartupState.h | 1 - 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/lib/ConvoyLeader/src/StartupState.cpp b/lib/ConvoyLeader/src/StartupState.cpp index ea3cd6ff..9cbd18c5 100644 --- a/lib/ConvoyLeader/src/StartupState.cpp +++ b/lib/ConvoyLeader/src/StartupState.cpp @@ -36,6 +36,7 @@ #include "StartupState.h" #include "IdleState.h" #include "RemoteControl.h" +#include #include /****************************************************************************** @@ -65,6 +66,12 @@ void StartupState::entry() { m_isActive = true; + + if (nullptr != m_pendingCommand) + { + delete m_pendingCommand; + m_pendingCommand = nullptr; + } } void StartupState::process(StateMachine& sm) @@ -77,7 +84,7 @@ void StartupState::process(StateMachine& sm) if (nullptr == m_pendingCommand) { /* Create new pending commmand. */ - m_pendingCommand = new Command{RemoteControl::CMD_ID_GET_MAX_SPEED}; + m_pendingCommand = new (std::nothrow) Command{RemoteControl::CMD_ID_GET_MAX_SPEED}; } else { @@ -89,11 +96,15 @@ void StartupState::process(StateMachine& sm) if (nullptr == m_pendingCommand) { /* Create new pending commmand. */ - m_pendingCommand = new Command; - m_pendingCommand->commandId = RemoteControl::CMD_ID_SET_INIT_POS; - m_pendingCommand->xPos = settings.getInitialXPosition(); - m_pendingCommand->yPos = settings.getInitialYPosition(); - m_pendingCommand->orientation = settings.getInitialHeading(); + m_pendingCommand = new (std::nothrow) Command; + + if (nullptr != m_pendingCommand) + { + m_pendingCommand->commandId = RemoteControl::CMD_ID_SET_INIT_POS; + m_pendingCommand->xPos = settings.getInitialXPosition(); + m_pendingCommand->yPos = settings.getInitialYPosition(); + m_pendingCommand->orientation = settings.getInitialHeading(); + } } else { diff --git a/lib/ConvoyLeader/src/StartupState.h b/lib/ConvoyLeader/src/StartupState.h index b3719f70..cd794635 100644 --- a/lib/ConvoyLeader/src/StartupState.h +++ b/lib/ConvoyLeader/src/StartupState.h @@ -45,7 +45,6 @@ #include #include -#include #include "SerialMuxChannels.h" /****************************************************************************** From 74e674b0a36a9181c20f515e08767fbf4b2331df Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 08:14:53 +0100 Subject: [PATCH 26/36] Added Idle State --- lib/ConvoyLeader/src/App.cpp | 29 +++++- lib/ConvoyLeader/src/App.h | 5 +- lib/ConvoyLeader/src/IdleState.cpp | 143 ++++++++++++++++++++++++++++ lib/ConvoyLeader/src/IdleState.h | 147 +++++++++++++++++++++++++++++ 4 files changed, 319 insertions(+), 5 deletions(-) create mode 100644 lib/ConvoyLeader/src/IdleState.cpp create mode 100644 lib/ConvoyLeader/src/IdleState.h diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index fdaf6401..a34c9e4c 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -42,6 +42,7 @@ #include #include #include "StartupState.h" +#include "IdleState.h" /****************************************************************************** * Compiler Switches @@ -178,7 +179,7 @@ void App::setup() /* Initialize timers. */ m_sendWaypointTimer.start(SEND_WAYPOINT_TIMER_INTERVAL); - m_initialCommandTimer.start(SEND_WAYPOINT_TIMER_INTERVAL); + m_commandTimer.start(SEND_COMMANDS_TIMER_INTERVAL); /* Start with startup state. */ m_systemStateMachine.setState(&StartupState::getInstance()); @@ -370,7 +371,7 @@ bool App::setupSerialMuxProt() void App::processPeriodicTasks() { - if ((true == m_initialCommandTimer.isTimeout()) && (true == m_smpServer.isSynced())) + if ((true == m_commandTimer.isTimeout()) && (true == m_smpServer.isSynced())) { Command* pendingCommand = StartupState::getInstance().getPendingCommand(); @@ -378,11 +379,23 @@ void App::processPeriodicTasks() { if (false == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, pendingCommand, sizeof(Command))) { - LOG_WARNING("Failed to send pending command to RU."); + LOG_WARNING("Failed to send StartupState pending command to RU."); } + } + else + { + pendingCommand = IdleState::getInstance().getPendingCommand(); - m_initialCommandTimer.restart(); + if (nullptr != pendingCommand) + { + if (false == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, pendingCommand, sizeof(Command))) + { + LOG_WARNING("Failed to send IdleState pending command to RU."); + } + } } + + m_commandTimer.restart(); } if (true == m_sendWaypointTimer.isTimeout()) @@ -435,6 +448,14 @@ void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize { StartupState::getInstance().notifyCommandProcessed(); } + else if (RemoteControl::CMD_ID_START_DRIVING == cmdRsp->commandId) + { + IdleState::getInstance().notifyCommandProcessed(); + } + else + { + LOG_WARNING("CMD_RSP: Unknown command ID: 0x%02X", cmdRsp->commandId); + } } else { diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index fc2afc96..21f03818 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -138,6 +138,9 @@ class App /** Send waypoint timer interval in ms. */ static const uint32_t SEND_WAYPOINT_TIMER_INTERVAL = 500U; + /** Send commands timer interval in ms. */ + static const uint32_t SEND_COMMANDS_TIMER_INTERVAL = 100U; + /** MQTT topic name for birth messages. */ static const char* TOPIC_NAME_BIRTH; @@ -191,7 +194,7 @@ class App /** * Timer for sending initial commands. */ - SimpleTimer m_initialCommandTimer; + SimpleTimer m_commandTimer; private: /** diff --git a/lib/ConvoyLeader/src/IdleState.cpp b/lib/ConvoyLeader/src/IdleState.cpp new file mode 100644 index 00000000..be596459 --- /dev/null +++ b/lib/ConvoyLeader/src/IdleState.cpp @@ -0,0 +1,143 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Idle State. + * @author Gabryel Reyes + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include "IdleState.h" +#include "DrivingState.h" +#include "RemoteControl.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void IdleState::entry() +{ + m_isActive = true; + m_isReleaseSuccessful = false; + + if (nullptr != m_pendingCommand) + { + delete m_pendingCommand; + m_pendingCommand = nullptr; + } +} + +void IdleState::process(StateMachine& sm) +{ + if (true == m_isReleaseSuccessful) + { + /* Release is successful. Switch to driving state. */ + sm.setState(&DrivingState::getInstance()); + } +} + +void IdleState::exit() +{ + m_isActive = false; +} + +Command* IdleState::getPendingCommand() +{ + return (true == m_isActive) ? m_pendingCommand : nullptr; +} + +void IdleState::notifyCommandProcessed() +{ + if (nullptr != m_pendingCommand) + { + delete m_pendingCommand; + m_pendingCommand = nullptr; + m_isReleaseSuccessful = true; + } +} + +bool IdleState::requestRelease() +{ + bool isSuccessful = false; + + if (true == m_isActive) + { + if (nullptr != m_pendingCommand) + { + delete m_pendingCommand; + m_pendingCommand = nullptr; + } + + m_pendingCommand = new (std::nothrow) Command{RemoteControl::CMD_ID_START_DRIVING}; + + if (nullptr != m_pendingCommand) + { + isSuccessful = true; + } + } + + return isSuccessful; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/ConvoyLeader/src/IdleState.h b/lib/ConvoyLeader/src/IdleState.h new file mode 100644 index 00000000..03f4f6ab --- /dev/null +++ b/lib/ConvoyLeader/src/IdleState.h @@ -0,0 +1,147 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Idle State. + * @author Gabryel Reyes + * + * @addtogroup Application + * + * @{ + */ +#ifndef IDLE_STATE_H +#define IDLE_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include +#include "SerialMuxChannels.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system idle state. */ +class IdleState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static IdleState& getInstance() + { + static IdleState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Get pending command. If there is no pending command or the state is not active, it will return nullptr. + * + * @return Pending command or nullptr. + */ + Command* getPendingCommand(); + + /** + * Notify the state, that the pending command is successfully processed by RU. + */ + void notifyCommandProcessed(); + + /** + * Release the vehicle to start driving. + * + * @return If the request to release the vehicle is successful, returns true. Otherwise, false. + */ + bool requestRelease(); + +protected: +private: + /** Flag: State is active. */ + bool m_isActive; + + /** Flag: Release is successful. */ + bool m_isReleaseSuccessful; + + /** Pending command. */ + Command* m_pendingCommand; + + /** + * Default constructor. + */ + IdleState() : IState(), m_isActive(false), m_isReleaseSuccessful(false), m_pendingCommand(nullptr) + { + } + + /** + * Default destructor. + */ + virtual ~IdleState() + { + } + + /* Not allowed. */ + IdleState(const IdleState& state); /**< Copy construction of an instance. */ + IdleState& operator=(const IdleState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IDLE_STATE_H */ +/** @} */ From ccb7bc8938c6a921f3480375c5e304e5ec217659 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 11:07:03 +0100 Subject: [PATCH 27/36] Added driving state. Replaced LongitudinalController with driving state --- lib/ConvoyLeader/src/App.cpp | 92 +++-------- lib/ConvoyLeader/src/App.h | 42 +---- ...tudinalController.cpp => DrivingState.cpp} | 94 +++++------ ...ongitudinalController.h => DrivingState.h} | 155 ++++++++---------- 4 files changed, 133 insertions(+), 250 deletions(-) rename lib/ConvoyLeader/src/{LongitudinalController.cpp => DrivingState.cpp} (67%) rename lib/ConvoyLeader/src/{LongitudinalController.h => DrivingState.h} (50%) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index a34c9e4c..ae84c904 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -43,6 +43,7 @@ #include #include "StartupState.h" #include "IdleState.h" +#include "DrivingState.h" /****************************************************************************** * Compiler Switches @@ -171,12 +172,6 @@ void App::setup() } else { - /* Setup longitudinal controller. */ - LongitudinalController::MotorSetpointCallback motorSetpointCallback = [this](const int16_t& topCenterSpeed) - { return this->motorSetpointCallback(topCenterSpeed); }; - - m_longitudinalController.setMotorSetpointCallback(motorSetpointCallback); - /* Initialize timers. */ m_sendWaypointTimer.start(SEND_WAYPOINT_TIMER_INTERVAL); m_commandTimer.start(SEND_COMMANDS_TIMER_INTERVAL); @@ -220,9 +215,6 @@ void App::loop() /* Process V2V Communication */ m_v2vClient.process(); - /* Process Longitudinal Controller */ - m_longitudinalController.process(); - /* Process System State Machine */ m_systemStateMachine.process(); @@ -230,52 +222,9 @@ void App::loop() processPeriodicTasks(); } -void App::currentVehicleChannelCallback(const VehicleData& vehicleData) -{ - m_latestVehicleData = Waypoint(vehicleData.xPos, vehicleData.yPos, vehicleData.orientation, vehicleData.left, - vehicleData.right, vehicleData.center); - - // latestVehicleData.debugPrint(); - - m_longitudinalController.calculateTopMotorSpeed(m_latestVehicleData); -} - -bool App::motorSetpointCallback(const int16_t topCenterSpeed) +void App::setLatestVehicleData(const Waypoint& waypoint) { - SpeedData payload; - payload.center = topCenterSpeed; - - return m_smpServer.sendData(m_serialMuxProtChannelIdMotorSpeeds, &payload, sizeof(payload)); -} - -void App::release() -{ - Command payload; - payload.commandId = RemoteControl::CMD_ID_START_DRIVING; - - /* Release robot. */ - if (false == m_longitudinalController.release()) - { - LOG_WARNING("Robot could not be released."); - } - else if (false == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &payload, sizeof(payload))) - { - LOG_WARNING("Failed to send release command to RU."); - } - else - { - LOG_INFO("Robot released."); - } -} - -void App::setMaxMotorSpeed(const int16_t maxMotorSpeed) -{ - m_longitudinalController.setMaxMotorSpeed(maxMotorSpeed); -} - -void App::setLastFollowerFeedback(const Waypoint& feedback) -{ - m_longitudinalController.setLastFollowerFeedback(feedback); + m_latestVehicleData = waypoint; } /****************************************************************************** @@ -329,20 +278,19 @@ bool App::setupMqttClient() } else { - isSuccessful = - m_mqttClient.subscribe(TOPIC_NAME_RELEASE, true, [this](const String& payload) { this->release(); }); - - isSuccessful &= m_mqttClient.subscribe("platoons/0/vehicles/0/feedback", false, - [this](const String& payload) - { - Waypoint* waypoint = Waypoint::deserialize(payload); - - if (nullptr != waypoint) - { - this->setLastFollowerFeedback(*waypoint); - delete waypoint; - } - }); + isSuccessful = m_mqttClient.subscribe( + TOPIC_NAME_RELEASE, true, [this](const String& payload) { IdleState::getInstance().requestRelease(); }); + + isSuccessful &= + m_mqttClient.subscribe("platoons/0/vehicles/0/feedback", false, + [this](const String& payload) + { + Waypoint* waypoint = Waypoint::deserialize(payload); + VehicleData feedback{waypoint->xPos, waypoint->yPos, waypoint->orientation, + waypoint->left, waypoint->right, waypoint->center}; + + DrivingState::getInstance().setLastFollowerFeedback(feedback); + }); } return isSuccessful; @@ -441,7 +389,7 @@ void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize else if (RemoteControl::CMD_ID_GET_MAX_SPEED == cmdRsp->commandId) { LOG_DEBUG("Max Speed: %d", cmdRsp->maxMotorSpeed); - application->setMaxMotorSpeed(cmdRsp->maxMotorSpeed); + DrivingState::getInstance().setMaxMotorSpeed(cmdRsp->maxMotorSpeed); StartupState::getInstance().notifyCommandProcessed(); } else if (RemoteControl::CMD_ID_SET_INIT_POS == cmdRsp->commandId) @@ -477,7 +425,11 @@ void App_currentVehicleChannelCallback(const uint8_t* payload, const uint8_t pay { const VehicleData* currentVehicleData = reinterpret_cast(payload); App* application = reinterpret_cast(userData); - application->currentVehicleChannelCallback(*currentVehicleData); + Waypoint dataAsWaypoint(currentVehicleData->xPos, currentVehicleData->yPos, currentVehicleData->orientation, + currentVehicleData->left, currentVehicleData->right, currentVehicleData->center); + + application->setLatestVehicleData(dataAsWaypoint); + DrivingState::getInstance().setVehicleData(*currentVehicleData); } else { diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index 21f03818..3c368f3e 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -50,7 +50,6 @@ #include #include #include "SerialMuxChannels.h" -#include "LongitudinalController.h" /****************************************************************************** * Macros @@ -73,7 +72,6 @@ class App m_smpServer(Board::getInstance().getDevice().getStream(), this), m_mqttClient(), m_v2vClient(m_mqttClient), - m_longitudinalController(), m_sendWaypointTimer() { } @@ -96,40 +94,11 @@ class App void loop(); /** - * Callback for the current vehicle data. + * Set latest vehicle data from RU. * - * @param[in] vehicleData Current vehicle data. + * @param[in] waypoint Latest vehicle data from RU. */ - void currentVehicleChannelCallback(const VehicleData& vehicleData); - - /** - * Motor setpoint callback. - * Called in order to send the motor speeds using SerialMuxProt to the robot. - * - * @param[in] topCenterSpeed Center motor speed [steps/s]. - * - * @return If the motor speed was sent successfully, returns true. Otherwise, false. - */ - bool motorSetpointCallback(const int16_t topCenterSpeed); - - /** - * Release robot and start driving. - */ - void release(); - - /** - * Set max motor speed. - * - * @param[in] maxMotorSpeed Max motor speed [steps/s]. - */ - void setMaxMotorSpeed(const int16_t maxMotorSpeed); - - /** - * Set incoming feedback from last follower. - * - * @param[in] feedback Feedback from last follower. - */ - void setLastFollowerFeedback(const Waypoint& feedback); + void setLatestVehicleData(const Waypoint& waypoint); private: /** Minimum battery level in percent. */ @@ -176,11 +145,6 @@ class App /** The system state machine. */ StateMachine m_systemStateMachine; - /** - * Longitudinal controller. - */ - LongitudinalController m_longitudinalController; - /** * Latest vehicle data from RU. */ diff --git a/lib/ConvoyLeader/src/LongitudinalController.cpp b/lib/ConvoyLeader/src/DrivingState.cpp similarity index 67% rename from lib/ConvoyLeader/src/LongitudinalController.cpp rename to lib/ConvoyLeader/src/DrivingState.cpp index 3d9f6c47..e9f5ef20 100644 --- a/lib/ConvoyLeader/src/LongitudinalController.cpp +++ b/lib/ConvoyLeader/src/DrivingState.cpp @@ -25,7 +25,7 @@ DESCRIPTION *******************************************************************************/ /** - * @brief Leader Longitudinal Controller. + * @brief Driving state. * @author Gabryel Reyes */ @@ -33,8 +33,7 @@ * Includes *****************************************************************************/ -#include "LongitudinalController.h" -#include +#include "DrivingState.h" /****************************************************************************** * Compiler Switches @@ -60,72 +59,55 @@ * Public Methods *****************************************************************************/ -LongitudinalController::LongitudinalController() : - m_maxMotorSpeed(0), - m_state(STATE_STARTUP), - m_lastFollowerFeedback(), - m_motorSetpointCallback(nullptr) +void DrivingState::entry() { + m_isActive = true; } -LongitudinalController::~LongitudinalController() +void DrivingState::process(StateMachine& sm) { + /* Check if the state is active. */ + if (false == m_isActive) + { + m_topMotorSpeed = 0; + } + else + { + /* Check limits. */ + + /* Check follower feedback. Calculate platoon length and react accordingly */ + + /* Calculate top motor speed. */ + m_topMotorSpeed = m_maxMotorSpeed; + } } -void LongitudinalController::process() +void DrivingState::exit() { - switch (m_state) - { - case STATE_STARTUP: - /* Max speed must be positive. */ - if (0 < m_maxMotorSpeed) - { - m_state = STATE_READY; - } - - break; - - case STATE_READY: - /* Wait for external release. */ - break; - - case STATE_DRIVING: - /* Allow top motor speed calculation. */ - break; - - case STATE_SAFE: - /* Stop the vehicle. Sent continously to prevent overwriting by other modules. */ - if (nullptr != m_motorSetpointCallback) - { - m_motorSetpointCallback(0); - } - - break; - - default: - /* Should never happen. */ - m_state = STATE_SAFE; - break; - } + m_isActive = false; } -void LongitudinalController::calculateTopMotorSpeed(const Waypoint& currentVehicleData) +void DrivingState::setMaxMotorSpeed(int16_t maxSpeed) { - if (STATE_DRIVING == m_state) - { - int16_t topMotorSpeed = 0; + m_maxMotorSpeed = maxSpeed; +} - /* TODO: Check follower feedback. Calculate platoon length and react accordingly. */ +bool DrivingState::getTopMotorSpeed(int16_t& topMotorSpeed) const +{ + topMotorSpeed = m_topMotorSpeed; - /* Calculate top motor speed. */ - topMotorSpeed = m_maxMotorSpeed; + /* Only valid if the state is active. */ + return m_isActive; +} - /* Send top motor speed. */ - if (nullptr != m_motorSetpointCallback) - { - m_motorSetpointCallback(topMotorSpeed); - } - } +void DrivingState::setVehicleData(const VehicleData& vehicleData) +{ + m_vehicleData = vehicleData; +} + +void DrivingState::setLastFollowerFeedback(const VehicleData& feedback) +{ + m_followerFeedback = feedback; } /****************************************************************************** diff --git a/lib/ConvoyLeader/src/LongitudinalController.h b/lib/ConvoyLeader/src/DrivingState.h similarity index 50% rename from lib/ConvoyLeader/src/LongitudinalController.h rename to lib/ConvoyLeader/src/DrivingState.h index bb9c8e57..a770e5fd 100644 --- a/lib/ConvoyLeader/src/LongitudinalController.h +++ b/lib/ConvoyLeader/src/DrivingState.h @@ -25,15 +25,15 @@ DESCRIPTION *******************************************************************************/ /** - * @brief Leader Longitudinal Controller. + * @brief Driving state. * @author Gabryel Reyes * * @addtogroup Application * * @{ */ -#ifndef LONGITUDINAL_CONTROLLER_H -#define LONGITUDINAL_CONTROLLER_H +#ifndef DRIVING_STATE_H +#define DRIVING_STATE_H /****************************************************************************** * Compile Switches @@ -44,8 +44,9 @@ *****************************************************************************/ #include -#include -#include +#include +#include +#include "SerialMuxChannels.h" /****************************************************************************** * Macros @@ -55,132 +56,116 @@ * Types and Classes *****************************************************************************/ -/** Leader Longitudinal Controller. */ -class LongitudinalController +/** The system driving state. */ +class DrivingState : public IState { public: /** - * Motor setpoint callback. - * Called in order to set the motor speeds. + * Get state instance. + * + * @return State instance. */ - typedef std::function MotorSetpointCallback; - - /** Controller states. */ - enum STATE : uint8_t + static DrivingState& getInstance() { - STATE_STARTUP = 0, /**< Startup state. */ - STATE_READY, /**< Ready state. */ - STATE_DRIVING, /**< Driving state. */ - STATE_SAFE, /**< Safe state. */ - }; + static DrivingState instance; - /** - * LongitudinalController constructor. - */ - LongitudinalController(); + /* Singleton idiom to force initialization during first usage. */ - /** - * Default destructor. - */ - ~LongitudinalController(); + return instance; + } /** - * Process the longitudinal controller. + * If the state is entered, this method will called once. */ - void process(); + void entry() final; /** - * Set the maximum motor speed of the robot. + * Processing the state. * - * @param[in] maxMotorSpeed Maximum motor speed. + * @param[in] sm State machine, which is calling this state. */ - void setMaxMotorSpeed(int16_t maxMotorSpeed) - { - m_maxMotorSpeed = maxMotorSpeed; - } + void process(StateMachine& sm) final; /** - * Release robot for driving state, only if in ready state. - * - * @return If the robot was released, returns true. Otherwise false. + * If the state is left, this method will be called once. */ - bool release() - { - bool isReleased = false; - - if (STATE_READY == m_state) - { - m_state = STATE_DRIVING; - isReleased = true; - } - - return isReleased; - } + void exit() final; /** - * Set incoming feedback from last follower. + * Set maximum motor speed. * - * @param[in] feedback Feedback from last follower. + * @param[in] speed Maximum motor speed. */ - void setLastFollowerFeedback(const Waypoint& feedback) - { - m_lastFollowerFeedback = feedback; - } + void setMaxMotorSpeed(int16_t maxSpeed); /** - * Get current state. + * Get the calculated top motor speed. * - * @return Current state. - */ - STATE getState() const - { - return m_state; - } - - /** - * Set safe state. This will stop the robot. + * @param[out] topMotorSpeed The calculated top motor speed. + * + * @returns true if the top motor speed is valid. Otherwise, false. */ - void setSafeState() - { - m_state = STATE_SAFE; - } + bool getTopMotorSpeed(int16_t& topMotorSpeed) const; /** - * Set motor setpoint callback. + * Set latest vehicle data. * - * @param[in] motorSetpointCallback Motor setpoint callback. + * @param[in] vehicleData Latest vehicle data. */ - void setMotorSetpointCallback(const MotorSetpointCallback& motorSetpointCallback) - { - m_motorSetpointCallback = motorSetpointCallback; - } + void setVehicleData(const VehicleData& vehicleData); /** - * Calculate the top motor speed and send it to the robot. + * Set last follower feedback. * - * @param[in] currentVehicleData Current vehicle data. + * @param[in] feedback Last follower feedback. */ - void calculateTopMotorSpeed(const Waypoint& currentVehicleData); + void setLastFollowerFeedback(const VehicleData& feedback); +protected: private: + /** Flag: State is active. */ + bool m_isActive; + /** Maximum motor speed. */ int16_t m_maxMotorSpeed; - /** Current state. */ - STATE m_state; + /** Calculated top motor speed. */ + int16_t m_topMotorSpeed; + + /** Latest vehicle data. */ + VehicleData m_vehicleData; - /** Feedback from last follower. */ - Waypoint m_lastFollowerFeedback; + /** Last follower feedback. */ + VehicleData m_followerFeedback; /** - * Motor setpoint callback. + * Default constructor. */ - MotorSetpointCallback m_motorSetpointCallback; + DrivingState() : + IState(), + m_isActive(false), + m_maxMotorSpeed(0), + m_topMotorSpeed(0), + m_vehicleData{0}, + m_followerFeedback{0} + { + } + + /** + * Default destructor. + */ + virtual ~DrivingState() + { + } + + /* Not allowed. */ + DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ + DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ }; /****************************************************************************** * Functions *****************************************************************************/ -#endif /* LONGITUDINAL_CONTROLLER_H */ +#endif /* DRIVING_STATE_H */ /** @} */ From 8056d5697d7dd678e3fa0b54279233233f28b341 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 11:11:28 +0100 Subject: [PATCH 28/36] fixed CI findings --- lib/ConvoyLeader/src/DrivingState.h | 2 +- lib/ConvoyLeader/src/SerialMuxChannels.h | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/lib/ConvoyLeader/src/DrivingState.h b/lib/ConvoyLeader/src/DrivingState.h index a770e5fd..0083ac53 100644 --- a/lib/ConvoyLeader/src/DrivingState.h +++ b/lib/ConvoyLeader/src/DrivingState.h @@ -94,7 +94,7 @@ class DrivingState : public IState /** * Set maximum motor speed. * - * @param[in] speed Maximum motor speed. + * @param[in] maxSpeed Maximum motor speed. */ void setMaxMotorSpeed(int16_t maxSpeed); diff --git a/lib/ConvoyLeader/src/SerialMuxChannels.h b/lib/ConvoyLeader/src/SerialMuxChannels.h index 6e7688b0..c3e8b2aa 100644 --- a/lib/ConvoyLeader/src/SerialMuxChannels.h +++ b/lib/ConvoyLeader/src/SerialMuxChannels.h @@ -82,8 +82,10 @@ typedef struct _Command { uint8_t commandId; /**< Command ID */ + /** Command payload. */ union { + /** Init data command payload. */ struct { int32_t xPos; /**< X position [mm]. */ From 58de6d88df4bb4f86004242dfbfc59bcb709332c Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 11:53:59 +0100 Subject: [PATCH 29/36] Ru must not be released, as it waits for a speed different to 0 --- lib/ConvoyLeader/src/App.cpp | 20 ------------- lib/ConvoyLeader/src/IdleState.cpp | 48 ++++-------------------------- lib/ConvoyLeader/src/IdleState.h | 21 ++----------- 3 files changed, 9 insertions(+), 80 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index ae84c904..72f01d64 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -330,18 +330,6 @@ void App::processPeriodicTasks() LOG_WARNING("Failed to send StartupState pending command to RU."); } } - else - { - pendingCommand = IdleState::getInstance().getPendingCommand(); - - if (nullptr != pendingCommand) - { - if (false == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, pendingCommand, sizeof(Command))) - { - LOG_WARNING("Failed to send IdleState pending command to RU."); - } - } - } m_commandTimer.restart(); } @@ -396,14 +384,6 @@ void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize { StartupState::getInstance().notifyCommandProcessed(); } - else if (RemoteControl::CMD_ID_START_DRIVING == cmdRsp->commandId) - { - IdleState::getInstance().notifyCommandProcessed(); - } - else - { - LOG_WARNING("CMD_RSP: Unknown command ID: 0x%02X", cmdRsp->commandId); - } } else { diff --git a/lib/ConvoyLeader/src/IdleState.cpp b/lib/ConvoyLeader/src/IdleState.cpp index be596459..077c3176 100644 --- a/lib/ConvoyLeader/src/IdleState.cpp +++ b/lib/ConvoyLeader/src/IdleState.cpp @@ -35,8 +35,6 @@ #include "IdleState.h" #include "DrivingState.h" -#include "RemoteControl.h" -#include /****************************************************************************** * Compiler Switches @@ -64,22 +62,16 @@ void IdleState::entry() { - m_isActive = true; - m_isReleaseSuccessful = false; - - if (nullptr != m_pendingCommand) - { - delete m_pendingCommand; - m_pendingCommand = nullptr; - } + m_isActive = true; + m_releaseRequested = false; } void IdleState::process(StateMachine& sm) { - if (true == m_isReleaseSuccessful) + if ((true == m_isActive) && (true == m_releaseRequested)) { - /* Release is successful. Switch to driving state. */ sm.setState(&DrivingState::getInstance()); + m_releaseRequested = false; } } @@ -88,42 +80,14 @@ void IdleState::exit() m_isActive = false; } -Command* IdleState::getPendingCommand() -{ - return (true == m_isActive) ? m_pendingCommand : nullptr; -} - -void IdleState::notifyCommandProcessed() -{ - if (nullptr != m_pendingCommand) - { - delete m_pendingCommand; - m_pendingCommand = nullptr; - m_isReleaseSuccessful = true; - } -} - bool IdleState::requestRelease() { - bool isSuccessful = false; - if (true == m_isActive) { - if (nullptr != m_pendingCommand) - { - delete m_pendingCommand; - m_pendingCommand = nullptr; - } - - m_pendingCommand = new (std::nothrow) Command{RemoteControl::CMD_ID_START_DRIVING}; - - if (nullptr != m_pendingCommand) - { - isSuccessful = true; - } + m_releaseRequested = true; } - return isSuccessful; + return m_isActive; } /****************************************************************************** diff --git a/lib/ConvoyLeader/src/IdleState.h b/lib/ConvoyLeader/src/IdleState.h index 03f4f6ab..b6c41577 100644 --- a/lib/ConvoyLeader/src/IdleState.h +++ b/lib/ConvoyLeader/src/IdleState.h @@ -90,18 +90,6 @@ class IdleState : public IState */ void exit() final; - /** - * Get pending command. If there is no pending command or the state is not active, it will return nullptr. - * - * @return Pending command or nullptr. - */ - Command* getPendingCommand(); - - /** - * Notify the state, that the pending command is successfully processed by RU. - */ - void notifyCommandProcessed(); - /** * Release the vehicle to start driving. * @@ -114,16 +102,13 @@ class IdleState : public IState /** Flag: State is active. */ bool m_isActive; - /** Flag: Release is successful. */ - bool m_isReleaseSuccessful; - - /** Pending command. */ - Command* m_pendingCommand; + /** Flag: Release is requested. */ + bool m_releaseRequested; /** * Default constructor. */ - IdleState() : IState(), m_isActive(false), m_isReleaseSuccessful(false), m_pendingCommand(nullptr) + IdleState() : IState(), m_isActive(false) { } From 981d57042c9b5020c6760bfa962580a0d5ba3d5b Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 13:33:20 +0100 Subject: [PATCH 30/36] Added Error State --- lib/ConvoyLeader/src/App.cpp | 7 ++ lib/ConvoyLeader/src/App.h | 5 ++ lib/ConvoyLeader/src/ErrorState.cpp | 93 +++++++++++++++++++++ lib/ConvoyLeader/src/ErrorState.h | 122 ++++++++++++++++++++++++++++ 4 files changed, 227 insertions(+) create mode 100644 lib/ConvoyLeader/src/ErrorState.cpp create mode 100644 lib/ConvoyLeader/src/ErrorState.h diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index 72f01d64..c6007ae6 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -44,6 +44,7 @@ #include "StartupState.h" #include "IdleState.h" #include "DrivingState.h" +#include "ErrorState.h" /****************************************************************************** * Compiler Switches @@ -227,6 +228,11 @@ void App::setLatestVehicleData(const Waypoint& waypoint) m_latestVehicleData = waypoint; } +void App::setErrorState() +{ + m_systemStateMachine.setState(&ErrorState::getInstance()); +} + /****************************************************************************** * Protected Methods *****************************************************************************/ @@ -373,6 +379,7 @@ void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize if (RemoteControl::RSP_ID_ERROR == cmdRsp->responseId) { /* Go to error state. */ + application->setErrorState(); } else if (RemoteControl::CMD_ID_GET_MAX_SPEED == cmdRsp->commandId) { diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index 3c368f3e..40eec92a 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -100,6 +100,11 @@ class App */ void setLatestVehicleData(const Waypoint& waypoint); + /** + * Set error state. + */ + void setErrorState(); + private: /** Minimum battery level in percent. */ static const uint8_t MIN_BATTERY_LEVEL = 10U; diff --git a/lib/ConvoyLeader/src/ErrorState.cpp b/lib/ConvoyLeader/src/ErrorState.cpp new file mode 100644 index 00000000..bb55196a --- /dev/null +++ b/lib/ConvoyLeader/src/ErrorState.cpp @@ -0,0 +1,93 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error State. + * @author Gabryel Reyes + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include "ErrorState.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + m_isActive = true; + m_releaseRequested = false; +} + +void ErrorState::process(StateMachine& sm) +{ + UTIL_NOT_USED(sm); +} + +void ErrorState::exit() +{ + m_isActive = false; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/ConvoyLeader/src/ErrorState.h b/lib/ConvoyLeader/src/ErrorState.h new file mode 100644 index 00000000..a8f4634d --- /dev/null +++ b/lib/ConvoyLeader/src/ErrorState.h @@ -0,0 +1,122 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error State. + * @author Gabryel Reyes + * + * @addtogroup Application + * + * @{ + */ +#ifndef ERROR_STATE_H +#define ERROR_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system error state. */ +class ErrorState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance + */ + static ErrorState& getInstance() + { + static ErrorState instance; /* singleton idiom to force initialization in the first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Flag: State is active. */ + bool m_isActive; + + /** Flag: Release is requested. */ + bool m_releaseRequested; + + /** + * Default constructor. + */ + ErrorState() : IState(), m_isActive(false) + { + } + + /** + * Default destructor. + */ + virtual ~ErrorState() + { + } + + /* Not allowed. */ + ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ + ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ERROR_STATE_H */ +/** @} */ From 9f13513227783c56b64c56825475d6ef7295b7d6 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 13:37:54 +0100 Subject: [PATCH 31/36] Moved RemoteControl Commands and responses to SerialMuxChannels The commands depend on the channels, so it makes sense to have them together --- lib/ConvoyLeader/src/App.cpp | 1 - lib/ConvoyLeader/src/RemoteControl.h | 87 ------------------------ lib/ConvoyLeader/src/SerialMuxChannels.h | 32 ++++++++- lib/ConvoyLeader/src/StartupState.cpp | 1 - 4 files changed, 29 insertions(+), 92 deletions(-) delete mode 100644 lib/ConvoyLeader/src/RemoteControl.h diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index c6007ae6..df47ff10 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -33,7 +33,6 @@ * Includes *****************************************************************************/ #include "App.h" -#include "RemoteControl.h" #include #include #include diff --git a/lib/ConvoyLeader/src/RemoteControl.h b/lib/ConvoyLeader/src/RemoteControl.h deleted file mode 100644 index 376d8ef9..00000000 --- a/lib/ConvoyLeader/src/RemoteControl.h +++ /dev/null @@ -1,87 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief RemoteControl common constants. - * @author Gabryel Reyes - * - * @addtogroup Application - * - * @{ - */ -#ifndef REMOTE_CONTROL_H -#define REMOTE_CONTROL_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** RemoteControl application constants */ -namespace RemoteControl -{ - /** Remote control commands. */ - typedef enum : uint8_t - { - CMD_ID_IDLE = 0, /**< Nothing to do. */ - CMD_ID_START_LINE_SENSOR_CALIB, /**< Start line sensor calibration. */ - CMD_ID_START_MOTOR_SPEED_CALIB, /**< Start motor speed calibration. */ - CMD_ID_REINIT_BOARD, /**< Re-initialize the board. Required for webots simulation. */ - CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ - CMD_ID_START_DRIVING, /**< Start driving. */ - CMD_ID_SET_INIT_POS, /**< Set initial position. */ - - } CmdId; - - /** Remote control command responses. */ - typedef enum : uint8_t - { - RSP_ID_OK = 0, /**< Command successful executed. */ - RSP_ID_PENDING, /**< Command is pending. */ - RSP_ID_ERROR /**< Command failed. */ - - } RspId; -} /* namespace RemoteControl */ - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* REMOTE_CONTROL_H */ -/** @} */ diff --git a/lib/ConvoyLeader/src/SerialMuxChannels.h b/lib/ConvoyLeader/src/SerialMuxChannels.h index c3e8b2aa..77fe1b46 100644 --- a/lib/ConvoyLeader/src/SerialMuxChannels.h +++ b/lib/ConvoyLeader/src/SerialMuxChannels.h @@ -77,10 +77,36 @@ /** SerialMuxProt Server with fixed template argument. */ typedef SerialMuxProtServer SMPServer; +/** RemoteControl application constants */ +namespace RemoteControl +{ + /** Remote control commands. */ + typedef enum : uint8_t + { + CMD_ID_IDLE = 0, /**< Nothing to do. */ + CMD_ID_START_LINE_SENSOR_CALIB, /**< Start line sensor calibration. */ + CMD_ID_START_MOTOR_SPEED_CALIB, /**< Start motor speed calibration. */ + CMD_ID_REINIT_BOARD, /**< Re-initialize the board. Required for webots simulation. */ + CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ + CMD_ID_START_DRIVING, /**< Start driving. */ + CMD_ID_SET_INIT_POS, /**< Set initial position. */ + + } CmdId; + + /** Remote control command responses. */ + typedef enum : uint8_t + { + RSP_ID_OK = 0, /**< Command successful executed. */ + RSP_ID_PENDING, /**< Command is pending. */ + RSP_ID_ERROR /**< Command failed. */ + + } RspId; +} /* namespace RemoteControl */ + /** Struct of the "Command" channel payload. */ typedef struct _Command { - uint8_t commandId; /**< Command ID */ + RemoteControl::CmdId commandId; /**< Command ID */ /** Command payload. */ union @@ -99,8 +125,8 @@ typedef struct _Command /** Struct of the "Command Response" channel payload. */ typedef struct _CommandResponse { - uint8_t commandId; /**< Command ID */ - uint8_t responseId; /**< Response to the command */ + RemoteControl::CmdId commandId; /**< Command ID */ + RemoteControl::RspId responseId; /**< Response to the command */ /** Response Payload. */ union diff --git a/lib/ConvoyLeader/src/StartupState.cpp b/lib/ConvoyLeader/src/StartupState.cpp index 9cbd18c5..7a9f57cf 100644 --- a/lib/ConvoyLeader/src/StartupState.cpp +++ b/lib/ConvoyLeader/src/StartupState.cpp @@ -35,7 +35,6 @@ #include "StartupState.h" #include "IdleState.h" -#include "RemoteControl.h" #include #include From c20b01ba3b6f4b2249802fee8ce099c243f4b89a Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 13:43:31 +0100 Subject: [PATCH 32/36] Fixed CI findings --- lib/ConvoyLeader/src/IdleState.h | 2 +- lib/ConvoyLeader/src/SerialMuxChannels.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/lib/ConvoyLeader/src/IdleState.h b/lib/ConvoyLeader/src/IdleState.h index b6c41577..0e772ff7 100644 --- a/lib/ConvoyLeader/src/IdleState.h +++ b/lib/ConvoyLeader/src/IdleState.h @@ -108,7 +108,7 @@ class IdleState : public IState /** * Default constructor. */ - IdleState() : IState(), m_isActive(false) + IdleState() : IState(), m_isActive(false), m_releaseRequested(false) { } diff --git a/lib/ConvoyLeader/src/SerialMuxChannels.h b/lib/ConvoyLeader/src/SerialMuxChannels.h index 77fe1b46..8390a8fb 100644 --- a/lib/ConvoyLeader/src/SerialMuxChannels.h +++ b/lib/ConvoyLeader/src/SerialMuxChannels.h @@ -91,7 +91,7 @@ namespace RemoteControl CMD_ID_START_DRIVING, /**< Start driving. */ CMD_ID_SET_INIT_POS, /**< Set initial position. */ - } CmdId; + } CmdId; /**< Command ID */ /** Remote control command responses. */ typedef enum : uint8_t @@ -100,7 +100,7 @@ namespace RemoteControl RSP_ID_PENDING, /**< Command is pending. */ RSP_ID_ERROR /**< Command failed. */ - } RspId; + } RspId; /**< Response ID */ } /* namespace RemoteControl */ /** Struct of the "Command" channel payload. */ From 5cf133cc773334d3e0aea88a7255b7ad9ac0f510 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 13:51:28 +0100 Subject: [PATCH 33/36] Fixed PR findings --- lib/ConvoyLeader/src/App.cpp | 39 ++++++++++++++++-------- lib/ConvoyLeader/src/SerialMuxChannels.h | 2 +- 2 files changed, 27 insertions(+), 14 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index df47ff10..62a6f6c3 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -283,19 +283,32 @@ bool App::setupMqttClient() } else { - isSuccessful = m_mqttClient.subscribe( - TOPIC_NAME_RELEASE, true, [this](const String& payload) { IdleState::getInstance().requestRelease(); }); - - isSuccessful &= - m_mqttClient.subscribe("platoons/0/vehicles/0/feedback", false, - [this](const String& payload) - { - Waypoint* waypoint = Waypoint::deserialize(payload); - VehicleData feedback{waypoint->xPos, waypoint->yPos, waypoint->orientation, - waypoint->left, waypoint->right, waypoint->center}; - - DrivingState::getInstance().setLastFollowerFeedback(feedback); - }); + /* Create Callbacks. */ + IMqttClient::TopicCallback releaseTopicCallback = [this](const String& payload) + { IdleState::getInstance().requestRelease(); }; + + IMqttClient::TopicCallback lastFollowerFeedbackCallback = [this](const String& payload) + { + Waypoint* waypoint = Waypoint::deserialize(payload); + VehicleData feedback{waypoint->xPos, waypoint->yPos, waypoint->orientation, + waypoint->left, waypoint->right, waypoint->center}; + + DrivingState::getInstance().setLastFollowerFeedback(feedback); + }; + + /* Register MQTT client callbacks. */ + if (false == m_mqttClient.subscribe(TOPIC_NAME_RELEASE, true, releaseTopicCallback)) + { + LOG_ERROR("Failed to subscribe to release topic."); + } + else if (false == m_mqttClient.subscribe("platoons/0/vehicles/0/feedback", false, lastFollowerFeedbackCallback)) + { + LOG_ERROR("Failed to subscribe to last follower feedback topic."); + } + else + { + isSuccessful = true; + } } return isSuccessful; diff --git a/lib/ConvoyLeader/src/SerialMuxChannels.h b/lib/ConvoyLeader/src/SerialMuxChannels.h index 8390a8fb..93b77aea 100644 --- a/lib/ConvoyLeader/src/SerialMuxChannels.h +++ b/lib/ConvoyLeader/src/SerialMuxChannels.h @@ -89,7 +89,7 @@ namespace RemoteControl CMD_ID_REINIT_BOARD, /**< Re-initialize the board. Required for webots simulation. */ CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ CMD_ID_START_DRIVING, /**< Start driving. */ - CMD_ID_SET_INIT_POS, /**< Set initial position. */ + CMD_ID_SET_INIT_POS /**< Set initial position. */ } CmdId; /**< Command ID */ From 89c4b4a6c7f7673a59c993d13132cf63d36e164c Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 14:20:55 +0100 Subject: [PATCH 34/36] Implemente concat for String class --- lib/ArduinoNative/src/WString.h | 29 +++++++++++++++++++++++++ test/test_WString/test_WString.cpp | 35 +++++++++++++++++++++++++----- 2 files changed, 59 insertions(+), 5 deletions(-) diff --git a/lib/ArduinoNative/src/WString.h b/lib/ArduinoNative/src/WString.h index 3ea925cc..ca143cea 100644 --- a/lib/ArduinoNative/src/WString.h +++ b/lib/ArduinoNative/src/WString.h @@ -41,6 +41,10 @@ * Compile Switches *****************************************************************************/ +#ifndef ARDUINOJSON_ENABLE_ARDUINO_STRING +#define ARDUINOJSON_ENABLE_ARDUINO_STRING 1 +#endif + /****************************************************************************** * Includes *****************************************************************************/ @@ -648,6 +652,31 @@ class String } } + bool concat(const String& s) + { + this->operator+=(s); + return true; + } + + bool concat(const char* cstr) + { + if ((nullptr == cstr) || (0U == strlen(cstr))) + { + return false; + } + else + { + this->operator+=(cstr); + return true; + } + } + + bool concat(char c) + { + this->operator+=(c); + return true; + } + private: size_t m_size; /**< String buffer size */ char* m_buffer; /**< String buffer */ diff --git a/test/test_WString/test_WString.cpp b/test/test_WString/test_WString.cpp index b8a25f4b..811a53a4 100644 --- a/test/test_WString/test_WString.cpp +++ b/test/test_WString/test_WString.cpp @@ -53,6 +53,7 @@ static void testWStringReplacement(void); static void testWStringAppend(void); +static void testWStringConcat(void); /****************************************************************************** * Local Variables @@ -88,6 +89,7 @@ extern int main(int argc, char** argv) RUN_TEST(testWStringReplacement); RUN_TEST(testWStringAppend); + RUN_TEST(testWStringConcat); return UNITY_END(); } @@ -154,11 +156,11 @@ static void testWStringReplacement(void) static void testWStringAppend(void) { String original = String("Im short"); - char classic[4]; - classic[0] = 'H'; - classic[1] = 'I'; - classic[2] = '!'; - classic[3] = '\0'; + char classic[4]; + classic[0] = 'H'; + classic[1] = 'I'; + classic[2] = '!'; + classic[3] = '\0'; original += String("- first amendment -"); original += classic; @@ -170,3 +172,26 @@ static void testWStringAppend(void) tester += "Im new"; TEST_ASSERT_EQUAL_STRING("Im new", tester.c_str()); } + +/** + * Test WString += implementation. + */ +static void testWStringConcat(void) +{ + String original = String("Im short"); + char classic[4]; + classic[0] = 'H'; + classic[1] = 'I'; + classic[2] = '!'; + classic[3] = '\0'; + + original.concat(String("- first amendment -")); + original.concat(classic); + original.concat('-'); + + TEST_ASSERT_EQUAL_STRING("Im short- first amendment -HI!-", original.c_str()); + + String tester = String(""); + tester.concat("Im new"); + TEST_ASSERT_EQUAL_STRING("Im new", tester.c_str()); +} From 34fc984a1a1d054dfe6ae881e565eb1d822be4bd Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 14:26:26 +0100 Subject: [PATCH 35/36] Checked birht message serialization --- lib/ConvoyLeader/src/App.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index 62a6f6c3..cfa859fe 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -257,12 +257,16 @@ bool App::setupMqttClient() bool isSuccessful = false; SettingsHandler& settings = SettingsHandler::getInstance(); StaticJsonDocument birthDoc; - char birthMsgArray[JSON_BIRTHMESSAGE_MAX_SIZE]; String birthMessage; birthDoc["name"] = settings.getRobotName(); - (void)serializeJson(birthDoc, birthMsgArray); - birthMessage = birthMsgArray; + + if (0U == serializeJson(birthDoc, birthMessage)) + { + /* Non-fatal error. Birth message will be empty. */ + LOG_ERROR("Failed to serialize birth message."); + birthMessage.clear(); + } MqttSettings mqttSettings = {settings.getRobotName(), settings.getMqttBrokerAddress(), From 0b6885175e25b1e0285c717182e76437cc6e58e0 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Wed, 24 Jan 2024 14:26:56 +0100 Subject: [PATCH 36/36] removed unnecessary member variable --- lib/ConvoyLeader/src/ErrorState.cpp | 3 +-- lib/ConvoyLeader/src/ErrorState.h | 3 --- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/lib/ConvoyLeader/src/ErrorState.cpp b/lib/ConvoyLeader/src/ErrorState.cpp index bb55196a..26bd16c4 100644 --- a/lib/ConvoyLeader/src/ErrorState.cpp +++ b/lib/ConvoyLeader/src/ErrorState.cpp @@ -62,8 +62,7 @@ void ErrorState::entry() { - m_isActive = true; - m_releaseRequested = false; + m_isActive = true; } void ErrorState::process(StateMachine& sm) diff --git a/lib/ConvoyLeader/src/ErrorState.h b/lib/ConvoyLeader/src/ErrorState.h index a8f4634d..352100d6 100644 --- a/lib/ConvoyLeader/src/ErrorState.h +++ b/lib/ConvoyLeader/src/ErrorState.h @@ -92,9 +92,6 @@ class ErrorState : public IState /** Flag: State is active. */ bool m_isActive; - /** Flag: Release is requested. */ - bool m_releaseRequested; - /** * Default constructor. */