add publishing message

src/object_detection/src/multiflex_reader.cpp

@@ -15,18 +15,6 @@ 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:
@@ -56,19 +44,28 @@ public:
 
     RCLCPP_INFO(this->get_logger(), "Multiflex initialized");
 
+
+    publisher_ = this->create_publisher<object_detection::msg::MultiflexReading>("drone/object_detection", 10);
     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;
+    terabee::DistanceData data = multiflex->getDistance();
+    auto msg = object_detection::msg::MultiflexReading();
+    for (int i = 0; i < data.size(); i++)
+    {
+      msg.distance_data[i] = data.distance[i];
+    }
+    publisher_->publish(msg);
   }
 
   std::unique_ptr<terabee::ITerarangerFactory> factory;
   std::unique_ptr<terabee::ITerarangerMultiflex> multiflex;
 
   rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<object_detection::msg::MultiflexReading>::SharedPtr publisher_;
 };
 
 int main(int argc, char *argv[])