From de9510c0961216de7c7278218b818d30014de561 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Mon, 24 Apr 2023 11:57:13 +0200
Subject: [PATCH] add example reading of multiflex data

---
 src/object_detection/src/multiflex_reader.cpp | 54 +++++++++++++++----
 1 file changed, 44 insertions(+), 10 deletions(-)

diff --git a/src/object_detection/src/multiflex_reader.cpp b/src/object_detection/src/multiflex_reader.cpp
index 1f5b3e40..413e8b9a 100644
--- a/src/object_detection/src/multiflex_reader.cpp
+++ b/src/object_detection/src/multiflex_reader.cpp
@@ -5,35 +5,69 @@
 #include <iostream>
 
 #include "rclcpp/rclcpp.hpp"
-#include "object_detection/msg/lidar_reading.hpp"
 
 #include <terabee/ITerarangerFactory.hpp>
-#include <terabee/ITerarangerTowerEvo.hpp>
-#include <terabee/TowerDistanceData.hpp>
-#include <terabee/ImuData.hpp>
+#include <terabee/ITerarangerMultiflex.hpp>
+#include <terabee/DistanceData.hpp>
 
-using terabee::ImuData;
-using terabee::ITerarangerTowerEvo;
-using terabee::TowerDistanceData;
+using terabee::DistanceData;
 
 using namespace std::chrono_literals;
 
+std::ostream &operator<<(std::ostream &os, const DistanceData &d)
+{
+  os << "[";
+  for (size_t i = 0; i < d.distance.size(); i++)
+  {
+    os << d.distance[i] << ", ";
+  }
+  os << "\b\b"
+     << " ]";
+  return os;
+}
+
 class MultiflexReader : public rclcpp::Node
 {
 public:
   MultiflexReader()
       : Node("multiflex_reader")
   {
-    //publisher_ = this->create_publisher<object_detection::msg::LidarReading>("drone/object_detection", 10);
-    //timer_ = this->create_wall_timer(500ms, std::bind(&MultiflexReader::read_multiflex_data, this));
+    factory = terabee::ITerarangerFactory::getFactory();
+    multiflex = factory->createTerarangerMultiflex("/dev/ttyACM0");
+
+    if (!multiflex)
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to create multiflex");
+      return;
+    }
+
+    if (!multiflex->initialize())
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to initialize multiflex");
+      return;
+    }
+
+    if (!multiflex->configureNumberOfSensors(0x3f))
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 6!");
+      return;
+    }
+
+    RCLCPP_INFO(this->get_logger(), "Multiflex initialized");
+
+    timer_ = this->create_wall_timer(500ms, std::bind(&MultiflexReader::read_multiflex_data, this));
   }
 
 private:
   void read_multiflex_data()
   {
-
+    std::cout << "Distance = " << multiflex->getDistance() << std::endl;
   }
 
+  std::unique_ptr<terabee::ITerarangerFactory> factory;
+  std::unique_ptr<terabee::ITerarangerMultiflex> multiflex;
+
+  rclcpp::TimerBase::SharedPtr timer_;
 };
 
 int main(int argc, char *argv[])