Merge branch 'main' into send_setpoints

.gitignore

@@ -1,3 +1,4 @@
+.vscode/
 build/
 install/
 log/

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

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;
       }

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+  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()

src/height/msg/HeightData.msg

@@ -0,0 +1,2 @@
+float32[4] heights
+float32 min_height

src/height/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>height</name>
+  <version>0.0.0</version>
+  <description>package to read Terabee node</description>
+  <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <build_depend>TerabeeApi</build_depend>
+  <exec_depend>TerabeeApi</exec_depend>
+  
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/height/src/height_reader.cpp

@@ -0,0 +1,97 @@
+#include <chrono>
+#include <iostream>
+
+#include "rclcpp/rclcpp.hpp"
+#include "height/msg/height_data.hpp"
+
+#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 << "[";
+  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<height::msg::HeightData>("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<height::msg::HeightData>::SharedPtr publisher_;
+
+  std::unique_ptr<terabee::ITerarangerFactory> factory;
+  std::unique_ptr<terabee::ITerarangerEvoMini> evo_mini;
+};
+
+int main(int argc, char **argv)
+{
+
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<HeightReader>());
+  rclcpp::shutdown();
+
+  return 0;
+}

src/object_detection/CMakeLists.txt

@@ -27,19 +27,36 @@ find_package(object_detection REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/LidarReading.msg"
+  "msg/MultiflexReading.msg"
  )
 
 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
+)
+
+target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES})
+target_include_directories(multiflex_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS})
+
 install(TARGETS
   lidar_reader
+  multiflex_reader
   DESTINATION lib/${PROJECT_NAME}
 )
 
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

src/object_detection/launch/object_detection_launch_both.py

@@ -0,0 +1,23 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package="object_detection",
+            executable="multiflex_reader",
+            name="multiflex_reader",
+            parameters=[
+                {"multiflex_serial_port": "/dev/ttyACM0"}
+            ]
+        ),
+        Node(
+            package="object_detection",
+            executable="lidar_reader",
+            name="lidar_reader",
+            parameters=[
+                {"lidar_serial_port": "/dev/ttyUSB0"} # TODO test
+            ]
+        )
+
+    ])

src/object_detection/launch/object_detection_launch_multiflex.py

@@ -0,0 +1,15 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package="object_detection",
+            executable="multiflex_reader",
+            name="multiflex_reader",
+            parameters=[
+                {"multiflex_serial_port": "/dev/ttyACM0"}
+            ]
+        )
+
+    ])

src/object_detection/msg/MultiflexReading.msg

@@ -0,0 +1 @@
+float32[6] distance_data

src/object_detection/src/lidar_reader.cpp

@@ -51,13 +51,19 @@ public:
   LidarReader()
       : Node("lidar_reader")
   {
-    publisher_ = this->create_publisher<object_detection::msg::LidarReading>("drone/object_detection", 10);
+
+    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("/dev/ttyACM0");
+    tower = factory->createTerarangerTowerEvo(this->get_parameter("lidar_serial_port").as_string());
 
     if (!tower)
     {
@@ -81,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<object_detection::msg::LidarReading>::SharedPtr publisher_;
+  // rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_;
   rclcpp::TimerBase::SharedPtr timer_;
 
   // terabee tower evo variables

src/object_detection/src/multiflex_reader.cpp

@@ -0,0 +1,81 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <iostream>
+
+#include "rclcpp/rclcpp.hpp"
+#include "object_detection/msg/multiflex_reading.hpp"
+
+#include <terabee/ITerarangerFactory.hpp>
+#include <terabee/ITerarangerMultiflex.hpp>
+#include <terabee/DistanceData.hpp>
+
+using terabee::DistanceData;
+
+using namespace std::chrono_literals;
+
+class MultiflexReader : public rclcpp::Node
+{
+public:
+  MultiflexReader()
+      : Node("multiflex_reader")
+  {
+    rcl_interfaces::msg::ParameterDescriptor serial_port_descriptor = rcl_interfaces::msg::ParameterDescriptor{};
+    serial_port_descriptor.description = "Serial port of the USB port that the multiflex PCB is connected to.";
+    this->declare_parameter("multiflex_serial_port", "/dev/ttyACM0", serial_port_descriptor);
+
+    factory = terabee::ITerarangerFactory::getFactory();
+    multiflex = factory->createTerarangerMultiflex(this->get_parameter("multiflex_serial_port").as_string());
+
+    if (!multiflex)
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to create multiflex");
+      return;
+    }
+
+    if (!multiflex->initialize())
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to initialize multiflex");
+      return;
+    }
+
+    if (!multiflex->configureNumberOfSensors(0x3f))
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 6!");
+      return;
+    }
+
+    RCLCPP_INFO(this->get_logger(), "Multiflex initialized");
+
+
+    publisher_ = this->create_publisher<object_detection::msg::MultiflexReading>("drone/object_detection", 10);
+    timer_ = this->create_wall_timer(500ms, std::bind(&MultiflexReader::read_multiflex_data, this));
+  }
+
+private:
+  void read_multiflex_data()
+  {
+    terabee::DistanceData data = multiflex->getDistance();
+    auto msg = object_detection::msg::MultiflexReading();
+    for (size_t i = 0; i < data.size(); i++)
+    {
+      msg.distance_data[i] = data.distance[i];
+    }
+    publisher_->publish(msg);
+  }
+
+  std::unique_ptr<terabee::ITerarangerFactory> factory;
+  std::unique_ptr<terabee::ITerarangerMultiflex> multiflex;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<object_detection::msg::MultiflexReading>::SharedPtr publisher_;
+};
+
+int main(int argc, char *argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<MultiflexReader>());
+  rclcpp::shutdown();
+  return 0;
+}