From 16192c6c8b3c34ccfe7939c28aefed28f653e69f Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Thu, 20 Apr 2023 11:50:24 +0200
Subject: [PATCH] add imu data to message

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

diff --git a/src/object_detection/src/lidar_reader.cpp b/src/object_detection/src/lidar_reader.cpp
index 97767d6b..af00203e 100644
--- a/src/object_detection/src/lidar_reader.cpp
+++ b/src/object_detection/src/lidar_reader.cpp
@@ -52,7 +52,7 @@ public:
       : 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));
+    timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this));
 
     ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc);
 
@@ -75,11 +75,20 @@ public:
   }
 
 private:
-  void timer_callback()
+  void read_lidar_data()
   {
     //TODO publish message with all data from lidar
     std::cout << "Distance = " << tower->getDistance() << std::endl;
     std::cout << "IMU = " << tower->getImuData() << std::endl;
+
+    auto message = object_detection::msg::LidarReading msg;
+
+    ImuData imu_data = tower->getImuData();
+
+    for (size_t i = 0; i < imu_data.data.size(); i++)
+    {
+      msg.imu_data.push_back(imu_data.data[i]);
+    }
   }
 
   rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_;