Merge branch 'master' into sns-optical_flow_hack

config/pod.toml

@@ -9,23 +9,18 @@ host = "192.168.1.0"
 port = 4556
 
 [raspberry]
-nodes = ["state_machine", "mqtt_broker", "navigator", "optical_flow"]
+nodes = ["state_machine", "mqtt_broker", "navigator", "accelerometer", "keyence", "optical_flow"]
 
 [state_machine]
 # Possible transition tables are "full_run",
 # "no_levitation", "levitation_only"
 transition_table = "full_run"
 
-[some_sensors]
-[some_sensors.accelerometer]
-count = 5
-upper_limit = 100
-lower_limit = 0
-name = 'Acceleration'
-topic = 'accelerometer'
-format = 'float'
-type = 'acceleration'
-unit = 'm/s²'
-
 [navigator]
 maxVelocity = 10
+
+[accelerometer]
+mux_channel = 3
+
+[keyence]
+pin = 4

lib/navigation/control/navigator.cpp

@@ -132,7 +132,7 @@ void Navigator::requestFailure()
 
   rapidjson::Value message_value;
   message_value.SetString(failure_message.c_str(), message_payload->GetAllocator());
-  message_payload->AddMember("transition", message_value, message_payload->GetAllocator());
+  message_payload->AddMember("state", message_value, message_payload->GetAllocator());
 
   const core::MqttMessage::Header header{
     .timestamp = static_cast<std::uint64_t>(time_.now().time_since_epoch().count()),

lib/sensors/CMakeLists.txt

@@ -8,4 +8,4 @@ include_directories(
   "${CMAKE_SOURCE_DIR}/lib"
   "${CMAKE_CURRENT_SOURCE_DIR}"
 )
-target_link_libraries(${target} PUBLIC tomlplusplus::tomlplusplus)
+target_link_libraries(${target} PUBLIC tomlplusplus::tomlplusplus hyped_io)

lib/sensors/accelerometer_node.cpp

@@ -1,5 +1,5 @@
-#include "accelerometer.hpp"
 #include "accelerometer_node.hpp"
+#include "i2c_mux.hpp"
 
 #include <utility>
 
@@ -27,8 +27,20 @@ core::Result AccelerometerNode::startNode(toml::v3::node_view<const toml::v3::no
     logger.log(core::LogLevel::kFatal, "Failed to create I2C bus");
     return core::Result::kFailure;
   }
-  auto i2c                    = *optional_i2c;
-  auto optional_accelerometer = Accelerometer::create(logger, i2c, accelerometerAddress::k1D);
+  auto i2c                  = *optional_i2c;
+  auto optional_mux_channel = config["mux_channel"].value<std::uint8_t>();
+  if (!optional_mux_channel) {
+    logger.log(core::LogLevel::kFatal, "No mux channel specified");
+    return core::Result::kFailure;
+  }
+  auto mux_channel      = *optional_mux_channel;
+  auto optional_i2c_mux = I2cMux::create(logger, i2c, kDefaultMuxAddress, mux_channel);
+  if (!optional_i2c_mux) {
+    logger.log(core::LogLevel::kFatal, "Failed to create I2C mux");
+    return core::Result::kFailure;
+  }
+  auto i2c_mux                = *optional_i2c_mux;
+  auto optional_accelerometer = Accelerometer::create(logger, i2c_mux, accelerometerAddress::k1D);
   if (!optional_accelerometer) {
     logger.log(core::LogLevel::kFatal, "Failed to create accelerometer");
     return core::Result::kFailure;
@@ -50,11 +62,32 @@ AccelerometerNode::AccelerometerNode(core::Logger &logger,
 {
 }
 
+void AccelerometerNode::requestFailure()
+{
+  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;
+  message_value.SetString(failure_message.c_str(), message_payload->GetAllocator());
+  message_payload->AddMember("state", message_value, message_payload->GetAllocator());
+
+  const core::MqttMessage::Header header{
+    .timestamp = static_cast<std::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);
+}
+
 void AccelerometerNode::run()
 {
   while (true) {
     auto result = accelerometer_->isValueReady();
-    if (!result) { continue; }
+    if (!result) {
+      requestFailure();
+      return;
+    }
     if (*result == core::Result::kFailure) { continue; }
     auto optional_acceration = accelerometer_->read();
     if (!optional_acceration) {

lib/sensors/accelerometer_node.hpp

@@ -20,6 +20,8 @@ class AccelerometerNode {
   void run();
 
  private:
+  void requestFailure();
+
   core::Logger logger_;
   core::ITimeSource &time_;
   std::shared_ptr<core::Mqtt> mqtt_;

lib/sensors/keyence_node.cpp

@@ -0,0 +1,93 @@
+#include "keyence.hpp"
+#include "keyence_node.hpp"
+
+#include <utility>
+
+#include "core/logger.hpp"
+#include "core/mqtt.hpp"
+#include "core/wall_clock.hpp"
+#include "io/hardware_gpio.hpp"
+
+namespace hyped::sensors {
+
+core::Result KeyenceNode::startNode(toml::v3::node_view<const toml::v3::node> config,
+                                    const std::string &mqtt_ip,
+                                    const std::uint32_t mqtt_port)
+{
+  auto time          = core::WallClock();
+  auto logger        = core::Logger("Keyence", core::LogLevel::kDebug, time);
+  auto optional_mqtt = core::Mqtt::create(logger, "Keyence", mqtt_ip, mqtt_port);
+  if (!optional_mqtt) {
+    logger.log(core::LogLevel::kFatal, "Failed to create MQTT client");
+    return core::Result::kFailure;
+  }
+  auto mqtt      = *optional_mqtt;
+  auto gpio      = std::make_shared<io::HardwareGpio>(logger);
+  const auto pin = config["pin"].value<std::uint8_t>();
+  if (!pin) {
+    logger.log(core::LogLevel::kFatal, "No pin specified");
+    return core::Result::kFailure;
+  }
+  auto optional_keyence = Keyence::create(logger, gpio, *pin);
+  if (!optional_keyence) {
+    logger.log(core::LogLevel::kFatal, "Failed to create keyence");
+    return core::Result::kFailure;
+  }
+  auto keyence = *optional_keyence;
+  auto node    = KeyenceNode(logger, time, mqtt, keyence);
+  node.run();
+  return core::Result::kSuccess;
+}
+
+KeyenceNode::KeyenceNode(core::Logger &logger,
+                         core::ITimeSource &time,
+                         std::shared_ptr<core::Mqtt> mqtt,
+                         std::shared_ptr<Keyence> keyence)
+    : logger_(logger),
+      time_(time),
+      mqtt_(std::move(mqtt)),
+      keyence_(std::move(keyence))
+{
+}
+
+void KeyenceNode::requestFailure()
+{
+  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;
+  message_value.SetString(failure_message.c_str(), message_payload->GetAllocator());
+  message_payload->AddMember("state", message_value, message_payload->GetAllocator());
+
+  const core::MqttMessage::Header header{
+    .timestamp = static_cast<std::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);
+}
+
+void KeyenceNode::run()
+{
+  while (true) {
+    auto result = keyence_->updateStripeCount();
+    if (result == core::Result::kFailure) {
+      requestFailure();
+      return;
+    }
+    const auto stripe_count = keyence_->getStripeCount();
+    const auto topic        = core::MqttTopic::kKeyence;
+    auto message_payload    = std::make_shared<rapidjson::Document>();
+    message_payload->SetObject();
+    rapidjson::Value stripe_count_json(stripe_count);
+    message_payload->AddMember("count", stripe_count_json, message_payload->GetAllocator());
+    const core::MqttMessage::Header header{
+      .timestamp = static_cast<std::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);
+  }
+}
+
+}  // namespace hyped::sensors

lib/sensors/keyence_node.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include "keyence.hpp"
+
+#include "core/mqtt.hpp"
+#include "core/types.hpp"
+#include <toml++/toml.hpp>
+
+namespace hyped::sensors {
+
+class KeyenceNode {
+ public:
+  static core::Result startNode(toml::v3::node_view<const toml::v3::node> config,
+                                const std::string &mqtt_ip,
+                                const std::uint32_t mqtt_port);
+  KeyenceNode(core::Logger &logger,
+              core::ITimeSource &time,
+              std::shared_ptr<core::Mqtt> mqtt,
+              std::shared_ptr<Keyence> keyence);
+  void run();
+
+ private:
+  void requestFailure();
+
+  core::Logger logger_;
+  core::ITimeSource &time_;
+  std::shared_ptr<core::Mqtt> mqtt_;
+  std::shared_ptr<Keyence> keyence_;
+};
+
+}  // namespace hyped::sensors

src/pod/CMakeLists.txt

@@ -14,5 +14,6 @@ target_link_libraries(
   hyped_navigation_control
   hyped_navigation_filters
   hyped_navigation_preprocessing
+  hyped_sensors
   tomlplusplus::tomlplusplus
 )

src/pod/main.cpp

@@ -8,6 +8,8 @@
 #include <core/wall_clock.hpp>
 #include <io/hardware_gpio.hpp>
 #include <navigation/control/navigator.hpp>
+#include <sensors/accelerometer_node.hpp>
+#include <sensors/keyence_node.hpp>
 #include <state_machine/state_machine.hpp>
 #include <state_machine/transition_table.hpp>
 #include <toml++/toml.hpp>
@@ -81,6 +83,12 @@ int main(int argc, char **argv)
         hyped::navigation::Navigator::startNode(navigator_config, mqtt_ip, mqtt_port);
       } else if (node_name == "optical_flow") {
         execl("/usr/bin/python3", "python", "optical_flow.py", nullptr);
+      } else if (node_name == "keyence") {
+        auto keyence_config = config["keyence"];
+        hyped::sensors::KeyenceNode::startNode(keyence_config, mqtt_ip, mqtt_port);
+      } else if (node_name == "accelerometer") {
+        auto accelerometer_config = config["accelerometer"];
+        hyped::sensors::AccelerometerNode::startNode(accelerometer_config, mqtt_ip, mqtt_port);
       } else {
         std::cerr << "Unknown node: " << node_name << "\n";
         return 1;