sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add comments and improve code for object_detection package

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-05 16:28:27 +02:00
parent 0a1ef8af3d
commit 44b3f135a9
2 changed files with 79 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
src/object_detection/src/lidar_reader.cpp
View File

@@ -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[])

112
src/object_detection/src/multiflex_reader.cpp
View File

@@ -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<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()
{
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<terabee::ITerarangerFactory> factory;
std::unique_ptr<terabee::ITerarangerMultiflex> multiflex;
// 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::spin(std::make_shared<MultiflexReader>());
rclcpp::shutdown();
return 0;
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MultiflexReader>());
rclcpp::shutdown();
return 0;
}