diff --git a/src/object_detection/src/multiflex_reader.cpp b/src/object_detection/src/multiflex_reader.cpp index 9110aa66..e07ac8d9 100644 --- a/src/object_detection/src/multiflex_reader.cpp +++ b/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("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 factory; std::unique_ptr multiflex; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; }; int main(int argc, char *argv[])