test

merge Heigth sensor into main

.gitignore

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

.vscode/c_cpp_properties.json

@@ -10,7 +10,9 @@
                 "/home/ubuntu/ros2_ws/src/beacon_positioning/include/**",
                 "/usr/include/**",
                 "/mnt/Homework/Avans/AFSTUDEERSTAGE/positioning_systems_api/rtls_driver/include/**",
-                "/mnt/Homework/Avans/AFSTUDEERSTAGE/positioning_systems_api/serial_communication/include/**"
+                "/mnt/Homework/Avans/AFSTUDEERSTAGE/positioning_systems_api/serial_communication/include/**",
+                "/mnt/Homework/Avans/AFSTUDEERSTAGE/terabee_api/include/**",
+                "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**"
             ],
             "name": "ROS",
             "intelliSenseMode": "gcc-x64",

.vscode/settings.json

@@ -1,7 +1,66 @@
 {
     "ros.distro": "humble",
     "files.associations": {
-        "chrono": "cpp"
+        "chrono": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "codecvt": "cpp",
+        "compare": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "csignal": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "deque": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "functional": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "memory": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "ostream": "cpp",
+        "ratio": "cpp",
+        "semaphore": "cpp",
+        "shared_mutex": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "stop_token": "cpp",
+        "streambuf": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "thread": "cpp",
+        "type_traits": "cpp",
+        "tuple": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "utility": "cpp",
+        "variant": "cpp"
     },
     "python.autoComplete.extraPaths": [
         "/home/ubuntu/ros2_ws/install/px4_msgs/lib/python3.8/site-packages",

get_build.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+echo "updating git..."
+./fgit.sh
+echo "building package..."
+colcon build --packages-select px4_connection
+#echo "launching controller..."
+#ros2 run px4_connection px4_controller

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/drone_controls/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+project(drone_controls)
+
+# 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)
+
+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/drone_controls/package.xml

@@ -0,0 +1,20 @@
+<?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>drone_controls</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 3.5)
+project(object_detection)
+
+# 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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(TerabeeApi REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+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
+)
+
+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
+  # 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/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/LidarReading.msg

@@ -0,0 +1,6 @@
+float32 sensor_1
+float32 sensor_2
+float32 sensor_3
+float32 sensor_4
+float32[] imu_data
+

src/object_detection/msg/MultiflexReading.msg

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

src/object_detection/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>object_detection</name>
+  <version>0.0.0</version>
+  <description>Package to read the TeraBee lidar and multiflex sensors</description>
+  <maintainer email="semmer99@gmail.com">sem</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>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>TerabeeApi</build_depend>
+  
+  <exec_depend>TerabeeApi</exec_depend>
+  
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/object_detection/src/lidar_reader.cpp

@@ -0,0 +1,121 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <iostream>
+
+#include "rclcpp/rclcpp.hpp"
+#include "object_detection/msg/lidar_reading.hpp"
+
+#include <terabee/ITerarangerFactory.hpp>
+#include <terabee/ITerarangerTowerEvo.hpp>
+#include <terabee/TowerDistanceData.hpp>
+#include <terabee/ImuData.hpp>
+
+using terabee::ImuData;
+using terabee::ITerarangerTowerEvo;
+using terabee::TowerDistanceData;
+
+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
+{
+public:
+  LidarReader()
+      : 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");
+      return;
+    }
+
+    tower->setImuMode(mode);
+
+    if (!tower->initialize())
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to initialize TerarangerTowerEvo");
+      return;
+    }
+  }
+
+private:
+  void read_lidar_data()
+  {
+    //TODO publish message with all data from lidar
+    // std::cout << "Distance = " << tower->getDistance() << std::endl;
+    // std::cout << "IMU = " << tower->getImuData() << std::endl;
+
+    // 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();
+
+    // 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::TimerBase::SharedPtr timer_;
+
+  // terabee tower evo variables
+  std::unique_ptr<terabee::ITerarangerTowerEvo> tower;
+  std::unique_ptr<terabee::ITerarangerFactory> factory;
+};
+
+int main(int argc, char *argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<LidarReader>());
+  rclcpp::shutdown();
+  return 0;
+}

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

src/px4_connection/CMakeLists.txt

@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 3.5)
+project(px4_connection)
+
+# 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(px4_ros_com REQUIRED)
+find_package(px4_msgs REQUIRED)
+find_package(px4_connection REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/SetAttitude.srv"
+)
+
+include_directories(include)
+add_executable(heartbeat src/heartbeat.cpp)
+ament_target_dependencies(
+    heartbeat
+    rclcpp
+    px4_ros_com
+    px4_msgs
+)
+
+add_executable(px4_controller src/px4_controller.cpp)
+ament_target_dependencies(
+    px4_controller
+    rclcpp
+    px4_ros_com
+    px4_msgs
+    px4_connection
+)
+
+target_include_directories(heartbeat PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
+
+target_include_directories(px4_controller PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
+
+install(TARGETS heartbeat px4_controller
+    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
+    # 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/px4_connection/include/attitude_msg_code.hpp

@@ -0,0 +1,8 @@
+#ifndef ATTITUDE_MSG_CODE_HPP
+#define ATTITUDE_MSG_CODE_HPP
+
+#define ATTITUDE_STATUS_OK 0
+#define ATTITUDE_STATUS_ERROR 1
+
+
+#endif

src/px4_connection/launch/px4_controller_heardbeat_launch.py

@@ -0,0 +1,15 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package="px4_connection",
+            executable="heartbeat"
+        ),
+        Node(
+            package="px4_connection",
+            executable="px4_controller"
+        )
+
+    ])

src/px4_connection/package.xml

@@ -0,0 +1,25 @@
+<?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>px4_connection</name>
+    <version>0.0.0</version>
+    <description>Package to communicate with PX4 through the XRCE-DDS bridge</description>
+    <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
+    <license>Apache License 2.0</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+    <buildtool_depend>rosidl_default_generators</buildtool_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>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

src/px4_connection/src/heartbeat.cpp

@@ -0,0 +1,75 @@
+/*
+
+We need to send attitude setpoints to be able to arm the drone:
+https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET
+We need attitude setpoints because we don't have a GPS:
+https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-manual-control-lost/31565/9
+
+*/
+
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <px4_msgs/msg/offboard_control_mode.hpp>
+// #include <px4_msgs/msg/timesync.hpp>
+
+using namespace std::chrono_literals;
+
+class HeartBeat : public rclcpp::Node
+{
+public:
+    HeartBeat() : Node("setpoint_sender")
+    {
+        // create a publisher on the offboard control mode topic
+        offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
+        // create timer to send heartbeat messages (offboard control) every 100ms
+        timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this));
+        start_time = this->get_clock()->now().seconds();
+        RCLCPP_INFO(this->get_logger(), "done initializing at %d seconds. Sending heartbeat...", start_time);
+    }
+
+private:
+
+/**
+ * @brief Publish offboard control mode messages as a heartbeat.
+ *        Only the attitude is enabled, because that is how the drone will be controlled.
+ * 
+ */
+    void send_heartbeat()
+    {
+        // set message to enable attitude
+        auto msg = px4_msgs::msg::OffboardControlMode();
+        msg.position = false;
+        msg.velocity = true;
+        msg.acceleration = false;
+        msg.attitude = false;
+        msg.body_rate = false;
+        msg.actuator = false;
+
+        // get timestamp and publish message
+        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+        offboard_control_mode_publisher_->publish(msg);
+        // RCLCPP_INFO(this->get_logger(), "sent offboard control mode message!");
+
+        // check if 5 seconds have elapsed
+        // if (this->get_clock()->now().seconds() - start_time > 5)
+        // {
+        //     RCLCPP_INFO(this->get_logger(), "5 seconds elapsed!");
+        // }
+
+
+        
+    }
+    rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    double start_time;
+};
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<HeartBeat>());
+    rclcpp::shutdown();
+    return 0;
+}

src/px4_connection/src/px4_controller.cpp

@@ -0,0 +1,277 @@
+/*
+
+We need to send attitude setpoints to be able to arm the drone:
+https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET
+We need attitude setpoints because we don't have a GPS:
+https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-manual-control-lost/31565/9
+
+*/
+
+#include <chrono>
+#include <math.h>
+
+#include "rclcpp/rclcpp.hpp"
+// #include "attitude_msg_code.hpp"
+
+#include <px4_msgs/msg/vehicle_attitude_setpoint.hpp>
+#include <px4_msgs/msg/trajectory_setpoint.hpp>
+// #include <px4_msgs/msg/timesync.hpp>
+#include <px4_msgs/msg/vehicle_command.hpp>
+#include <px4_msgs/msg/vehicle_control_mode.hpp>
+
+#include <px4_connection/srv/set_attitude.hpp>
+// #include <px4_msgs/msg/offboard_control_mode.hpp>
+
+#define D_SPEED(x) -x - 9.81
+
+using namespace std::chrono_literals;
+
+class PX4Controller : public rclcpp::Node
+{
+public:
+    PX4Controller() : Node("px4_controller")
+    {
+        // create a publisher on the vehicle attitude setpoint topic
+        vehicle_setpoint_publisher_ = this->create_publisher<px4_msgs::msg::VehicleAttitudeSetpoint>("/fmu/in/vehicle_attitude_setpoint", 10);
+        vehicle_command_publisher_ = this->create_publisher<px4_msgs::msg::VehicleCommand>("/fmu/in/vehicle_command", 10);
+        trajectory_setpoint_publisher = this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
+        // offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
+
+        set_attitude_service_ = this->create_service<px4_connection::srv::SetAttitude>("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3));
+
+        //         service_ptr_ = this->create_service<std_srvs::srv::Empty>(
+        //         "test_service",
+        //         std::bind(&ServiceNode::service_callback, this, _1, _2, _3)
+        // );
+
+        // create timer to send vehicle attitude setpoints every second
+        timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this));
+
+        start_time_ = this->get_clock()->now().seconds();
+        RCLCPP_INFO(this->get_logger(), "finished initializing at %f seconds.", start_time_);
+    }
+
+private:
+    rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr vehicle_setpoint_publisher_;
+    rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher;
+    rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_;
+    rclcpp::Service<px4_connection::srv::SetAttitude>::SharedPtr set_attitude_service_;
+
+    // rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
+
+    rclcpp::TimerBase::SharedPtr timer_;
+    double start_time_;
+    bool has_sent_status = false;
+    bool flying = false;
+    bool has_swithed = false;
+    int setpoint_count = 0;
+    float thrust = 0.5;
+    bool ready_to_fly = false;
+    float cur_yaw = 0;
+
+    float last_setpoint[3] = {0,0,0};
+    float last_angle = 0;
+
+    void set_setpoint(
+        const std::shared_ptr<rmw_request_id_t> request_header,
+        const std::shared_ptr<px4_connection::srv::SetAttitude::Request> request,
+        const std::shared_ptr<px4_connection::srv::SetAttitude::Response> response)
+    {
+        last_setpoint[0] = request->x_speed;
+        last_setpoint[1] = request->y_speed;
+        last_setpoint[2] = request->x_speed;
+
+        last_angle = degrees_to_radians(request->angle);
+
+        response->status = 0;
+    }
+
+    void send_trajectory_setpoint()
+    {
+
+        auto msg = px4_msgs::msg::TrajectorySetpoint();
+        if (this->get_clock()->now().seconds() - start_time_ < 10)
+        {
+            msg.velocity[0] = 0;
+            msg.velocity[1] = 0;
+            msg.velocity[2] = D_SPEED(10);
+            msg.yawspeed = 0;
+            msg.yaw = -3.14;
+        }
+        else
+        {
+            if (!has_swithed)
+            {
+                RCLCPP_INFO(this->get_logger(), "switching to 0 vel");
+                has_swithed = true;
+            }
+
+            msg.velocity[0] = last_setpoint[0];
+            msg.velocity[1] = last_setpoint[1];
+            msg.velocity[2] = D_SPEED(last_setpoint[2]);
+            msg.yawspeed = 0.5;
+            msg.yaw = last_angle;
+        }
+
+        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+
+        trajectory_setpoint_publisher->publish(msg);
+    }
+
+    void send_attitude_setpoint()
+    {
+        if (setpoint_count % 20 == 0 && thrust <= 1)
+        {
+            thrust += 0.1;
+            RCLCPP_INFO(this->get_logger(), "increasing thrust to %f", thrust);
+        }
+
+        // set message to enable attitude
+        auto msg = px4_msgs::msg::VehicleAttitudeSetpoint();
+        // result quaternion
+        std::array<float, 4> q = {0, 0, 0, 0};
+
+        if (this->get_clock()->now().seconds() - start_time_ < 10)
+        {
+            // move up?
+            msg.thrust_body[0] = 0;    // north
+            msg.thrust_body[1] = 0;    // east
+            msg.thrust_body[2] = -0.8; // down, 100% thrust up
+
+            calculate_quaternion(q, 0, 0, 0);
+        }
+
+        else if (this->get_clock()->now().seconds() - start_time_ < 30)
+        {
+            if (!has_swithed)
+            {
+                RCLCPP_INFO(this->get_logger(), "changing to 0.5 thrust");
+                has_swithed = true;
+            }
+
+            msg.thrust_body[0] = 0;    // north
+            msg.thrust_body[1] = 0;    // east
+            msg.thrust_body[2] = -0.5; // down, 100% thrust up
+
+            calculate_quaternion(q, 0, 0, 0);
+        }
+        else
+        {
+            if (flying)
+            {
+                publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0);
+                flying = false;
+                RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds");
+            }
+        }
+
+        msg.q_d[0] = q.at(0);
+        msg.q_d[1] = q.at(1);
+        msg.q_d[2] = q.at(2);
+        msg.q_d[3] = q.at(3);
+
+        msg.yaw_sp_move_rate = 0;
+        msg.reset_integral = false;
+        msg.fw_control_yaw_wheel = false;
+
+        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+        vehicle_setpoint_publisher_->publish(msg);
+    }
+
+    /**
+     * @brief Send setpoints to PX4. First, 20 setpoints will be sent before offboard mode will be engaged and the drone will be armed.
+     *
+     */
+    void send_setpoint()
+    {
+        // increase amount of setpoints sent
+        setpoint_count++;
+
+        ready_to_fly = setpoint_count == 20;
+        // after 20 setpoints, send arm command to make drone actually fly
+        if (ready_to_fly)
+        {
+            // switch to offboard mode and arm
+
+            // set to offboard mode
+            this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
+            RCLCPP_INFO(this->get_logger(), "Set to offboard mode");
+            // arm the drone
+            this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0);
+
+            RCLCPP_INFO(this->get_logger(), "Arm command sent");
+            flying = true;
+        }
+
+        send_trajectory_setpoint();
+    }
+
+    /**
+     * @brief Publish vehicle commands
+     * @param command   Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
+     * @param param1    Command parameter 1
+     * @param param2    Command parameter 2
+     */
+    void publish_vehicle_command(uint16_t command, float param1, float param2)
+    {
+        px4_msgs::msg::VehicleCommand msg{};
+        msg.param1 = param1;
+        msg.param2 = param2;
+        msg.command = command;
+        msg.target_system = 1;
+        msg.target_component = 1;
+        msg.source_system = 1;
+        msg.source_component = 1;
+        msg.from_external = true;
+        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+        vehicle_command_publisher_->publish(msg);
+    }
+
+    /**
+     * @brief calculates a quaternion based on the given input values
+     *
+     * @param ptr array of result quaternion. Must be of size 4.
+     * @param heading desired heading (yaw) in radians.
+     * @param attitude desired attitude (pitch) in radians.
+     * @param bank desired bank (roll) in radians.
+     */
+    static void calculate_quaternion(std::array<float, 4> ptr, float heading, float attitude, float bank)
+    {
+
+        float c1 = cos(heading / 2);
+        float c2 = cos(attitude / 2);
+        float c3 = cos(bank / 2);
+        float s1 = sin(heading / 2);
+        float s2 = sin(attitude / 2);
+        float s3 = sin(bank / 2);
+
+        float c1c2 = c1 * c2;
+        float s1s2 = s1 * s2;
+
+        ptr.at(0) = c1c2 * c3 - s1s2 * s3;       // w
+        ptr.at(1) = c1c2 * s3 + s1s2 * c3;       // x
+        ptr.at(2) = s1 * c2 * c3 + c1 * s2 * s3; // y
+        ptr.at(3) = c1 * s2 * c3 - s1 * c2 * s3; // z
+    }
+
+    /**
+     * @brief converts degrees to radians
+     *
+     * @param deg angle in degrees
+     * @return float new angle in radians
+     */
+    static float degrees_to_radians(float deg)
+    {
+        return deg * (M_PI / 180.0);
+    }
+};
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+
+    rclcpp::Node::SharedPtr node = std::make_shared<PX4Controller>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/px4_connection/srv/SetAttitude.srv

@@ -0,0 +1,9 @@
+#all speeds are in meters/second
+float32 x_speed
+float32 y_speed
+float32 z_speed
+
+#angle is in degrees
+float32 angle
+---
+int8 status