From 2bce40dce00363ff4dbfbf664ff974e26dce2909 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 1 Feb 2024 13:52:07 +0100 Subject: [PATCH 01/13] Waypoint topics set up in a separate method --- lib/PlatoonService/src/V2VClient.cpp | 114 ++++++++++++++++----------- lib/PlatoonService/src/V2VClient.h | 21 ++++- 2 files changed, 84 insertions(+), 51 deletions(-) diff --git a/lib/PlatoonService/src/V2VClient.cpp b/lib/PlatoonService/src/V2VClient.cpp index 665eb225..06f1f193 100644 --- a/lib/PlatoonService/src/V2VClient.cpp +++ b/lib/PlatoonService/src/V2VClient.cpp @@ -55,6 +55,9 @@ * Local Variables *****************************************************************************/ +/* MQTT subtopic name for waypoint reception. */ +const char* V2VClient::TOPIC_NAME_WAYPOINT_RX = "inputWaypoint"; + /****************************************************************************** * Public Methods *****************************************************************************/ @@ -62,8 +65,8 @@ V2VClient::V2VClient(MqttClient& mqttClient) : m_mqttClient(mqttClient), m_waypointQueue(), - m_inputTopic(), - m_outputTopic(), + m_waypointInputTopic(), + m_waypointOutputTopic(), m_isLeader(false) { } @@ -74,69 +77,34 @@ V2VClient::~V2VClient() bool V2VClient::init(uint8_t platoonId, uint8_t vehicleId) { - bool isSuccessful = false; - char inputTopicBuffer[MAX_TOPIC_LENGTH]; - char outputTopicBuffer[MAX_TOPIC_LENGTH]; - uint8_t followerVehicleId = vehicleId + 1U; /* Output is published to next vehicle. */ - const char* outputSubtopic = "targetWaypoint"; + bool isSuccessful = false; if (PLATOON_LEADER_ID == vehicleId) { /* Its the leader. */ m_isLeader = true; } - else if (MAX_FOLLOWERS == vehicleId) + else if (NUMBER_OF_FOLLOWERS == vehicleId) { - /* Last follower. Sends data to the leader. */ - followerVehicleId = 0U; - outputSubtopic = "feedback"; + /* Last follower. */ } else { ; /* Its a normal follower. Nothing to do */ } - if (MAX_FOLLOWERS < vehicleId) + if (NUMBER_OF_FOLLOWERS < vehicleId) { /* Invalid ID. */ - LOG_ERROR("Invalid vehicle ID: %d. Maximum followers: %d.", vehicleId, MAX_FOLLOWERS); - } - else if (0 >= snprintf(inputTopicBuffer, MAX_TOPIC_LENGTH, "platoons/%d/vehicles/%d/targetWaypoint", platoonId, - vehicleId)) - { - LOG_ERROR("Failed to create input topic."); + LOG_ERROR("Invalid vehicle ID: %d. Maximum followers: %d.", vehicleId, NUMBER_OF_FOLLOWERS); } - else if (0 >= snprintf(outputTopicBuffer, MAX_TOPIC_LENGTH, "platoons/%d/vehicles/%d/%s", platoonId, - followerVehicleId, outputSubtopic)) + else if (false == setupWaypointTopics(platoonId, vehicleId)) { - LOG_ERROR("Failed to create output topic."); + LOG_ERROR("Failed to setup waypoint topics."); } else { - /* Set topics. */ - m_inputTopic = inputTopicBuffer; - m_outputTopic = outputTopicBuffer; - - LOG_DEBUG("Input Topic: %s", m_inputTopic.c_str()); - LOG_DEBUG("Output Topic: %s", m_outputTopic.c_str()); - - IMqttClient::TopicCallback lambdaTargetWaypointTopicCallback = [this](const String& payload) - { targetWaypointTopicCallback(payload); }; - - if ((true == m_inputTopic.isEmpty()) || (true == m_outputTopic.isEmpty())) - { - LOG_ERROR("Failed to create Platoon MQTT topics."); - } - /* Subscribe to Input Topic. */ - else if (false == m_mqttClient.subscribe(m_inputTopic, false, lambdaTargetWaypointTopicCallback)) - { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_inputTopic.c_str()); - } - else - { - - isSuccessful = true; - } + isSuccessful = true; } return isSuccessful; @@ -157,9 +125,9 @@ bool V2VClient::sendWaypoint(const Waypoint& waypoint) { LOG_DEBUG("Failed to serialize waypoint."); } - else if (false == m_mqttClient.publish(m_outputTopic, false, payload)) + else if (false == m_mqttClient.publish(m_waypointOutputTopic, false, payload)) { - LOG_ERROR("Failed to publish MQTT message to %s.", m_outputTopic.c_str()); + LOG_ERROR("Failed to publish MQTT message to %s.", m_waypointOutputTopic.c_str()); } else { @@ -218,6 +186,58 @@ void V2VClient::targetWaypointTopicCallback(const String& payload) } } +bool V2VClient::setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId) +{ + bool isSuccessful = false; + uint8_t nextVehicleId = vehicleId + 1U; /* Output is published to next vehicle. */ + char inputTopicBuffer[MAX_TOPIC_LENGTH]; + char outputTopicBuffer[MAX_TOPIC_LENGTH]; + + /* Input Topic. */ + if (0 >= snprintf(inputTopicBuffer, MAX_TOPIC_LENGTH, "platoons/%d/vehicles/%d/%s", platoonId, vehicleId, + TOPIC_NAME_WAYPOINT_RX)) + { + LOG_ERROR("Failed to create input topic."); + } + /* Output Topic. */ + else if (0 >= snprintf(outputTopicBuffer, MAX_TOPIC_LENGTH, "platoons/%d/vehicles/%d/%s", platoonId, nextVehicleId, + TOPIC_NAME_WAYPOINT_RX)) + { + LOG_ERROR("Failed to create output topic."); + } + else + { + /* Set topics. */ + m_waypointInputTopic = inputTopicBuffer; + m_waypointOutputTopic = outputTopicBuffer; + + if ((true == m_waypointInputTopic.isEmpty()) || (true == m_waypointOutputTopic.isEmpty())) + { + LOG_ERROR("Failed to create Platoon MQTT topics."); + } + else + { + /* Create lambda callback function for the waypoint input topic. */ + IMqttClient::TopicCallback lambdaWaypointInputTopicCallback = [this](const String& payload) + { this->targetWaypointTopicCallback(payload); }; + + /* Subscribe to Input Topic. */ + if (false == m_mqttClient.subscribe(m_waypointInputTopic, false, lambdaWaypointInputTopicCallback)) + { + LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_waypointInputTopic.c_str()); + } + else + { + LOG_DEBUG("Waypoint Input Topic: %s", m_waypointInputTopic.c_str()); + LOG_DEBUG("Waypoint Output Topic: %s", m_waypointOutputTopic.c_str()); + isSuccessful = true; + } + } + } + + return isSuccessful; +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/PlatoonService/src/V2VClient.h b/lib/PlatoonService/src/V2VClient.h index 76d07274..242611ab 100644 --- a/lib/PlatoonService/src/V2VClient.h +++ b/lib/PlatoonService/src/V2VClient.h @@ -117,8 +117,11 @@ class V2VClient /** Max topic length */ static const uint8_t MAX_TOPIC_LENGTH = 64U; - /** Maximum number of followers. */ - static const uint8_t MAX_FOLLOWERS = 1U; + /** Number of followers. */ + static const uint8_t NUMBER_OF_FOLLOWERS = 1U; + + /** MQTT subtopic name for waypoint reception. */ + static const char* TOPIC_NAME_WAYPOINT_RX; /** MQTTClient Instance. */ MqttClient& m_mqttClient; @@ -134,10 +137,10 @@ class V2VClient std::queue m_waypointQueue; /** Topic to receive target Waypoints. */ - String m_inputTopic; + String m_waypointInputTopic; /** Topic to send target Waypoints. */ - String m_outputTopic; + String m_waypointOutputTopic; /** Leader flag. */ bool m_isLeader; @@ -150,6 +153,16 @@ class V2VClient */ void targetWaypointTopicCallback(const String& payload); + /** + * Setup waypoint input and output topics. + * + * @param[in] platoonId ID of the platoon. + * @param[in] vehicleId ID of the vehicle inside the platoon. + * + * @return If the topics were setup successfully, returns true. Otherwise, false. + */ + bool setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId); + /** * Default constructor. */ From 50794c1b38f71b4b1150feb186f7d9a7cf8c9425 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 1 Feb 2024 14:00:44 +0100 Subject: [PATCH 02/13] Participant type enum added --- lib/PlatoonService/src/V2VClient.cpp | 23 ++++++++++++----------- lib/PlatoonService/src/V2VClient.h | 13 +++++++++++-- 2 files changed, 23 insertions(+), 13 deletions(-) diff --git a/lib/PlatoonService/src/V2VClient.cpp b/lib/PlatoonService/src/V2VClient.cpp index 06f1f193..434003ba 100644 --- a/lib/PlatoonService/src/V2VClient.cpp +++ b/lib/PlatoonService/src/V2VClient.cpp @@ -67,7 +67,7 @@ V2VClient::V2VClient(MqttClient& mqttClient) : m_waypointQueue(), m_waypointInputTopic(), m_waypointOutputTopic(), - m_isLeader(false) + m_participantType(PARTICIPANT_TYPE_UNKNOWN) { } @@ -79,24 +79,25 @@ bool V2VClient::init(uint8_t platoonId, uint8_t vehicleId) { bool isSuccessful = false; - if (PLATOON_LEADER_ID == vehicleId) + if (NUMBER_OF_FOLLOWERS < vehicleId) { - /* Its the leader. */ - m_isLeader = true; + /* Invalid ID. */ + LOG_ERROR("Invalid vehicle ID: %d. Maximum followers: %d.", vehicleId, NUMBER_OF_FOLLOWERS); } - else if (NUMBER_OF_FOLLOWERS == vehicleId) + else if (PLATOON_LEADER_ID == vehicleId) { - /* Last follower. */ + /* Its the leader. */ + m_participantType = PARTICIPANT_TYPE_LEADER; } else { - ; /* Its a normal follower. Nothing to do */ + /* Its a follower. */ + m_participantType = PARTICIPANT_TYPE_FOLLOWER; } - if (NUMBER_OF_FOLLOWERS < vehicleId) + if (PARTICIPANT_TYPE_UNKNOWN == m_participantType) { - /* Invalid ID. */ - LOG_ERROR("Invalid vehicle ID: %d. Maximum followers: %d.", vehicleId, NUMBER_OF_FOLLOWERS); + LOG_ERROR("Failed to determine participant type."); } else if (false == setupWaypointTopics(platoonId, vehicleId)) { @@ -213,7 +214,7 @@ bool V2VClient::setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId) if ((true == m_waypointInputTopic.isEmpty()) || (true == m_waypointOutputTopic.isEmpty())) { - LOG_ERROR("Failed to create Platoon MQTT topics."); + LOG_ERROR("Failed to create Waypoint MQTT topics."); } else { diff --git a/lib/PlatoonService/src/V2VClient.h b/lib/PlatoonService/src/V2VClient.h index 242611ab..8d0e922c 100644 --- a/lib/PlatoonService/src/V2VClient.h +++ b/lib/PlatoonService/src/V2VClient.h @@ -61,6 +61,14 @@ class V2VClient /** Platoon leader vehicle ID. */ static const uint8_t PLATOON_LEADER_ID = 0U; + /** Type of platoon participant. */ + enum ParticipantType : uint8_t + { + PARTICIPANT_TYPE_UNKNOWN = 0U, /**< Unknown participant type */ + PARTICIPANT_TYPE_LEADER, /**< Platoon leader */ + PARTICIPANT_TYPE_FOLLOWER /**< Platoon follower */ + }; + /** * Constructs a V2V client. * @@ -142,8 +150,9 @@ class V2VClient /** Topic to send target Waypoints. */ String m_waypointOutputTopic; - /** Leader flag. */ - bool m_isLeader; + + /** Type of Platoon Participant.*/ + ParticipantType m_participantType; private: /** From 301ab19945ca27d32e0ab6ca1f0ccef91bafc53d Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 1 Feb 2024 15:41:11 +0100 Subject: [PATCH 03/13] Subcription to Heartbeat topic --- lib/PlatoonService/src/V2VClient.cpp | 66 ++++++++++++++++++++++++++++ lib/PlatoonService/src/V2VClient.h | 25 +++++++++++ 2 files changed, 91 insertions(+) diff --git a/lib/PlatoonService/src/V2VClient.cpp b/lib/PlatoonService/src/V2VClient.cpp index 434003ba..70eb7532 100644 --- a/lib/PlatoonService/src/V2VClient.cpp +++ b/lib/PlatoonService/src/V2VClient.cpp @@ -58,6 +58,9 @@ /* MQTT subtopic name for waypoint reception. */ const char* V2VClient::TOPIC_NAME_WAYPOINT_RX = "inputWaypoint"; +/* MQTT subtopic name for platoon heartbeat. */ +const char* V2VClient::TOPIC_NAME_PLATOON_HEARTBEAT = "heartbeat"; + /****************************************************************************** * Public Methods *****************************************************************************/ @@ -67,6 +70,8 @@ V2VClient::V2VClient(MqttClient& mqttClient) : m_waypointQueue(), m_waypointInputTopic(), m_waypointOutputTopic(), + m_platoonHeartbeatTopic(), + m_vehicleHeartbeatTopic(), m_participantType(PARTICIPANT_TYPE_UNKNOWN) { } @@ -103,6 +108,10 @@ bool V2VClient::init(uint8_t platoonId, uint8_t vehicleId) { LOG_ERROR("Failed to setup waypoint topics."); } + else if (false == setupHeartbeatTopics(platoonId, vehicleId)) + { + LOG_ERROR("Failed to setup heartbeat topics."); + } else { isSuccessful = true; @@ -187,6 +196,11 @@ void V2VClient::targetWaypointTopicCallback(const String& payload) } } +void V2VClient::platoonHeartbeatTopicCallback(const String& payload) +{ + /* TODO: Receive Platoon Heartbeat, and send own vehicle heartbeat. */ +} + bool V2VClient::setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId) { bool isSuccessful = false; @@ -239,6 +253,58 @@ bool V2VClient::setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId) return isSuccessful; } +bool V2VClient::setupHeartbeatTopics(uint8_t platoonId, uint8_t vehicleId) +{ + bool isSuccessful = false; + char platoonHeartbeatTopicBuffer[MAX_TOPIC_LENGTH]; + char vehicleHeartbeatTopicBuffer[MAX_TOPIC_LENGTH]; + + /* Platoon Heartbeat Topic. */ + if (0 >= snprintf(platoonHeartbeatTopicBuffer, MAX_TOPIC_LENGTH, "platoons/%d/%s", platoonId, + TOPIC_NAME_PLATOON_HEARTBEAT)) + { + LOG_ERROR("Failed to create Platoon Heartbeat topic."); + } + /* Heartbeat Response Topic. */ + else if (0 >= snprintf(vehicleHeartbeatTopicBuffer, MAX_TOPIC_LENGTH, "platoons/%d/vehicles/%d/%s", platoonId, + vehicleId, TOPIC_NAME_PLATOON_HEARTBEAT)) + { + LOG_ERROR("Failed to create Heartbeat Response topic."); + } + else + { + /* Set topics. */ + m_platoonHeartbeatTopic = platoonHeartbeatTopicBuffer; + m_vehicleHeartbeatTopic = vehicleHeartbeatTopicBuffer; + + LOG_DEBUG("Platoon Heartbeat Topic: %s", m_platoonHeartbeatTopic.c_str()); + LOG_DEBUG("Vehicle Heartbeat Topic: %s", m_vehicleHeartbeatTopic.c_str()); + + if ((true == m_platoonHeartbeatTopic.isEmpty()) || (true == m_vehicleHeartbeatTopic.isEmpty())) + { + LOG_ERROR("Failed to create Platoon Heartbeat MQTT topics."); + } + else + { + /* Create lambda callback function for the platoon heartbeat topic. */ + IMqttClient::TopicCallback lambdaHeartbeatInputTopicCallback = [this](const String& payload) + { this->platoonHeartbeatTopicCallback(payload); }; + + /* Subscribe to platoon heartbeat Topic. */ + if (false == m_mqttClient.subscribe(m_platoonHeartbeatTopic, false, lambdaHeartbeatInputTopicCallback)) + { + LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str()); + } + else + { + isSuccessful = true; + } + } + } + + return isSuccessful; +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/PlatoonService/src/V2VClient.h b/lib/PlatoonService/src/V2VClient.h index 8d0e922c..1e7ffdfd 100644 --- a/lib/PlatoonService/src/V2VClient.h +++ b/lib/PlatoonService/src/V2VClient.h @@ -131,6 +131,9 @@ class V2VClient /** MQTT subtopic name for waypoint reception. */ static const char* TOPIC_NAME_WAYPOINT_RX; + /** MQTT subtopic name for platoon heartbeat. */ + static const char* TOPIC_NAME_PLATOON_HEARTBEAT; + /** MQTTClient Instance. */ MqttClient& m_mqttClient; @@ -150,6 +153,11 @@ class V2VClient /** Topic to send target Waypoints. */ String m_waypointOutputTopic; + /** Topic to receive platoon heartbeat messages. */ + String m_platoonHeartbeatTopic; + + /** Topic to send vehicle heartbeat messages. */ + String m_vehicleHeartbeatTopic; /** Type of Platoon Participant.*/ ParticipantType m_participantType; @@ -162,6 +170,13 @@ class V2VClient */ void targetWaypointTopicCallback(const String& payload); + /** + * Callback for Platoon Heartbeat MQTT Topic. + * + * @param[in] payload Payload of the MQTT message. + */ + void platoonHeartbeatTopicCallback(const String& payload); + /** * Setup waypoint input and output topics. * @@ -172,6 +187,16 @@ class V2VClient */ bool setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId); + /** + * Setup heartbeat input and output topics. + * + * @param[in] platoonId ID of the platoon. + * @param[in] vehicleId ID of the vehicle inside the platoon. + * + * @return If the topics were setup successfully, returns true. Otherwise, false. + */ + bool setupHeartbeatTopics(uint8_t platoonId, uint8_t vehicleId); + /** * Default constructor. */ From cf97bbe5167060aa91e36081b020e2d3c3c4661e Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 1 Feb 2024 16:18:10 +0100 Subject: [PATCH 04/13] Response to platoon heartbeat --- lib/PlatoonService/src/V2VClient.cpp | 39 +++++++++++++++++++++++----- lib/PlatoonService/src/V2VClient.h | 3 +++ 2 files changed, 35 insertions(+), 7 deletions(-) diff --git a/lib/PlatoonService/src/V2VClient.cpp b/lib/PlatoonService/src/V2VClient.cpp index 70eb7532..1ba0c97a 100644 --- a/lib/PlatoonService/src/V2VClient.cpp +++ b/lib/PlatoonService/src/V2VClient.cpp @@ -34,6 +34,7 @@ *****************************************************************************/ #include "V2VClient.h" #include +#include /****************************************************************************** * Compiler Switches @@ -61,6 +62,9 @@ const char* V2VClient::TOPIC_NAME_WAYPOINT_RX = "inputWaypoint"; /* MQTT subtopic name for platoon heartbeat. */ const char* V2VClient::TOPIC_NAME_PLATOON_HEARTBEAT = "heartbeat"; +/** Buffer size for JSON serialization of heartbeat messages. */ +static const uint32_t JSON_HEARTBEAT_MAX_SIZE = 64U; + /****************************************************************************** * Public Methods *****************************************************************************/ @@ -72,7 +76,8 @@ V2VClient::V2VClient(MqttClient& mqttClient) : m_waypointOutputTopic(), m_platoonHeartbeatTopic(), m_vehicleHeartbeatTopic(), - m_participantType(PARTICIPANT_TYPE_UNKNOWN) + m_participantType(PARTICIPANT_TYPE_UNKNOWN), + m_vehicleId(0U) { } @@ -84,12 +89,14 @@ bool V2VClient::init(uint8_t platoonId, uint8_t vehicleId) { bool isSuccessful = false; - if (NUMBER_OF_FOLLOWERS < vehicleId) + m_vehicleId = vehicleId; + + if (NUMBER_OF_FOLLOWERS < m_vehicleId) { /* Invalid ID. */ - LOG_ERROR("Invalid vehicle ID: %d. Maximum followers: %d.", vehicleId, NUMBER_OF_FOLLOWERS); + LOG_ERROR("Invalid vehicle ID: %d. Maximum followers: %d.", m_vehicleId, NUMBER_OF_FOLLOWERS); } - else if (PLATOON_LEADER_ID == vehicleId) + else if (PLATOON_LEADER_ID == m_vehicleId) { /* Its the leader. */ m_participantType = PARTICIPANT_TYPE_LEADER; @@ -104,11 +111,11 @@ bool V2VClient::init(uint8_t platoonId, uint8_t vehicleId) { LOG_ERROR("Failed to determine participant type."); } - else if (false == setupWaypointTopics(platoonId, vehicleId)) + else if (false == setupWaypointTopics(platoonId, m_vehicleId)) { LOG_ERROR("Failed to setup waypoint topics."); } - else if (false == setupHeartbeatTopics(platoonId, vehicleId)) + else if (false == setupHeartbeatTopics(platoonId, m_vehicleId)) { LOG_ERROR("Failed to setup heartbeat topics."); } @@ -198,7 +205,25 @@ void V2VClient::targetWaypointTopicCallback(const String& payload) void V2VClient::platoonHeartbeatTopicCallback(const String& payload) { - /* TODO: Receive Platoon Heartbeat, and send own vehicle heartbeat. */ + /* Send vehicle heartbeat. */ + StaticJsonDocument heartbeatDoc; + String heartbeatPayload; + + heartbeatDoc["id"] = m_vehicleId; + heartbeatDoc["timestamp"] = millis(); + + if (0U == serializeJson(heartbeatDoc, heartbeatPayload)) + { + LOG_ERROR("Failed to serialize heartbeat."); + } + else if (false == m_mqttClient.publish(m_vehicleHeartbeatTopic, false, heartbeatPayload)) + { + LOG_ERROR("Failed to publish MQTT message to %s.", m_vehicleHeartbeatTopic.c_str()); + } + else + { + LOG_DEBUG("Sent heartbeat: %s", heartbeatPayload.c_str()); + } } bool V2VClient::setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId) diff --git a/lib/PlatoonService/src/V2VClient.h b/lib/PlatoonService/src/V2VClient.h index 1e7ffdfd..de574377 100644 --- a/lib/PlatoonService/src/V2VClient.h +++ b/lib/PlatoonService/src/V2VClient.h @@ -162,6 +162,9 @@ class V2VClient /** Type of Platoon Participant.*/ ParticipantType m_participantType; + /** Vehicle ID. */ + uint8_t m_vehicleId; + private: /** * Callback for Position Setpoint MQTT Topic. From 7fc92285e71b6ab4fe698f82e6bc7cf8844cffe4 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 1 Feb 2024 16:50:02 +0100 Subject: [PATCH 05/13] Setup leader topics --- lib/PlatoonService/src/V2VClient.cpp | 57 +++++++++++++++++++++++----- lib/PlatoonService/src/V2VClient.h | 22 ++++++++++- 2 files changed, 69 insertions(+), 10 deletions(-) diff --git a/lib/PlatoonService/src/V2VClient.cpp b/lib/PlatoonService/src/V2VClient.cpp index 1ba0c97a..b6bdcfdd 100644 --- a/lib/PlatoonService/src/V2VClient.cpp +++ b/lib/PlatoonService/src/V2VClient.cpp @@ -62,6 +62,9 @@ const char* V2VClient::TOPIC_NAME_WAYPOINT_RX = "inputWaypoint"; /* MQTT subtopic name for platoon heartbeat. */ const char* V2VClient::TOPIC_NAME_PLATOON_HEARTBEAT = "heartbeat"; +/* MQTT subtopic name for platoon heartbeat. */ +const char* V2VClient::TOPIC_NAME_PLATOON_HEARTBEAT_RESPONSE = "heartbeatResponse"; + /** Buffer size for JSON serialization of heartbeat messages. */ static const uint32_t JSON_HEARTBEAT_MAX_SIZE = 64U; @@ -75,7 +78,7 @@ V2VClient::V2VClient(MqttClient& mqttClient) : m_waypointInputTopic(), m_waypointOutputTopic(), m_platoonHeartbeatTopic(), - m_vehicleHeartbeatTopic(), + m_heartbeatResponseTopic(), m_participantType(PARTICIPANT_TYPE_UNKNOWN), m_vehicleId(0U) { @@ -124,6 +127,11 @@ bool V2VClient::init(uint8_t platoonId, uint8_t vehicleId) isSuccessful = true; } + if ((true == isSuccessful) && (PARTICIPANT_TYPE_LEADER == m_participantType)) + { + isSuccessful = setupLeaderTopics(); + } + return isSuccessful; } @@ -216,9 +224,9 @@ void V2VClient::platoonHeartbeatTopicCallback(const String& payload) { LOG_ERROR("Failed to serialize heartbeat."); } - else if (false == m_mqttClient.publish(m_vehicleHeartbeatTopic, false, heartbeatPayload)) + else if (false == m_mqttClient.publish(m_heartbeatResponseTopic, false, heartbeatPayload)) { - LOG_ERROR("Failed to publish MQTT message to %s.", m_vehicleHeartbeatTopic.c_str()); + LOG_ERROR("Failed to publish MQTT message to %s.", m_heartbeatResponseTopic.c_str()); } else { @@ -226,6 +234,12 @@ void V2VClient::platoonHeartbeatTopicCallback(const String& payload) } } +void V2VClient::vehicleHeartbeatTopicCallback(const String& payload) +{ + /* TODO: Leader processing of vehicle heartbeats. */ + LOG_DEBUG("Received heartbeat from vehicle: %s", payload.c_str()); +} + bool V2VClient::setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId) { bool isSuccessful = false; @@ -291,21 +305,21 @@ bool V2VClient::setupHeartbeatTopics(uint8_t platoonId, uint8_t vehicleId) LOG_ERROR("Failed to create Platoon Heartbeat topic."); } /* Heartbeat Response Topic. */ - else if (0 >= snprintf(vehicleHeartbeatTopicBuffer, MAX_TOPIC_LENGTH, "platoons/%d/vehicles/%d/%s", platoonId, - vehicleId, TOPIC_NAME_PLATOON_HEARTBEAT)) + else if (0 >= snprintf(vehicleHeartbeatTopicBuffer, MAX_TOPIC_LENGTH, "platoons/%d/%s", platoonId, + TOPIC_NAME_PLATOON_HEARTBEAT_RESPONSE)) { LOG_ERROR("Failed to create Heartbeat Response topic."); } else { /* Set topics. */ - m_platoonHeartbeatTopic = platoonHeartbeatTopicBuffer; - m_vehicleHeartbeatTopic = vehicleHeartbeatTopicBuffer; + m_platoonHeartbeatTopic = platoonHeartbeatTopicBuffer; + m_heartbeatResponseTopic = vehicleHeartbeatTopicBuffer; LOG_DEBUG("Platoon Heartbeat Topic: %s", m_platoonHeartbeatTopic.c_str()); - LOG_DEBUG("Vehicle Heartbeat Topic: %s", m_vehicleHeartbeatTopic.c_str()); + LOG_DEBUG("Vehicle Heartbeat Topic: %s", m_heartbeatResponseTopic.c_str()); - if ((true == m_platoonHeartbeatTopic.isEmpty()) || (true == m_vehicleHeartbeatTopic.isEmpty())) + if ((true == m_platoonHeartbeatTopic.isEmpty()) || (true == m_heartbeatResponseTopic.isEmpty())) { LOG_ERROR("Failed to create Platoon Heartbeat MQTT topics."); } @@ -330,6 +344,31 @@ bool V2VClient::setupHeartbeatTopics(uint8_t platoonId, uint8_t vehicleId) return isSuccessful; } +bool V2VClient::setupLeaderTopics() +{ + bool isSuccessful = false; + + /* Participant is the leader. */ + IMqttClient::TopicCallback lambdaVehiclesHeartbeatTopicCallback = [this](const String& payload) + { this->vehicleHeartbeatTopicCallback(payload); }; + + if (true == m_heartbeatResponseTopic.isEmpty()) + { + LOG_ERROR("Failed to create Leader Heartbeat Response MQTT topic."); + } + /* Subscribe to Vehicles Heartbeat Topics. */ + else if (false == m_mqttClient.subscribe(m_heartbeatResponseTopic, false, lambdaVehiclesHeartbeatTopicCallback)) + { + LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str()); + } + else + { + isSuccessful = true; + } + + return isSuccessful; +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/PlatoonService/src/V2VClient.h b/lib/PlatoonService/src/V2VClient.h index de574377..eddf0361 100644 --- a/lib/PlatoonService/src/V2VClient.h +++ b/lib/PlatoonService/src/V2VClient.h @@ -134,6 +134,9 @@ class V2VClient /** MQTT subtopic name for platoon heartbeat. */ static const char* TOPIC_NAME_PLATOON_HEARTBEAT; + /** MQTT subtopic name for platoon heartbeat reponse. */ + static const char* TOPIC_NAME_PLATOON_HEARTBEAT_RESPONSE; + /** MQTTClient Instance. */ MqttClient& m_mqttClient; @@ -157,11 +160,14 @@ class V2VClient String m_platoonHeartbeatTopic; /** Topic to send vehicle heartbeat messages. */ - String m_vehicleHeartbeatTopic; + String m_heartbeatResponseTopic; /** Type of Platoon Participant.*/ ParticipantType m_participantType; + /** Platoon ID. */ + uint8_t m_platoonId; + /** Vehicle ID. */ uint8_t m_vehicleId; @@ -180,6 +186,13 @@ class V2VClient */ void platoonHeartbeatTopicCallback(const String& payload); + /** + * Callback for Vehicle Heartbeat MQTT Topic. Only used by the leader. + * + * @param[in] payload Payload of the MQTT message. + */ + void vehicleHeartbeatTopicCallback(const String& payload); + /** * Setup waypoint input and output topics. * @@ -200,6 +213,13 @@ class V2VClient */ bool setupHeartbeatTopics(uint8_t platoonId, uint8_t vehicleId); + /** + * Leader-specific setup. Sets up the topics for the leader, which must take care of its followers. + * + * @return If the topics were setup successfully, returns true. Otherwise, false. + */ + bool setupLeaderTopics(); + /** * Default constructor. */ From fee9c1eca6784af11516622628104f2ba404270e Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 1 Feb 2024 18:06:11 +0100 Subject: [PATCH 06/13] Send and respond to heartbeat --- lib/PlatoonService/src/V2VClient.cpp | 149 +++++++++++++++++++++++---- lib/PlatoonService/src/V2VClient.h | 26 +++++ 2 files changed, 157 insertions(+), 18 deletions(-) diff --git a/lib/PlatoonService/src/V2VClient.cpp b/lib/PlatoonService/src/V2VClient.cpp index b6bdcfdd..3a311ac9 100644 --- a/lib/PlatoonService/src/V2VClient.cpp +++ b/lib/PlatoonService/src/V2VClient.cpp @@ -66,7 +66,7 @@ const char* V2VClient::TOPIC_NAME_PLATOON_HEARTBEAT = "heartbeat"; const char* V2VClient::TOPIC_NAME_PLATOON_HEARTBEAT_RESPONSE = "heartbeatResponse"; /** Buffer size for JSON serialization of heartbeat messages. */ -static const uint32_t JSON_HEARTBEAT_MAX_SIZE = 64U; +static const uint32_t JSON_HEARTBEAT_MAX_SIZE = 128U; /****************************************************************************** * Public Methods @@ -80,7 +80,9 @@ V2VClient::V2VClient(MqttClient& mqttClient) : m_platoonHeartbeatTopic(), m_heartbeatResponseTopic(), m_participantType(PARTICIPANT_TYPE_UNKNOWN), - m_vehicleId(0U) + m_platoonId(0U), + m_vehicleId(0U), + m_followerResponseCounter(0U) { } @@ -130,6 +132,7 @@ bool V2VClient::init(uint8_t platoonId, uint8_t vehicleId) if ((true == isSuccessful) && (PARTICIPANT_TYPE_LEADER == m_participantType)) { isSuccessful = setupLeaderTopics(); + m_platoonHeartbeatTimer.start(PLATOON_HEARTBEAT_TIMER_INTERVAL); } return isSuccessful; @@ -137,7 +140,42 @@ bool V2VClient::init(uint8_t platoonId, uint8_t vehicleId) void V2VClient::process() { - /* Nothing to do here yet. */ + /* Send Platoon Heartbeat. Only active as leader. */ + if (true == m_platoonHeartbeatTimer.isTimeout()) + { + /* Send Platoon Heartbeat */ + if (false == sendPlatoonHeartbeat()) + { + LOG_ERROR("Failed to send platoon heartbeat."); + } + else + { + /* Start timeout timer. */ + m_vehicleHeartbeatTimeoutTimer.start(VEHICLE_HEARTBEAT_TIMEOUT_TIMER_INTERVAL); + + /* Reset follower response counter. */ + m_followerResponseCounter = 0U; + } + + /* Reset timer. */ + m_platoonHeartbeatTimer.restart(); + } + + /* Check participants heartbeats. Only active as leader. */ + if (true == m_vehicleHeartbeatTimeoutTimer.isTimeout()) + { + if (NUMBER_OF_FOLLOWERS != m_followerResponseCounter) + { + LOG_ERROR("Not all participants responded to heartbeat."); + } + else + { + LOG_DEBUG("All participants responded to heartbeat."); + } + + /* Stop timer. */ + m_vehicleHeartbeatTimeoutTimer.stop(); + } } bool V2VClient::sendWaypoint(const Waypoint& waypoint) @@ -213,31 +251,77 @@ void V2VClient::targetWaypointTopicCallback(const String& payload) void V2VClient::platoonHeartbeatTopicCallback(const String& payload) { - /* Send vehicle heartbeat. */ - StaticJsonDocument heartbeatDoc; - String heartbeatPayload; - - heartbeatDoc["id"] = m_vehicleId; - heartbeatDoc["timestamp"] = millis(); + /* Deserialize payload. */ + StaticJsonDocument jsonPayload; + DeserializationError error = deserializeJson(jsonPayload, payload.c_str()); - if (0U == serializeJson(heartbeatDoc, heartbeatPayload)) + if (DeserializationError::Ok != error) { - LOG_ERROR("Failed to serialize heartbeat."); - } - else if (false == m_mqttClient.publish(m_heartbeatResponseTopic, false, heartbeatPayload)) - { - LOG_ERROR("Failed to publish MQTT message to %s.", m_heartbeatResponseTopic.c_str()); + LOG_ERROR("JSON Deserialization Error %d.", error); } else { - LOG_DEBUG("Sent heartbeat: %s", heartbeatPayload.c_str()); + /* Send vehicle heartbeat. */ + JsonVariant jsonTimestamp = jsonPayload["timestamp"]; /* Timestamp [ms]. */ + StaticJsonDocument heartbeatDoc; + String heartbeatPayload; + + if (false == jsonTimestamp.isNull()) + { + heartbeatDoc["id"] = m_vehicleId; + heartbeatDoc["timestamp"] = jsonTimestamp.as(); + + if (0U == serializeJson(heartbeatDoc, heartbeatPayload)) + { + LOG_ERROR("Failed to serialize heartbeat."); + } + else if (false == m_mqttClient.publish(m_heartbeatResponseTopic, false, heartbeatPayload)) + { + LOG_ERROR("Failed to publish MQTT message to %s.", m_heartbeatResponseTopic.c_str()); + } + else + { + LOG_DEBUG("Sent heartbeat: %s", heartbeatPayload.c_str()); + } + } } } void V2VClient::vehicleHeartbeatTopicCallback(const String& payload) { - /* TODO: Leader processing of vehicle heartbeats. */ - LOG_DEBUG("Received heartbeat from vehicle: %s", payload.c_str()); + /* Deserialize payload. */ + StaticJsonDocument jsonPayload; + DeserializationError error = deserializeJson(jsonPayload, payload.c_str()); + + if (DeserializationError::Ok != error) + { + LOG_ERROR("JSON Deserialization Error %d.", error); + } + else + { + JsonVariant jsonId = jsonPayload["id"]; /* Vehicle ID. */ + JsonVariant jsonTimestamp = jsonPayload["timestamp"]; /* Timestamp [ms]. */ + + if ((false == jsonId.isNull()) && (false == jsonTimestamp.isNull())) + { + uint8_t id = jsonId.as(); + uint32_t timestamp = jsonTimestamp.as(); + + if (m_vehicleId == id) + { + /* This is me, the leader! */ + } + else if (timestamp != m_lastPlatoonHeartbeatTimestamp) + { + LOG_ERROR("Received heartbeat from vehicle %d with timestamp %d, expected %d.", id, timestamp, + m_lastPlatoonHeartbeatTimestamp); + } + else + { + ++m_followerResponseCounter; + } + } + } } bool V2VClient::setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId) @@ -369,6 +453,35 @@ bool V2VClient::setupLeaderTopics() return isSuccessful; } +bool V2VClient::sendPlatoonHeartbeat() +{ + bool isSuccessful = false; + + /* Send platoon heartbeat. */ + StaticJsonDocument heartbeatDoc; + String heartbeatPayload; + uint32_t timestamp = millis(); + + heartbeatDoc["timestamp"] = timestamp; + + if (0U == serializeJson(heartbeatDoc, heartbeatPayload)) + { + LOG_ERROR("Failed to serialize heartbeat."); + } + else if (false == m_mqttClient.publish(m_platoonHeartbeatTopic, false, heartbeatPayload)) + { + LOG_ERROR("Failed to publish MQTT message to %s.", m_platoonHeartbeatTopic.c_str()); + } + else + { + LOG_DEBUG("Sent platoon heartbeat: %s", heartbeatPayload.c_str()); + isSuccessful = true; + m_lastPlatoonHeartbeatTimestamp = timestamp; + } + + return isSuccessful; +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/PlatoonService/src/V2VClient.h b/lib/PlatoonService/src/V2VClient.h index eddf0361..741cb38d 100644 --- a/lib/PlatoonService/src/V2VClient.h +++ b/lib/PlatoonService/src/V2VClient.h @@ -45,6 +45,7 @@ #include #include #include +#include /****************************************************************************** * Macros @@ -128,6 +129,12 @@ class V2VClient /** Number of followers. */ static const uint8_t NUMBER_OF_FOLLOWERS = 1U; + /** Vehicle heartbeat timeout timer interval in ms. */ + static const uint32_t VEHICLE_HEARTBEAT_TIMEOUT_TIMER_INTERVAL = 1000U; + + /** Send platoon heartbeat timer interval in ms. */ + static const uint32_t PLATOON_HEARTBEAT_TIMER_INTERVAL = 2U * VEHICLE_HEARTBEAT_TIMEOUT_TIMER_INTERVAL; + /** MQTT subtopic name for waypoint reception. */ static const char* TOPIC_NAME_WAYPOINT_RX; @@ -171,6 +178,18 @@ class V2VClient /** Vehicle ID. */ uint8_t m_vehicleId; + /** Last Platoon heartbeat timestamp. */ + uint32_t m_lastPlatoonHeartbeatTimestamp; + + /** Follower response counter. */ + uint8_t m_followerResponseCounter; + + /** Platoon Heartbeat timer. */ + SimpleTimer m_platoonHeartbeatTimer; + + /** Vehicle heartbeat timeout timer. */ + SimpleTimer m_vehicleHeartbeatTimeoutTimer; + private: /** * Callback for Position Setpoint MQTT Topic. @@ -220,6 +239,13 @@ class V2VClient */ bool setupLeaderTopics(); + /** + * Send the platoon heartbeat message. + * + * @return If the message was sent successfully, returns true. Otherwise, false. + */ + bool sendPlatoonHeartbeat(); + /** * Default constructor. */ From 7ed5ccba1f2e80f6ecc63e86395554dc4c6fd401 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Thu, 1 Feb 2024 18:14:09 +0100 Subject: [PATCH 07/13] Added missing members to the initialization list --- lib/PlatoonService/src/V2VClient.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/lib/PlatoonService/src/V2VClient.cpp b/lib/PlatoonService/src/V2VClient.cpp index 3a311ac9..6d9d7ef9 100644 --- a/lib/PlatoonService/src/V2VClient.cpp +++ b/lib/PlatoonService/src/V2VClient.cpp @@ -82,7 +82,10 @@ V2VClient::V2VClient(MqttClient& mqttClient) : m_participantType(PARTICIPANT_TYPE_UNKNOWN), m_platoonId(0U), m_vehicleId(0U), - m_followerResponseCounter(0U) + m_lastPlatoonHeartbeatTimestamp(0U), + m_followerResponseCounter(0U), + m_platoonHeartbeatTimer(), + m_vehicleHeartbeatTimeoutTimer() { } From 55dc95c7cfb408a0ba7138b59c713a8f5a101d83 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Fri, 2 Feb 2024 13:59:00 +0100 Subject: [PATCH 08/13] Renamed V2V module --- lib/ConvoyFollower/src/App.cpp | 12 +++--- lib/ConvoyFollower/src/App.h | 8 ++-- lib/ConvoyLeader/src/App.cpp | 8 ++-- lib/ConvoyLeader/src/App.h | 8 ++-- .../src/{V2VClient.cpp => V2VCommManager.cpp} | 38 +++++++++---------- .../src/{V2VClient.h => V2VCommManager.h} | 32 ++++++++-------- 6 files changed, 53 insertions(+), 53 deletions(-) rename lib/PlatoonService/src/{V2VClient.cpp => V2VCommManager.cpp} (90%) rename lib/PlatoonService/src/{V2VClient.h => V2VCommManager.h} (88%) diff --git a/lib/ConvoyFollower/src/App.cpp b/lib/ConvoyFollower/src/App.cpp index a35f3248..92318a42 100644 --- a/lib/ConvoyFollower/src/App.cpp +++ b/lib/ConvoyFollower/src/App.cpp @@ -158,12 +158,12 @@ void App::setup() { LOG_FATAL("Failed to setup MQTT client."); } - else if (V2VClient::PLATOON_LEADER_ID == settings.getPlatoonVehicleId()) + else if (V2VCommManager::PLATOON_LEADER_ID == settings.getPlatoonVehicleId()) { /* Correct config.json file loaded? */ LOG_FATAL("Platoon Vehicle ID must not be 0 for a follower."); } - else if (false == m_v2vClient.init(settings.getPlatoonPlatoonId(), settings.getPlatoonVehicleId())) + else if (false == m_v2vCommManager.init(settings.getPlatoonPlatoonId(), settings.getPlatoonVehicleId())) { LOG_FATAL("Failed to initialize V2V client."); } @@ -216,7 +216,7 @@ void App::loop() m_mqttClient.process(); /* Process V2V Communication */ - m_v2vClient.process(); + m_v2vCommManager.process(); /* Process System State Machine */ m_systemStateMachine.process(); @@ -225,12 +225,12 @@ void App::loop() processPeriodicTasks(); /* Process V2V event queue. */ - while (0U < m_v2vClient.getWaypointQueueSize()) + while (0U < m_v2vCommManager.getWaypointQueueSize()) { /* Waypoints are pending. */ Waypoint waypoint; - if (false == m_v2vClient.getNextWaypoint(waypoint)) + if (false == m_v2vCommManager.getNextWaypoint(waypoint)) { LOG_WARNING("Failed to get next waypoint from V2V client."); } @@ -412,7 +412,7 @@ void App::processPeriodicTasks() { LOG_WARNING("Failed to get waypoint from driving state."); } - else if (false == m_v2vClient.sendWaypoint(payload)) + else if (false == m_v2vCommManager.sendWaypoint(payload)) { LOG_WARNING("Waypoint could not be sent."); } diff --git a/lib/ConvoyFollower/src/App.h b/lib/ConvoyFollower/src/App.h index 0fe09000..232e2576 100644 --- a/lib/ConvoyFollower/src/App.h +++ b/lib/ConvoyFollower/src/App.h @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include "SerialMuxChannels.h" /****************************************************************************** @@ -72,7 +72,7 @@ class App m_serialMuxProtChannelIdStatus(0U), m_smpServer(Board::getInstance().getDevice().getStream(), this), m_mqttClient(), - m_v2vClient(m_mqttClient), + m_v2vCommManager(m_mqttClient), m_systemStateMachine(), m_sendWaypointTimer(), m_commandTimer(), @@ -166,9 +166,9 @@ class App MqttClient m_mqttClient; /** - * V2V client instance. + * V2V communication manager instance. */ - V2VClient m_v2vClient; + V2VCommManager m_v2vCommManager; /** * The system state machine. diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index 7fb47b5c..2a7454b4 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -158,12 +158,12 @@ void App::setup() { LOG_FATAL("Failed to setup MQTT client."); } - else if (V2VClient::PLATOON_LEADER_ID != settings.getPlatoonVehicleId()) + else if (V2VCommManager::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())) + else if (false == m_v2vCommManager.init(settings.getPlatoonPlatoonId(), settings.getPlatoonVehicleId())) { LOG_FATAL("Failed to initialize V2V client."); } @@ -220,7 +220,7 @@ void App::loop() m_mqttClient.process(); /* Process V2V Communication */ - m_v2vClient.process(); + m_v2vCommManager.process(); /* Process System State Machine */ m_systemStateMachine.process(); @@ -390,7 +390,7 @@ void App::processPeriodicTasks() if ((true == m_sendWaypointTimer.isTimeout()) && (true == m_mqttClient.isConnected())) { - if (false == m_v2vClient.sendWaypoint(m_latestVehicleData)) + if (false == m_v2vCommManager.sendWaypoint(m_latestVehicleData)) { LOG_WARNING("Waypoint could not be sent."); } diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index 59cd699c..2ca361d6 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include "SerialMuxChannels.h" /****************************************************************************** @@ -72,7 +72,7 @@ class App m_serialMuxProtChannelIdStatus(0U), m_smpServer(Board::getInstance().getDevice().getStream(), this), m_mqttClient(), - m_v2vClient(m_mqttClient), + m_v2vCommManager(m_mqttClient), m_systemStateMachine(), m_latestVehicleData(), m_sendWaypointTimer(), @@ -167,9 +167,9 @@ class App MqttClient m_mqttClient; /** - * V2V client instance. + * V2V communication manager instance. */ - V2VClient m_v2vClient; + V2VCommManager m_v2vCommManager; /** * The system state machine. diff --git a/lib/PlatoonService/src/V2VClient.cpp b/lib/PlatoonService/src/V2VCommManager.cpp similarity index 90% rename from lib/PlatoonService/src/V2VClient.cpp rename to lib/PlatoonService/src/V2VCommManager.cpp index 6d9d7ef9..2e844d06 100644 --- a/lib/PlatoonService/src/V2VClient.cpp +++ b/lib/PlatoonService/src/V2VCommManager.cpp @@ -25,14 +25,14 @@ DESCRIPTION *******************************************************************************/ /** - * @brief Vehicle to Vehicle (V2V) communication client. + * @brief Vehicle to Vehicle (V2V) communication manager. * @author Gabryel Reyes */ /****************************************************************************** * Includes *****************************************************************************/ -#include "V2VClient.h" +#include "V2VCommManager.h" #include #include @@ -57,13 +57,13 @@ *****************************************************************************/ /* MQTT subtopic name for waypoint reception. */ -const char* V2VClient::TOPIC_NAME_WAYPOINT_RX = "inputWaypoint"; +const char* V2VCommManager::TOPIC_NAME_WAYPOINT_RX = "inputWaypoint"; /* MQTT subtopic name for platoon heartbeat. */ -const char* V2VClient::TOPIC_NAME_PLATOON_HEARTBEAT = "heartbeat"; +const char* V2VCommManager::TOPIC_NAME_PLATOON_HEARTBEAT = "heartbeat"; /* MQTT subtopic name for platoon heartbeat. */ -const char* V2VClient::TOPIC_NAME_PLATOON_HEARTBEAT_RESPONSE = "heartbeatResponse"; +const char* V2VCommManager::TOPIC_NAME_PLATOON_HEARTBEAT_RESPONSE = "heartbeatResponse"; /** Buffer size for JSON serialization of heartbeat messages. */ static const uint32_t JSON_HEARTBEAT_MAX_SIZE = 128U; @@ -72,7 +72,7 @@ static const uint32_t JSON_HEARTBEAT_MAX_SIZE = 128U; * Public Methods *****************************************************************************/ -V2VClient::V2VClient(MqttClient& mqttClient) : +V2VCommManager::V2VCommManager(MqttClient& mqttClient) : m_mqttClient(mqttClient), m_waypointQueue(), m_waypointInputTopic(), @@ -89,11 +89,11 @@ V2VClient::V2VClient(MqttClient& mqttClient) : { } -V2VClient::~V2VClient() +V2VCommManager::~V2VCommManager() { } -bool V2VClient::init(uint8_t platoonId, uint8_t vehicleId) +bool V2VCommManager::init(uint8_t platoonId, uint8_t vehicleId) { bool isSuccessful = false; @@ -141,7 +141,7 @@ bool V2VClient::init(uint8_t platoonId, uint8_t vehicleId) return isSuccessful; } -void V2VClient::process() +void V2VCommManager::process() { /* Send Platoon Heartbeat. Only active as leader. */ if (true == m_platoonHeartbeatTimer.isTimeout()) @@ -181,7 +181,7 @@ void V2VClient::process() } } -bool V2VClient::sendWaypoint(const Waypoint& waypoint) +bool V2VCommManager::sendWaypoint(const Waypoint& waypoint) { bool isSuccessful = false; String payload; @@ -203,7 +203,7 @@ bool V2VClient::sendWaypoint(const Waypoint& waypoint) return isSuccessful; } -bool V2VClient::getNextWaypoint(Waypoint& waypoint) +bool V2VCommManager::getNextWaypoint(Waypoint& waypoint) { bool isSuccessful = false; @@ -225,7 +225,7 @@ bool V2VClient::getNextWaypoint(Waypoint& waypoint) return isSuccessful; } -size_t V2VClient::getWaypointQueueSize() const +size_t V2VCommManager::getWaypointQueueSize() const { return m_waypointQueue.size(); } @@ -238,7 +238,7 @@ size_t V2VClient::getWaypointQueueSize() const * Private Methods *****************************************************************************/ -void V2VClient::targetWaypointTopicCallback(const String& payload) +void V2VCommManager::targetWaypointTopicCallback(const String& payload) { Waypoint* waypoint = Waypoint::deserialize(payload); @@ -252,7 +252,7 @@ void V2VClient::targetWaypointTopicCallback(const String& payload) } } -void V2VClient::platoonHeartbeatTopicCallback(const String& payload) +void V2VCommManager::platoonHeartbeatTopicCallback(const String& payload) { /* Deserialize payload. */ StaticJsonDocument jsonPayload; @@ -290,7 +290,7 @@ void V2VClient::platoonHeartbeatTopicCallback(const String& payload) } } -void V2VClient::vehicleHeartbeatTopicCallback(const String& payload) +void V2VCommManager::vehicleHeartbeatTopicCallback(const String& payload) { /* Deserialize payload. */ StaticJsonDocument jsonPayload; @@ -327,7 +327,7 @@ void V2VClient::vehicleHeartbeatTopicCallback(const String& payload) } } -bool V2VClient::setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId) +bool V2VCommManager::setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId) { bool isSuccessful = false; uint8_t nextVehicleId = vehicleId + 1U; /* Output is published to next vehicle. */ @@ -379,7 +379,7 @@ bool V2VClient::setupWaypointTopics(uint8_t platoonId, uint8_t vehicleId) return isSuccessful; } -bool V2VClient::setupHeartbeatTopics(uint8_t platoonId, uint8_t vehicleId) +bool V2VCommManager::setupHeartbeatTopics(uint8_t platoonId, uint8_t vehicleId) { bool isSuccessful = false; char platoonHeartbeatTopicBuffer[MAX_TOPIC_LENGTH]; @@ -431,7 +431,7 @@ bool V2VClient::setupHeartbeatTopics(uint8_t platoonId, uint8_t vehicleId) return isSuccessful; } -bool V2VClient::setupLeaderTopics() +bool V2VCommManager::setupLeaderTopics() { bool isSuccessful = false; @@ -456,7 +456,7 @@ bool V2VClient::setupLeaderTopics() return isSuccessful; } -bool V2VClient::sendPlatoonHeartbeat() +bool V2VCommManager::sendPlatoonHeartbeat() { bool isSuccessful = false; diff --git a/lib/PlatoonService/src/V2VClient.h b/lib/PlatoonService/src/V2VCommManager.h similarity index 88% rename from lib/PlatoonService/src/V2VClient.h rename to lib/PlatoonService/src/V2VCommManager.h index 741cb38d..5b0dbb8d 100644 --- a/lib/PlatoonService/src/V2VClient.h +++ b/lib/PlatoonService/src/V2VCommManager.h @@ -25,15 +25,15 @@ DESCRIPTION *******************************************************************************/ /** - * @brief Vehicle to Vehicle (V2V) communication client. + * @brief Vehicle to Vehicle (V2V) communication manager. * @author Gabryel Reyes * * @addtogroup PlatoonService * * @{ */ -#ifndef V2V_CLIENT_H -#define V2V_CLIENT_H +#ifndef V2V_COMM_MANAGER_H +#define V2V_COMM_MANAGER_H /****************************************************************************** * Compile Switches @@ -55,8 +55,8 @@ * Types and Classes *****************************************************************************/ -/** V2V Client for external communication in the platooning context. */ -class V2VClient +/** V2V Communication Manager for external communication in the platooning context. */ +class V2VCommManager { public: /** Platoon leader vehicle ID. */ @@ -71,29 +71,29 @@ class V2VClient }; /** - * Constructs a V2V client. + * Constructs a V2V manager. * * @param[in] mqttClient MQTT client instance. */ - V2VClient(MqttClient&); + V2VCommManager(MqttClient&); /** * Default destructor. */ - ~V2VClient(); + ~V2VCommManager(); /** - * Initialize the V2V client. + * Initialize the V2V communication manager. * * @param[in] platoonId ID of the platoon. * @param[in] vehicleId ID of the vehicle inside the platoon. * - * @return If the V2V client was initialized successfully, returns true. Otherwise, false. + * @return If the V2V communication manager was initialized successfully, returns true. Otherwise, false. */ bool init(uint8_t platoonId, uint8_t vehicleId); /** - * Process the V2V client. + * Process the V2V communication manager. */ void process(); @@ -107,7 +107,7 @@ class V2VClient bool sendWaypoint(const Waypoint& waypoint); /** - * Get the next recevied Waypoint from the V2V client. + * Get the next recevied Waypoint from the V2V communication manager. * * @param[out] waypoint Next waypoint to receive. * @@ -249,22 +249,22 @@ class V2VClient /** * Default constructor. */ - V2VClient(); + V2VCommManager(); /** * Copy constructor. */ - V2VClient(const V2VClient&); + V2VCommManager(const V2VCommManager&); /** * Assignment operator. */ - V2VClient& operator=(const V2VClient&); + V2VCommManager& operator=(const V2VCommManager&); }; /****************************************************************************** * Functions *****************************************************************************/ -#endif /* V2V_CLIENT_H */ +#endif /* V2V_COMM_MANAGER_H */ /** @} */ From 675fc3912f6452fb3a689fea80a1061f68bb113e Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 5 Feb 2024 09:50:10 +0100 Subject: [PATCH 09/13] V2V module returns a status on process --- lib/ConvoyLeader/src/App.cpp | 31 +++++++++- lib/ConvoyLeader/src/App.h | 5 ++ lib/PlatoonService/src/V2VCommManager.cpp | 75 ++++++++++++++--------- lib/PlatoonService/src/V2VCommManager.h | 16 ++++- 4 files changed, 95 insertions(+), 32 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index 2a7454b4..1e0f7254 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -220,7 +220,7 @@ void App::loop() m_mqttClient.process(); /* Process V2V Communication */ - m_v2vCommManager.process(); + processV2VCommunication(); /* Process System State Machine */ m_systemStateMachine.process(); @@ -449,6 +449,35 @@ void App::processPeriodicTasks() } } +void App::processV2VCommunication() +{ + V2VCommManager::V2VStatus status = m_v2vCommManager.process(); + + switch (status) + { + case V2VCommManager::V2V_STATUS_OK: + /* All good. Nothing to do. */ + break; + + case V2VCommManager::V2V_STATUS_NOT_INIT: + LOG_WARNING("V2V not initialized."); + break; + + case V2VCommManager::V2V_STATUS_LOST_FOLLOWER: + LOG_ERROR("Lost follower."); + setErrorState(); + break; + + case V2VCommManager::V2V_STATUS_GENERAL_ERROR: + LOG_ERROR("V2V Communication error."); + setErrorState(); + break; + + default: + break; + } +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/ConvoyLeader/src/App.h b/lib/ConvoyLeader/src/App.h index 2ca361d6..16f82837 100644 --- a/lib/ConvoyLeader/src/App.h +++ b/lib/ConvoyLeader/src/App.h @@ -231,6 +231,11 @@ class App */ void processPeriodicTasks(); + /** + * Process V2V communication. + */ + void processV2VCommunication(); + private: /* Not allowed. */ App(const App& app); /**< Copy construction of an instance. */ diff --git a/lib/PlatoonService/src/V2VCommManager.cpp b/lib/PlatoonService/src/V2VCommManager.cpp index 2e844d06..d5bcc6bd 100644 --- a/lib/PlatoonService/src/V2VCommManager.cpp +++ b/lib/PlatoonService/src/V2VCommManager.cpp @@ -85,7 +85,8 @@ V2VCommManager::V2VCommManager(MqttClient& mqttClient) : m_lastPlatoonHeartbeatTimestamp(0U), m_followerResponseCounter(0U), m_platoonHeartbeatTimer(), - m_vehicleHeartbeatTimeoutTimer() + m_vehicleHeartbeatTimeoutTimer(), + m_status(V2V_STATUS_NOT_INIT) { } @@ -138,47 +139,61 @@ bool V2VCommManager::init(uint8_t platoonId, uint8_t vehicleId) m_platoonHeartbeatTimer.start(PLATOON_HEARTBEAT_TIMER_INTERVAL); } + if (true == isSuccessful) + { + m_status = V2V_STATUS_OK; + } + return isSuccessful; } -void V2VCommManager::process() +V2VCommManager::V2VStatus V2VCommManager::process() { - /* Send Platoon Heartbeat. Only active as leader. */ - if (true == m_platoonHeartbeatTimer.isTimeout()) + /* Is MQTT client connected? */ + if (true == m_mqttClient.isConnected()) { - /* Send Platoon Heartbeat */ - if (false == sendPlatoonHeartbeat()) + /* Send Platoon Heartbeat. Only active as leader. */ + if (true == m_platoonHeartbeatTimer.isTimeout()) { - LOG_ERROR("Failed to send platoon heartbeat."); - } - else - { - /* Start timeout timer. */ - m_vehicleHeartbeatTimeoutTimer.start(VEHICLE_HEARTBEAT_TIMEOUT_TIMER_INTERVAL); - - /* Reset follower response counter. */ - m_followerResponseCounter = 0U; - } + /* Send Platoon Heartbeat */ + if (false == sendPlatoonHeartbeat()) + { + LOG_ERROR("Failed to send platoon heartbeat."); + m_status = V2V_STATUS_GENERAL_ERROR; + } + else + { + /* Start timeout timer. */ + m_vehicleHeartbeatTimeoutTimer.start(VEHICLE_HEARTBEAT_TIMEOUT_TIMER_INTERVAL); - /* Reset timer. */ - m_platoonHeartbeatTimer.restart(); - } + /* Reset follower response counter. */ + m_followerResponseCounter = 0U; + } - /* Check participants heartbeats. Only active as leader. */ - if (true == m_vehicleHeartbeatTimeoutTimer.isTimeout()) - { - if (NUMBER_OF_FOLLOWERS != m_followerResponseCounter) - { - LOG_ERROR("Not all participants responded to heartbeat."); + /* Reset timer. */ + m_platoonHeartbeatTimer.restart(); } - else + + /* Check participants heartbeats. Only active as leader. */ + if (true == m_vehicleHeartbeatTimeoutTimer.isTimeout()) { - LOG_DEBUG("All participants responded to heartbeat."); - } + if (NUMBER_OF_FOLLOWERS != m_followerResponseCounter) + { + LOG_ERROR("Not all participants responded to heartbeat."); + m_status = V2V_STATUS_LOST_FOLLOWER; + } + else + { + LOG_DEBUG("All participants responded to heartbeat."); + m_status = V2V_STATUS_OK; + } - /* Stop timer. */ - m_vehicleHeartbeatTimeoutTimer.stop(); + /* Stop timer. */ + m_vehicleHeartbeatTimeoutTimer.stop(); + } } + + return m_status; } bool V2VCommManager::sendWaypoint(const Waypoint& waypoint) diff --git a/lib/PlatoonService/src/V2VCommManager.h b/lib/PlatoonService/src/V2VCommManager.h index 5b0dbb8d..b5a36512 100644 --- a/lib/PlatoonService/src/V2VCommManager.h +++ b/lib/PlatoonService/src/V2VCommManager.h @@ -70,6 +70,15 @@ class V2VCommManager PARTICIPANT_TYPE_FOLLOWER /**< Platoon follower */ }; + /** V2VCommunication Manager Status. */ + enum V2VStatus : uint8_t + { + V2V_STATUS_OK = 0U, /**< Status OK */ + V2V_STATUS_NOT_INIT, /**< Not initialized */ + V2V_STATUS_LOST_FOLLOWER, /**< Lost follower */ + V2V_STATUS_GENERAL_ERROR /**< General error */ + }; + /** * Constructs a V2V manager. * @@ -94,8 +103,10 @@ class V2VCommManager /** * Process the V2V communication manager. + * + * @return V2VCommManager Status. */ - void process(); + V2VStatus process(); /** * Send a Waypoint to the next vehicle in the platoon. @@ -190,6 +201,9 @@ class V2VCommManager /** Vehicle heartbeat timeout timer. */ SimpleTimer m_vehicleHeartbeatTimeoutTimer; + /** Current Status. */ + V2VStatus m_status; + private: /** * Callback for Position Setpoint MQTT Topic. From f60c9d7afaa731402eba38a2424ea498f9628824 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 5 Feb 2024 10:07:59 +0100 Subject: [PATCH 10/13] Received heartbeat from unknown follower --- lib/PlatoonService/src/V2VCommManager.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/lib/PlatoonService/src/V2VCommManager.cpp b/lib/PlatoonService/src/V2VCommManager.cpp index d5bcc6bd..efcf9e45 100644 --- a/lib/PlatoonService/src/V2VCommManager.cpp +++ b/lib/PlatoonService/src/V2VCommManager.cpp @@ -329,6 +329,10 @@ void V2VCommManager::vehicleHeartbeatTopicCallback(const String& payload) { /* This is me, the leader! */ } + else if (id > NUMBER_OF_FOLLOWERS) + { + LOG_ERROR("Received heartbeat from unknown vehicle %d.", id); + } else if (timestamp != m_lastPlatoonHeartbeatTimestamp) { LOG_ERROR("Received heartbeat from vehicle %d with timestamp %d, expected %d.", id, timestamp, From 1ceea49fabb63a9bc1570ae044a43de363743679 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 5 Feb 2024 11:44:20 +0100 Subject: [PATCH 11/13] Vehicle status sent in V2V payload --- lib/ConvoyFollower/src/App.cpp | 40 ++++++- lib/ConvoyFollower/src/App.h | 5 + lib/ConvoyLeader/src/App.cpp | 15 ++- lib/PlatoonService/src/Follower.h | 127 ++++++++++++++++++++++ lib/PlatoonService/src/V2VCommManager.cpp | 41 +++++-- lib/PlatoonService/src/V2VCommManager.h | 30 ++++- 6 files changed, 238 insertions(+), 20 deletions(-) create mode 100644 lib/PlatoonService/src/Follower.h diff --git a/lib/ConvoyFollower/src/App.cpp b/lib/ConvoyFollower/src/App.cpp index 92318a42..be5bcc76 100644 --- a/lib/ConvoyFollower/src/App.cpp +++ b/lib/ConvoyFollower/src/App.cpp @@ -216,7 +216,7 @@ void App::loop() m_mqttClient.process(); /* Process V2V Communication */ - m_v2vCommManager.process(); + m_v2vCommManager.process(V2VCommManager::VEHICLE_STATUS_ERROR); /* Process System State Machine */ m_systemStateMachine.process(); @@ -478,6 +478,44 @@ void App::processPeriodicTasks() } } +void App::processV2VCommunication() +{ + V2VCommManager::V2VStatus v2vStatus = V2VCommManager::V2V_STATUS_OK; + V2VCommManager::VehicleStatus vehicleStatus = V2VCommManager::VEHICLE_STATUS_OK; + + if (true == ErrorState::getInstance().isActive()) + { + vehicleStatus = V2VCommManager::VEHICLE_STATUS_ERROR; + } + + v2vStatus = m_v2vCommManager.process(vehicleStatus); + + switch (v2vStatus) + { + case V2VCommManager::V2V_STATUS_OK: + /* All good. Nothing to do. */ + break; + + case V2VCommManager::V2V_STATUS_NOT_INIT: + LOG_WARNING("V2V not initialized."); + break; + + case V2VCommManager::V2V_STATUS_LOST_FOLLOWER: + case V2VCommManager::V2V_STATUS_FOLLOWER_ERROR: + LOG_ERROR("Follower Error"); + setErrorState(); + break; + + case V2VCommManager::V2V_STATUS_GENERAL_ERROR: + LOG_ERROR("V2V Communication error."); + setErrorState(); + break; + + default: + break; + } +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/ConvoyFollower/src/App.h b/lib/ConvoyFollower/src/App.h index 232e2576..76a10663 100644 --- a/lib/ConvoyFollower/src/App.h +++ b/lib/ConvoyFollower/src/App.h @@ -200,6 +200,11 @@ class App */ SimpleTimer m_statusTimeoutTimer; + /** + * Process V2V communication. + */ + void processV2VCommunication(); + private: /** * Handler of fatal errors in the Application. diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index 1e0f7254..dd16adf4 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -451,9 +451,17 @@ void App::processPeriodicTasks() void App::processV2VCommunication() { - V2VCommManager::V2VStatus status = m_v2vCommManager.process(); + V2VCommManager::V2VStatus v2vStatus = V2VCommManager::V2V_STATUS_OK; + V2VCommManager::VehicleStatus vehicleStatus = V2VCommManager::VEHICLE_STATUS_OK; - switch (status) + if (true == ErrorState::getInstance().isActive()) + { + vehicleStatus = V2VCommManager::VEHICLE_STATUS_ERROR; + } + + v2vStatus = m_v2vCommManager.process(vehicleStatus); + + switch (v2vStatus) { case V2VCommManager::V2V_STATUS_OK: /* All good. Nothing to do. */ @@ -464,7 +472,8 @@ void App::processV2VCommunication() break; case V2VCommManager::V2V_STATUS_LOST_FOLLOWER: - LOG_ERROR("Lost follower."); + case V2VCommManager::V2V_STATUS_FOLLOWER_ERROR: + LOG_ERROR("Follower Error"); setErrorState(); break; diff --git a/lib/PlatoonService/src/Follower.h b/lib/PlatoonService/src/Follower.h new file mode 100644 index 00000000..dd9a2c24 --- /dev/null +++ b/lib/PlatoonService/src/Follower.h @@ -0,0 +1,127 @@ +/* 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 Follower class for V2V communication. + * @author Gabryel Reyes + * + * @addtogroup PlatoonService + * + * @{ + */ +#ifndef FOLLOWER_H +#define FOLLOWER_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Follower class. */ +class Follower +{ +public: + /** + * Follower Constructor. + */ + Follower() : m_lastHeartbeatTimestamp(0U), m_status(0U) + { + } + + /** + * Follower Destructor. + */ + ~Follower() + { + } + + /** + * Get the last heartbeat timestamp. + * + * @return Last heartbeat timestamp. + */ + uint32_t getLastHeartbeatTimestamp() const + { + return m_lastHeartbeatTimestamp; + } + + /** + * Set the last heartbeat timestamp. + * + * @param[in] timestamp Last heartbeat timestamp. + */ + void setLastHeartbeatTimestamp(uint32_t timestamp) + { + m_lastHeartbeatTimestamp = timestamp; + } + + /** + * Get the status. + * + * @return Status. + */ + uint8_t getStatus() const + { + return m_status; + } + + /** + * Set the status. + * + * @param[in] status Status. + */ + void setStatus(uint8_t status) + { + m_status = status; + } + +private: + /** Last heartbeat timestamp. */ + uint32_t m_lastHeartbeatTimestamp; + + /** Status. */ + uint8_t m_status; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* FOLLOWER_H */ +/** @} */ diff --git a/lib/PlatoonService/src/V2VCommManager.cpp b/lib/PlatoonService/src/V2VCommManager.cpp index efcf9e45..830bef9b 100644 --- a/lib/PlatoonService/src/V2VCommManager.cpp +++ b/lib/PlatoonService/src/V2VCommManager.cpp @@ -86,7 +86,9 @@ V2VCommManager::V2VCommManager(MqttClient& mqttClient) : m_followerResponseCounter(0U), m_platoonHeartbeatTimer(), m_vehicleHeartbeatTimeoutTimer(), - m_status(V2V_STATUS_NOT_INIT) + m_v2vStatus(V2V_STATUS_NOT_INIT), + m_vehicleStatus(), + m_followers{} { } @@ -141,16 +143,18 @@ bool V2VCommManager::init(uint8_t platoonId, uint8_t vehicleId) if (true == isSuccessful) { - m_status = V2V_STATUS_OK; + m_v2vStatus = V2V_STATUS_OK; } return isSuccessful; } -V2VCommManager::V2VStatus V2VCommManager::process() +V2VCommManager::V2VStatus V2VCommManager::process(VehicleStatus status) { + m_vehicleStatus = status; + /* Is MQTT client connected? */ - if (true == m_mqttClient.isConnected()) + if ((true == m_mqttClient.isConnected()) && (V2V_STATUS_OK == m_v2vStatus)) { /* Send Platoon Heartbeat. Only active as leader. */ if (true == m_platoonHeartbeatTimer.isTimeout()) @@ -159,7 +163,7 @@ V2VCommManager::V2VStatus V2VCommManager::process() if (false == sendPlatoonHeartbeat()) { LOG_ERROR("Failed to send platoon heartbeat."); - m_status = V2V_STATUS_GENERAL_ERROR; + m_v2vStatus = V2V_STATUS_GENERAL_ERROR; } else { @@ -180,12 +184,12 @@ V2VCommManager::V2VStatus V2VCommManager::process() if (NUMBER_OF_FOLLOWERS != m_followerResponseCounter) { LOG_ERROR("Not all participants responded to heartbeat."); - m_status = V2V_STATUS_LOST_FOLLOWER; + m_v2vStatus = V2V_STATUS_LOST_FOLLOWER; } else { LOG_DEBUG("All participants responded to heartbeat."); - m_status = V2V_STATUS_OK; + m_v2vStatus = V2V_STATUS_OK; } /* Stop timer. */ @@ -193,7 +197,7 @@ V2VCommManager::V2VStatus V2VCommManager::process() } } - return m_status; + return m_v2vStatus; } bool V2VCommManager::sendWaypoint(const Waypoint& waypoint) @@ -288,6 +292,7 @@ void V2VCommManager::platoonHeartbeatTopicCallback(const String& payload) { heartbeatDoc["id"] = m_vehicleId; heartbeatDoc["timestamp"] = jsonTimestamp.as(); + heartbeatDoc["status"] = m_vehicleStatus; if (0U == serializeJson(heartbeatDoc, heartbeatPayload)) { @@ -319,13 +324,15 @@ void V2VCommManager::vehicleHeartbeatTopicCallback(const String& payload) { JsonVariant jsonId = jsonPayload["id"]; /* Vehicle ID. */ JsonVariant jsonTimestamp = jsonPayload["timestamp"]; /* Timestamp [ms]. */ + JsonVariant jsonStatus = jsonPayload["status"]; /* Vehicle status. */ - if ((false == jsonId.isNull()) && (false == jsonTimestamp.isNull())) + if ((false == jsonId.isNull()) && (false == jsonTimestamp.isNull()) && (false == jsonStatus.isNull())) { uint8_t id = jsonId.as(); uint32_t timestamp = jsonTimestamp.as(); + uint8_t status = jsonStatus.as(); - if (m_vehicleId == id) + if (PLATOON_LEADER_ID == id) { /* This is me, the leader! */ } @@ -340,7 +347,21 @@ void V2VCommManager::vehicleHeartbeatTopicCallback(const String& payload) } else { + uint8_t followerArrayIndex = id - 1U; /*Already checked id != 0U */ + Follower& follower = m_followers[followerArrayIndex]; + + /* Update follower. */ + follower.setLastHeartbeatTimestamp(timestamp); + follower.setStatus(status); + ++m_followerResponseCounter; + + /* Check follower status. */ + if (VEHICLE_STATUS_OK != status) + { + LOG_ERROR("Follower %d status: %d.", id, status); + m_v2vStatus = V2V_STATUS_FOLLOWER_ERROR; + } } } } diff --git a/lib/PlatoonService/src/V2VCommManager.h b/lib/PlatoonService/src/V2VCommManager.h index b5a36512..e1167ec0 100644 --- a/lib/PlatoonService/src/V2VCommManager.h +++ b/lib/PlatoonService/src/V2VCommManager.h @@ -46,6 +46,7 @@ #include #include #include +#include "Follower.h" /****************************************************************************** * Macros @@ -73,10 +74,19 @@ class V2VCommManager /** V2VCommunication Manager Status. */ enum V2VStatus : uint8_t { - V2V_STATUS_OK = 0U, /**< Status OK */ - V2V_STATUS_NOT_INIT, /**< Not initialized */ - V2V_STATUS_LOST_FOLLOWER, /**< Lost follower */ - V2V_STATUS_GENERAL_ERROR /**< General error */ + V2V_STATUS_OK = 0U, /**< Status OK */ + V2V_STATUS_NOT_INIT, /**< Not initialized */ + V2V_STATUS_LOST_FOLLOWER, /**< Lost follower */ + V2V_STATUS_FOLLOWER_ERROR, /**< Follower error */ + V2V_STATUS_GENERAL_ERROR /**< General error */ + }; + + /** Vehicle Status. */ + enum VehicleStatus : uint8_t + { + VEHICLE_STATUS_UNKNOWN = 0U, /**< Vehicle status unknown */ + VEHICLE_STATUS_OK, /**< Vehicle status OK */ + VEHICLE_STATUS_ERROR /**< Vehicle status error */ }; /** @@ -104,9 +114,11 @@ class V2VCommManager /** * Process the V2V communication manager. * + * @param[in] status Vehicle status. + * * @return V2VCommManager Status. */ - V2VStatus process(); + V2VStatus process(VehicleStatus status); /** * Send a Waypoint to the next vehicle in the platoon. @@ -202,7 +214,13 @@ class V2VCommManager SimpleTimer m_vehicleHeartbeatTimeoutTimer; /** Current Status. */ - V2VStatus m_status; + V2VStatus m_v2vStatus; + + /** Vehicle Status. */ + VehicleStatus m_vehicleStatus; + + /** Followers. */ + Follower m_followers[NUMBER_OF_FOLLOWERS]; private: /** From da6925d057c6c41fb4e1141081b74852f0d35b2f Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 5 Feb 2024 11:50:26 +0100 Subject: [PATCH 12/13] Fixed calling processing of V2V --- lib/ConvoyFollower/src/App.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/ConvoyFollower/src/App.cpp b/lib/ConvoyFollower/src/App.cpp index be5bcc76..91bf2498 100644 --- a/lib/ConvoyFollower/src/App.cpp +++ b/lib/ConvoyFollower/src/App.cpp @@ -216,7 +216,7 @@ void App::loop() m_mqttClient.process(); /* Process V2V Communication */ - m_v2vCommManager.process(V2VCommManager::VEHICLE_STATUS_ERROR); + processV2VCommunication(); /* Process System State Machine */ m_systemStateMachine.process(); From a55f7d24ee1edd2eaa146220b4ad359d26edba94 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 6 Feb 2024 13:17:49 +0100 Subject: [PATCH 13/13] Fixed review findings --- lib/PlatoonService/src/V2VCommManager.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/lib/PlatoonService/src/V2VCommManager.cpp b/lib/PlatoonService/src/V2VCommManager.cpp index 830bef9b..43bf9ce0 100644 --- a/lib/PlatoonService/src/V2VCommManager.cpp +++ b/lib/PlatoonService/src/V2VCommManager.cpp @@ -94,6 +94,12 @@ V2VCommManager::V2VCommManager(MqttClient& mqttClient) : V2VCommManager::~V2VCommManager() { + while (0U != m_waypointQueue.size()) + { + Waypoint* nextWaypoint = m_waypointQueue.front(); + m_waypointQueue.pop(); + delete nextWaypoint; + } } bool V2VCommManager::init(uint8_t platoonId, uint8_t vehicleId) @@ -284,14 +290,16 @@ void V2VCommManager::platoonHeartbeatTopicCallback(const String& payload) else { /* Send vehicle heartbeat. */ - JsonVariant jsonTimestamp = jsonPayload["timestamp"]; /* Timestamp [ms]. */ - StaticJsonDocument heartbeatDoc; - String heartbeatPayload; + JsonVariant jsonTimestamp = jsonPayload["timestamp"]; /* Timestamp [ms]. */ if (false == jsonTimestamp.isNull()) { + StaticJsonDocument heartbeatDoc; + String heartbeatPayload; + + /* Timestamp is sent back to acknowledge synchronization. */ + heartbeatDoc["timestamp"] = jsonTimestamp.as(); /* Timestamp [ms]. */ heartbeatDoc["id"] = m_vehicleId; - heartbeatDoc["timestamp"] = jsonTimestamp.as(); heartbeatDoc["status"] = m_vehicleStatus; if (0U == serializeJson(heartbeatDoc, heartbeatPayload)) @@ -518,7 +526,9 @@ bool V2VCommManager::sendPlatoonHeartbeat() else { LOG_DEBUG("Sent platoon heartbeat: %s", heartbeatPayload.c_str()); - isSuccessful = true; + isSuccessful = true; + + /* Save last timestamp in which the follower heartbeat was present. May be used for debugging in the future. */ m_lastPlatoonHeartbeatTimestamp = timestamp; }