From 4d4647f34872ca23286a4db8f0c1d5be29e28969 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Wed, 19 Apr 2023 16:52:29 +0200
Subject: [PATCH] add example code from terabee

---
 src/object_detection/src/lidar_reader.cpp | 77 +++++++++++++++++++----
 1 file changed, 64 insertions(+), 13 deletions(-)

diff --git a/src/object_detection/src/lidar_reader.cpp b/src/object_detection/src/lidar_reader.cpp
index 566dd20b..6b33d620 100644
--- a/src/object_detection/src/lidar_reader.cpp
+++ b/src/object_detection/src/lidar_reader.cpp
@@ -2,6 +2,7 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <iostream>
 
 #include "rclcpp/rclcpp.hpp"
 #include "object_detection/msg/lidar_reading.hpp"
@@ -11,34 +12,84 @@
 #include <terabee/TowerDistanceData.hpp>
 #include <terabee/ImuData.hpp>
 
+using terabee::ImuData;
 using terabee::ITerarangerTowerEvo;
 using terabee::TowerDistanceData;
-using terabee::ImuData;
 
 using namespace std::chrono_literals;
 
 /* This example creates a subclass of Node and uses std::bind() to register a
-* member function as a callback from the timer. */
+ * member function as a callback from the timer. */
+
+std::ostream &operator<<(std::ostream &os, const TowerDistanceData &d)
+{
+  os << "[";
+  for (size_t i = 0; i < d.distance.size(); i++)
+  {
+    os << d.distance[i] << (d.mask[i] ? " <new>, " : " <old>, ");
+  }
+  os << "\b\b"
+     << " ]";
+  return os;
+}
+
+std::ostream &operator<<(std::ostream &os, const ImuData &d)
+{
+  os << "[";
+  for (size_t i = 0; i < d.data.size(); i++)
+  {
+    os << d.data[i] << ", ";
+  }
+  os << "\b\b"
+     << " ]";
+  return os;
+}
 
 class LidarReader : public rclcpp::Node
 {
-  public:
-    LidarReader()
-    : Node("lidar_reader")
-    { 
-      publisher_ = this->create_publisher<object_detection::msg::LidarReading>("drone/object_detection", 10);
-    }
+public:
+  LidarReader()
+      : Node("lidar_reader")
+  {
+    publisher_ = this->create_publisher<object_detection::msg::LidarReading>("drone/object_detection", 10);
+    timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::timer_callback, this));
 
-  private:
-    void timer_callback()
+    ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc);
+
+    factory = terabee::ITerarangerFactory::getFactory();
+    tower = factory.createTerarangerTowerEvo("/dev/ttyACM0");
+
+    if (!tower)
     {
-      RCLCPP_INFO(this->get_logger(), "yeet");
+      RCLCPP_ERROR(this->get_logger(), "Failed to create TerarangerTowerEvo");
+      return;
     }
 
-    rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_;
+    tower->setImuMode(mode);
+
+    if (!tower->initialize())
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to initialize TerarangerTowerEvo");
+      return;
+    }
+  }
+
+private:
+  void timer_callback()
+  {
+    std::cout << "Distance = " << tower->getDistance() << std::endl;
+    std::cout << "IMU = " << tower->getImuData() << std::endl;
+  }
+
+  rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  // terabee tower evo variables
+  terabee::ITerarangerTowerEvo tower;
+  terabee::ITerarangerFactory factory;
 };
 
-int main(int argc, char * argv[])
+int main(int argc, char *argv[])
 {
   rclcpp::init(argc, argv);
   rclcpp::spin(std::make_shared<LidarReader>());