added sequence of events for reading, todo: read patload

Hyp-ed · May 29, 2024 · 48b564b · 48b564b
1 parent e44814a
commit 48b564b
Show file tree

Hide file tree

Showing 2 changed files with 62 additions and 25 deletions.
diff --git a/lib/navigation/control/navigator.cpp b/lib/navigation/control/navigator.cpp
@@ -206,44 +206,77 @@ void Navigator::run()
   }
 
   while (true) {
-    mqtt_->consume();
-
-    auto keyence_msg       = mqtt_->getMessage();
-    auto optical_msg       = mqtt_->getMessage();
-    auto accelerometer_msg = mqtt_->getMessage();
-
-    auto keyence_payload          = keyence_msg->payload;
-    auto optical_payload          = optical_msg->payload;
-    auto raw_acceleration_payload = accelerometer_msg->payload;
-
-    core::KeyenceData dummy_keyence_data = {0, 0};
+    bool sensor_data_received = false;
+
+    std::optional<core::KeyenceData> most_recent_keyence_data = std::nullopt;
+
+    std::optional<core::OpticalData> most_recent_optical_data = std::nullopt;
+
+    std::optional<std::array<core::RawAccelerationData, core::kNumAccelerometers>>
+      most_recent_accelerometer_data = std::nullopt;
+
+    while (!sensor_data_received) {
+      mqtt_->consume();
+
+      auto msg = mqtt_->getMessage();
+
+      if (msg.has_value() && msg.value().topic == core::MqttTopic::kKeyence) {
+        auto payload = msg->payload;
+        // TODOLater: read the payload
+        most_recent_keyence_data = core::KeyenceData{0, 0};
+      } else if (msg.has_value() && msg.value().topic == core::MqttTopic::kOpticalFlow) {
+        auto payload = msg->payload;
+        // TODOLater: read the payload
+        core::OpticalData most_recent_optical_data;
+        for (auto &data : most_recent_optical_data) {
+          data = {0.0F, 0.0F};
+        }
+      } else if (msg.has_value() && msg.value().topic == core::MqttTopic::kAccelerometer) {
+        auto payload = msg->payload;
+        // TODOLater: read the payload
+        std::array<core::RawAccelerationData, core::kNumAccelerometers>
+          most_recent_accelerometer_data
+          = {core::RawAccelerationData(0, 0, 0, core::TimePoint{}, false),
+             core::RawAccelerationData(0, 0, 0, core::TimePoint{}, false),
+             core::RawAccelerationData(0, 0, 0, core::TimePoint{}, false),
+             core::RawAccelerationData(0, 0, 0, core::TimePoint{}, false)};
+      }
+
+      if (most_recent_keyence_data && most_recent_optical_data && most_recent_accelerometer_data) {
+        sensor_data_received = true;
+      }
+    }
 
-    core::OpticalData dummy_optical_data;
-    for (auto &data : dummy_optical_data) {
+    core::OpticalData default_optical_data;
+    for (auto &data : default_optical_data) {
       data = {0.0F, 0.0F};
     }
 
-    std::array<core::RawAccelerationData, core::kNumAccelerometers> dummy_accelerometer_data
-      = {core::RawAccelerationData(0, 0, 0, core::TimePoint{}, false),
-         core::RawAccelerationData(0, 0, 0, core::TimePoint{}, false),
-         core::RawAccelerationData(0, 0, 0, core::TimePoint{}, false),
-         core::RawAccelerationData(0, 0, 0, core::TimePoint{}, false)};
+    if (most_recent_keyence_data.has_value()) {
+      auto keyence_update_result = keyenceUpdate(most_recent_keyence_data.value());
+    }
+
+    if (most_recent_optical_data.has_value()) {
+      auto optical_update_result = opticalUpdate(most_recent_optical_data.value());
+      // If no optical data received, use {0,0}
+    } else {
+      auto optical_update_result = opticalUpdate(default_optical_data);
+    }
 
-    auto keyence_update_result       = keyenceUpdate(dummy_keyence_data);
-    auto optical_update_result       = opticalUpdate(dummy_optical_data);
-    auto accelerometer_update_result = accelerometerUpdate(dummy_accelerometer_data);
+    if (most_recent_accelerometer_data.has_value()) {
+      auto accelerometer_update_result
+        = accelerometerUpdate(most_recent_accelerometer_data.value());
+    }
 
     if (keyence_update_result == core::Result::kFailure
         || optical_update_result == core::Result::kFailure
         || accelerometer_update_result == core::Result::kFailure) {
       logger_.log(core::LogLevel::kFatal, "Failed to update sensor data");
       requestFailure();
-
-      break;
+      return;
     }
-
     publishCurrentTrajectory();
   }
 }
 
-}  // namespace hyped::navigation
+}  // namespace hyped::navigation
diff --git a/lib/navigation/control/navigator.hpp b/lib/navigation/control/navigator.hpp
@@ -83,6 +83,10 @@ class Navigator : public INavigator {
   core::Float previous_keyence_reading_;
   core::Float previous_accelerometer_data_;
 
+  core::Result keyence_update_result = core::Result::kSuccess;
+  core::Result optical_update_result = core::Result::kSuccess;
+  core::Result accelerometer_update_result = core::Result::kSuccess;
+
   // current navigation trajectory
   core::Trajectory trajectory_;
 };