diff --git a/.gitignore b/.gitignore index 13456571..577388bd 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +.vscode/ build/ install/ log/ diff --git a/src/beacon_positioning/CMakeLists.txt b/src/beacon_positioning/CMakeLists.txt index 8841d161..8ce3803a 100644 --- a/src/beacon_positioning/CMakeLists.txt +++ b/src/beacon_positioning/CMakeLists.txt @@ -31,7 +31,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} ) add_executable(tracker_position src/tracker_position.cpp) -ament_target_dependencies(tracker_position rclcpp std_msgs beacon_positioning) +ament_target_dependencies(tracker_position rclcpp std_msgs + beacon_positioning +) target_link_libraries(tracker_position positioning_systems_api::serial_communication 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/height/CMakeLists.txt b/src/height/CMakeLists.txt new file mode 100644 index 00000000..32394387 --- /dev/null +++ b/src/height/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.5) +project(height) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(TerabeeApi REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(height REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/HeightData.msg" + ) + +add_executable(height_reader src/height_reader.cpp) +target_include_directories(height_reader PUBLIC + $ + $) + + target_link_libraries(height_reader ${TerabeeApi_LIBRARIES}) + target_include_directories(height_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS}) + +ament_target_dependencies( + height_reader + rclcpp + TerabeeApi + height +) + +install(TARGETS height_reader + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/height/msg/HeightData.msg b/src/height/msg/HeightData.msg new file mode 100644 index 00000000..88d28cb5 --- /dev/null +++ b/src/height/msg/HeightData.msg @@ -0,0 +1,2 @@ +float32[4] heights +float32 min_height \ No newline at end of file diff --git a/src/height/package.xml b/src/height/package.xml new file mode 100644 index 00000000..ceb7933a --- /dev/null +++ b/src/height/package.xml @@ -0,0 +1,27 @@ + + + + height + 0.0.0 + package to read Terabee node + ubuntu + Apache License 2.0 + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + rclcpp + + ament_lint_auto + ament_lint_common + + TerabeeApi + TerabeeApi + + + + ament_cmake + + diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp new file mode 100644 index 00000000..d8f70b06 --- /dev/null +++ b/src/height/src/height_reader.cpp @@ -0,0 +1,97 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "height/msg/height_data.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +using terabee::DistanceData; +using terabee::ITerarangerEvoMini; + +std::ostream &operator<<(std::ostream &os, const DistanceData &d) +{ + os << "["; + for (size_t i = 0; i < d.distance.size(); i++) + { + 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)); + publisher_ = this->create_publisher("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++) + { + msg.heights[i] = distance_data.distance[i]; + if (distance_data.distance[i] < min && distance_data.distance[i] >= 0) + { + min = distance_data.distance[i]; + } + } + msg.min_height = min; + publisher_->publish(msg); + + RCLCPP_INFO(this->get_logger(),"publishing message with min distance %f",msg.min_height); + } + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + + std::unique_ptr factory; + std::unique_ptr evo_mini; +}; + +int main(int argc, char **argv) +{ + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/object_detection/CMakeLists.txt b/src/object_detection/CMakeLists.txt index d5009b96..be10d823 100644 --- a/src/object_detection/CMakeLists.txt +++ b/src/object_detection/CMakeLists.txt @@ -31,13 +31,17 @@ rosidl_generate_interfaces(${PROJECT_NAME} ) add_executable(lidar_reader src/lidar_reader.cpp) -ament_target_dependencies(lidar_reader rclcpp object_detection) +ament_target_dependencies(lidar_reader rclcpp + object_detection +) target_link_libraries(lidar_reader ${TerabeeApi_LIBRARIES}) 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) +ament_target_dependencies(multiflex_reader rclcpp + object_detection +) target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES}) target_include_directories(multiflex_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS}) 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