Merge pull request #8 from SemvdH/main

fix some errors

.vscode/c_cpp_properties.json

@@ -12,7 +12,8 @@
                 "/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/**",
-                "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**"
+                "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**",
+                "/home/ubuntu/ros2_ws/src/px4_connection/include/**"
             ],
             "name": "ROS",
             "intelliSenseMode": "gcc-x64",

call_attitude.sh

@@ -0,0 +1 @@
+ros2 service call /drone/set_attitude drone_services/srv/SetAttitude "{pitch: $1, yaw: $2, roll: $3, thrust: $4}"

drone_scripts/find_id.sh

@@ -0,0 +1,32 @@
+#!/bin/bash
+
+lidarserial=00000000001A
+heightserial=369A378F3439
+
+find_by_id(){
+    v=${1%:*}; p=${1#*:}  # split vid:pid into 2 vars
+    v=${v#${v%%[!0]*}}; p=${p#${p%%[!0]*}}  # strip leading zeros
+    grep -il "^PRODUCT=$v/$p" /sys/bus/usb/devices/*:*/uevent |
+    sed s,uevent,, |
+    xargs -r grep -r '^DEVNAME=' --include uevent
+}
+ids=$(find_by_id 0483:5740)
+echo $ids
+
+for device in $ids
+do
+	id=$(echo $device | cut -d "=" -f 2)
+	serial=$(/bin/udevadm info --name=/dev/$id | grep SERIAL_SHORT | cut -d = -f 2)
+	echo "device "$id" has serial number "$serial
+	if [ $serial = $lidarserial ]
+	then
+		echo "LIDAR on port" $id
+		echo $id > /home/ubuntu/drone_conf/lidar.conf
+	elif [ $serial = $heightserial ]
+	then
+		echo "height sensor on port" $id
+		echo $id > /home/ubuntu/drone_conf/height.conf
+	else
+		echo "unknown serial number" $serial
+	fi
+done

drone_scripts/start_height_sensor.sh

@@ -0,0 +1,10 @@
+#!/bin/bash
+
+. /home/ubuntu/source_ros2.sh
+
+SERIAL=$(cat /home/ubuntu/drone_conf/height.conf)
+echo "serial port is "$SERIAL
+echo ros2 run height height_reader --ros-args -p height_serial_port:=/dev/${SERIAL}
+ros2 run height height_reader --ros-args -p height_serial_port:=/dev/${SERIAL} | tee /home/ubuntu/drone_log/height.log
+
+

drone_scripts/start_lidar.sh

@@ -0,0 +1,10 @@
+#!/bin/bash
+
+. /home/ubuntu/source_ros2.sh
+
+SERIAL=$(cat /home/ubuntu/drone_conf/lidar.conf)
+echo "serial port is "$SERIAL
+echo ros2 run object_detection lidar_reader --ros-args -p lidar_serial_port:=/dev/${SERIAL}
+ros2 run object_detection lidar_reader --ros-args -p lidar_serial_port:=/dev/${SERIAL} | tee /home/ubuntu/drone_log/lidar.log
+
+

get_usb_device_port.sh

@@ -0,0 +1 @@
+/bin/udevadm info --name=/dev/ttyACM0 | grep SERIAL_SHORT | cut -d = -f 2

services/drone_find_usb_devices.service

@@ -0,0 +1,12 @@
+[Unit]
+Description=Service to start the script that sets the serial ports for the USB sensors
+
+[Service]
+Type=simple
+Restart=on-failure
+User=ubuntu
+ExecStart=/home/ubuntu/drone_scripts/find_id.sh
+WorkingDirectory=/home/ubuntu
+
+[Install]
+WantedBy=multi-user.target

services/drone_height_sensor.service

@@ -0,0 +1,14 @@
+[Unit]
+Description=Height sensor for drone running in ROS 2
+After=drone_find_usb_devices.service
+
+[Service]
+Type=simple
+Restart=on-failure
+User=ubuntu
+ExecStart=/home/ubuntu/drone_scripts/start_height_sensor.sh
+Environment="HOME=root"
+WorkingDirectory=/home/ubuntu
+
+[Install]
+WantedBy=multi-user.target

services/drone_lidar.service

@@ -0,0 +1,14 @@
+[Unit]
+Description=Height sensor for drone running in ROS 2
+After=drone_find_usb_devices.service
+
+[Service]
+Type=simple
+Restart=on-failure
+User=ubuntu
+ExecStart=/home/ubuntu/drone_scripts/start_lidar.sh
+Environment="HOME=root"
+WorkingDirectory=/home/ubuntu
+
+[Install]
+WantedBy=multi-user.target

src/camera/camera/camera_controller.py

@@ -0,0 +1,50 @@
+import rclpy
+from rclpy.node import Node
+
+from drone_services.srv import TakePicture
+import os
+from datetime import datetime
+
+# import cv2
+
+RES_4K_H = 3496
+RES_4K_W = 4656
+
+
+class CameraController(Node):
+    def __init__(self):
+        super().__init__('camera_controller')
+
+        self.get_logger().info("Camera controller started. Waiting for service call...")
+        self.srv = self.create_service(
+            TakePicture, 'drone/picture', self.take_picture_callback)
+
+    def take_picture_callback(self, request, response):
+        if (request.input_name == "default"):
+            self.get_logger().info("Taking picture with default filename")
+            now = datetime.now().strftime("droneimage_%Y-%m-%d_%H-%M-%S")
+            imagename = "/home/ubuntu/drone_img/" + now + ".jpg"
+            response.filename = imagename
+        else:
+            response.filename = request.input_name
+        os.system('fswebcam -r ' + RES_4K_W + 'x' + RES_4K_H + ' ' + response.filename)
+        self.get_logger().info("Picture saved as " + response.filename)
+
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    test_controller = CameraController()
+    rclpy.spin(test_controller)
+
+    # 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()

src/camera/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>camera</name>
+    <version>0.0.0</version>
+    <description>Package for controlling the camera of the drone</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/camera/setup.cfg

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

src/camera/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'camera'
+
+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='Package for controlling the camera of the drone',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'camera_controller = camera.camera_controller:main'
+        ],
+    },
+)

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

@@ -0,0 +1,46 @@
+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/SetTrajectory.srv"
+  "srv/SetVelocity.srv"  
+  "srv/TakePicture.srv"
+  "srv/SetVehicleControl.srv"
+  "srv/ArmDrone.srv"
+  "srv/DisarmDrone.srv"
+  "srv/ControlRelais.srv"
+  "msg/DroneControlMode.msg"
+)
+
+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/msg/DroneArmStatus.msg

@@ -0,0 +1 @@
+bool armed false

src/drone_services/msg/DroneControlMode.msg

@@ -0,0 +1,5 @@
+# control mode of the drone
+# 1: Attitude
+# 2: Velocity
+# 3: Position
+int8 control_mode

src/drone_services/msg/DroneRouteStatus.msg

@@ -0,0 +1,2 @@
+int32 current_setpoint_index
+float32[5] current_setpoint # x,y,z,angle,take_picture

src/drone_services/msg/DroneStatus.msg

@@ -0,0 +1,5 @@
+float32 battery_percentage
+float32 cpu_usage
+int32 route_setpoint # -1 if no route
+wstring control_mode
+bool armed

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/ArmDrone.srv

@@ -0,0 +1,2 @@
+---
+bool success

src/drone_services/srv/ControlRelais.srv

@@ -0,0 +1,5 @@
+#to turn the relais on or off
+bool relais1_on false
+bool relais2_on false
+---
+int8 bits # relais 1 = bit 0, relais 2 is bit 1

src/drone_services/srv/DisarmDrone.srv

@@ -0,0 +1,2 @@
+---
+bool success

src/drone_services/srv/SetAttitude.srv

@@ -0,0 +1,11 @@
+#service to set attitude setpoints for manual attitude control
+
+#all angles are in degrees
+float32 yaw
+float32 pitch
+float32 roll
+
+#thrust between -1 and 1
+float32 thrust
+---
+bool success

src/drone_services/srv/SetTrajectory.srv

@@ -0,0 +1,7 @@
+# control mode velocity or position, refer to drone_services/msg/DroneControlMode.msg
+int8 control_mode
+# x, y, z values
+float32[3] values
+float32 yaw
+---
+bool success

src/drone_services/srv/SetVehicleControl.srv

@@ -0,0 +1,16 @@
+# service to set control mode for heartbeat messages
+# bitmask for control:
+# bit 0: actuator
+# bit 1: body rate
+# bit 2: attitude
+# bit 3: acceleration
+# bit 4: velocity
+# bit 5: position
+
+# used control modes:
+# 4: attitude and thrust
+# 16: velocity
+# 32: position
+int32 control # control bitmask
+---
+bool success # if operation succeeded

src/drone_services/srv/SetVelocity.srv

@@ -6,4 +6,4 @@ float32 z_speed
 #angle is in degrees
 float32 angle
 ---
-int8 status
+bool success

src/drone_services/srv/TakePicture.srv

@@ -0,0 +1,3 @@
+wstring input_name "default" # name of the input file
+---
+wstring filename # output file name

src/drone_status/drone_status/drone_status.py

@@ -0,0 +1,63 @@
+import rclpy
+from rclpy.node import Node
+
+from drone_services.msg import DroneStatus
+from drone_services.msg import DroneControlMode
+from drone_services.msg import DroneArmStatus
+from drone_services.msg import DroneRouteStatus
+from px4_msgs.msg import BatteryStatus
+from px4_msgs.msg import Cpuload
+
+CONTROL_MODE_ATTITUDE = 1
+CONTROL_MODE_VELOCITY = 2
+CONTROL_MODE_POSITION = 3
+
+class DroneStatus(Node):
+    def __init__(self):
+        super().__init__('drone_status')
+        #publish to drone/status topic
+        self.publisher = self.create_publisher(DroneStatus, '/drone/status', 10)
+        self.control_mode_subscriber = self.create_subscription(DroneControlMode, '/drone/control_mode', self.control_mode_callback, 10)
+        self.arm_status_subscriber = self.create_subscription(DroneArmStatus, '/drone/arm_status', self.arm_status_callback, 10)
+        self.route_status_subscriber = self.create_subscription(DroneRouteStatus, '/drone/route_status', self.route_status_callback, 10)
+        self.battery_status_subscriber = self.create_subscription(BatteryStatus, '/fmu/out/battery_status', self.battery_status_callback, 10)
+        self.cpu_load_subscriber = self.create_subscription(Cpuload, '/fmu/out/cpuload', self.cpu_load_callback, 10)
+        #publish every 0.5 seconds
+        self.timer = self.create_timer(0.5, self.publish_status)
+        self.armed = False
+        self.control_mode = "attitude"
+        self.battery_percentage = 100.0
+        self.cpu_usage = 0.0
+        self.route_setpoint = 0
+    
+    def publish_status(self):
+        msg = DroneStatus()
+        msg.armed = self.armed
+        msg.control_mode = self.control_mode
+        msg.battery_percentage = self.battery_percentage
+        msg.cpu_usage = self.cpu_usage
+        msg.route_setpoint = self.route_setpoint
+        self.publisher.publish(msg)
+        self.get_logger().info('Publishing: "%s"' % msg.status)
+
+    def control_mode_callback(self,msg):
+        if msg.control_mode == CONTROL_MODE_ATTITUDE:
+            self.control_mode = "attitude"
+        elif msg.control_mode == CONTROL_MODE_VELOCITY:
+            self.control_mode = "velocity"
+        elif msg.control_mode == CONTROL_MODE_POSITION:
+            self.control_mode = "position"
+        else:
+            self.control_mode = "unknown"
+
+    def arm_status_callback(self,msg):
+        self.armed = msg.armed
+    
+    def route_status_callback(self,msg):
+        self.route_setpoint = msg.current_setpoint_index
+
+    def battery_status_callback(self, msg):
+        self.battery_percentage = msg.remaining * 100.0
+    
+    def cpu_load_callback(self, msg):
+        self.cpu_usage = msg.load

src/drone_status/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>drone_status</name>
+    <version>0.0.0</version>
+    <description>Package for combining several data points from the drone into a single topic</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>
+    <exec_depend>px4_msgs</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/drone_status/setup.cfg

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

src/drone_status/setup.py

@@ -0,0 +1,25 @@
+from setuptools import setup
+
+package_name = 'drone_status'
+
+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='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)

src/drone_status/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/drone_status/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/drone_status/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/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

@@ -1,8 +1,4 @@
 #include <chrono>
-#include <functional>
-#include <memory>
-#include <string>
-#include <iostream>
 
 #include "rclcpp/rclcpp.hpp"
 #include "object_detection/msg/lidar_reading.hpp"
@@ -18,62 +14,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 +49,40 @@ public:
     }
 
 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_;
+    // publisher for lidar data
+    rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_;
+    // timer for publishing readings
     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

@@ -1,8 +1,4 @@
 #include <chrono>
-#include <functional>
-#include <memory>
-#include <string>
-#include <iostream>
 
 #include "rclcpp/rclcpp.hpp"
 #include "object_detection/msg/multiflex_reading.hpp"
@@ -11,6 +7,8 @@
 #include <terabee/ITerarangerMultiflex.hpp>
 #include <terabee/DistanceData.hpp>
 
+// #include <iostream>
+
 using terabee::DistanceData;
 
 using namespace std::chrono_literals;
@@ -21,55 +19,68 @@ 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(0x7)) // check if all 6 distance sensors work
         {
-      RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 6!");
+            RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 3!");
             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);
-  }
-
+    // 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++)
+        {
+            RCLCPP_INFO(this->get_logger(), "distance %f", data.distance[i]);
+            //            msg.distance_data[i] = data.distance[i];
+        }
+
+        // publish message
+        publisher_->publish(msg);
+    }
 };
 
 int main(int argc, char *argv[])

src/px4_connection/CMakeLists.txt

@@ -20,20 +20,18 @@ 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"
-)
+find_package(drone_services REQUIRED)
+find_package(std_srvs REQUIRED)
 
 include_directories(include)
 add_executable(heartbeat src/heartbeat.cpp)
 ament_target_dependencies(
     heartbeat
     rclcpp
+    drone_services
     px4_ros_com
     px4_msgs
+    drone_services
 )
 
 add_executable(px4_controller src/px4_controller.cpp)
@@ -42,7 +40,8 @@ ament_target_dependencies(
     rclcpp
     px4_ros_com
     px4_msgs
-    px4_connection
+    drone_services
+    std_srvs
 )
 
 target_include_directories(heartbeat PUBLIC

src/px4_connection/include/drone_control_modes.h

@@ -0,0 +1,20 @@
+#ifndef DRONE_CONTROL_MODES_H
+#define DRONE_CONTROL_MODES_H
+
+#define CONTROL_MODE_ATTITUDE 1
+#define CONTROL_MODE_VELOCITY 2
+#define CONTROL_MODE_POSITION 3
+
+#define CONTROL_MODE_MIN CONTROL_MODE_ATTITUDE
+#define CONTROL_MODE_MAX CONTROL_MODE_POSITION
+
+#define CONTROL_ACTUATOR_POS 0
+#define CONTROL_BODY_RATE_POS 1
+#define CONTROL_ATTITUDE_POS 2
+#define CONTROL_ACCELERATION_POS 3
+#define CONTROL_VELOCITY_POS 4
+#define CONTROL_POSITION_POS 5
+
+
+
+#endif

src/px4_connection/package.xml

@@ -8,13 +8,12 @@
     <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>
+    <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>

src/px4_connection/src/heartbeat.cpp

@@ -1,26 +1,31 @@
-/*
-
-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
 
+/**
+ * @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>
-// #include <px4_msgs/msg/timesync.hpp>
+#include <drone_services/srv/set_vehicle_control.hpp>
+#include <drone_services/msg/drone_control_mode.hpp>
+
+#include "drone_control_modes.h"
+
+
 
 using namespace std::chrono_literals;
 
 class HeartBeat : public rclcpp::Node
 {
 public:
-    HeartBeat() : Node("setpoint_sender")
+    HeartBeat() : Node("heartbeat")
     {
+        vehicle_control_mode_service_ = this->create_service<drone_services::srv::SetVehicleControl>("/drone/set_vehicle_control", std::bind(&HeartBeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+        RCLCPP_INFO(this->get_logger(), "initialized service /drone/set_vehicle_control");
+        // create a publisher on the drone control mode topic
+        drone_control_mode_publisher_ = this->create_publisher<drone_services::msg::DroneControlMode>("/drone/control_mode", 10);
         // 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
@@ -30,7 +35,19 @@ public:
     }
 
 private:
+    // publisher for offboard control mode messages
+    rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
+    // publisher for the control mode of the drone
+    rclcpp::Publisher<drone_services::msg::DroneControlMode>::SharedPtr drone_control_mode_publisher_;
+    // timer for sending the heartbeat
+    rclcpp::TimerBase::SharedPtr timer_;
+    // service to change vehicle_mode
+    rclcpp::Service<drone_services::srv::SetVehicleControl>::SharedPtr vehicle_control_mode_service_;
 
+    // start time of node in seconds
+    double start_time;
+    // which level of control is needed, only attitude control by default
+    int control_mode = 1 << CONTROL_ATTITUDE_POS;
     /**
      * @brief Publish offboard control mode messages as a heartbeat.
      *        Only the attitude is enabled, because that is how the drone will be controlled.
@@ -38,32 +55,70 @@ private:
      */
     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;
         
+        msg.position = (this->control_mode >> CONTROL_POSITION_POS) & 1 ? true : false;
+        msg.velocity = (this->control_mode >> CONTROL_VELOCITY_POS) & 1 ? true : false;
+        msg.acceleration = (this->control_mode >> CONTROL_ACCELERATION_POS) & 1 ? true : false;
+        msg.attitude = (this->control_mode >> CONTROL_ATTITUDE_POS) & 1 ? true : false;
+        msg.body_rate = (this->control_mode >> CONTROL_BODY_RATE_POS) & 1 ? true : false;
+        msg.actuator = (this->control_mode >> CONTROL_ACTUATOR_POS) & 1 ? true : 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;
+
+/**
+ * @brief Publish the current control mode of the drone onto the 'drone/control_mode' topic
+ * 
+ */
+    void publish_new_control_mode()
+    {
+        auto msg = drone_services::msg::DroneControlMode();
+        if (this->control_mode & (1 << CONTROL_ATTITUDE_POS))
+        {
+            msg.control_mode = CONTROL_MODE_ATTITUDE;
+            RCLCPP_INFO(this->get_logger(), "set control mode to attitude");
+        }
+        else if (this->control_mode & (1 << CONTROL_VELOCITY_POS))
+        {
+            msg.control_mode = CONTROL_MODE_VELOCITY;
+            RCLCPP_INFO(this->get_logger(), "set control mode to velocity");
+        }
+        else if (this->control_mode & (1 << CONTROL_POSITION_POS))
+        {
+            msg.control_mode = CONTROL_MODE_POSITION;
+            RCLCPP_INFO(this->get_logger(), "set control mode to position");
+        }
+        RCLCPP_INFO(this->get_logger(), "publishing new control mode %d", msg.control_mode);
+        drone_control_mode_publisher_->publish(msg);
+    }
+
+    /**
+     * @brief Handle a request to change the vehicle control mode
+     *
+     * @param request_header the header of the service request
+     * @param request the request that was sent to the service
+     * @param response the reponse that the service sends back to the client.
+     */
+
+    void handle_vehicle_control_change(
+        const std::shared_ptr<rmw_request_id_t> request_header,
+        const std::shared_ptr<drone_services::srv::SetVehicleControl::Request> request,
+        const std::shared_ptr<drone_services::srv::SetVehicleControl::Response> response)
+    {
+        if (request->control < 0 || request->control > (1 << CONTROL_POSITION_POS))
+        {
+            response->success = false;
+        }
+        else
+        {
+            this->control_mode = request->control;
+            RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode);
+            publish_new_control_mode();
+            response->success = true;
+        }
+    }
 };
 
 int main(int argc, char *argv[])

src/px4_connection/src/px4_controller.cpp

@@ -1,28 +1,35 @@
-/*
-
-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
-
+/**
+ * @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 "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_msgs/msg/vehicle_attitude.hpp>
+#include <px4_msgs/msg/vehicle_local_position.hpp>
 
-#include <px4_connection/srv/set_attitude.hpp>
-// #include <px4_msgs/msg/offboard_control_mode.hpp>
+#include <drone_services/srv/set_attitude.hpp>
+#include <drone_services/srv/set_trajectory.hpp>
+#include <drone_services/srv/arm_drone.hpp>
+#include <drone_services/srv/disarm_drone.hpp>
+#include <drone_services/msg/drone_control_mode.hpp>
+#include <drone_services/msg/drone_arm_status.hpp>
 
-#define D_SPEED(x) -x - 9.81
+#include <std_srvs/srv/empty.hpp>
+
+#include "drone_control_modes.h"
+
+#define DEFAULT_YAW_SPEED 0.6 // default turning speed in radians per second
+#define D_SPEED(x) -x - 9.81  // a speed up of x m/s has to be ajusted for the earths gravity
 
 using namespace std::chrono_literals;
 
@@ -31,19 +38,26 @@ 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);
+        arm_status_publisher_ = this->create_publisher<drone_services::msg::DroneArmStatus>("/drone/arm_status", 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));
+        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));
+        vehicle_local_position_subscription_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1));
+        control_mode_subscription_ = this->create_subscription<drone_services::msg::DroneControlMode>("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_receive, this, std::placeholders::_1));
 
-        //         service_ptr_ = this->create_service<std_srvs::srv::Empty>(
-        //         "test_service",
-        //         std::bind(&ServiceNode::service_callback, this, _1, _2, _3)
-        // );
 
+        set_attitude_service_ = this->create_service<drone_services::srv::SetAttitude>("/drone/set_attitude", std::bind(&PX4Controller::handle_attitude_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+        set_trajectory_service_ = this->create_service<drone_services::srv::SetTrajectory>("/drone/set_trajectory", std::bind(&PX4Controller::handle_trajectory_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+        disarm_service_ = this->create_service<drone_services::srv::DisarmDrone>("/drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+        arm_service_ = this->create_service<drone_services::srv::ArmDrone>("/drone/arm", std::bind(&PX4Controller::handle_arm_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));
 
@@ -55,120 +69,234 @@ 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<drone_services::msg::DroneArmStatus>::SharedPtr arm_status_publisher_;
+
+    rclcpp::Subscription<px4_msgs::msg::VehicleAttitude>::SharedPtr vehicle_attitude_subscription_;
+    rclcpp::Subscription<px4_msgs::msg::VehicleLocalPosition>::SharedPtr vehicle_local_position_subscription_;
+    rclcpp::Subscription<drone_services::msg::DroneControlMode>::SharedPtr control_mode_subscription_;
+
+    rclcpp::Service<drone_services::srv::SetAttitude>::SharedPtr set_attitude_service_;
+    rclcpp::Service<drone_services::srv::SetTrajectory>::SharedPtr set_trajectory_service_;
+    rclcpp::Service<drone_services::srv::DisarmDrone>::SharedPtr disarm_service_;
+    rclcpp::Service<drone_services::srv::ArmDrone>::SharedPtr arm_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 user_in_control = false; // if user has taken over control
+    bool armed = false;
     bool has_swithed = false;
     int setpoint_count = 0;
-    float thrust = 0.5;
+    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_angle = 0;  // angle in radians (-PI .. PI)
+    float last_thrust = 0; // default 10% thrust for when the drone gets armed
 
-    void set_setpoint(
+    float velocity[3] = {0, 0, 0};
+    float position[3] = {0, 0, 0};
+
+    float base_q[4] = {0, 0, 0, 0};
+    int base_q_amount = 0;
+
+    char current_control_mode = CONTROL_MODE_ATTITUDE; // start with attitude control
+
+    bool start_trajectory = false;
+    const float omega = 0.3; // angular speed of the POLAR trajectory
+    const float K = 0;       // [m] gain that regulates the spiral pitch
+
+    float rho_0 = 0;
+    float theta_0 = 0;
+    const float p0_z = -0.0;
+    float p0_x = rho_0 * cos(theta_0);
+    float p0_y = rho_0 * sin(theta_0);
+    float des_x = p0_x, des_y = p0_y, des_z = p0_z;
+    float dot_des_x = 0.0, dot_des_y = 0.0;
+    float gamma = M_PI_4;
+
+    float X;
+    float Y;
+
+    float local_x = 0;
+    float local_y = 0;
+
+    uint32_t discrete_time_index = 0;
+
+    // result quaternion
+    std::array<float, 4> q = {0, 0, 0, 0};
+
+    void handle_attitude_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)
+        const std::shared_ptr<drone_services::srv::SetAttitude::Request> request,
+        const std::shared_ptr<drone_services::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()
+        if (armed)
         {
-
-        auto msg = px4_msgs::msg::TrajectorySetpoint();
-        if (this->get_clock()->now().seconds() - start_time_ < 10)
+            if (request->yaw == 0 && request->pitch == 0 && request->roll == 0 && request->thrust == 0)
             {
-            msg.velocity[0] = 0;
-            msg.velocity[1] = 0;
-            msg.velocity[2] = D_SPEED(10);
-            msg.yawspeed = 0;
-            msg.yaw = -3.14;
+                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
             {
-            if (!has_swithed)
+                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);
+
+            response->success = true;
+        }
+        else
         {
-                RCLCPP_INFO(this->get_logger(), "switching to 0 vel");
-                has_swithed = true;
+            last_thrust = 0;
+            last_setpoint[0] = 0;
+            last_setpoint[1] = 0;
+            last_setpoint[2] = 0;
+            response->success = false;
+        }
     }
 
-            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;
+    void handle_trajectory_setpoint(
+        const std::shared_ptr<rmw_request_id_t> request_header,
+        const std::shared_ptr<drone_services::srv::SetTrajectory::Request> request,
+        const std::shared_ptr<drone_services::srv::SetTrajectory::Response> response)
+    {
+        if (!(request->control_mode == CONTROL_MODE_VELOCITY || request->control_mode == CONTROL_MODE_POSITION))
+        {
+            RCLCPP_INFO(this->get_logger(), "Got invalid trajectory control mode: %d", request->control_mode);
+            response->success = false;
+        }
+        else
+        {
+            RCLCPP_INFO(this->get_logger(), "Got new trajectory setpoint with control mode: %d", request->control_mode);
+            if (request->control_mode == CONTROL_MODE_VELOCITY)
+            {
+                for (int i = 0; i < 3; i++)
+                {
+                    velocity[i] += request->values[i];
+                }
+                RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]);
+            }
+            else if (request->control_mode == CONTROL_MODE_POSITION)
+            {
+                position[0] += request->values[0];
+                position[1] += request->values[1];
+                position[2] -= request->values[2]; // height is negative
+
+                RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]);
             }
 
-        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+            last_angle += request->yaw;
+            new_setpoint = true;
+            RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle);
+            response->success = true;
+        }
+    }
 
-        trajectory_setpoint_publisher->publish(msg);
+    /**
+     * @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<drone_services::srv::DisarmDrone::Request> request,
+        const std::shared_ptr<drone_services::srv::DisarmDrone::Response> response)
+    {
+        RCLCPP_INFO(this->get_logger(), "Got disarm request...");
+
+        if (armed)
+        {
+            armed = false;
+            user_in_control = false;
+            publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0);
+
+            auto msg = drone_services::msg::DroneArmStatus();
+            msg.armed = false;
+            arm_status_publisher_->publish(msg);
+            
+            response->success = true;
+        }
+        else
+        {
+            RCLCPP_ERROR(this->get_logger(), "Got disarm request but drone was not armed!");
+            response->success = false;
+        }
+    }
+
+    /**
+     * @brief arms the drone when a call to the arm service is made
+     *
+     * @param request_header the header for the service request
+     * @param request the request (empty)
+     * @param response the response (empty)
+     */
+    void handle_arm_request(
+        const std::shared_ptr<rmw_request_id_t> request_header,
+        const std::shared_ptr<drone_services::srv::ArmDrone::Request> request,
+        const std::shared_ptr<drone_services::srv::ArmDrone::Response> response)
+    {
+        RCLCPP_INFO(this->get_logger(), "Got arm request...");
+
+        if (!armed)
+        {
+            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");
+            this->last_thrust = -0.1; // spin motors at 10% thrust
+            armed = true;
+
+            auto msg = drone_services::msg::DroneArmStatus();
+            msg.armed = true;
+            arm_status_publisher_->publish(msg);
+
+            response->success = true;
+        }
+        else
+        {
+            RCLCPP_ERROR(this->get_logger(), "Arm request received but the drone is already armed!");
+            response->success = false;
+        }
     }
 
     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.thrust_body[2] = -last_thrust;
 
+        calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]);
+        // set quaternion
         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.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);
 
         msg.yaw_sp_move_rate = 0;
         msg.reset_integral = false;
@@ -178,32 +306,169 @@ private:
         vehicle_setpoint_publisher_->publish(msg);
     }
 
+    void send_velocity_setpoint()
+    {
+        auto msg = px4_msgs::msg::TrajectorySetpoint();
+
+        msg.velocity[0] = velocity[0];
+        msg.velocity[1] = velocity[1];
+        msg.velocity[2] = -velocity[2];
+        for (int i = 0; i < 3; i++)
+        {
+            msg.position[i] = NAN;
+        }
+
+        publish_trajectory_setpoint(msg);
+    }
+
+    void send_position_setpoint()
+    {
+        auto msg = px4_msgs::msg::TrajectorySetpoint();
+
+        RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z);
+        RCLCPP_INFO(this->get_logger(),"local position: %f %f", local_x, local_y);
+        msg.position = {local_x, local_y, des_z};
+        msg.velocity = {dot_des_x, dot_des_y, 0.0};
+        msg.yaw = gamma; //-3.14; // [-PI:PI]
+
+        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+        trajectory_setpoint_publisher->publish(msg);
+    }
+
+    void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg)
+    {
+        msg.yaw = last_angle;
+        msg.yawspeed = DEFAULT_YAW_SPEED;
+        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+        trajectory_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++;
+        // the spiral, in polar coordinates (theta, rho), is given by
+        // theta = theta_0 + omega*t
+        // rho = rho_0 + K*theta
+        float theta = theta_0 + omega * 0.1 * discrete_time_index;
+        float rho = rho_0 + K * theta;
 
-        ready_to_fly = setpoint_count == 20;
-        // after 20 setpoints, send arm command to make drone actually fly
-        if (ready_to_fly)
+        // from polar to cartesian coordinates
+        des_x = rho * cos(theta);
+        des_y = rho * sin(theta);
+        des_z = position[2]; // the z position can be set to the received height
+
+        // velocity computation
+        float dot_rho = K * omega;
+        dot_des_x = dot_rho * cos(theta) - rho * sin(theta) * omega;
+        dot_des_y = dot_rho * sin(theta) + rho * cos(theta) * omega;
+        // desired heading direction
+        gamma = atan2(dot_des_y, dot_des_x);
+
+        if (!user_in_control)
         {
-            // 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;
+            // RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint");
+            send_attitude_setpoint();
+        }
+        else
+        {
+            if (current_control_mode == CONTROL_MODE_ATTITUDE)
+            {
+                // RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint");
+                send_attitude_setpoint();
+            }
+            else
+            {
+                if (!new_setpoint)
+                {
+                    return;
+                }
+                if (current_control_mode == CONTROL_MODE_VELOCITY)
+                {
+                    // RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint");
+                    send_velocity_setpoint();
+                }
+                else if (current_control_mode == CONTROL_MODE_POSITION)
+                {
+                    // RCLCPP_INFO(this->get_logger(), "Sending position setpoint");
+                    send_position_setpoint();
+                }
+                new_setpoint = false;
+            }
+        }
+        if (start_trajectory)
+        {
+            discrete_time_index++;
+        }
     }
 
-        send_trajectory_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]);
+        }
+    }
+
+    void on_local_position_receive(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg)
+    {
+        // set start values to current position
+        if (!user_in_control)
+        {
+            // https://www.math.usm.edu/lambers/mat169/fall09/lecture33.pdf
+            rho_0 = pow(msg->x,2) + pow(msg->y,2);
+            theta_0 = atan2(msg->y, msg->x);
+            last_angle = msg->heading;
+
+            local_x = msg->x;
+            local_y = msg->y;
+        }
+        X = msg->x;
+        Y = msg->y;
+        float Z = msg->z;
+        if (!start_trajectory && (p0_x + 1.0 > X && p0_x - 1.0 < X) && (p0_y + 1.0 > Y && p0_y - 1.0 < Y) && (p0_z + 1.0 > Z && p0_z - 1.0 < Z))
+        {
+            start_trajectory = true;
+            RCLCPP_INFO(this->get_logger(), "start trajectory");
+        }
+    }
+
+    void on_control_mode_receive(const drone_services::msg::DroneControlMode::SharedPtr msg)
+    {
+        if (msg->control_mode >= CONTROL_MODE_MIN && msg->control_mode <= CONTROL_MODE_MAX)
+        {
+            current_control_mode = msg->control_mode;
+            RCLCPP_INFO(this->get_logger(), "Got valid control mode");
+            user_in_control = true; // user has taken over control
+        }
+        else
+        {
+            RCLCPP_ERROR(this->get_logger(), "Received invalid control mode %d", msg->control_mode);
+        }
+        RCLCPP_INFO(this->get_logger(), "Control mode set to %d", current_control_mode);
     }
 
     /**
@@ -235,7 +500,7 @@ private:
      * @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)
+    static void calculate_quaternion(std::array<float, 4> &ptr, float heading, float attitude, float bank)
     {
 
         float c1 = cos(heading / 2);

src/relais_control/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>relais_control</name>
+    <version>0.0.0</version>
+    <description>package to control the relais that enables Pixhawk RX and TX communication</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/relais_control/relais_control/relais_controller.py

@@ -0,0 +1,66 @@
+import rclpy
+from rclpy.node import Node
+
+try:
+    import RPi.GPIO as GPIO
+except RuntimeError:
+    print("Error importing RPi.GPIO!  This is probably because you need superuser privileges.  You can achieve this by using 'sudo' to run your script")
+
+from drone_services.srv import ControlRelais
+class RelaisController(Node):
+    def __init__(self):
+        super().__init__('relais_controller')
+        self.srv = self.create_service(ControlRelais, '/drone/control_relais', self.control_relais_callback)
+
+        self.relais1_pin = 17
+        self.relais2_pin = 27
+        self.init_gpio()
+        self.turn_relais_on()
+    
+    def init_gpio(self):
+        GPIO.setwarnings(False)
+
+        self.get_logger().info(str(GPIO.RPI_INFO))
+
+        GPIO.setmode(GPIO.BCM)
+
+        GPIO.setup(self.relais1_pin, GPIO.OUT)
+        GPIO.setup(self.relais2_pin, GPIO.OUT)
+        self.get_logger().info("GPIO initialized")
+
+    def turn_relais_on(self):
+        GPIO.output(self.relais1_pin, GPIO.HIGH)
+        GPIO.output(self.relais2_pin, GPIO.HIGH)
+        self.get_logger().info("Relais turned on")
+
+    def control_relais_callback(self, request, response):
+        if request.relais1_on:
+            GPIO.output(self.relais1_pin, GPIO.HIGH)
+            response.bits = response.bits | 1
+        else:
+            GPIO.output(self.relais1_pin, GPIO.LOW)
+            response.bits = response.bits & ~(1 << 0)
+        if request.relais2_on:
+            GPIO.output(self.relais2_pin, GPIO.HIGH)
+            response.bits = response.bits | (1 << 1)
+        else:
+            GPIO.output(self.relais2_pin, GPIO.LOW)
+            response.bits = response.bits & ~(1 << 1)
+        return response
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    relais_controller = RelaisController()
+
+    rclpy.spin(relais_controller)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    relais_controller.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/relais_control/setup.cfg

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

src/relais_control/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'relais_control'
+
+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='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'relais_controller = relais_control.relais_controller:main'
+        ],
+    },
+)

src/relais_control/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/relais_control/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/relais_control/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/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,273 @@
+import rclpy
+from rclpy.node import Node
+
+from sshkeyboard import listen_keyboard_manual
+import asyncio
+
+from drone_services.srv import SetAttitude
+from drone_services.srv import SetTrajectory
+from drone_services.srv import SetVehicleControl
+from drone_services.srv import ArmDrone
+
+CONTROL_MODE_ATTITUDE = 4
+CONTROL_MODE_VELOCITY = 16
+CONTROL_MODE_POSITION = 32
+
+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('set attitude service not available, waiting again...')
+        self.get_logger().info('successfully connected to set attitude service')
+        self.vehicle_control_cli = self.create_client(
+            SetVehicleControl, '/drone/set_vehicle_control')
+        while not self.vehicle_control_cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('set vehicle control service not available, waiting again...')
+        self.get_logger().info('successfully connected to set vehicle control service')
+        self.traj_cli = self.create_client(SetTrajectory, '/drone/set_trajectory')
+        while not self.traj_cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('set trajectory service not available, waiting again...')
+        self.get_logger().info('successfully connected to set trajectory service')
+        self.arm_cli = self.create_client(ArmDrone, '/drone/arm')
+        while not self.arm_cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('arm service not available, waiting again...')
+        self.get_logger().info('successfully connected to arm service')
+
+        self.get_logger().info('all services available')
+        self.control_mode = 1
+        
+        self.attitude_req = SetAttitude.Request()
+        self.vehicle_control_req = SetVehicleControl.Request()
+        self.traj_req = SetTrajectory.Request()
+        self.arm_req = ArmDrone.Request()
+
+        self.get_logger().info("\nControls:\n1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nCTRL-C - 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_arm(self):
+        self.future = self.arm_cli.call_async(self.arm_req)
+        rclpy.spin_until_future_complete(self, self.future)
+        self.get_logger().info('publishing message arm msg on service')
+        return self.future.result()
+    
+    def send_control_mode(self):
+        self.vehicle_control_req.control = self.control_mode
+        self.future = self.vehicle_control_cli.call_async(self.vehicle_control_req)
+        rclpy.spin_until_future_complete(self, self.future)
+        self.get_logger().info('publishing message vehicle control msg on service')
+        return self.future.result()
+
+    def send_attitude_request(self, yaw, pitch, roll, thrust):
+        self.attitude_req.yaw = yaw
+        self.attitude_req.pitch = pitch
+        self.attitude_req.roll = roll
+        self.attitude_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.attitude_req)
+        rclpy.spin_until_future_complete(self, self.future)
+        self.get_logger().info('publishing attitude message on service')
+        return self.future.result()
+
+    def send_velocity_request(self, x, y, z, angle):
+        self.traj_req.control_mode = 2
+        self.traj_req.yaw = angle
+        self.traj_req.values = [x, y, z]
+        self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle))
+        self.future = self.traj_cli.call_async(self.traj_req)
+        rclpy.spin_until_future_complete(self, self.future)
+        self.get_logger().info('publishing velocity message on service')
+        return self.future.result()
+
+    def send_position_request(self, x, y, z, angle):
+        self.traj_req.control_mode = 3
+        self.traj_req.yaw = angle
+        self.traj_req.values = [x, y, z]
+        self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle))
+        self.future = self.traj_cli.call_async(self.traj_req)
+        rclpy.spin_until_future_complete(self, self.future)
+        self.get_logger().info('publishing position message on service')
+        return self.future.result()
+
+    def on_release(self, key):
+        # self.get_logger().info('released ' + str(key))
+        pass
+
+    def move_up(self):
+        pass
+        if (self.control_mode == CONTROL_MODE_ATTITUDE):
+            self.send_attitude_request(pitch=0.0, yaw=0.0,
+                                       roll=0.0, thrust=0.05)
+        elif (self.control_mode == CONTROL_MODE_VELOCITY):
+            self.send_velocity_request(x=0.0, y=0.0, z=0.1, angle=0.0)
+        else:
+            self.send_position_request(x=0.0, y=0.0, z=0.5, angle=0.0)
+
+    def move_right(self):
+        if (self.control_mode == CONTROL_MODE_ATTITUDE):
+            self.send_attitude_request(pitch=0.0, yaw=0.0,
+                                       roll=1.0, thrust=0.0)
+        elif (self.control_mode == CONTROL_MODE_VELOCITY):
+            self.send_velocity_request(x=1.0, y=0.0, z=0.0, angle=0.0)
+        else:
+            self.send_position_request(x=1.0, y=0.0, z=0.0, angle=0.0)
+
+    def move_down(self):
+        if (self.control_mode == CONTROL_MODE_ATTITUDE):
+            self.send_attitude_request(pitch=0.0, yaw=0.0,
+                                       roll=0.0, thrust=-0.05)
+        elif (self.control_mode == CONTROL_MODE_VELOCITY):
+            self.send_velocity_request(x=0.0, y=0.0, z=-0.1, angle=0.0)
+        else:
+            self.send_position_request(x=0.0, y=0.0, z=-0.5, angle=0.0)
+
+    def move_left(self):
+        if (self.control_mode == CONTROL_MODE_ATTITUDE):
+            self.send_attitude_request(pitch=0.0, yaw=0.0,
+                                       roll=-1.0, thrust=0.0)
+        elif (self.control_mode == CONTROL_MODE_VELOCITY):
+            self.send_velocity_request(x=-1.0, y=0.0, z=0.0, angle=0.0)
+        else:
+            self.send_position_request(x=-1.0, y=0.0, z=0.0, angle=0.0)
+
+    def rotate_right(self):
+        if (self.control_mode == CONTROL_MODE_ATTITUDE):
+            self.send_attitude_request(pitch=0.0, yaw=1.0,
+                                       roll=0.0, thrust=0.0)
+        elif (self.control_mode == CONTROL_MODE_VELOCITY):
+            self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=1.0)
+        else:
+            self.send_position_request(x=0.0, y=0.0, z=0.0, angle=1.0)
+
+    def rotate_left(self):
+        if (self.control_mode == CONTROL_MODE_ATTITUDE):
+            self.send_attitude_request(pitch=0.0, yaw=-1.0,
+                                       roll=0.0, thrust=0.0)
+        elif (self.control_mode == CONTROL_MODE_VELOCITY):
+            self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=-1.0)
+        else:
+            self.send_position_request(x=0.0, y=0.0, z=0.0, angle=-1.0)
+
+    def move_forward(self):
+        if (self.control_mode == CONTROL_MODE_ATTITUDE):
+            self.send_attitude_request(pitch=-1.0, yaw=0.0,
+                                       roll=0.0, thrust=0.0)
+        elif (self.control_mode == CONTROL_MODE_VELOCITY):
+            self.send_velocity_request(x=0.0, y=1.0, z=0.0, angle=0.0)
+        else:
+            self.send_position_request(x=0.0, y=1.0, z=0.0, angle=0.0)
+
+    def move_backward(self):
+        if (self.control_mode == CONTROL_MODE_ATTITUDE):
+            self.send_attitude_request(pitch=1.0, yaw=0.0,
+                                       roll=0.0, thrust=0.0)
+        elif (self.control_mode == CONTROL_MODE_VELOCITY):
+            self.send_velocity_request(x=0.0, y=-1.0, z=0.0, angle=0.0)
+        else:
+            self.send_position_request(x=0.0, y=-1.0, z=0.0, angle=0.0)
+
+    def stop(self):
+        if (self.control_mode == CONTROL_MODE_ATTITUDE):
+            self.send_attitude_request(pitch=0.0, yaw=0.0,
+                                       roll=0.0, thrust=0.0)
+        elif (self.control_mode == CONTROL_MODE_VELOCITY):
+            self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=0.0)
+        else:
+            self.send_position_request(x=0.0, y=0.0, z=0.0, angle=0.0)
+
+    def move_up_little(self):
+        if (self.control_mode == CONTROL_MODE_ATTITUDE):
+            self.send_attitude_request(pitch=0.0, yaw=0.0,
+                                       roll=0.0, thrust=0.01)
+        elif (self.control_mode == CONTROL_MODE_VELOCITY):
+            self.send_velocity_request(x=0.0, y=0.0, z=0.05, angle=0.0)
+        else:
+            self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0)
+
+    def move_down_little(self):
+        if (self.control_mode == CONTROL_MODE_ATTITUDE):
+            self.send_attitude_request(pitch=0.0, yaw=0.0,
+                                       roll=0.0, thrust=-0.01)
+        elif (self.control_mode == CONTROL_MODE_VELOCITY):
+            self.send_velocity_request(x=0.0, y=0.0, z=-0.05, angle=0.0)
+        else:
+            self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0)
+
+    def on_press(self, key):
+        try:
+            # self.get_logger().info('pressed ' + char)
+            if key == 'w':
+                self.get_logger().info('forward')
+                self.move_forward()
+            if key == 's':
+                self.get_logger().info('backward')
+                self.move_backward()
+            if key == 'a':
+                self.get_logger().info('left')
+                self.move_left()
+            if key == 'd':
+                self.get_logger().info('right')
+                self.move_right()
+            if key == 'q':
+                self.get_logger().info('rotate left')
+                self.rotate_left()
+            if key == 'e':
+                self.get_logger().info('rotate right')
+                self.rotate_right()
+            if key == 'z':
+                self.get_logger().info('down')
+                self.move_down()
+            if key == 'space':
+                self.get_logger().info('up')
+                self.move_up()
+            if key == 'v':
+                self.get_logger().info('down a little')
+                self.move_down_little()
+            if key == 'f':
+                self.get_logger().info('up a little')
+                self.move_up_little()
+            if key == 'n':
+                self.get_logger().info('stop')
+                self.stop()
+            if key == '1':
+                self.get_logger().info('attitude control')
+                self.control_mode = CONTROL_MODE_ATTITUDE
+                self.send_control_mode()
+            if key == '2':
+                self.get_logger().info('velocity control')
+                self.control_mode = CONTROL_MODE_VELOCITY
+                self.send_control_mode()
+            if key == '3':
+                self.get_logger().info('position control')
+                self.control_mode = CONTROL_MODE_POSITION
+                self.send_control_mode()
+            if key == '/':
+                self.get_logger().info('arming')
+                self.send_arm()
+
+        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()