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

add comments

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-05 12:57:48 +02:00
parent 07f0202d51
commit 6a29220bc9
3 changed files with 86 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
src/height/src/height_reader.cpp
View File

@@ -1,5 +1,10 @@
/**
* @file height_reader.cpp
* @author Sem van der Hoeven (sem.hoeven@gmail.com)
* @brief Uses the Terabee API to read the Terabee Evo Mini
* height sensor data and publishes it on a /drone/height topic
*/
#include <chrono>
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "height/msg/height_data.hpp"
@@ -16,70 +21,81 @@ using terabee::ITerarangerEvoMini;
class HeightReader : public rclcpp::Node
{
public:
HeightReader() : rclcpp::Node("height_reader")
{
rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{};
descriptor.description = "serial port for the USB port of the height sensor";
this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor);
factory = terabee::ITerarangerFactory::getFactory();
evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string());
if (!evo_mini)
HeightReader() : rclcpp::Node("height_reader")
{
RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!");
return;
rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{};
descriptor.description = "serial port for the USB port of the height sensor";
this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor);
factory = terabee::ITerarangerFactory::getFactory();
evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string());
if (!evo_mini)
{
RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!");
return;
}
evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode);
if (!evo_mini->initialize())
{
RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!");
return;
}
RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!");
timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this));
publisher_ = this->create_publisher<height::msg::HeightData>("drone/height", 10);
}
evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode);
if (!evo_mini->initialize())
{
RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!");
return;
}
RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!");
timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this));
publisher_ = this->create_publisher<height::msg::HeightData>("drone/height", 10);
}
private:
void read_height()
{
auto msg = height::msg::HeightData();
float min = 255;
terabee::DistanceData distance_data = evo_mini->getDistance();
for (size_t i = 0; i < distance_data.size(); i++)
/**
* @brief reads the height value from the Terabee Evo Mini sensor and publishes a
*
*/
void read_height()
{
msg.heights[i] = distance_data.distance[i];
if (distance_data.distance[i] < min && distance_data.distance[i] >= 0)
{
min = distance_data.distance[i];
}
auto msg = height::msg::HeightData();
// high initial minimal value
float min = 255;
terabee::DistanceData distance_data = evo_mini->getDistance();
// add readings and calculate mimimum value
for (size_t i = 0; i < distance_data.size(); i++)
{
msg.heights[i] = distance_data.distance[i];
if (distance_data.distance[i] < min && distance_data.distance[i] >= 0)
{
min = distance_data.distance[i];
}
}
// add minimum value and publish message
msg.min_height = min;
publisher_->publish(msg);
RCLCPP_INFO(this->get_logger(), "publishing message with min distance %f", msg.min_height);
}
msg.min_height = min;
publisher_->publish(msg);
RCLCPP_INFO(this->get_logger(),"publishing message with min distance %f",msg.min_height);
}
// timer for publishing height readings
rclcpp::TimerBase::SharedPtr timer_;
// publisher for height readings
rclcpp::Publisher<height::msg::HeightData>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<height::msg::HeightData>::SharedPtr publisher_;
std::unique_ptr<terabee::ITerarangerFactory> factory;
std::unique_ptr<terabee::ITerarangerEvoMini> evo_mini;
// factory for height sensor
std::unique_ptr<terabee::ITerarangerFactory> factory;
// height sensor object pointer
std::unique_ptr<terabee::ITerarangerEvoMini> evo_mini;
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<HeightReader>());
rclcpp::shutdown();
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<HeightReader>());
rclcpp::shutdown();
return 0;
return 0;
}