Revert "add node with basic code to read sensor"
This reverts commit 5f98cb2f10.
This commit is contained in:
@@ -1,81 +1,10 @@
|
|||||||
#include <chrono>
|
#include <cstdio>
|
||||||
#include <iostream>
|
|
||||||
|
|
||||||
#include "rclcpp/rclcpp.hpp"
|
int main(int argc, char ** argv)
|
||||||
|
|
||||||
#include <terabee/ITerarangerFactory.hpp>
|
|
||||||
#include <terabee/ITerarangerEvoMini.hpp>
|
|
||||||
#include <terabee/DistanceData.hpp>
|
|
||||||
|
|
||||||
using namespace std::chrono_literals;
|
|
||||||
|
|
||||||
using terabee::DistanceData;
|
|
||||||
using terabee::ITerarangerEvoMini;
|
|
||||||
|
|
||||||
std::ostream &operator<<(std::ostream &os, const DistanceData &d)
|
|
||||||
{
|
{
|
||||||
os << "[";
|
(void) argc;
|
||||||
for (size_t i = 0; i < d.distance.size(); i++)
|
(void) argv;
|
||||||
{
|
|
||||||
os << d.distance[i] << ", ";
|
|
||||||
}
|
|
||||||
os << "\b\b"
|
|
||||||
<< " ]";
|
|
||||||
return os;
|
|
||||||
}
|
|
||||||
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
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));
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
void read_height()
|
|
||||||
{
|
|
||||||
std::cout << "Distance = " << evo_mini->getDistance() << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
rclcpp::TimerBase::SharedPtr timer_;
|
|
||||||
|
|
||||||
std::unique_ptr<terabee::ITerarangerFactory> factory;
|
|
||||||
std::unique_ptr<terabee::ITerarangerEvoMini> evo_mini;
|
|
||||||
};
|
|
||||||
|
|
||||||
int main(int argc, char **argv)
|
|
||||||
{
|
|
||||||
(void)argc;
|
|
||||||
(void)argv;
|
|
||||||
|
|
||||||
rclcpp::init(argc, argv);
|
|
||||||
|
|
||||||
rclcpp::shutdown();
|
|
||||||
|
|
||||||
|
printf("hello world height package\n");
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user