add comments and improve code for object_detection package

src/object_detection/src/lidar_reader.cpp

@@ -33,21 +33,19 @@ public:
         publisher_ = this->create_publisher<object_detection::msg::LidarReading>("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<object_detection::msg::LidarReading>::SharedPtr publisher_;
+    // timer for publishing readings
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    // terabee tower evo variables
+    std::unique_ptr<terabee::ITerarangerTowerEvo> tower;
+    std::unique_ptr<terabee::ITerarangerFactory> 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<object_detection::msg::LidarReading>::SharedPtr publisher_;
-    // timer for publishing readings
-    rclcpp::TimerBase::SharedPtr timer_;
-
-    // terabee tower evo variables
-    std::unique_ptr<terabee::ITerarangerTowerEvo> tower;
-    std::unique_ptr<terabee::ITerarangerFactory> factory;
 };
 
 int main(int argc, char *argv[])

src/object_detection/src/multiflex_reader.cpp

@@ -18,64 +18,76 @@ using namespace std::chrono_literals;
 class MultiflexReader : public rclcpp::Node
 {
 public:
-  MultiflexReader()
+    MultiflexReader()
-      : Node("multiflex_reader")
+        : 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)
     {
-      RCLCPP_ERROR(this->get_logger(), "Failed to create multiflex");
+        // get serial port of sensor through parameter
-      return;
+        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<object_detection::msg::MultiflexReading>("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<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()
+    // factory and object for multiflex sensor
-  {
+    std::unique_ptr<terabee::ITerarangerFactory> factory;
-    terabee::DistanceData data = multiflex->getDistance();
+    std::unique_ptr<terabee::ITerarangerMultiflex> multiflex;
-    auto msg = object_detection::msg::MultiflexReading();
+
-    for (size_t i = 0; i < data.size(); i++)
+    // timer and publisher for publishing message onto topic
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Publisher<object_detection::msg::MultiflexReading>::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<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[])
 {
-  rclcpp::init(argc, argv);
+    rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<MultiflexReader>());
+    rclcpp::spin(std::make_shared<MultiflexReader>());
-  rclcpp::shutdown();
+    rclcpp::shutdown();
-  return 0;
+    return 0;
 }