From 44b3f135a9e76f34271ee5c36c3032b2d4f051da Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 5 May 2023 16:28:27 +0200 Subject: [PATCH] add comments and improve code for object_detection package --- src/object_detection/src/lidar_reader.cpp | 33 +++--- src/object_detection/src/multiflex_reader.cpp | 112 ++++++++++-------- 2 files changed, 79 insertions(+), 66 deletions(-) diff --git a/src/object_detection/src/lidar_reader.cpp b/src/object_detection/src/lidar_reader.cpp index df21a116..265d35a2 100644 --- a/src/object_detection/src/lidar_reader.cpp +++ b/src/object_detection/src/lidar_reader.cpp @@ -33,21 +33,19 @@ public: publisher_ = this->create_publisher("drone/object_detection", 10); timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this)); - // set lidar to measure including IMU - ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc); - factory = terabee::ITerarangerFactory::getFactory(); tower = factory->createTerarangerTowerEvo(this->get_parameter("lidar_serial_port").as_string()); - if (!tower) + if (!tower) // check if the object could be created { RCLCPP_ERROR(this->get_logger(), "Failed to create TerarangerTowerEvo"); return; } - tower->setImuMode(mode); + // set lidar to measure including IMU + tower->setImuMode(ITerarangerTowerEvo::QuaternionLinearAcc); - if (!tower->initialize()) + if (!tower->initialize()) // check if communication with the sensor works { RCLCPP_ERROR(this->get_logger(), "Failed to initialize TerarangerTowerEvo"); return; @@ -55,6 +53,19 @@ public: } private: + // publisher for lidar data + rclcpp::Publisher::SharedPtr publisher_; + // timer for publishing readings + rclcpp::TimerBase::SharedPtr timer_; + + // terabee tower evo variables + std::unique_ptr tower; + std::unique_ptr factory; + + /** + * @brief Reads the data from the LIDAR distance modules and the IMU, and publishes it onto the drone/object_detection topic + * + */ void read_lidar_data() { auto msg = object_detection::msg::LidarReading(); @@ -67,7 +78,6 @@ private: // read data from built-in IMU 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]); @@ -77,15 +87,6 @@ private: publisher_->publish(msg); RCLCPP_INFO(this->get_logger(), "Publishing message"); } - - // publisher for lidar data - rclcpp::Publisher::SharedPtr publisher_; - // timer for publishing readings - rclcpp::TimerBase::SharedPtr timer_; - - // terabee tower evo variables - std::unique_ptr tower; - std::unique_ptr factory; }; int main(int argc, char *argv[]) diff --git a/src/object_detection/src/multiflex_reader.cpp b/src/object_detection/src/multiflex_reader.cpp index b92b3db6..f2e92272 100644 --- a/src/object_detection/src/multiflex_reader.cpp +++ b/src/object_detection/src/multiflex_reader.cpp @@ -18,64 +18,76 @@ using namespace std::chrono_literals; class MultiflexReader : public rclcpp::Node { public: - MultiflexReader() - : Node("multiflex_reader") - { - rcl_interfaces::msg::ParameterDescriptor serial_port_descriptor = rcl_interfaces::msg::ParameterDescriptor{}; - serial_port_descriptor.description = "Serial port of the USB port that the multiflex PCB is connected to."; - this->declare_parameter("multiflex_serial_port", "/dev/ttyACM0", serial_port_descriptor); - - factory = terabee::ITerarangerFactory::getFactory(); - multiflex = factory->createTerarangerMultiflex(this->get_parameter("multiflex_serial_port").as_string()); - - if (!multiflex) + MultiflexReader() + : Node("multiflex_reader") { - RCLCPP_ERROR(this->get_logger(), "Failed to create multiflex"); - return; + // get serial port of sensor through parameter + rcl_interfaces::msg::ParameterDescriptor serial_port_descriptor = rcl_interfaces::msg::ParameterDescriptor{}; + serial_port_descriptor.description = "Serial port of the USB port that the multiflex PCB is connected to."; + this->declare_parameter("multiflex_serial_port", "/dev/ttyACM0", serial_port_descriptor); + + // create the sensor object + factory = terabee::ITerarangerFactory::getFactory(); + multiflex = factory->createTerarangerMultiflex(this->get_parameter("multiflex_serial_port").as_string()); + + if (!multiflex) // check if the object can be created + { + RCLCPP_ERROR(this->get_logger(), "Failed to create multiflex"); + return; + } + + if (!multiflex->initialize()) // check if communication with the sensor works + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize multiflex"); + return; + } + + if (!multiflex->configureNumberOfSensors(0x3f)) // check if all 6 distance sensors work + { + RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 6!"); + return; + } + + 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)); } - 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"); - - - 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() - { - terabee::DistanceData data = multiflex->getDistance(); - auto msg = object_detection::msg::MultiflexReading(); - for (size_t i = 0; i < data.size(); i++) + // factory and object for multiflex sensor + std::unique_ptr factory; + std::unique_ptr multiflex; + + // timer and publisher for publishing message onto topic + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + + /** + * @brief Reads distance data from the sensor and publishes it onto the drone/object_detection topic + * + */ + void read_multiflex_data() { - msg.distance_data[i] = data.distance[i]; + // get distance reading + terabee::DistanceData data = multiflex->getDistance(); + + // populate message with readings + auto msg = object_detection::msg::MultiflexReading(); + for (size_t i = 0; i < data.size(); i++) + { + msg.distance_data[i] = data.distance[i]; + } + + // publish message + publisher_->publish(msg); } - 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[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; }