From e44814ad5d6000435547f5d4d36f36cf703c37bf Mon Sep 17 00:00:00 2001
From: Michael Belyaev <michael7belyaev@gmail.com>
Date: Sat, 25 May 2024 21:56:32 +0100
Subject: [PATCH] checks subscriptions and published faiklure to kState

---
 lib/navigation/control/navigator.cpp | 27 +++++++++++++++------------
 lib/navigation/control/navigator.hpp | 12 ++++++++++++
 2 files changed, 27 insertions(+), 12 deletions(-)

diff --git a/lib/navigation/control/navigator.cpp b/lib/navigation/control/navigator.cpp
index e69222e1..01707c03 100644
--- a/lib/navigation/control/navigator.cpp
+++ b/lib/navigation/control/navigator.cpp
@@ -135,8 +135,9 @@ core::Result Navigator::accelerometerUpdate(
 // Publish failure message to kState topic
 void Navigator::requestFailure()
 {
-  failure_message = "kFailure" const auto topic = core::MqttTopic::kState;
-  auto message_payload                          = std::make_shared<rapidjson::Document>();
+  std::string failure_message = "kFailure";
+  const auto topic            = core::MqttTopic::kState;
+  auto message_payload        = std::make_shared<rapidjson::Document>();
   message_payload->SetObject();
 
   rapidjson::Value message_value;
@@ -170,6 +171,17 @@ void Navigator::publishCurrentTrajectory()
   mqtt_->publish(message, core::MqttMessageQos::kExactlyOnce);
 }
 
+bool Navigator::subscribeAndCheck(core::MqttTopic topic)
+{
+  auto result = mqtt_->subscribe(topic);
+  if (result == core::Result::kFailure) {
+    requestFailure();
+    logger_.log(core::LogLevel::kFatal, "Failed to subscribe to topic");
+    return false;
+  }
+  return true;
+}
+
 void Navigator::run()
 {
   const auto topic     = core::MqttTopic::kStarted;
@@ -186,15 +198,6 @@ void Navigator::run()
   mqtt_->publish(message, core::MqttMessageQos::kExactlyOnce);
 
   // Subscribe to all required topics
-  bool Navigator::subscribeAndCheck(core::MqttTopic topic)
-  {
-    if (!mqtt_->subscribe(topic)) {
-      publishFailure();
-      logger_.log(core::LogLevel::kFatal, "Failed to subscribe to topic");
-      return false;
-    }
-    return true;
-  }
 
   if (!subscribeAndCheck(core::MqttTopic::kKeyence)
       || !subscribeAndCheck(core::MqttTopic::kOpticalFlow)
@@ -234,7 +237,7 @@ void Navigator::run()
         || optical_update_result == core::Result::kFailure
         || accelerometer_update_result == core::Result::kFailure) {
       logger_.log(core::LogLevel::kFatal, "Failed to update sensor data");
-      requestFailure("Failed to update sensor data");
+      requestFailure();
 
       break;
     }
diff --git a/lib/navigation/control/navigator.hpp b/lib/navigation/control/navigator.hpp
index b638a513..1646a0ad 100644
--- a/lib/navigation/control/navigator.hpp
+++ b/lib/navigation/control/navigator.hpp
@@ -53,6 +53,18 @@ class Navigator : public INavigator {
    */
   void publishCurrentTrajectory();
 
+  /**
+   * @brief Publishes a failure message to the MQTT broker
+   */
+  void requestFailure();
+
+  /**
+   * @brief Checks if subscribing to a topic was successful
+   *
+   * @param message
+   */
+  bool subscribeAndCheck(core::MqttTopic topic);
+
  private:
   core::ILogger &logger_;
   const core::ITimeSource &time_;