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

add comments and code to lidar node

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-05 16:19:24 +02:00
parent 6a29220bc9
commit 0a1ef8af3d
2 changed files with 57 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,104 +18,80 @@ using terabee::TowerDistanceData;
using namespace std::chrono_literals; using namespace std::chrono_literals;
/* This example creates a subclass of Node and uses std::bind() to register a
* member function as a callback from the timer. */
std::ostream &operator<<(std::ostream &os, const TowerDistanceData &d)
{
os << "[";
for (size_t i = 0; i < d.distance.size(); i++)
{
os << i << " " << d.distance[i] << (d.mask[i] ? " <new>, " : " <old>, ");
}
os << "\b\b"
<< " ]";
return os;
}
std::ostream &operator<<(std::ostream &os, const ImuData &d)
{
os << "[";
for (size_t i = 0; i < d.data.size(); i++)
{
os << d.data[i] << ", ";
}
os << "\b\b"
<< " ]";
return os;
}
class LidarReader : public rclcpp::Node class LidarReader : public rclcpp::Node
{ {
public: public:
LidarReader() LidarReader()
: Node("lidar_reader") : Node("lidar_reader")
{
rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{};
descriptor.description = "serial port for the USB port of the LIDAR";
this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor);
// 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));
ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc);
factory = terabee::ITerarangerFactory::getFactory();
tower = factory->createTerarangerTowerEvo(this->get_parameter("lidar_serial_port").as_string());
if (!tower)
{ {
RCLCPP_ERROR(this->get_logger(), "Failed to create TerarangerTowerEvo"); // get serial port of sensor through parameter
return; rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{};
} descriptor.description = "serial port for the USB port of the LIDAR";
this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor);
tower->setImuMode(mode); // create publisher and bind timer to publish function
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));
if (!tower->initialize()) // set lidar to measure including IMU
{ ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc);
RCLCPP_ERROR(this->get_logger(), "Failed to initialize TerarangerTowerEvo");
return; factory = terabee::ITerarangerFactory::getFactory();
tower = factory->createTerarangerTowerEvo(this->get_parameter("lidar_serial_port").as_string());
if (!tower)
{
RCLCPP_ERROR(this->get_logger(), "Failed to create TerarangerTowerEvo");
return;
}
tower->setImuMode(mode);
if (!tower->initialize())
{
RCLCPP_ERROR(this->get_logger(), "Failed to initialize TerarangerTowerEvo");
return;
}
} }
}
private: private:
void read_lidar_data() void read_lidar_data()
{ {
//TODO publish message with all data from lidar auto msg = object_detection::msg::LidarReading();
// std::cout << "Distance = " << tower->getDistance() << std::endl;
// std::cout << "IMU = " << tower->getImuData() << std::endl;
// auto msg = object_detection::msg::LidarReading(); // read distance data from all sensors
msg.sensor_1 = tower->getDistance().distance.at(0);
msg.sensor_2 = tower->getDistance().distance.at(2);
msg.sensor_3 = tower->getDistance().distance.at(4);
msg.sensor_4 = tower->getDistance().distance.at(6);
// msg.sensor_1 = tower->getDistance().distance.at(0); // read data from built-in IMU
// msg.sensor_2 = tower->getDistance().distance.at(2); ImuData imu_data = tower->getImuData();
// msg.sensor_3 = tower->getDistance().distance.at(4);
// msg.sensor_4 = tower->getDistance().distance.at(6);
// 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]);
}
// for (size_t i = 0; i < imu_data.data.size(); i++) // publish message
// { publisher_->publish(msg);
// msg.imu_data.push_back(imu_data.data[i]); RCLCPP_INFO(this->get_logger(), "Publishing message");
// } }
// // publisher_->publish(msg);
// RCLCPP_INFO(this->get_logger(), "Publishing message");
}
// rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_; // publisher for lidar data
rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_;
// timer for publishing readings
rclcpp::TimerBase::SharedPtr timer_;
// terabee tower evo variables // terabee tower evo variables
std::unique_ptr<terabee::ITerarangerTowerEvo> tower; std::unique_ptr<terabee::ITerarangerTowerEvo> tower;
std::unique_ptr<terabee::ITerarangerFactory> factory; std::unique_ptr<terabee::ITerarangerFactory> factory;
}; };
int main(int argc, char *argv[]) int main(int argc, char *argv[])
{ {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<LidarReader>()); rclcpp::spin(std::make_shared<LidarReader>());
rclcpp::shutdown(); rclcpp::shutdown();
return 0; return 0;
} }

2
src/px4_connection/src/heartbeat.cpp
View File

@@ -5,9 +5,7 @@
* and PX4 flight controller alive by sending OffboardControl messages * and PX4 flight controller alive by sending OffboardControl messages
*/ */
#include <chrono> #include <chrono>
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include <px4_msgs/msg/offboard_control_mode.hpp> #include <px4_msgs/msg/offboard_control_mode.hpp>
using namespace std::chrono_literals; using namespace std::chrono_literals;