From 9af542d4adfce0dd13e8d7167a3aa04ecd90cd88 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 09:11:44 +0000 Subject: [PATCH] everything built --- .../src/tracker_position.cpp | 4 +-- src/object_detection/CMakeLists.txt | 6 ++-- src/object_detection/src/lidar_reader.cpp | 28 +++++++++---------- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/beacon_positioning/src/tracker_position.cpp b/src/beacon_positioning/src/tracker_position.cpp index c7f35511..0b4d9f52 100644 --- a/src/beacon_positioning/src/tracker_position.cpp +++ b/src/beacon_positioning/src/tracker_position.cpp @@ -149,7 +149,7 @@ int main(int argc, char **argv) if (tracker_msg.is_valid_position) { RCLCPP_INFO(node->get_logger(), "x = %f, y = %f, z = %f", tracker_msg.tracker_position_xyz.at(0), tracker_msg.tracker_position_xyz.at(1), tracker_msg.tracker_position_xyz.at(2)); - for (int i = 0; i < tracker_msg.anchors_data.size(); i++) + for (size_t i = 0; i < tracker_msg.anchors_data.size(); i++) { RCLCPP_INFO(node->get_logger(), "anchor number= %d, distance = %d, x = %d, y = %d, z = %d", tracker_msg.anchors_data[i].number, tracker_msg.anchors_data[i].distance, tracker_msg.anchors_data[i].pos_x, tracker_msg.anchors_data[i].pos_y, tracker_msg.anchors_data[i].pos_z); } @@ -160,7 +160,7 @@ int main(int argc, char **argv) message.y_pos = tracker_msg.tracker_position_xyz.at(1); message.z_pos = tracker_msg.tracker_position_xyz.at(2); message.anchor_distances = {0, 0, 0, 0}; - for (int i = 0; i < tracker_msg.anchors_data.size(); i++) + for (size_t i = 0; i < tracker_msg.anchors_data.size(); i++) { message.anchor_distances[i] = tracker_msg.anchors_data[i].distance; } diff --git a/src/object_detection/CMakeLists.txt b/src/object_detection/CMakeLists.txt index 9c705f64..be10d823 100644 --- a/src/object_detection/CMakeLists.txt +++ b/src/object_detection/CMakeLists.txt @@ -23,7 +23,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(TerabeeApi REQUIRED) find_package(rosidl_default_generators REQUIRED) -#find_package(object_detection REQUIRED) +find_package(object_detection REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/LidarReading.msg" @@ -32,7 +32,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} add_executable(lidar_reader src/lidar_reader.cpp) ament_target_dependencies(lidar_reader rclcpp -# object_detection + object_detection ) target_link_libraries(lidar_reader ${TerabeeApi_LIBRARIES}) @@ -40,7 +40,7 @@ target_include_directories(lidar_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS}) add_executable(multiflex_reader src/multiflex_reader.cpp) ament_target_dependencies(multiflex_reader rclcpp -# object_detection + object_detection ) target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES}) diff --git a/src/object_detection/src/lidar_reader.cpp b/src/object_detection/src/lidar_reader.cpp index 31647ba8..eaa98bd8 100644 --- a/src/object_detection/src/lidar_reader.cpp +++ b/src/object_detection/src/lidar_reader.cpp @@ -57,7 +57,7 @@ public: this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor); - publisher_ = this->create_publisher("drone/object_detection", 10); + // publisher_ = this->create_publisher("drone/object_detection", 10); timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this)); ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc); @@ -87,24 +87,24 @@ private: // std::cout << "Distance = " << tower->getDistance() << 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); + // 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++) - { - msg.imu_data.push_back(imu_data.data[i]); - } - publisher_->publish(msg); - RCLCPP_INFO(this->get_logger(), "Publishing message"); + // for (size_t i = 0; i < imu_data.data.size(); i++) + // { + // msg.imu_data.push_back(imu_data.data[i]); + // } + // // publisher_->publish(msg); + // RCLCPP_INFO(this->get_logger(), "Publishing message"); } - rclcpp::Publisher::SharedPtr publisher_; + // rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr timer_; // terabee tower evo variables