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

add putting reading values in message

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-20 13:21:54 +02:00
parent 988c546f15
commit 091fe9a4ee
1 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -78,17 +78,24 @@ private:
void read_lidar_data() void read_lidar_data()
{ {
//TODO publish message with all data from lidar //TODO publish message with all data from lidar
std::cout << "Distance = " << tower->getDistance() << std::endl; // std::cout << "Distance = " << tower->getDistance() << std::endl;
std::cout << "IMU = " << tower->getImuData() << std::endl; // std::cout << "IMU = " << tower->getImuData() << std::endl;
auto msg = object_detection::msg::LidarReading(); auto msg = object_detection::msg::LidarReading();
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);
ImuData imu_data = tower->getImuData(); ImuData imu_data = tower->getImuData();
for (size_t i = 0; i < imu_data.data.size(); i++) for (size_t i = 0; i < imu_data.data.size(); i++)
{ {
msg.imu_data.push_back(imu_data.data[i]); msg.imu_data.push_back(imu_data.data[i]);
} }
publisher_->publish(msg);
RCLCPP_INFO(this->get_logger(), "Publishing message");
} }
rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_; rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_;