Separate state requests and broadcasts

lib/core/mqtt_topics.hpp

@@ -6,31 +6,35 @@
 namespace hyped::core {
 
 enum class MqttTopic {
-  kTest,
   kState,
+  kStateRequest,
   kAccelerometer,
   kOpticalFlow,
   kKeyence,
   kStarted,
-  kNavigationData
+  kDisplacement,
+  kVelocity,
+  kAcceleration,
 };
 
 const std::unordered_map<MqttTopic, std::string> mqtt_topic_to_string
-  = {{MqttTopic::kTest, "test"},
-     {MqttTopic::kState, "state"},
-     {MqttTopic::kAccelerometer, "accelerometer"},
-     {MqttTopic::kOpticalFlow, "optical_flow"},
-     {MqttTopic::kKeyence, "keyence"},
-     {MqttTopic::kStarted, "started"},
-     {MqttTopic::kNavigationData, "navigation_data"}};
+  = {{MqttTopic::kState, "hyped/cart_2024/state/state"},
+     {MqttTopic::kStateRequest, "hypd/cart_2024/state/state_request"},
+     {MqttTopic::kAccelerometer, "hyped/cart_2024/measurement/accelerometer"},
+     {MqttTopic::kOpticalFlow, "hypd/cart_2024/measurement/optical_flow"},
+     {MqttTopic::kKeyence, "hyped/cart_2024/measurement/keyence"},
+     {MqttTopic::kDisplacement, "hypd/cart_2024/navigation/displacement"},
+     {MqttTopic::kVelocity, "hypd/cart_2024/navigation/velocity"},
+     {MqttTopic::kAcceleration, "hypd/cart_2024/navigation/acceleration"}};
 
 const std::unordered_map<std::string, MqttTopic> mqtt_string_to_topic
-  = {{"test", MqttTopic::kTest},
-     {"state", MqttTopic::kState},
-     {"accelerometer", MqttTopic::kAccelerometer},
-     {"optical_flow", MqttTopic::kOpticalFlow},
-     {"keyence", MqttTopic::kKeyence},
-     {"started", MqttTopic::kStarted},
-     {"navigation_data", MqttTopic::kNavigationData}};
+  = {{"hyped/cart_2024/state/state", MqttTopic::kState},
+     {"hypd/cart_2024/state/state_request", MqttTopic::kState},
+     {"hyped/cart_2024/measurement/accelerometer", MqttTopic::kAccelerometer},
+     {"hyped/cart_2024/measurement/optical_flow", MqttTopic::kOpticalFlow},
+     {"hyped/cart_2024/measurement/keyence", MqttTopic::kKeyence},
+     {"hypd/cart_2024/navigation/displacement", MqttTopic::kDisplacement},
+     {"hypd/cart_2024/navigation/velocity", MqttTopic::kVelocity},
+     {"hypd/cart_2024/navigation/acceleration", MqttTopic::kAcceleration}};
 
 }  // namespace hyped::core

lib/navigation/control/navigator.cpp

@@ -158,27 +158,50 @@ void Navigator::requestBraking()
 }
 
 // Publish current trajectory to kTest topic
-void Navigator::publishCurrentTrajectory()
+core::Result Navigator::publishCurrentTrajectory()
 {
-  const auto topic     = core::MqttTopic::kNavigationData;
-  auto message_payload = std::make_shared<rapidjson::Document>();
-  message_payload->SetObject();
-
   const auto trajectory = currentTrajectory();
-  if (!trajectory) { logger_.log(core::LogLevel::kFatal, "Failed to get current trajectory"); }
-
-  rapidjson::Value displacement(trajectory->displacement);
-  rapidjson::Value velocity(trajectory->velocity);
-  rapidjson::Value acceleration(previous_accelerometer_data_);
-  message_payload->AddMember("displacement", displacement, message_payload->GetAllocator());
-  message_payload->AddMember("velocity", velocity, message_payload->GetAllocator());
-  message_payload->AddMember("acceleration", acceleration, message_payload->GetAllocator());
-
-  const core::MqttMessage::Header header{
-    .timestamp = static_cast<uint64_t>(time_.now().time_since_epoch().count()),
-    .priority  = core::MqttMessagePriority::kNormal};
-  const core::MqttMessage message{topic, header, message_payload};
-  mqtt_->publish(message, core::MqttMessageQos::kExactlyOnce);
+  if (!trajectory) {
+    logger_.log(core::LogLevel::kFatal, "Failed to get current trajectory");
+    return core::Result::kFailure;
+  }
+  {
+    const auto topic     = core::MqttTopic::kDisplacement;
+    auto message_payload = std::make_shared<rapidjson::Document>();
+    message_payload->SetObject();
+    rapidjson::Value displacement(trajectory->displacement);
+    message_payload->AddMember("displacement", displacement, message_payload->GetAllocator());
+    const core::MqttMessage::Header header{
+      .timestamp = static_cast<uint64_t>(time_.now().time_since_epoch().count()),
+      .priority  = core::MqttMessagePriority::kNormal};
+    const core::MqttMessage message{topic, header, message_payload};
+    mqtt_->publish(message, core::MqttMessageQos::kExactlyOnce);
+  }
+  {
+    const auto topic     = core::MqttTopic::kVelocity;
+    auto message_payload = std::make_shared<rapidjson::Document>();
+    message_payload->SetObject();
+    rapidjson::Value velocity(trajectory->velocity);
+    message_payload->AddMember("velocity", velocity, message_payload->GetAllocator());
+    const core::MqttMessage::Header header{
+      .timestamp = static_cast<uint64_t>(time_.now().time_since_epoch().count()),
+      .priority  = core::MqttMessagePriority::kNormal};
+    const core::MqttMessage message{topic, header, message_payload};
+    mqtt_->publish(message, core::MqttMessageQos::kExactlyOnce);
+  }
+  {
+    const auto topic     = core::MqttTopic::kAcceleration;
+    auto message_payload = std::make_shared<rapidjson::Document>();
+    message_payload->SetObject();
+    rapidjson::Value acceleration(trajectory->acceleration);
+    message_payload->AddMember("acceleration", acceleration, message_payload->GetAllocator());
+    const core::MqttMessage::Header header{
+      .timestamp = static_cast<uint64_t>(time_.now().time_since_epoch().count()),
+      .priority  = core::MqttMessagePriority::kNormal};
+    const core::MqttMessage message{topic, header, message_payload};
+    mqtt_->publish(message, core::MqttMessageQos::kExactlyOnce);
+  }
+  return core::Result::kSuccess;
 }
 
 void Navigator::publishStart()
@@ -297,7 +320,12 @@ void Navigator::run()
 
     updateSensorData(
       *most_recent_keyence_data, *most_recent_optical_data, *most_recent_accelerometer_data);
-    publishCurrentTrajectory();
+    const auto result = publishCurrentTrajectory();
+    if (result == core::Result::kFailure) {
+      logger_.log(core::LogLevel::kFatal, "Failed to publish current trajectory");
+      requestFailure();
+      return;
+    }
   }
 }
 

lib/navigation/control/navigator.hpp

@@ -58,7 +58,7 @@ class Navigator : public INavigator {
   /**
    * @brief Publishes the current trajectory to the MQTT broker
    */
-  void publishCurrentTrajectory();
+  core::Result publishCurrentTrajectory();
 
   /**
    * @brief Publishes a failure message to the MQTT broker

lib/navigation/filtering/kalman_matrices.cpp

@@ -33,6 +33,7 @@ const StateTransitionCovarianceMatrix transition_covariance
 const MeasurementNoiseCovarianceMatrix kMeasurementNoiseCovarianceMatrix
   = (MeasurementNoiseCovarianceMatrix() << 0.01, 0, 0, 0).finished();
 
+// TODOLater: this is entirely arbitrary, someone needs to work out what it should be
 const ErrorCovarianceMatrix kErrorCovarianceMatrix
   = (ErrorCovarianceMatrix() << 0.01, 0, 0, 0.01).finished();
 

lib/state_machine/state_machine.cpp

@@ -13,7 +13,7 @@ StateMachine::StateMachine(std::shared_ptr<core::IMqtt> mqtt, TransitionTable tr
       mqtt_(std::move(mqtt)),
       transition_to_state_(std::move(transition_table))
 {
-  mqtt_->subscribe(core::MqttTopic::kTest);
+  mqtt_->subscribe(core::MqttTopic::kStateRequest);
 }
 
 State StateMachine::stringToState(const std::string &state_name)
@@ -55,7 +55,7 @@ void StateMachine::update()
 void StateMachine::publishCurrentState()
 {
   const auto state_string = stateToString(getCurrentState());
-  const auto topic        = core::MqttTopic::kTest;
+  const auto topic        = core::MqttTopic::kState;
   auto message_payload    = std::make_shared<rapidjson::Document>();
   message_payload->SetObject();
   rapidjson::Value state_value(state_string.c_str(), message_payload->GetAllocator());