Merge pull request #5 from SemvdH/service

merge Service branch into main

.vscode/c_cpp_properties.json

@@ -11,7 +11,8 @@
                 "/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/terabee_api/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",

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

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5)
+project(drone_services)
+
+# 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(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/SetAttitude.srv"
+  "srv/SetVelocity.srv"  
+)
+
+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_services/package.xml

@@ -0,0 +1,23 @@
+<?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_services</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>
+    <buildtool_depend>rosidl_default_generators</buildtool_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>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

src/drone_services/srv/SetAttitude.srv

@@ -0,0 +1,9 @@
+#all angles are in degrees
+float32 yaw
+float32 pitch
+float32 roll
+
+#thrust between -1 and 1
+float32 thrust
+---
+int8 status

src/drone_services/srv/SetVelocity.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

src/height/src/height_reader.cpp

@@ -1,5 +1,10 @@
+/**
+ * @file height_reader.cpp
+ * @author Sem van der Hoeven (sem.hoeven@gmail.com)
+ * @brief Uses the Terabee API to read the Terabee Evo Mini
+ * height sensor data and publishes it on a /drone/height topic
+ */
 #include <chrono>
-#include <iostream>
 
 #include "rclcpp/rclcpp.hpp"
 #include "height/msg/height_data.hpp"
@@ -13,18 +18,6 @@ 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:
@@ -59,12 +52,18 @@ public:
     }
 
 private:
+    /**
+     * @brief reads the height value from the Terabee Evo Mini sensor and publishes a
+     *
+     */
     void read_height()
     {
         auto msg = height::msg::HeightData();
 
+        // high initial minimal value
         float min = 255;
         terabee::DistanceData distance_data = evo_mini->getDistance();
+        // add readings and calculate mimimum value
         for (size_t i = 0; i < distance_data.size(); i++)
         {
             msg.heights[i] = distance_data.distance[i];
@@ -73,16 +72,21 @@ private:
                 min = distance_data.distance[i];
             }
         }
+        // add minimum value and publish message
         msg.min_height = min;
         publisher_->publish(msg);
 
         RCLCPP_INFO(this->get_logger(), "publishing message with min distance %f", msg.min_height);
     }
 
+    // timer for publishing height readings
     rclcpp::TimerBase::SharedPtr timer_;
+    // publisher for height readings
     rclcpp::Publisher<height::msg::HeightData>::SharedPtr publisher_;
 
+    // factory for height sensor
     std::unique_ptr<terabee::ITerarangerFactory> factory;
+    // height sensor object pointer
     std::unique_ptr<terabee::ITerarangerEvoMini> evo_mini;
 };
 

src/object_detection/src/lidar_reader.cpp

@@ -18,62 +18,34 @@ 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")
     {
-
+        // get serial port of sensor through parameter
         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);
+        // create publisher and bind timer to publish function
+        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)
+        if (!tower) // check if the object could be created
         {
             RCLCPP_ERROR(this->get_logger(), "Failed to create TerarangerTowerEvo");
             return;
         }
 
-    tower->setImuMode(mode);
+        // set lidar to measure including IMU
+        tower->setImuMode(ITerarangerTowerEvo::QuaternionLinearAcc);
 
-    if (!tower->initialize())
+        if (!tower->initialize()) // check if communication with the sensor works
         {
             RCLCPP_ERROR(this->get_logger(), "Failed to initialize TerarangerTowerEvo");
             return;
@@ -81,35 +53,40 @@ public:
     }
 
 private:
-  void read_lidar_data()
+    // publisher for lidar data
-  {
+    rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_;
-    //TODO publish message with all data from lidar
+    // timer for publishing readings
-    // 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;
+
+    /**
+     * @brief Reads the data from the LIDAR distance modules and the IMU, and publishes it onto the drone/object_detection topic
+     * 
+     */
+    void read_lidar_data()
+    {
+        auto msg = object_detection::msg::LidarReading();
+
+        // read distance data from all sensors
+        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);
+
+        // read data from built-in IMU
+        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]);
+        }
+
+        // publish message
+        publisher_->publish(msg);
+        RCLCPP_INFO(this->get_logger(), "Publishing message");
+    }
 };
 
 int main(int argc, char *argv[])

src/object_detection/src/multiflex_reader.cpp

@@ -21,26 +21,28 @@ public:
     MultiflexReader()
         : Node("multiflex_reader")
     {
+        // get serial port of sensor through parameter
         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);
 
+        // create the sensor object
         factory = terabee::ITerarangerFactory::getFactory();
         multiflex = factory->createTerarangerMultiflex(this->get_parameter("multiflex_serial_port").as_string());
 
-    if (!multiflex)
+        if (!multiflex) // check if the object can be created
         {
             RCLCPP_ERROR(this->get_logger(), "Failed to create multiflex");
             return;
         }
 
-    if (!multiflex->initialize())
+        if (!multiflex->initialize()) // check if communication with the sensor works
         {
             RCLCPP_ERROR(this->get_logger(), "Failed to initialize multiflex");
             return;
         }
 
-    if (!multiflex->configureNumberOfSensors(0x3f))
+        if (!multiflex->configureNumberOfSensors(0x3f)) // check if all 6 distance sensors work
         {
             RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 6!");
             return;
@@ -48,28 +50,38 @@ public:
 
         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:
+    // factory and object for multiflex sensor
+    std::unique_ptr<terabee::ITerarangerFactory> factory;
+    std::unique_ptr<terabee::ITerarangerMultiflex> multiflex;
+
+    // timer and publisher for publishing message onto topic
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Publisher<object_detection::msg::MultiflexReading>::SharedPtr publisher_;
+
+    /**
+     * @brief Reads distance data from the sensor and publishes it onto the drone/object_detection topic
+     *
+     */
     void read_multiflex_data()
     {
+        // get distance reading
         terabee::DistanceData data = multiflex->getDistance();
+
+        // populate message with readings
         auto msg = object_detection::msg::MultiflexReading();
         for (size_t i = 0; i < data.size(); i++)
         {
             msg.distance_data[i] = data.distance[i];
         }
+
+        // publish message
         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[])

src/px4_connection/CMakeLists.txt

@@ -0,0 +1,73 @@
+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(drone_services REQUIRED)
+find_package(std_srvs REQUIRED)
+
+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
+    drone_services
+    std_srvs
+)
+
+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,24 @@
+<?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>
+
+    <depend>rclcpp</depend>
+    <depend>px4_ros_com</depend>
+    <depend>px4_msgs</depend>
+    <depend>drone_services</depend>
+    <depend>std_srvs</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,62 @@
+/**
+ * @file heartbeat.cpp
+ * @author Sem van der Hoeven (sem.hoeven@gmail.com)
+ * @brief Heartbeat node that keeps the connection between the flight computer 
+ * and PX4 flight controller alive by sending OffboardControl messages
+ */
+#include <chrono>
+#include "rclcpp/rclcpp.hpp"
+#include <px4_msgs/msg/offboard_control_mode.hpp>
+
+using namespace std::chrono_literals;
+
+class HeartBeat : public rclcpp::Node
+{
+public:
+    HeartBeat() : Node("heartbeat")
+    {
+        // 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:
+    // publisher for offboard control mode messages
+    rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
+    // timer for sending the heartbeat
+    rclcpp::TimerBase::SharedPtr timer_;
+    // start time of node in seconds
+    double start_time;
+
+    /**
+     * @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 = false;
+        msg.acceleration = false;
+        msg.attitude = true;
+        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);
+    }
+};
+
+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,355 @@
+/**
+ * @file px4_controller.cpp
+ * @author Sem van der Hoeven (sem.hoeven@gmail.com)
+ * @brief Controller node to contol the PX4 using attitude or trajectory setpoints. 
+ * It subscribes to the /drone/set_attitude topic to receive control commands
+ */
+
+#include <chrono>
+#include <math.h>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <px4_msgs/msg/vehicle_attitude_setpoint.hpp>
+#include <px4_msgs/msg/trajectory_setpoint.hpp>
+#include <px4_msgs/msg/vehicle_command.hpp>
+#include <px4_msgs/msg/vehicle_control_mode.hpp>
+#include <px4_msgs/msg/vehicle_attitude.hpp>
+
+#include <drone_services/srv/set_attitude.hpp>
+
+#include <std_srvs/srv/empty.hpp>
+
+#define D_SPEED(x) -x - 9.81
+
+using namespace std::chrono_literals;
+
+class PX4Controller : public rclcpp::Node
+{
+public:
+    PX4Controller() : Node("px4_controller")
+    {
+
+        rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
+        auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
+
+        // 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);
+
+        vehicle_attitude_subscription_ = this->create_subscription<px4_msgs::msg::VehicleAttitude>("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this, std::placeholders::_1));
+
+        set_attitude_service_ = this->create_service<drone_services::srv::SetAttitude>("drone/set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+        disarm_service_ = this->create_service<std_srvs::srv::Empty>("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_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::Subscription<px4_msgs::msg::VehicleAttitude>::SharedPtr vehicle_attitude_subscription_;
+
+    rclcpp::Service<drone_services::srv::SetAttitude>::SharedPtr set_attitude_service_;
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr disarm_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 armed = false;
+    bool has_swithed = false;
+    int setpoint_count = 0;
+    float thrust = 0.0;
+    bool ready_to_fly = false;
+    float cur_yaw = 0;
+    bool new_setpoint = false; // for printing new q_d when a new setpoint has been received
+
+    float last_setpoint[3] = {0, 0, 0};
+    float last_angle = 0;
+    float last_thrust = 0;
+
+    float base_q[4] = {0, 0, 0, 0};
+    int base_q_amount = 0;
+
+    // result quaternion
+    std::array<float, 4> q = {0, 0, 0, 0};
+
+    void set_setpoint(
+        const std::shared_ptr<rmw_request_id_t> request_header,
+        const std::shared_ptr<drone_services::srv::SetAttitude::Request> request,
+        const std::shared_ptr<drone_services::srv::SetAttitude::Response> response)
+    {
+        if (armed)
+        {
+            if (request->yaw == 0 && request->pitch == 0 && request->roll == 0 && request->thrust == 0)
+            {
+                last_setpoint[0] = degrees_to_radians(request->yaw);
+                last_setpoint[1] = degrees_to_radians(request->pitch);
+                last_setpoint[2] = degrees_to_radians(request->roll);
+                last_thrust = request->thrust;
+                RCLCPP_INFO(this->get_logger(), "STOPPING MOTORS");
+            }
+            else
+            {
+                last_setpoint[0] += degrees_to_radians(request->yaw);
+                last_setpoint[1] += degrees_to_radians(request->pitch);
+                last_setpoint[2] += degrees_to_radians(request->roll);
+                last_thrust += request->thrust;
+                if (last_thrust > 1)
+                    last_thrust = 1;
+                else if (last_thrust < 0)
+                    last_thrust = 0;
+            }
+
+            RCLCPP_INFO(this->get_logger(), "got values: yaw:%f pitch:%f roll:%f thrust:%f", request->yaw, request->pitch, request->roll, request->thrust);
+            RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust);
+            new_setpoint = true;
+
+            response->status = 0;
+        }
+        else
+        {
+            last_thrust = 0;
+            last_setpoint[0] = 0;
+            last_setpoint[1] = 0;
+            last_setpoint[2] = 0;
+            response->status = 1;
+        }
+    }
+
+    /**
+     * @brief disarms the drone when a call to the disarm service is made
+     *
+     * @param request_header the header for the service request
+     * @param request the request (empty)
+     * @param response the response (empty)
+     */
+    void handle_disarm_request(
+        const std::shared_ptr<rmw_request_id_t> request_header,
+        const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+        const std::shared_ptr<std_srvs::srv::Empty::Response> response)
+    {
+        RCLCPP_INFO(this->get_logger(), "Got disarm request...");
+
+        if (armed)
+        {
+            armed = false;
+            publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0);
+        }
+    }
+
+    void send_trajectory_setpoint()
+    {
+
+        auto msg = px4_msgs::msg::TrajectorySetpoint();
+        if (this->get_clock()->now().seconds() - start_time_ < 10)
+        {
+            msg.acceleration[0] = 0;
+            msg.acceleration[1] = 0;
+            msg.acceleration[2] = D_SPEED(10);
+            msg.yawspeed = 0;
+            msg.yaw = -3.14;
+        }
+        else
+        {
+            if (!has_swithed)
+            {
+                RCLCPP_INFO(this->get_logger(), "waiting for service input...");
+                has_swithed = true;
+            }
+
+            msg.acceleration[0] = last_setpoint[0];
+            msg.acceleration[1] = last_setpoint[1];
+            msg.acceleration[2] = D_SPEED(last_setpoint[2]);
+            msg.yawspeed = 0.5;
+        }
+
+        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+
+        trajectory_setpoint_publisher->publish(msg);
+    }
+
+    void send_attitude_setpoint()
+    {
+
+        // set message to enable attitude
+        auto msg = px4_msgs::msg::VehicleAttitudeSetpoint();
+
+        // if (this->get_clock()->now().seconds() - start_time_ < 30)
+        // {
+        // msg.yaw_body = last_setpoint[0];
+        // msg.pitch_body = last_setpoint[1];
+        // msg.roll_body = last_setpoint[2];
+        // move up?
+        msg.thrust_body[0] = 0; // north
+        msg.thrust_body[1] = 0; // east
+        msg.thrust_body[2] = -last_thrust;
+
+        calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]);
+        // RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f", q.at(0), q.at(1), q.at(2), last_thrust);
+        // }
+        // 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");
+        //     }
+        // }
+
+        // set quaternion
+        msg.q_d[0] = q.at(0);
+        msg.q_d[1] = base_q[1] + q.at(1);
+        msg.q_d[2] = base_q[2] + q.at(2);
+        msg.q_d[3] = base_q[3] + q.at(3);
+
+        if (new_setpoint)
+        {
+            RCLCPP_INFO(this->get_logger(), "q_d: [%f, %f, %f, %f]", msg.q_d[0], msg.q_d[1], msg.q_d[2], msg.q_d[3]);
+            new_setpoint = false;
+        }
+
+        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;
+            armed = true;
+        }
+
+        send_attitude_setpoint();
+    }
+
+    void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg)
+    {
+        /*
+        average q:
+        - 0.7313786745071411
+        - 0.005042835604399443
+        - 0.0002370707516092807
+        - 0.6819528937339783
+        */
+        if (!armed)
+        {
+            if (base_q_amount > 2)
+            {
+                base_q[1] = (base_q[1] + msg->q[1]) / 2;
+                base_q[2] = (base_q[2] + msg->q[2]) / 2;
+                base_q[3] = (base_q[3] + msg->q[3]) / 2;
+            } else {
+                base_q[1] = msg->q[1];
+                base_q[2] = msg->q[2];
+                base_q[3] = msg->q[3];
+                base_q_amount++;
+            }
+
+            RCLCPP_INFO(this->get_logger(), "base_q: %f %f %f %f", base_q[0], base_q[1], base_q[2], base_q[3]);
+        }
+    }
+
+    /**
+     * @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/test_controls/package.xml

@@ -0,0 +1,21 @@
+<?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>test_controls</name>
+  <version>0.0.0</version>
+  <description>test controls</description>
+  <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>drone_services</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/test_controls/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/test_controls
+[install]
+install-scripts=$base/lib/test_controls

src/test_controls/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'test_controls'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='ubuntu',
+    maintainer_email='semmer99@gmail.com',
+    description='test controls',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'test_controller = test_controls.test_controller:main'
+        ],
+    },
+)

src/test_controls/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/test_controls/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/test_controls/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/test_controls/test_controls/test_controller.py

@@ -0,0 +1,118 @@
+import rclpy
+from rclpy.node import Node
+
+from sshkeyboard import listen_keyboard_manual
+import asyncio
+
+from drone_services.srv import SetAttitude
+
+
+class TestController(Node):
+
+    def __init__(self):
+        super().__init__('test_controller')
+        self.cli = self.create_client(SetAttitude, 'drone/set_attitude')
+        while not self.cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('service not available, waiting again...')
+        self.req = SetAttitude.Request()
+
+        self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nN - emergency stop\nEsc - exit")
+
+    def spin(self):
+        while rclpy.ok():
+            asyncio.run(listen_keyboard_manual(on_press=self.on_press))
+            rclpy.spin_once(self, timeout_sec=0.1)
+
+    def send_request(self, yaw, pitch, roll, thrust):
+        self.req.yaw = yaw
+        self.req.pitch = pitch
+        self.req.roll = roll
+        self.req.thrust = thrust
+        self.get_logger().info('set request to %f %f %f %f' % (yaw, pitch, roll, thrust))
+        self.future = self.cli.call_async(self.req)
+        rclpy.spin_until_future_complete(self, self.future)
+        self.get_logger().info('publishing message on service')
+        return self.future.result()
+
+    def on_release(self, key):
+        # self.get_logger().info('released ' + str(key))
+        pass
+
+    def on_press(self, key):
+        try:
+            # self.get_logger().info('pressed ' + char)
+            if key == 'w':
+                self.get_logger().info('forward')
+                self.send_request(pitch=-10.0, yaw=0.0,
+                                  roll=0.0, thrust=0.0)
+            if key == 's':
+                self.get_logger().info('backward')
+                self.send_request(pitch=10.0, yaw=0.0,
+                                  roll=0.0, thrust=0.0)
+            if key == 'a':
+                self.get_logger().info('left')
+                self.send_request(pitch=0.0, yaw=0.0,
+                                  roll=-10.0, thrust=0.0)
+            if key == 'd':
+                self.get_logger().info('right')
+                self.send_request(pitch=0.0, yaw=0.0,
+                                  roll=10.0, thrust=0.0)
+            if key == 'q':
+                self.get_logger().info('rotate left')
+                self.send_request(pitch=0.0, yaw=-10.0,
+                                  roll=0.0, thrust=0.0)
+            if key == 'e':
+                self.get_logger().info('rotate right')
+                self.send_request(pitch=0.0, yaw=10.0,
+                                  roll=0.0, thrust=0.0)
+            if key == 'z':
+                self.get_logger().info('down')
+                self.send_request(pitch=0.0, yaw=0.0,
+                                  roll=0.0, thrust=-0.05)
+            if key == 'space':
+                self.get_logger().info('up')
+                self.send_request(pitch=0.0, yaw=0.0,
+                                  roll=0.0, thrust=0.05)
+            if key == 'n':
+                self.get_logger().info('stop')
+                self.send_request(pitch=0.0, yaw=0.0,
+                                  roll=0.0, thrust=0.0)
+            # else:
+            #     try:
+            #         # known keys like spacebar, ctrl
+            #         name = key.name
+            #         vk = key.value.vk
+            #     except AttributeError:
+            #         # unknown keys like headphones skip song button
+            #         name = 'UNKNOWN'
+            #         vk = key.vk
+            #     # self.get_logger().info('pressed {} ({})'.format(name, vk))
+            #     if vk == 32:
+            #         self.get_logger().info('up')
+            #         self.send_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.05)
+            #     if vk == 65505:
+            #         self.get_logger().info('down')
+            #         self.send_request(pitch=0.0, yaw=0.0,
+            #                           roll=0.0, thrust=-0.05)
+
+        except Exception as e:
+            self.get_logger().error(str(e))
+            raise e
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    test_controller = TestController()
+
+    test_controller.spin()
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    test_controller.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()