NAV - MQTT node for navigation (#119)

Co-authored-by: Tom Lonergan <tomlonergan91@gmail.com>
Hyp-ed · Jul 1, 2024 · 4f0f768 · 4f0f768
1 parent 6e72202
commit 4f0f768
Show file tree

Hide file tree

Showing 18 changed files with 382 additions and 132 deletions.
diff --git a/config/pod.toml b/config/pod.toml
@@ -9,7 +9,7 @@ host = "192.168.1.0"
 port = 4556
 
 [raspberry]
-nodes = ["state_machine", "mqtt_broker"]
+nodes = ["state_machine", "mqtt_broker", "navigator"]
 
 [state_machine]
 # Possible transition tables are "full_run",
@@ -26,3 +26,6 @@ topic = 'accelerometer'
 format = 'float'
 type = 'acceleration'
 unit = 'm/s²'
+
+[navigator]
+maxVelocity = 10
diff --git a/lib/core/mqtt_logger.cpp b/lib/core/mqtt_logger.cpp
@@ -20,7 +20,7 @@ MqttLogger::MqttLogger(const char *const label,
 void MqttLogger::log(const LogLevel level, const char *format, ...)
 {
   logger_.log(level, format);
-  const auto topic                                     = MqttTopic::kTest;
+  const auto topic                                     = MqttTopic::kLogs;
   std::shared_ptr<rapidjson::Document> message_payload = std::make_shared<rapidjson::Document>();
   message_payload->SetObject();
   message_payload->AddMember("log", *format, message_payload->GetAllocator());

diff --git a/lib/core/mqtt_topics.hpp b/lib/core/mqtt_topics.hpp
@@ -5,11 +5,39 @@
 
 namespace hyped::core {
 
-enum class MqttTopic { kTest, kState };
+enum class MqttTopic {
+  kState,
+  kStateRequest,
+  kAccelerometer,
+  kOpticalFlow,
+  kKeyence,
+  kStarted,
+  kDisplacement,
+  kVelocity,
+  kAcceleration,
+  kLogs
+};
 
 const std::unordered_map<MqttTopic, std::string> mqtt_topic_to_string
-  = {{MqttTopic::kTest, "test"}, {MqttTopic::kState, "state"}};
+  = {{MqttTopic::kState, "hyped/cart_2024/state/state"},
+     {MqttTopic::kStateRequest, "hyped/cart_2024/state/state_request"},
+     {MqttTopic::kAccelerometer, "hyped/cart_2024/measurement/accelerometer"},
+     {MqttTopic::kOpticalFlow, "hyped/cart_2024/measurement/optical_flow"},
+     {MqttTopic::kKeyence, "hyped/cart_2024/measurement/keyence"},
+     {MqttTopic::kDisplacement, "hyped/cart_2024/navigation/displacement"},
+     {MqttTopic::kVelocity, "hyped/cart_2024/navigation/velocity"},
+     {MqttTopic::kAcceleration, "hyped/cart_2024/navigation/acceleration"},
+     {MqttTopic::kLogs, "hyped/cart_2024/logs"}};
+
 const std::unordered_map<std::string, MqttTopic> mqtt_string_to_topic
-  = {{"test", MqttTopic::kTest}, {"state", MqttTopic::kState}};
+  = {{"hyped/cart_2024/state/state", MqttTopic::kState},
+     {"hyped/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},
+     {"hyped/cart_2024/navigation/displacement", MqttTopic::kDisplacement},
+     {"hyped/cart_2024/navigation/velocity", MqttTopic::kVelocity},
+     {"hyped/cart_2024/navigation/acceleration", MqttTopic::kAcceleration},
+     {"hyped/cart_2024/logs", MqttTopic::kLogs}};
 
-}  // namespace hyped::core
+}  // namespace hyped::core
diff --git a/lib/core/types.hpp b/lib/core/types.hpp
@@ -23,10 +23,9 @@ struct Trajectory {
 };
 
 // number of each type of sensors
-static constexpr std::uint8_t kNumAccelerometers = 4;
+static constexpr std::uint8_t kNumAccelerometers = 1;
 static constexpr std::uint8_t kNumAxis           = 3;
-static constexpr std::uint8_t kNumEncoders       = 4;
-static constexpr std::uint8_t kNumKeyence        = 2;
+static constexpr std::uint8_t kNumKeyence        = 1;
 static constexpr std::uint8_t kNumOptical        = 1;
 
 // data format for raw sensor data
@@ -38,25 +37,11 @@ using OpticalData          = std::array<std::array<Float, 2>, kNumOptical>;
 // data produced by the accelerometer sensor
 // values are in milli-g (standard gravity)
 struct RawAccelerationData {
-  RawAccelerationData(const std::int32_t x,
-                      const std::int32_t y,
-                      const std::int32_t z,
-                      const TimePoint measured_at,
-                      const bool is_sensor_active)
-      : x(x),
-        y(y),
-        z(z),
-        measured_at(measured_at),
-        is_sensor_active(is_sensor_active)
-
-  {
-  }
-
-  const std::int32_t x;
-  const std::int32_t y;
-  const std::int32_t z;
-  const TimePoint measured_at;
-  const bool is_sensor_active;
+  std::int32_t x;
+  std::int32_t y;
+  std::int32_t z;
+  TimePoint measured_at;
+  bool is_sensor_active;
 };
 
 }  // namespace hyped::core
diff --git a/lib/navigation/control/CMakeLists.txt b/lib/navigation/control/CMakeLists.txt
@@ -9,4 +9,11 @@ include_directories(
   "${CMAKE_CURRENT_SOURCE_DIR}"
   "${CMAKE_SOURCE_DIR}/lib/navigation"
 )
-target_link_libraries(${target} Eigen3::Eigen hyped_core)
+target_link_libraries(
+  ${target}
+  Eigen3::Eigen
+  hyped_core
+  tomlplusplus::tomlplusplus
+  hyped_navigation_filters
+  hyped_navigation_preprocessing
+)
diff --git a/lib/navigation/control/consts.hpp b/lib/navigation/control/consts.hpp
@@ -8,8 +8,8 @@
 namespace hyped::navigation {
 
 // TODOLater: move most of these to a config file
-static constexpr core::Float kTrackLength     = 100.0;  // m
-static constexpr core::Float kBrakingDistance = 20.0;   // m TODOLater:check!
+static constexpr core::Float kTrackLength     = 6.0;  // m
+static constexpr core::Float kBrakingDistance = 2.0;  // m TODOLater:check!
 static constexpr core::Float kStripeDistance  = 5.0;
 static constexpr bool kIsKeyenceActive        = true;
 
@@ -30,4 +30,5 @@ class INavigator {
     = 0;
   virtual core::Result opticalUpdate(const core::OpticalData &optical_data) = 0;
 };
+
 }  // namespace hyped::navigation