add pdf docs

add correct land service
add optical flow enabled and emergency force stop
2023-07-10 15:56:21 +02:00 · 2023-07-10 15:18:24 +02:00 · 2023-07-10 15:13:57 +02:00 · 2023-07-10 15:00:51 +02:00 · 2023-07-10 13:47:58 +02:00 · 2023-07-10 13:08:14 +02:00
53 changed files with 2463 additions and 390 deletions
--- a/README.md
+++ b/README.md
@@ -1 +1,152 @@
 # 5G RCID
+## 5G Remote Controlled Inspection Drone
+By Sem van der Hoeven
+
+## Table of contents
+- [Introduction](#introduction)
+- [EMERGENCY FORCE STOP](#emergency-force-stop)
+- [Pin layout](#pin-layout)
+- [Connecting to API](#connecting-to-api)
+- [Installing API](#installing-api)
+- [Installation on new flight computer](#installation-on-new-flight-computer)
+- [Building PX4 firmware](#building-px4-firmware)
+- [ROS2 nodes overview](#ros2-nodes-overview)
+- [PX4 parameters](#px4-parameters)
+- [Optical flow](#optical-flow)
+
+## Introduction
+This is the code for the 5G RCID of the 5G Hub. All ROS2 nodes and the API code can be found here. The flight computer currently already contains the latest version of the software. 
+- The code for the ROS2 nodes can be found in the [src folder (src/)](/src/)
+- The code for the API can be found in the [api folder (api/)](/api/)
+- Further information about how the drone is built and how it works can be found in my report in the [docs folder (doc/)](/doc/)
+
+To add new ROS2 functionality, you can edit the code, push to the repository, pull on the flight computer (ubuntu@10.100.0.40, password is `raspberrypi`) and build the ROS2 workspace again. An example of how to do this is shown below:
+```bash
+# (On your computer) commit changes
+git commit -a -m "Added new functionality"
+git push
+# (On the flight computer) pull changes
+cd /home/ubuntu/ros2_ws
+git fetch
+git pull
+# (On the flight computer) build workspace
+colcon build --allow-overriding drone_services
+# (On the flight computer) source workspace
+source install/setup.bash
+# (On the flight computer) restart ROS2 nodes
+drone_scripts/restart_services.sh
+```
+To add new API functionality, you can do the same, but for the code in the api folder. Then, log in on the edge computer and pull the changes. The API is automatically restarted when the code is changed. If you want to manually restart the API, you can do so using the following command:
+```bash
+# (On the edge computer) restart API
+sudo systemctl restart webserver
+```
+## EMERGENCY FORCE STOP
+If, for whatever reason, the drone does not respond to the API, you can force stop all nodes on the drone and force it to land through the pixhawk using the following commands:
+```bash
+# on your computer, ssh into the drone
+ssh ubuntu@10.100.0.40
+# cd into the ROS2 workspace
+cd ros2_ws
+# stop all ROS2 nodes
+drone_scripts/stop_services.sh
+```
+## Pin layout
+A pinout of the raspberry pi can be found [here](https://www.raspberrypi.com/documentation/computers/raspberry-pi.html). The connections are visible in the table below:
+|Raspberry Pi pin|Function|Connected to|
+|---|---|---|
+|2|5V|Power input from PDB|
+|4|5V|Power to relais|
+|6|GND|Ground from PDB|
+|8|UART TX|To RX of Pixhawk TELEM 1|
+|9|GND|Ground to relais|
+|10|UART RX|To TX of Pixhawk TELEM 1|
+|11|GPIO 17|Relais|
+|13|GPIO 27|Relais|
+
+## Connecting to API
+To connect to the API, make sure you are connected to the 5G Hub network. Then, the API is located at http://10.1.1.41:8080/. When the drone is finished booting (the relais is switched on), you can connect to the drone using the `Connect` button.
+
+## Installing API
+To be able to run the API, npm and nodejs must be installed:
+```bash
+curl -o- https://raw.githubusercontent.com/nvm-sh/nvm/v0.39.3/install.sh | bash
+nvm install node
+# to be able to run npm and nodejs as sudo, run the following commands:
+n=$(which node)
+n=${n%/bin/node}
+chmod -R 755 $n/bin/* 
+sudo cp -r $n/{bin,lib,share} /usr/local 
+```
+To install the API on a new edge computer, you must first clone this repository. Then you can run the NodeJS webserver using npm. You can do that using the following commands:
+```bash
+git clone https://github.com/etmeddi/5g_drone_ROS2.git
+cd 5g_drone_ROS2/api
+npm start
+```
+## Installation on new flight computer
+The drone currently has a Raspberry Pi that contains a ROS2 installation. The Raspberry Pi runs Ubuntu 20.04 and ROS 2 Foxy. If you want to install this on a new Pi, you should [get Ubuntu Server 20.04](https://ubuntu.com/download/server#downloads), and [install ROS2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) on it. Then, you should clone this repository into a `ros2_ws` folder. You can do that using the following commands:
+```bash
+git clone git@github.com:etmeddi/5g_drone_ROS2.git
+mv 5g_drone_ROS2 ros2_ws
+```
+
+After that, to make sure the connection to PX4 works, you should follow the [PX4 ROS2 User Guide](https://docs.px4.io/main/en/ros/ros2_comm.html#installation-setup) provided by PX4.
+
+## Building PX4 firmware
+The RCID uses a custom version of the PX4 firmware to include the battery percentage and CPU load of the flight controller. Before building the firmware, make sure the [developer toolchain](https://docs.px4.io/main/en/dev_setup/dev_env.html) is set up correctly. To build the firmware, first clone the repository (from [the PX4 docs](https://docs.px4.io/main/en/dev_setup/building_px4.html)):
+```bash
+# enter home directory
+cd
+# clone git repository
+git clone https://github.com/PX4/PX4-Autopilot.git --recursive
+```
+In the PX4 directory, edit the file `src/modules/uxrce_dds_client/dds_topics.yaml`. Above the line that says `subscriptions:`, add the following:
+```yaml
+  - topic: /fmu/out/battery_status
+    type: px4_msgs::msg::BatteryStatus
+  - topic: /fmu/out/cpuload
+    type: px4_msgs::msg::Cpuload
+```
+The file can also be found in this repository at [dds_topics.yaml](dds_topics.yaml). After changing the file, you can build the firmware using the following command:
+```bash
+# navigate to root of PX4 directory
+cd ~/PX4-Autopilot
+# build firmware
+make px4_fmu-v5_default
+```
+The built firmware file will be located at `PX4-Autopilot/build/px4_fmu-v4_default/px4_fmu-v4_default.px4`. You can then flash this to the flight controller using [QGroundControl](http://qgroundcontrol.com/). Make sure to select the [custom firmware](https://docs.px4.io/main/en/config/firmware.html#installing-px4-master-beta-or-custom-firmware) option when flashing, and select the built firmware file.
+## ROS2 nodes overview
+An overview of all the ROS2 nodes, services and topics can be found below (also visible in my report)
+
+![ROS2 nodes overview](/doc/ROSNodes.jpg)
+
+## PX4 parameters
+To enable communication with the flight computer, the following parameters should be set in QGroundControl:
+|Parameter|Value|Function|
+|---|---|---|
+|UXRCE_DDS_CFG|101|run microxrce-dds on TELEM 1|
+|MAV_0_CONFIG|TELEM 4|run mavlink on TELEM 4 because XRCE-DDS runs on TELEM 1|
+|SER_TEL1_BAUD|921600|high baud rate because serial|
+|COM_RC_IN_MODE|4 (Stick input disabled)|allow arming without GPS|
+|COM_RCL_EXCEPT|5|Ignore loss of RC remote signal|
+
+## Optical flow
+The optical flow sensor is a [Hex HereFlow PMW3901](https://docs.px4.io/main/en/sensor/pmw3901.html#hex-hereflow-pmw3901-optical-flow-sensor) and it's connected to the CAN port of the pixhawk. The parameters to enable the optical flow sensor are:
+|Parameter|Value|Function|
+|---|---|---|
+|EKF2_OF_CTRL |1 (Enabled)|Enable optical flow sensor fusion|
+|UAVCAN_ENABLE|2 (Sensors automatic config)|Enable UAVCAN for sensors|
+|UAVCAN_SUB_FLOW|Enabled|subscribe to optical flow messages|
+|UAVCAN_SUB_GPS|Disabled|subscribe to GPS messages|
+
+To reverse this, and be able to use GPS, the parameters should be:
+
+|Parameter|Value|Function|
+|---|---|---|
+|EKF2_OF_CTRL |0 (Disabled)|Disable optical flow sensor fusion|
+|UAVCAN_ENABLE|0 (Disabled)|Disable UAVCAN for sensors|
+|UAVCAN_SUB_FLOW|Disabled|don't subscribe to optical flow messages|
+|UAVCAN_SUB_GPS|Enabled|subscribe to GPS messages|
+
+
--- a/api/index.js
+++ b/api/index.js
@@ -1,19 +1,10 @@
 var express = require("express");
 var app = express();
-const WebSocket = require("ws");

-var ws = new WebSocket("ws://10.100.0.40:9001/");
+let sse_clients = [];

-ws.on('open', function open() {
-    console.log("connected");
-  });
-
-ws.on("message", function message(message) {
-  var msg = JSON.parse(message);
-  console.log("RECEIVED: " + msg);
-});
-
-ws.on("error", console.error);
+app.use(express.static("public"));
+app.use(express.json());

 // set the view engine to ejs
 app.set("view engine", "ejs");
@@ -22,8 +13,10 @@ app.set("view engine", "ejs");

 // index page
 app.get("/", function (req, res) {
-  res.render("index");
+  res.render("index", { api_connected: api_connected });
 });

+app.get("/events", handle_sse_client);
+
 app.listen(8080);
 console.log("Server is listening on port 8080");
--- a/api/public/css/stylesheet.css
+++ b/api/public/css/stylesheet.css
@@ -0,0 +1,66 @@
+body {
+    background-color: azure;
+    font-family: 'Segoe UI', Tahoma, Geneva, Verdana, sans-serif;
+}
+
+html, body {
+    overflow: auto;
+}
+
+.header {
+    color: black;
+    text-align: center;
+}
+
+.video {
+    width: 100%;
+    overflow: auto;
+}
+
+.mainvideo {
+    width: 50%;
+    float: left;
+    height: 100%;
+    border: 1px solid blue;
+    margin-right: 10px;
+    padding: 10px;
+    overflow: visible;
+}
+
+.lastpicture {
+    width: 40%;
+    float: right;
+    /* height: 400px; */
+    border: 1px solid red;
+    padding: 10px;
+}
+
+#connectedbuttons {
+    overflow: auto;
+}
+
+#buttons {
+    float: left;
+}
+
+#connectedstatus {
+    float:left;
+    width: 80%;
+}
+
+#take_picture {
+    float:right;
+}
+#arm_disarm {
+    float: right;
+}
+
+#button_estop {
+    background-color: red;
+    color: white;
+}
+
+.headerimg {
+    height: 200px;
+    /* float: right; */
+}
--- a/api/public/img/droneimage_2023-05-30_14-55-57.jpg
+++ b/api/public/img/droneimage_2023-05-30_14-55-57.jpg
--- a/api/views/index.ejs
+++ b/api/views/index.ejs
@@ -1,12 +1,275 @@
 <!DOCTYPE html>
 <html lang="en">
-    <head>
-        <meta charset="UTF-8">
-        <meta name="viewport" content="width=device-width, initial-scale=1.0">
-        
-        <title>5G drone API</title>
-    </head>
-    <body>
-        <div>Hello World!</div>
-    </body>
-</html>
+
+<head>
+    <meta charset="UTF-8">
+    <meta name="viewport" content="width=device-width, initial-scale=1.0">
+    <link rel='stylesheet' href='/css/stylesheet.css' />
+
+    <title>5G drone API</title>
+</head>
+
+<body style="height: 100%;">
+    <div>
+        <h1 class="header">5G Drone API</h1>
+    </div>
+    <div class="mainvideo">
+        <p id="cameraview">Camera view: Not connected</p>
+        <canvas id="result-video" style="border: 1px solid blue;" width="800" height="600"></canvas>
+        <div id="connectedbuttons">
+            <div id="connectedstatus">
+                <h2 id="connectedlabel">Not connected to drone</h2>
+                <button id="connectbutton" onclick="local_connect()">Connect</button>
+            </div>
+            <div id="buttons">
+                <button id="take_picture" onclick="take_picture()">Take picture</button>
+                <br>
+                <button id="arm_disarm" onclick="arm_disarm()">Arm/Disarm</button>
+            </div>
+        </div>
+        <div id="controls">
+            <h2>Controls</h2>
+            <div id="buttons">
+                <button class="movebutton" id="button_turnleft">Turn left</button>
+                <button class="movebutton" id="button_turnright">Turn right</button>
+                <button class="movebutton" id="button_up">Up</button>
+                <button class="movebutton" id="button_down">Down</button>
+                <button class="movebutton" id="button_forward">Forward</button>
+                <button class="movebutton" id="button_backward">Backward</button>
+                <button class="movebutton" id="button_left">Left</button>
+                <button class="movebutton" id="button_right">Right</button>
+                <button id="button_land" onclick="land()">Land</button>
+                <button id="button_estop" onclick="estop()"><strong>Emergency Stop</strong></button>
+
+            </div>
+        </div>
+    </div>
+    <div class="lastpicture">
+        <p>Last picture:</p>
+        <div id="lastimg">
+            <img id="picture" style="width: 400px;">
+        </div>
+        <h2>Drone status</h2>
+        <p id="batterypercentage">Battery percentage</p>
+        <p id="cpuload">CPU load</p>
+        <p id="armed"></p>
+        <p id="control_mode"></p>
+        <p id="speed">Current speed</p>
+        <p id="position">Current position</p>
+        <p id="height">Height</p>
+        <p id="failsafe">Failsafe not activated</p>
+        <img class="headerimg"
+            src="https://upload.wikimedia.org/wikipedia/commons/thumb/e/e9/Ericsson_logo.svg/2341px-Ericsson_logo.svg.png"
+            alt="ericsson logo">
+        <img class="headerimg" src="https://hightechcampus.com/storage/1069/5ghub-logo.png" alt="5g hub logo">
+    </div>
+</body>
+
+<script>
+    var ws;
+    assign_button_callbacks();
+    openSocket = () => {
+
+        document.getElementById("cameraview").innerHTML = "Camera view: Connecting...";
+        socket = new WebSocket("ws://10.100.0.40:9002/");
+        let msg = document.getElementById("result-video");
+        socket.addEventListener('open', (e) => {
+            console.log("Connected to video")
+            document.getElementById("cameraview").innerHTML = "Camera view: Connected";
+        });
+        socket.addEventListener('message', (e) => {
+            let ctx = msg.getContext("2d");
+            let image = new Image();
+            image.src = URL.createObjectURL(e.data);
+            image.addEventListener("load", (e) => {
+                ctx.drawImage(image, 0, 0, 800, 600);
+            });
+        });
+        socket.addEventListener('close', (e) => {
+            console.log("Disconnected from video")
+            document.getElementById("cameraview").innerHTML = "Camera view: Disconnected. Reload the page to reconnect";
+        });
+        socket.addEventListener('error', (e) => {
+            console.log("Error in video connection")
+            document.getElementById("cameraview").innerHTML = "Camera view: Error. Reload the page to reconnect";
+        });
+    }
+
+    function assign_button_callbacks() {
+        var buttons = document.getElementsByClassName("movebutton");
+        for (var i = 0; i < buttons.length; i++) {
+            buttons[i].addEventListener("mouseup", function () {
+                stop();
+            });
+        }
+        document.getElementById("button_forward").addEventListener("mousedown", function () { forward(); });
+        document.getElementById("button_backward").addEventListener("mousedown", function () { backward(); });
+        document.getElementById("button_left").addEventListener("mousedown", function () { left(); });
+        document.getElementById("button_right").addEventListener("mousedown", function () { right(); });
+        document.getElementById("button_turnleft").addEventListener("mousedown", function () { turn_left(); });
+        document.getElementById("button_turnright").addEventListener("mousedown", function () { turn_right(); });
+        document.getElementById("button_up").addEventListener("mousedown", function () { up(); });
+        document.getElementById("button_down").addEventListener("mousedown", function () { down(); });
+    }
+
+    function send_move_request(data) {
+        console.log("sending move request");
+        if (ws != null)
+            ws.send(data);
+    }
+
+    function turn_left() {
+        console.log("turnleft");
+        send_move_request(JSON.stringify({ "command": 3, "up_down": 0.0, "forward_backward": 0.0, "left_right": 0.0, "yaw": -10.0 }));
+    }
+    function turn_right() {
+        console.log("turnright");
+        send_move_request(JSON.stringify({ "command": 3, "up_down": 0.0, "forward_backward": 0.0, "left_right": 0.0, "yaw": 10.0 }));
+    }
+    function up() {
+        console.log("up");
+        send_move_request(JSON.stringify({ "command": 3, "up_down": 1.0, "forward_backward": 0.0, "left_right": 0.0, "yaw": 0.0 }));
+
+    }
+    function down() {
+        console.log("down");
+        send_move_request(JSON.stringify({ "command": 3, "up_down": -1.0, "forward_backward": 0.0, "left_right": 0.0, "yaw": 0.0 }));
+
+    }
+    function forward() {
+        console.log("forward"); send_move_request(JSON.stringify({ "command": 3, "up_down": 0.0, "forward_backward": 1.0, "left_right": 0.0, "yaw": 0.0 }));
+
+    }
+    function backward() {
+        console.log("backward");
+        send_move_request(JSON.stringify({ "command": 3, "up_down": 0.0, "forward_backward": -1.0, "left_right": 0.0, "yaw": 0.0 }));
+    }
+    function left() {
+        console.log("left");
+        send_move_request(JSON.stringify({ "command": 3, "up_down": 0.0, "forward_backward": 0.0, "left_right": -1.0, "yaw": 0.0 }));
+    }
+    function right() {
+        console.log("right");
+        send_move_request(JSON.stringify({ "command": 3, "up_down": 0.0, "forward_backward": 0.0, "left_right": 1.0, "yaw": 0.0 }));
+    }
+    function stop() {
+        console.log("stop");
+        send_move_request(JSON.stringify({ "command": 3, "up_down": 0.0, "forward_backward": 0.0, "left_right": 0.0, "yaw": 0.0 }));
+    }
+
+    function land() {
+        console.log("land");
+        var request = JSON.stringify({ command: 0 });
+        ws.send(request);
+
+    }
+
+    function estop() {
+        console.log("estop");
+        var request = JSON.stringify({
+            command: 6,
+        });
+        ws.send(request);
+    }
+
+    function take_picture() {
+        console.log("take picture");
+        var image = new Image();
+        image.src = document.getElementById("result-video").toDataURL();
+        image.width = 400;
+        document.getElementById("lastimg").innerHTML = "";
+        document.getElementById("lastimg").appendChild(image);
+    }
+
+    function arm_disarm() {
+        console.log("arm/disarm");
+        var request = JSON.stringify({ command: 1 });
+        ws.send(request);
+    }
+
+    function handle_ws_message(data) {
+        set_timout = false;
+        if (data.type == "STATUS") {
+            document.getElementById("batterypercentage").innerHTML = "Battery percentage: " + data.data.battery_percentage.toString().substring(0, 4) + "%";
+            document.getElementById("cpuload").innerHTML = "CPU load: " + data.data.cpu_usage.toString().substring(0, 6).substring(2, 4) + "%";
+            document.getElementById("armed").innerHTML = "Armed: " + data.data.armed;
+            document.getElementById("control_mode").innerHTML = "Control mode: " + data.data.control_mode;
+            document.getElementById("speed").innerHTML = "Current speed (m/s): x: " + data.data.velocity[0].toString().substring(0,4) + " y: " + data.data.velocity[1].toString().substring(0,4) + " z: " + data.data.velocity[2].toString().substring(0,4);
+            document.getElementById("position").innerHTML = "Current position (m): x: " + data.data.position[0].toString().substring(0,4) + " y: " + data.data.position[1].toString().substring(0,4) + " z: " + data.data.position[2].toString().substring(0,4);
+        } else if (data.type == "FAILSAFE") {
+            document.getElementById("failsafe").innerHTML = "Failsafe: ACTIVATED";
+            document.getElementById("failsafe").style.backgroundColor = "red";
+            document.getElementById("failsafe").style.color = "white";
+            document.getElementById("failsafe").style.textDecoration = "bold";
+            alert("Failsafe enabled! Drone is landing. The failsafe message is:\n" + data.message);
+        } else if (data.type == "API_HEARTBEAT") {
+            concole.log("Got heartbeat from API")
+            clearTimeout(api_timout);
+        }
+    }
+
+    function local_connect() {
+        console.log("Connecting to API");
+        ws = new WebSocket("ws://10.100.0.40:9001/");
+
+        ws.addEventListener("open", function open() {
+            console.log("connected with websockets to API!");
+            document.getElementById("connectedlabel").innerHTML = "Connected to drone";
+            document.getElementById("connectbutton").disabled = true;
+            openSocket();
+        });
+
+        ws.addEventListener("message", function message(message) {
+            try {
+                var msg = JSON.parse(message.data);
+                handle_ws_message(msg);
+
+            } catch (error) {
+                console.log("could not parse as json");
+                console.error(error)
+            }
+        });
+
+        ws.addEventListener("error", function error(err) {
+            console.log("there was an error! " + err);
+            console.log("Showing alert!");
+            alert("There was an error connecting to the API!");
+            document.getElementById("connectedlabel").innerHTML = "Not connected to drone";
+            document.getElementById("connectbutton").disabled = false;
+        });
+
+        ws.addEventListener("close", function close(event) {
+            console.log("connection closed");
+            alert("Connection to API closed!")
+            document.getElementById("connectedlabel").innerHTML = "Not connected to drone";
+            document.getElementById("connectbutton").disabled = false;
+        });
+    }
+
+    function connect() {
+        var received = false;
+        var xhr = new XMLHttpRequest();
+        xhr.open("GET", "/connect", true);
+        xhr.onreadystatechange = function () {
+            if (this.status == 200) {
+                console.log(this.responseText);
+                if (this.responseText.length > 0) {
+                    var status = JSON.parse(this.responseText);
+                    document.getElementById("connectedlabel").innerHTML = "Connected to drone";
+                    document.getElementById("connectbutton").disabled = true;
+                }
+            } else {
+                console.log("error");
+                document.getElementById("connectedlabel").innerHTML = "Not connected to drone";
+                if (!received) {
+                    alert("Could not connect to API!");
+                    received = true;
+
+                }
+            }
+        };
+        xhr.send();
+    }
+</script>
+
+</html>
--- a/api/views/test.ejs
+++ b/api/views/test.ejs
@@ -0,0 +1,37 @@
+<!DOCTYPE html>
+<html>
+
+<head>
+    <title>Python_Websocket_Live_Streaming</title>
+    <link rel="stylesheet" type="text/css" href="style.css">
+</head>
+
+<body onload="openSocket()">
+    <div id="status">
+        Connection failed. Somebody may be using the socket.
+    </div>
+    <div style="text-align: center">
+        <canvas id="msg" width="960" height="720" style="display:inline-block" />
+    </div>
+
+</body>
+<script>
+    openSocket = () => {
+
+        socket = new WebSocket("ws://10.100.0.40:9001/");
+        let msg = document.getElementById("msg");
+        socket.addEventListener('open', (e) => {
+            document.getElementById("status").innerHTML = "Opened";
+        });
+        socket.addEventListener('message', (e) => {
+            let ctx = msg.getContext("2d");
+            let image = new Image();
+            image.src = URL.createObjectURL(e.data);
+            image.addEventListener("load", (e) => {
+                ctx.drawImage(image, 0, 0, msg.width, msg.height);
+            });
+        });
+    }
+</script>
+
+</html>
--- a/Presentatie.pdf
+++ b/Presentatie.pdf
--- a/doc/Afstudeerverslag_Sem_van_der_Hoeven_2162609_final.pdf
+++ b/doc/Afstudeerverslag_Sem_van_der_Hoeven_2162609_final.pdf
--- a/doc/ROSNodes.jpg
+++ b/doc/ROSNodes.jpg
--- a/drone_scripts/restart_services.sh
+++ b/drone_scripts/restart_services.sh
@@ -0,0 +1,14 @@
+echo "waiting 5 secs before restarting..."
+sleep 5
+echo "restarting"
+
+sudo systemctl restart drone_api.service
+sudo systemctl restart drone_camera.service
+sudo systemctl restart drone_failsafe.service
+sudo systemctl restart drone_find_usb_devices.service
+sudo systemctl restart drone_height_sensor.service
+sudo systemctl restart drone_lidar.service
+sudo systemctl restart drone_positionchanger.service
+sudo systemctl restart drone_px4_connection.service
+sudo systemctl restart drone_relais.service
+sudo systemctl restart drone_status.service
--- a/drone_scripts/start_api.sh
+++ b/drone_scripts/start_api.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+. /home/ubuntu/source_ros2.sh
+
+ros2 run api_communication api_listener
+
+
--- a/drone_scripts/start_failsafe.sh
+++ b/drone_scripts/start_failsafe.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+. /home/ubuntu/source_ros2.sh
+
+ros2 run failsafe failsafe
+
+
--- a/drone_scripts/start_positionchanger.sh
+++ b/drone_scripts/start_positionchanger.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+. /home/ubuntu/source_ros2.sh
+
+ros2 run drone_controls position_changer
+
+
--- a/drone_scripts/start_px4_connection.sh
+++ b/drone_scripts/start_px4_connection.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+. /home/ubuntu/source_ros2.sh
+
+ros2 launch px4_connection px4_controller_heardbeat_launch.py
+
+
--- a/drone_scripts/stop_services.sh
+++ b/drone_scripts/stop_services.sh
@@ -0,0 +1,10 @@
+sudo systemctl stop drone_api.service
+sudo systemctl stop drone_camera.service
+sudo systemctl stop drone_failsafe.service
+sudo systemctl stop drone_find_usb_devices.service
+sudo systemctl stop drone_height_sensor.service
+sudo systemctl stop drone_lidar.service
+sudo systemctl stop drone_positionchanger.service
+sudo systemctl stop drone_px4_connection.service
+sudo systemctl stop drone_relais.service
+sudo systemctl stop drone_status.service
--- a/img.png
+++ b/img.png
--- a/restart_services.sh
+++ b/restart_services.sh
@@ -0,0 +1,10 @@
+sudo systemctl restart drone_api.service
+sudo systemctl restart drone_camera.service
+sudo systemctl restart drone_failsafe.service
+sudo systemctl restart drone_find_usb_devices.service
+sudo systemctl restart drone_height_sensor.service
+sudo systemctl restart drone_lidar.service
+sudo systemctl restart drone_positionchanger.service
+sudo systemctl restart drone_px4_connection.service
+sudo systemctl restart drone_relais.service
+sudo systemctl restart drone_status.service
--- a/src/api_communication/api_communication/api_listener.py
+++ b/src/api_communication/api_communication/api_listener.py
@@ -2,46 +2,84 @@ import rclpy
 from rclpy.node import Node

 from drone_services.msg import DroneStatus
+from drone_services.msg import FailsafeMsg
 from drone_services.srv import TakePicture
+from drone_services.srv import MovePosition
+from drone_services.srv import EnableFailsafe
+from drone_services.srv import ArmDrone
+from drone_services.srv import DisarmDrone
+from drone_services.srv import ReadyDrone
+from drone_services.srv import Land

 import asyncio
 import websockets.server
 import threading
 import json
 from enum import Enum
-
-# communication: client always sends commands that have a command id.
-# server always sends messages back that have a message type
-
+from functools import partial
+from functools import partial

 class RequestCommand(Enum):
-    GET_COMMANDS_TYPES = -1  # to get the available commands and types
-    TAKEOFF = 0
-    LAND = 1
+    """
+    Enum for the commands that can be sent to the API
+    """
+    GET_COMMANDS_TYPES = -1 
+    LAND = 0
+    ARM_DISARM = 1
    MOVE_POSITION = 2
    MOVE_DIRECTION = 3
-    TAKE_PICTURE = 5
-
+    EMERGENCY_STOP = 6

 class ResponseMessage(Enum):
+    """
+    Enum for the messages that can be sent to the client
+    """
    ALL_REQUESTS_RESPONSES = -1
    STATUS = 0
-    IMAGE = 1
-
+    MOVE_DIRECTION_RESULT = 2
+    FAILSAFE = 3

 class ApiListener(Node):
+    """
+    Node that listens to the client and sends the commands to the drone
+    """
+
    def __init__(self):
        super().__init__('api_listener')
        self.get_logger().info('ApiListener node started')
-        self.drone_status_subscriber = self.create_subscription(
-            DroneStatus, '/drone/status', self.drone_status_callback, 10)
+        self.drone_status_subscriber = self.create_subscription(DroneStatus, '/drone/status', self.drone_status_callback, 10)
+        self.failsafe_subscriber = self.create_subscription(FailsafeMsg, "/drone/failsafe", self.failsafe_callback, 10)
+
        self.timer = self.create_timer(1, self.publish_status)
-        self.take_picture_client = self.create_client(
-            TakePicture, '/drone/picture')
-        while not self.take_picture_client.wait_for_service(timeout_sec=1.0):
-            self.get_logger().info('Take picture service not available, waiting again...')
+
+        self.take_picture_client = self.create_client(TakePicture, '/drone/picture')
+        self.wait_for_service(self.take_picture_client, "Take picture")
        self.take_picture_request = TakePicture.Request()

+        self.move_position_client = self.create_client(MovePosition, '/drone/move_position')
+        self.wait_for_service(self.move_position_client, "Move position")
+        self.move_position_request = MovePosition.Request()
+
+        self.enable_failsafe_client = self.create_client(EnableFailsafe, "/drone/enable_failsafe")
+        self.wait_for_service(self.enable_failsafe_client, "Enable failsafe")
+        self.enable_failsafe_request = EnableFailsafe.Request()
+
+        self.arm_drone_client = self.create_client(ArmDrone, "/drone/arm")
+        self.wait_for_service(self.arm_drone_client, "Arm drone")
+        self.arm_drone_request = ArmDrone.Request()
+
+        self.disarm_drone_client = self.create_client(DisarmDrone, "/drone/disarm")
+        self.wait_for_service(self.disarm_drone_client, "Disarm drone")
+        self.disarm_drone_request = DisarmDrone.Request()
+
+        self.ready_drone_client = self.create_client(ReadyDrone, "/drone/ready")
+        self.wait_for_service(self.ready_drone_client, "Ready drone")
+        self.ready_drone_request = ReadyDrone.Request()
+
+        self.land_client = self.create_client(Land, "/drone/land")
+        self.wait_for_service(self.land_client, "Land drone")
+        self.land_request = Land.Request()
+
        self.status_data = {}
        self.status_data_received = False
        self.last_message = ""
@@ -57,53 +95,114 @@ class ApiListener(Node):
            target=self.handle_responses, daemon=True)
        self.response_thread.start()

-    def drone_status_callback(self, msg):
-        self.status_data_received = True
-        self.status_data['battery_percentage'] = msg.battery_percentage
-        self.status_data['cpu_usage'] = msg.cpu_usage
-        self.status_data['armed'] = msg.armed
-        self.status_data['control_mode'] = msg.control_mode
-        self.status_data['route_setpoint'] = msg.route_setpoint
+        self.event_loop = None
+        self.armed = False
+        self.failsafe_enabled = False
+        self.has_sent_failsafe_msg = False

-    def publish_message(self, message):
-        self.get_logger().info(f'Publishing message: {message}')
-        asyncio.run(self.websocket.send(message))
+    def wait_for_service(self, client, service_name):
+        """Waits for a client service to be available
+
+        Args:
+            client (ROS2 service client): The client to use to wait fo the service
+            service_name (str): The client name for logging
+        """
+        waiting = 0
+        while not client.wait_for_service(timeout_sec=1.0):
+            if (waiting > 10):
+                self.get_logger().error(service_name + ' service not available, exiting...')
+                exit(-1)
+            self.get_logger().info(service_name + 'service not available, waiting again...')
+            waiting = waiting + 1
+
+    def drone_status_callback(self, msg):
+        """Callback for when a drone_status message is received
+
+        Args:
+            msg (DroneStatus): The received message 
+        """
+        try:
+            self.status_data_received = True
+            self.status_data['battery_percentage'] = msg.battery_percentage
+            if msg.battery_percentage < 15:
+                self.enable_failsafe("Battery level too low! Failsafe enabled to prevent damage to battery (" + str(msg.battery_percentage ) + "%)")
+            self.status_data['cpu_usage'] = msg.cpu_usage
+            self.status_data['armed'] = msg.armed
+            self.armed = msg.armed
+            self.status_data['control_mode'] = msg.control_mode
+            self.status_data['route_setpoint'] = msg.route_setpoint
+            self.status_data['velocity'] = [float(msg.velocity[0]), float(
+                msg.velocity[1]), float(msg.velocity[2])]
+            self.status_data['position'] = [float(msg.position[0]), float(
+            msg.position[1]), float(msg.position[2])]
+            self.status_data['failsafe'] = msg.failsafe
+            self.status_data['height'] = msg.height
+        except Exception as e:
+            self.get_logger().error(f'Error while parsing drone status message: {e}')
+
+    def failsafe_callback(self, msg):
+        """Callback for when the failsafe gets enabled. Queues a FAILSAFE message to the client
+
+        Args:
+            msg (FailSAfe): The message that was received
+        """
+        if self.failsafe_enabled:
+            return
+
+        if not self.has_sent_failsafe_msg:
+            self.has_sent_failsafe_msg = True
+            self.status_data['failsafe'] = msg.enabled
+            self.message_queue.append(json.dumps({'type': ResponseMessage.FAILSAFE.name, 'message': msg.msg}))
+
+    async def publish_message(self, message):
+        """publishes a message to the NodeJS client
+
+        Args:
+            message (JSON object): the message to send
+        """
+        # self.get_logger().info(f'Publishing message: {message}')
+        if self.websocket is not None:
+            try:
+                await self.websocket.send(message)
+            except Exception as e:
+                self.get_logger().error('Something went wrong while sending a message to the websocket: ' + str(e))
+        else:
+            self.get_logger().error('Trying to publish message but no websocket connection')

    def publish_status(self):
+        """publishes the current status to the client by queueing a STATUS message
+        """
        if self.status_data_received:
+            self.status_data_received = False
            if self.websocket is not None:
-                self.message_queue.append(json.dumps(
-                    {'type': ResponseMessage.STATUS.name, 'data': self.status_data}))
+                try:
+                    self.message_queue.append(json.dumps({'type': ResponseMessage.STATUS.name, 'data': self.status_data}))
+                except Exception as e:
+                    self.get_logger().error('Something went wrong while publishing the status: ' + str(e))

    def handle_responses(self):
+        """Thread to handle responses to send to the client
+        """
        while True:
-            if len(self.message_queue) > 0:
-                self.publish_message(self.message_queue.pop(0))
+            if len(self.message_queue) > 0 and self.websocket is not None:
+                # self.get_logger().info("sending message")
+                asyncio.run(self.publish_message(self.message_queue.pop(0)))

    def start_api_thread(self):
+        """Starts the API thread"""
        asyncio.run(self.handle_api())

    async def handle_api(self):
+        """Handles the API requests and starts the websockets server"""
        self.get_logger().info('Starting API')
+        self.event_loop = asyncio.get_event_loop()
        self.server = await websockets.serve(self.api_handler, '0.0.0.0', 9001)
        self.get_logger().info('API started on port 9001')
        await self.server.wait_closed()

-    def process_image_request(self, message_json):
-        self.get_logger().info('Take picture command received')
-        if message_json['filename']:
-            self.get_logger().info(
-                f'Filename: {message_json["filename"]}')
-            self.take_picture_request.input_name = message_json['filename']
-        self.future = self.cli.call_async(self.take_picture_request)
-        rclpy.spin_until_future_complete(self, self.future)
-        result_filename = self.future.result()
-        with open(result_filename, 'rb') as f:
-            image_data = f.read()
-            self.message_queue.append(json.dumps(
-                {'type': ResponseMessage.IMAGE.name, 'image': image_data}))
-
    def send_available_commands(self):
+        """Sends the available commands to the client
+        """
        print('Sending available commands')
        requestcommands = {}
        messagetypes = {}
@@ -114,10 +213,146 @@ class ApiListener(Node):
            messagetypes[message_type.name] = message_type.value
        result['request_commands'] = requestcommands
        result['response_messages'] = messagetypes
-        self.message_queue.append(json.dumps(
-            {'type': ResponseMessage.ALL_REQUESTS_RESPONSES.name, 'data': result}))
+        self.message_queue.append(json.dumps({'type': ResponseMessage.ALL_REQUESTS_RESPONSES.name, 'data': result}))
+
+    def handle_direction_message(self, message):
+        """Calls the move position service with the given values
+
+        Args:
+            message (JSON object): the message containing the direction values
+        """
+        self.move_position_request.up_down = float(message['up_down'])
+        self.move_position_request.left_right = float(message['left_right'])
+        self.move_position_request.front_back = float(
+            message['forward_backward'])
+        self.move_position_request.angle = float(message['yaw'])
+        self.get_logger().info(f'Calling move position service with request: {str(self.move_position_request)}')
+
+        self.send_move_position_request()
+
+    def send_move_position_request(self):
+        """Sends the move position request to the move position service"""
+        future = self.move_position_client.call_async(
+            self.move_position_request)
+        future.add_done_callback(partial(self.move_position_service_callback))
+
+    def move_position_service_callback(self, future):
+        """Callback for the move position service
+
+        Args:
+            future (Future): Future object that holds the result
+        """
+        try:
+            result = future.result()
+            message_result = {}
+            message_result['type'] = ResponseMessage.MOVE_DIRECTION_RESULT.name
+            self.get_logger().info(f'Move position service call result: {str(result)}')
+            if result.success == True:
+                self.get_logger().info('Move position service call success')
+                message_result['success'] = True
+            else:
+                self.get_logger().error('Move position service call failed')
+                message_result['success'] = False
+            self.message_queue.append(json.dumps(message_result))
+        except Exception as e:
+            self.get_logger().error('Something went wrong while sending a move position request and waiting for the response: %r' % (e))
+
+    def enable_failsafe(self, message):
+        self.get_logger().info("Enabling failsafe")
+        self.enable_failsafe_request.message = message
+        future = self.enable_failsafe_client.call_async(
+            self.enable_failsafe_request)
+        future.add_done_callback(partial(self.enable_failsafe_callback))
+
+    def enable_failsafe_callback(self, future):
+        try:
+            result = future.result()
+            if (result.enabled == True):
+                self.get_logger().info("Failsafe activated")
+        except Exception as e:
+            self.get_logger().error("Something went wrong while trying to enable failsafe!\n" + str(e))
+
+    def emergency_stop(self):
+        """Sends an emergency stop request to the failsafe service"""
+        self.get_logger().info('Activating emergency stop')
+        self.enable_failsafe("Emergency stop activated")
+
+    def takeoff(self):
+        """Sends a takeoff request to the move position service"""
+        self.get_logger().info('Takeoff command received')
+        self.move_position_request.up_down = 0.1
+        self.move_position_request.left_right = 0.0
+        self.move_position_request.front_back = 0.0
+        self.move_position_request.angle = 0.0
+        self.send_move_position_request()
+
+    def land(self):
+        """Sends a land request to the move position service"""
+        self.get_logger().info('Land command received')
+        self.move_position_request.up_down = -0.1
+        self.move_position_request.left_right = 0.0
+        self.move_position_request.front_back = 0.0
+        self.move_position_request.angle = 0.0
+        self.send_move_position_request()
+        future = self.land_client.call_async(self.land_request)
+        future.add_done_callback(partial(self.land_service_callback))
+
+    def arm_disarm(self):
+        """Sends an arm or disarm request to the PX4Controller"""
+        if self.armed:
+            self.get_logger().info('Disarm command received')
+            future = self.disarm_drone_client.call_async(
+                self.disarm_drone_request)
+            future.add_done_callback(partial(self.disarm_service_callback))
+        else:
+            self.get_logger().info('Arm command received')
+            future = self.ready_drone_client.call_async(
+                self.ready_drone_request)
+            future.add_done_callback(partial(self.ready_drone_callback))
+    
+    def ready_drone_callback(self, future):
+        try:
+            result = future.result()
+            if result.success:
+                self.get_logger().info('Ready service call success')
+            else:
+                self.get_logger().error('Ready service call failed')
+        except Exception as e:
+            self.get_logger().error('Something went wrong while calling the ready service!\n' + str(e))
+
+    def arm_service_callback(self, future):
+        try:
+            result = future.result()
+            if result.success:
+                self.get_logger().info('Arm service call success')
+            else:
+                self.get_logger().error('Arm service call failed')
+        except Exception as e:
+            self.get_logger().error('Something went wrong while calling the arm service!\n' + str(e))
+
+    def disarm_service_callback(self, future):
+        try:
+            result = future.result()
+            if result.success:
+                self.get_logger().info('Disarm service call success')
+                self.armed = False
+            else:
+                self.get_logger().error('Disarm service call failed')
+        except Exception as e:
+            self.get_logger().error('Something went wrong while calling the disarm service!\n' + str(e))
+            
+    def land_service_callback(self, future):
+        try:
+            result = future.result()
+            if result.is_landing:
+                self.get_logger().info('Land service call success')
+            else:
+                self.get_logger().error('Land service call failed')
+        except Exception as e:
+            self.get_logger().error('Something went wrong while calling the land service!\n' + str(e))

    def consume_message(self, message):
+        """Consumes a message from the client"""
        self.get_logger().info(f'Consuming message: {message}')
        try:
            message_json = json.loads(str(message))
@@ -129,37 +364,41 @@ class ApiListener(Node):
            else:
                self.get_logger().info(
                    f'Received command: {message_json["command"]}')
-                if message_json['command'] == RequestCommand.TAKEOFF.value:
-                    self.get_logger().info('Takeoff command received')
-                elif message_json['command'] == RequestCommand.LAND.value:
+                if message_json['command'] == RequestCommand.LAND.value:
                    self.get_logger().info('Land command received')
-                elif message_json['command'] == RequestCommand.MOVE_POSITION.value:
-                    self.get_logger().info('Move position command received')
+                    self.land()
+                elif message_json['command'] == RequestCommand.ARM_DISARM.value:
+                    self.get_logger().info('Arm/disarm command received')
+                    self.arm_disarm()
                elif message_json['command'] == RequestCommand.MOVE_DIRECTION.value:
                    self.get_logger().info('Move direction command received')
-                elif message_json['command'] == RequestCommand.TAKE_PICTURE.value:
-                    self.process_image_request(message_json)
-                elif message_json['command'] == RequestCommand.GET.value:
+                    self.handle_direction_message(message_json)
+                elif message_json['command'] == RequestCommand.GET_COMMANDS_TYPES.value:
                    self.get_logger().info('Get command received')
                    self.send_available_commands()
+                elif message_json['command'] == RequestCommand.EMERGENCY_STOP.value:
+                    self.get_logger().info('Emergency stop command received')
+                    self.emergency_stop()
                else:
-                    self.get_logger().error('Received unknown command')
+                    self.get_logger().error('Received unknown command ' + str(message_json['command']))
                    self.send_available_commands()
        except TypeError:
-            self.get_logger().error('Received unknown command')
+            self.get_logger().error('Received unknown type: ' + str(type(message)) + " " + str(message))
            self.send_available_commands()
        except json.JSONDecodeError:
            self.get_logger().error('Received invalid JSON')
            self.send_available_commands()
        except Exception as e:
            self.get_logger().error('Something went wrong!')
-            self.get_logger().error(str(e))
+            self.get_logger().error(str(type(e)))
+            self.get_logger().error(e)

    async def api_handler(self, websocket):
+        """Handles the websocket connection
+        Args:
+            websocket (websockets object): the websocket connection
+        """
        self.get_logger().info('New connection')
-        # if self.websocket is not None:
-        #     self.get_logger().error('Got a new websocket connection but I am already connected!')
-        #     return

        self.websocket = websocket
        try:
@@ -175,17 +414,17 @@ class ApiListener(Node):
            self.get_logger().error(str(e))
            self.websocket = None

-
 def main(args=None):
+    """Main function"""
    rclpy.init(args=args)

    api_listener = ApiListener()
-
-    rclpy.spin(api_listener)
+    multithreaded_executor = rclpy.executors.MultiThreadedExecutor()
+    multithreaded_executor.add_node(api_listener)
+    multithreaded_executor.spin()

    api_listener.destroy_node()
    rclpy.shutdown()

-
 if __name__ == '__main__':
    main()
--- a/src/api_communication/package.xml
+++ b/src/api_communication/package.xml
@@ -8,10 +8,6 @@
  <license>Apache License 2.0</license>
  <depend>rclpy</depend>
  <depend>drone_services</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>
--- a/src/api_communication/test/test_api_listener.py
+++ b/src/api_communication/test/test_api_listener.py
@@ -0,0 +1,110 @@
+import os
+import sys
+import unittest
+import time
+
+import launch
+import launch_ros
+import launch_ros.actions
+import launch_testing.actions
+import pytest
+import rclpy
+
+from drone_services.msg import FailsafeMsg
+from drone_services.msg import DroneStatus
+
+@pytest.mark.rostest
+def generate_test_description():
+    file_path = os.path.dirname(__file__)
+    api_listener_node = launch_ros.actions.Node(
+        executable=sys.executable,
+        arguments=[os.path.join(
+            file_path, '..', 'api_communication', 'api_listener.py')],
+        additional_env={'PYTHONUNBUFFERED': '1'}
+    )
+    failsafe_node = launch_ros.actions.Node(
+        package='failsafe', executable='failsafe')
+    camera_node = launch_ros.actions.Node(
+        package='camera', executable='camera_controller')
+    position_changer_node = launch_ros.actions.Node(
+        package='drone_controls', executable='position_changer')
+    px4_controller_node = launch_ros.actions.Node(
+        package='px4_connection', executable='px4_controller')
+    heartbeat_node = launch_ros.actions.Node(
+        package='px4_connection', executable='heartbeat')
+
+    return (
+        launch.LaunchDescription([
+            api_listener_node,
+            failsafe_node,
+            camera_node,
+            position_changer_node,
+            px4_controller_node,
+            heartbeat_node,
+            launch_testing.actions.ReadyToTest(),
+        ]),
+        {
+            'api_listener_node': api_listener_node,
+            'failsafe_node': failsafe_node,
+            'camera_node': camera_node,
+            'position_changer_node': position_changer_node,
+            'px4_controller_node': px4_controller_node,
+            'heartbeat_node': heartbeat_node
+        }
+    )
+
+
+class ApiListenerTest(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+
+    def setUp(self):
+        self.node = rclpy.create_node('test_api_listener')
+        self.published_battery_status = False
+        self.received_failsafe_callback = False
+
+    def tearDown(self):
+        self.node.destroy_node()
+
+    def failsafe_callback(self,msg):
+        self.assertTrue(msg.enabled, "Failsafe was not enabled!")
+        self.assertTrue("Battery level too low! Failsafe enabled to prevent damage to battery" in msg.msg, "Failsafe message was not correct!")
+        self.received_failsafe_callback = True
+
+    def status_callback(self,msg):
+        self.node.get_logger().info("Received status callback " + str(msg))
+
+    def test_api_listener_battery(self, api_listener_node, proc_output):
+        failsafe_subscriber = self.node.create_subscription(FailsafeMsg, '/drone/failsafe', self.failsafe_callback, 10)
+        drone_status_publisher = self.node.create_publisher(DroneStatus, '/drone/status', 10)
+        msg = DroneStatus()
+        msg.battery_percentage = 10.0
+        msg.armed = True
+        msg.height = 10.0
+        msg.control_mode = "attitude"
+        msg.cpu_usage = 10.0
+        msg.route_setpoint = 0
+        msg.position = [0.0,0.0,0.0]
+        msg.velocity = [0.0,0.0,0.0]
+
+        time.sleep(5) # wait for nodes to start
+        self.node.get_logger().info("Starting publishing")
+        end_time = time.time() + 10.0
+
+        try:
+            while time.time() < end_time:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+                # self.node.get_logger().info("publishing message")
+                drone_status_publisher.publish(msg)
+                if self.received_failsafe_callback:
+                    break
+            self.assertTrue(self.received_failsafe_callback, "Failsafe was not enabled!")
+        finally:
+            self.node.destroy_subscription(failsafe_subscriber)
+            self.node.destroy_publisher(drone_status_publisher)
--- a/src/api_communication/test/test_copyright.py
+++ b/src/api_communication/test/test_copyright.py
@@ -1,23 +0,0 @@
-# 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'
--- a/src/api_communication/test/test_flake8.py
+++ b/src/api_communication/test/test_flake8.py
@@ -1,25 +0,0 @@
-# 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)
--- a/src/api_communication/test/test_pep257.py
+++ b/src/api_communication/test/test_pep257.py
@@ -1,23 +0,0 @@
-# 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'
--- a/src/beacon_positioning/src/tracker_position.cpp
+++ b/src/beacon_positioning/src/tracker_position.cpp
@@ -4,10 +4,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "beacon_positioning/msg/tracker_position.hpp"
-
 #include "rtls_driver/rtls_driver.hpp"
-
-// from the example: https://github.com/Terabee/positioning_systems_api/blob/master/examples/rtls_tracker_example.cpp
 #include "serial_communication/serial.hpp"

 #define TRACKER_0_PORT "/dev/ttyUSB0"
--- a/src/camera/camera/camera_controller.py
+++ b/src/camera/camera/camera_controller.py
@@ -5,19 +5,54 @@ from sensor_msgs.msg import Image
 import os
 from datetime import datetime

+import asyncio
+import websockets.server
+import threading
+import cv2
+import sys
+import requests
+
 #resolution of the camera
 RES_4K_H = 3496
 RES_4K_W = 4656

 class CameraController(Node):
    def __init__(self):
+        """Initialize the camera controller node.
+
+        Initializes the service and the video stream for the websocket connection.
+        
+        """
        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)
+            TakePicture, '/drone/picture', self.take_picture_callback)
+        
+        self.websocket = None
+        self.server = None
+        self.event_loop = None        
+        self.should_exit = False
+
+        self.timer = self.create_timer(1, self.timer_callback)
+
+        self.video_thread = threading.Thread(target=self.setup_websocket)
+        self.video_thread.start()
+
+
+
+    def timer_callback(self):
+        """Callback function for shutting down if an error occurred from a different thread."""
+        if self.should_exit:
+            self.get_logger().info("Shutting down...")
+            self.destroy_node()
+            sys.exit(-1)

    def take_picture_callback(self, request, response):
+        """Callback function for the service call to the /drone/picture service.
+            takes a picture with the given filename and saves it to the drone_img folder.
+            Sends the filename back to the caller.
+        """
        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")
@@ -25,11 +60,57 @@ class CameraController(Node):
            response.filename = imagename
        else:
            response.filename = request.input_name
-        os.system('fswebcam -r ' + RES_4K_W + 'x' + RES_4K_H + ' ' + response.filename)
+        os.system('fswebcam -r ' + str(RES_4K_W) + 'x' + str(RES_4K_H) + ' ' + response.filename)
        self.get_logger().info("Picture saved as " + response.filename)

        return response

+    def setup_websocket(self):
+        """Sets up the websocket connection for the video stream on port 9002."""
+        loop = asyncio.new_event_loop()
+        connected = False
+        while not connected:
+            try:
+                start_server = websockets.serve(self.websocket_video, "0.0.0.0", 9002,loop=loop)
+                connected = True
+            except Exception as e:
+                self.get_logger().error("error " + str(e))
+                connected = False 
+        loop.run_until_complete(start_server)
+        loop.run_forever()
+        try:
+            self.destroy_node()
+        except Exception as e:
+            self.get_logger().error("error " + str(e))
+            sys.exit(-1)
+    
+    async def websocket_video(self,websocket,path):
+        """Function for the websocket connection on port 9002.
+        This continuously captures frames from the video and sends them to the websocket client.
+        A resolution of 1920x1080 is chosen for performance reasons."""
+        vid = cv2.VideoCapture(0,cv2.CAP_V4L)
+
+        vid.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
+        vid.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
+        error_amount = 0
+        while True:
+            try:
+                while(vid.isOpened()):
+                    img, frame = vid.read()
+                    encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 100]
+                    man = cv2.imencode('.jpg', frame)[1]
+                    await websocket.send(man.tobytes())
+                self.get_logger().error("Not opened")
+                error_amount += 1
+            except Exception as e:
+                self.get_logger().error("error " + str(e))
+                error_amount += 1
+            if error_amount > 20:
+                self.get_logger().error("Too many errors, closing node")
+                self.should_exit = True
+                sys.exit(-1)
+
+       
 def main(args=None):
    rclpy.init(args=args)

--- a/src/camera/package.xml
+++ b/src/camera/package.xml
@@ -11,9 +11,6 @@
    <exec_depend>drone_services</exec_depend>
    <exec_depend>sensor_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>
--- a/src/camera/test/test_camera.py
+++ b/src/camera/test/test_camera.py
@@ -0,0 +1,72 @@
+import os
+import sys
+import unittest
+import time
+
+import launch
+import launch_ros
+import launch_ros.actions
+import launch_testing.actions
+import pytest
+import rclpy
+
+from drone_services.srv import TakePicture
+
+@pytest.mark.rostest
+def generate_test_description():
+    file_path = os.path.dirname(__file__)
+    camera_node = launch_ros.actions.Node(
+        executable=sys.executable,
+        arguments=[os.path.join(file_path, '..', 'camera', 'camera_controller.py')],
+        additional_env={'PYTHONUNBUFFERED': '1'}
+    )
+
+    return (
+        launch.LaunchDescription([
+            camera_node,
+            launch_testing.actions.ReadyToTest(),
+        ]),
+        {
+            'camera_node': camera_node,
+        }
+    )
+
+class CameraUnitTest(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+    
+    def setUp(self):
+        self.node = rclpy.create_node('test_camera')
+        self.service_called = False
+
+    def tearDown(self):
+        self.node.destroy_node()
+
+    def service_call_callback(self,future):
+        self.assertIsNotNone(future.result())
+        self.assertEqual(future.result().filename, "/home/ubuntu/test_image.jpg")
+        self.assertTrue(os.path.exists("/home/ubuntu/test_image.jpg"))
+        self.service_called = True
+
+    def test_camera_save_image(self,camera_node,proc_output):
+        # call camera service
+        camera_client = self.node.create_client(TakePicture, '/drone/picture')
+        while not camera_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('camera service not available, waiting again...')
+        request = TakePicture.Request()
+        request.input_name = "/home/ubuntu/test_image.jpg"
+        try:
+            while True:
+                rclpy.spin_once(self.node,timeout_sec=0.1)
+                if not self.service_called:
+                    camera_client.call_async(request).add_done_callback(self.service_call_callback)
+                else:
+                    break
+        finally:
+            self.node.destroy_client(camera_client)
--- a/src/camera/test/test_copyright.py
+++ b/src/camera/test/test_copyright.py
@@ -1,23 +0,0 @@
-# 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'
--- a/src/camera/test/test_flake8.py
+++ b/src/camera/test/test_flake8.py
@@ -1,25 +0,0 @@
-# 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)
--- a/src/camera/test/test_pep257.py
+++ b/src/camera/test/test_pep257.py
@@ -1,23 +0,0 @@
-# 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'
--- a/src/drone_controls/CMakeLists.txt
+++ b/src/drone_controls/CMakeLists.txt
@@ -2,11 +2,12 @@ cmake_minimum_required(VERSION 3.8)
 project(drone_controls)

 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+    add_compile_options(-Wall -Wextra -Wpedantic)
 endif()

 # find dependencies
 find_package(ament_cmake REQUIRED)
+
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -25,15 +26,18 @@ install(TARGETS position_changer
    DESTINATION lib/${PROJECT_NAME})

 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
+    # find_package(ament_lint_auto REQUIRED)
+
+    # the following line skips the linter which checks for copyrights
+    # comment the line when a copyright and license is added to all source files
+    # set(ament_cmake_copyright_FOUND TRUE)
+    # the following line skips cpplint (only works in a git repo)
+    # comment the line when this package is in a git repo and when
+    # a copyright and license is added to all source files
+    # set(ament_cmake_cpplint_FOUND TRUE)
+    # ament_lint_auto_find_test_dependencies()
+    find_package(launch_testing_ament_cmake REQUIRED)
+    add_launch_test(test/test_positionchanger.py)
 endif()

 ament_package()
--- a/src/drone_controls/src/PositionChanger.cpp
+++ b/src/drone_controls/src/PositionChanger.cpp
@@ -5,7 +5,11 @@
 #include <drone_services/msg/height_data.hpp>
 #include <drone_services/srv/set_vehicle_control.hpp>
 #include <drone_services/srv/set_trajectory.hpp>
+#include <drone_services/srv/set_attitude.hpp>
 #include <drone_services/srv/move_position.hpp>
+#include <drone_services/srv/ready_drone.hpp>
+#include <drone_services/srv/arm_drone.hpp>
+#include <drone_services/srv/land.hpp>

 #include <drone_services/srv/enable_failsafe.hpp>
 #include <drone_services/msg/failsafe_msg.hpp>
@@ -25,10 +29,13 @@

 #define MIN_DISTANCE 1.0 // in meters

+#define CONTROL_MODE_ATTITUDE 4 // attitude control mode bitmask
 #define DEFAULT_CONTROL_MODE 16 // default velocity control bitmask
 // converts bitmask control mode to control mode used by PX4Controller
 #define PX4_CONTROLLER_CONTROL_MODE(x) ((x) == 4 ? 1 : ((x) == 16 ? 2 : ((x) == 32 ? 3 : -1)))

+#define HEIGHT_DELTA 0.1
+
 using namespace std::chrono_literals;

 struct Quaternion
@@ -45,26 +52,36 @@ public:
        auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);

        this->lidar_subscription = this->create_subscription<drone_services::msg::LidarReading>("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, 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));
+        this->height_subscription = this->create_subscription<drone_services::msg::HeightData>("/drone/height", qos, std::bind(&PositionChanger::handle_height_message, this, std::placeholders::_1));
        this->odometry_subscription = this->create_subscription<px4_msgs::msg::VehicleOdometry>("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1));
        this->move_position_service = this->create_service<drone_services::srv::MovePosition>("/drone/move_position", std::bind(&PositionChanger::handle_move_position_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+        this->ready_drone_service = this->create_service<drone_services::srv::ReadyDrone>("/drone/ready", std::bind(&PositionChanger::handle_ready_drone_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+        this->land_service = this->create_service<drone_services::srv::Land>("/drone/land", std::bind(&PositionChanger::handle_land_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

        this->failsafe_publisher = this->create_publisher<drone_services::msg::FailsafeMsg>("/drone/failsafe", 10);

        this->trajectory_client = this->create_client<drone_services::srv::SetTrajectory>("/drone/set_trajectory");
+        this->attitude_client = this->create_client<drone_services::srv::SetAttitude>("/drone/set_attitude");
        this->vehicle_control_client = this->create_client<drone_services::srv::SetVehicleControl>("/drone/set_vehicle_control");
        this->failsafe_client = this->create_client<drone_services::srv::EnableFailsafe>("/drone/enable_failsafe");
+        this->arm_drone_client = this->create_client<drone_services::srv::ArmDrone>("/drone/arm");

        RCLCPP_INFO(this->get_logger(), "waiting for trajectory service...");
-        wait_for_service<rclcpp::Client<drone_services::srv::SetTrajectory>::SharedPtr>(this->trajectory_client);
+        wait_for_service<rclcpp::Client<drone_services::srv::SetTrajectory>::SharedPtr>(this->trajectory_client, "/drone/set_trajectory");
+        RCLCPP_INFO(this->get_logger(), "waiting for attitude service...");
+        wait_for_service<rclcpp::Client<drone_services::srv::SetAttitude>::SharedPtr>(this->attitude_client, "/drone/set_attitude");
        RCLCPP_INFO(this->get_logger(), "waiting for vehicle control service...");
-        wait_for_service<rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedPtr>(this->vehicle_control_client);
+        wait_for_service<rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedPtr>(this->vehicle_control_client, "/drone/set_vehicle_control");
        RCLCPP_INFO(this->get_logger(), "waiting for failsafe service...");
-        wait_for_service<rclcpp::Client<drone_services::srv::EnableFailsafe>::SharedPtr>(this->failsafe_client);
+        wait_for_service<rclcpp::Client<drone_services::srv::EnableFailsafe>::SharedPtr>(this->failsafe_client, "/drone/enable_failsafe");
+        RCLCPP_INFO(this->get_logger(), "waiting for arm service...");
+        wait_for_service<rclcpp::Client<drone_services::srv::ArmDrone>::SharedPtr>(this->arm_drone_client, "/drone/arm");

        this->trajectory_request = std::make_shared<drone_services::srv::SetTrajectory::Request>();
+        this->attitude_request = std::make_shared<drone_services::srv::SetAttitude::Request>();
        this->vehicle_control_request = std::make_shared<drone_services::srv::SetVehicleControl::Request>();
        this->failsafe_request = std::make_shared<drone_services::srv::EnableFailsafe::Request>();
+        this->arm_drone_request = std::make_shared<drone_services::srv::ArmDrone::Request>();

        lidar_health_timer = this->create_wall_timer(1s, std::bind(&PositionChanger::check_lidar_health, this));

@@ -84,6 +101,57 @@ public:
        if (status == std::future_status::ready)
        {
            RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", future.get()->success);
+            if (this->got_ready_drone_request && future.get()->success)
+            {
+                auto arm_response = this->arm_drone_client->async_send_request(this->arm_drone_request, std::bind(&PositionChanger::arm_drone_service_callback, this, std::placeholders::_1));
+            }
+        }
+        else
+        {
+            RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
+        }
+    }
+
+/**
+ * @brief callback function for the /drone/arm service
+ * 
+ * @param future the future object that holds the result of the service call
+ */
+    void arm_drone_service_callback(rclcpp::Client<drone_services::srv::ArmDrone>::SharedFuture future)
+    {
+        auto status = future.wait_for(1s);
+        if (status == std::future_status::ready)
+        {
+
+            RCLCPP_INFO(this->get_logger(), "Arm result: %d", future.get()->success);
+            if (this->got_ready_drone_request)
+            {
+                this->got_ready_drone_request = false;
+
+                this->attitude_request->pitch = 0.0;
+                this->attitude_request->yaw = 0.0;
+                this->attitude_request->roll = 0.0;
+                this->attitude_request->thrust = 0.15;
+                auto attitude_response = this->attitude_client->async_send_request(this->attitude_request, std::bind(&PositionChanger::attitude_message_callback, this, std::placeholders::_1));
+            }
+        }
+        else
+        {
+            RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
+        }
+    }
+
+/**
+ * @brief callback function for the /drone/set_attitude service
+ * 
+ * @param future the future object that holds the result of the service call
+ */
+    void attitude_message_callback(rclcpp::Client<drone_services::srv::SetAttitude>::SharedFuture future)
+    {
+        auto status = future.wait_for(1s);
+        if (status == std::future_status::ready)
+        {
+            RCLCPP_INFO(this->get_logger(), "Attitude set result: %d", future.get()->success);
        }
        else
        {
@@ -133,7 +201,6 @@ public:
     */
    void send_trajectory_message()
    {
-        check_move_direction_allowed();
        this->trajectory_request->values[0] = this->current_speed_x;
        this->trajectory_request->values[1] = this->current_speed_y;
        this->trajectory_request->values[2] = this->current_speed_z;
@@ -145,10 +212,10 @@ public:

    /**
     * @brief Enables the failsafe with the specified message
-     * 
+     *
     * @param message the message indicating the cause of the failsafe
     */
-    void enable_failsafe(std::string message)
+    void enable_failsafe(std::u16string message)
    {
        this->failsafe_enabled = true;
        this->failsafe_request->message = message;
@@ -162,33 +229,25 @@ public:
     */
    void apply_collision_weights()
    {
-        if (this->current_speed_x > 0) // if moving forward
+        if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT])
        {
-            if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT])
-            {
-                this->current_speed_x = collision_prevention_weights[MOVE_DIRECTION_FRONT];
-            }
+            RCLCPP_INFO(this->get_logger(), "Collision prevention front: %f", collision_prevention_weights[MOVE_DIRECTION_FRONT]);
+            get_x_y_with_rotation(collision_prevention_weights[MOVE_DIRECTION_FRONT], 0, this->current_yaw, &this->current_speed_x, &this->current_speed_y);
        }
-        else // moving backward
+        if (!this->move_direction_allowed[MOVE_DIRECTION_BACK])
        {
-            if (!this->move_direction_allowed[MOVE_DIRECTION_BACK])
-            {
-                this->current_speed_x = collision_prevention_weights[MOVE_DIRECTION_BACK];
-            }
+            RCLCPP_INFO(this->get_logger(), "Collision prevention back: %f", collision_prevention_weights[MOVE_DIRECTION_BACK]);
+            get_x_y_with_rotation(collision_prevention_weights[MOVE_DIRECTION_BACK], 0, this->current_yaw, &this->current_speed_x, &this->current_speed_y);
        }
-        if (this->current_speed_y > 0) // moving right
+        if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT])
        {
-            if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT])
-            {
-                this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_RIGHT];
-            }
+            RCLCPP_INFO(this->get_logger(), "Collision prevention right: %f", collision_prevention_weights[MOVE_DIRECTION_RIGHT]);
+            get_x_y_with_rotation(0, collision_prevention_weights[MOVE_DIRECTION_RIGHT], this->current_yaw, &this->current_speed_x, &this->current_speed_y);
        }
-        else // moving left
+        if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT])
        {
-            if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT])
-            {
-                this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_LEFT];
-            }
+            RCLCPP_INFO(this->get_logger(), "Collision prevention left: %f", collision_prevention_weights[MOVE_DIRECTION_LEFT]);
+            get_x_y_with_rotation(0, collision_prevention_weights[MOVE_DIRECTION_LEFT], this->current_yaw, &this->current_speed_x, &this->current_speed_y);
        }
    }

@@ -212,10 +271,80 @@ public:
                                                                                                              : (MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RL], this->lidar_sensor_values[LIDAR_SENSOR_RR]));
        collision_prevention_weights[MOVE_DIRECTION_RIGHT] = this->move_direction_allowed[MOVE_DIRECTION_RIGHT] ? 0
                                                                                                                : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RR], this->lidar_sensor_values[LIDAR_SENSOR_FR]));
-
        apply_collision_weights();
    }

+    /**
+     * @brief Callback for when a set_attitude request is made while landing
+     *
+     * @param future the future object containing the result of the request.
+     */
+    void attitude_land_callback(rclcpp::Client<drone_services::srv::SetAttitude>::SharedFuture future)
+    {
+        auto status = future.wait_for(1s);
+        if (status == std::future_status::ready)
+        {
+            if (future.get()->success)
+            {
+                RCLCPP_INFO(this->get_logger(), "Attitude set to 0 for landing, landing done");
+                this->has_landed = true;
+            }
+        }
+        else
+        {
+            RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
+        }
+    }
+
+    /**
+     * @brief Callback for when a set_vehicle_control request is made while landing
+     *
+     * @param future the future object containing the result of the request.
+     */
+    void vehicle_control_land_request_callback(rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedFuture future)
+    {
+        auto status = future.wait_for(1s);
+        if (status == std::future_status::ready)
+        {
+            if (future.get()->success)
+            {
+                RCLCPP_INFO(this->get_logger(), "Vehicle Control mode set to attitude for landing");
+                this->attitude_request->pitch = 0;
+                this->attitude_request->roll = 0;
+                this->attitude_request->yaw = 0;
+                this->attitude_request->thrust = 0;
+                auto attitude_response = this->attitude_client->async_send_request(this->attitude_request, std::bind(&PositionChanger::attitude_land_callback, this, std::placeholders::_1));
+            }
+        }
+        else
+        {
+            RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
+        }
+    }
+
+    /**
+     * @brief Callback method for receiving new height data
+     *
+     * @param message the message containing the height data
+     */
+    void handle_height_message(const drone_services::msg::HeightData::SharedPtr message)
+    {
+        this->current_height = message->min_height;
+        if (!this->got_ready_drone_request)
+        {
+            this->start_height = message->min_height;
+        }
+        if (this->is_landing)
+        {
+            if (this->current_height <= this->start_height + HEIGHT_DELTA)
+            {
+                this->vehicle_control_request->control = 4;
+                auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request,
+                                                                                              std::bind(&PositionChanger::vehicle_control_land_request_callback, this, std::placeholders::_1));
+            }
+        }
+    }
+
    /**
     * @brief Callback function for receiving new LIDAR data
     *
@@ -223,6 +352,7 @@ public:
     */
    void handle_lidar_message(const drone_services::msg::LidarReading::SharedPtr message)
    {
+        this->has_received_first_lidar_message = true;

        this->received_lidar_message = true;
        if (message->sensor_1 > 0)
@@ -252,14 +382,17 @@ public:

    /**
     * @brief Checks if the LIDAR is still sending messages. If not, enable failsafe
-     * 
+     *
     */
    void check_lidar_health()
    {
-        if (!this->received_lidar_message)
+        if (this->has_received_first_lidar_message)
        {
-            RCLCPP_WARN(this->get_logger(), "Lidar not sending messages, enabling failsafe");
-            enable_failsafe("No healthy connection to LIDAR!");
+            if (!this->received_lidar_message)
+            {
+                RCLCPP_WARN(this->get_logger(), "Lidar not sending messages, enabling failsafe");
+                enable_failsafe(u"No healthy connection to LIDAR! Check the LIDAR USB cable and restart the drone.");
+            }
        }
        this->received_lidar_message = false;
    }
@@ -291,6 +424,11 @@ public:
    {
        if (!this->failsafe_enabled)
        {
+            if (!this->has_received_first_lidar_message)
+            {
+                this->enable_failsafe(u"Waiting for LIDAR timed out! Check the LIDAR USB connection and consider restarting the drone.");
+                return;
+            }
            RCLCPP_INFO(this->get_logger(), "Incoming request\nfront_back: %f\nleft_right: %f\nup_down: %f\nangle: %f", request->front_back, request->left_right, request->up_down, request->angle);
            if (request->angle > 360 || request->angle < -360)
            {
@@ -298,17 +436,72 @@ public:
                response->success = false;
                return;
            }
+            if (this->vehicle_control_request->control != DEFAULT_CONTROL_MODE)
+            {
+                this->vehicle_control_request->control = DEFAULT_CONTROL_MODE;
+                auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request,
+                                                                                              std::bind(&PositionChanger::vehicle_control_service_callback, this, std::placeholders::_1));
+            }
            this->current_yaw += request->angle * (M_PI / 180.0); // get the angle in radians
            this->current_speed_z = request->up_down;
            get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y);
+            check_move_direction_allowed();
            RCLCPP_INFO(this->get_logger(), "Calculated speed x: %f, y: %f", this->current_speed_x, this->current_speed_y);
            send_trajectory_message();
            response->success = true;
-        } else {
+        }
+        else
+        {
            response->success = false;
        }
    }

+    /**
+     * @brief Callback function for when the /drone/land service is called
+     *
+     * @param request_header the header of the request
+     * @param request the request to land
+     * @param response the response to send back.
+     */
+    void handle_land_request(
+        const std::shared_ptr<rmw_request_id_t> request_header,
+        const std::shared_ptr<drone_services::srv::Land::Request> request,
+        const std::shared_ptr<drone_services::srv::Land::Response> response)
+    {
+        this->is_landing = true;
+        response->is_landing = this->is_landing;
+    }
+
+    /**
+     * @brief Callback function for when the /drone/ready_drone service is called
+     *
+     * @param request_header the header of the request
+     * @param request the request to ready the drone
+     * @param response the response to send back. Contains true if the request was successful, false otherwise
+     */
+    void handle_ready_drone_request(
+        const std::shared_ptr<rmw_request_id_t> request_header,
+        const std::shared_ptr<drone_services::srv::ReadyDrone::Request> request,
+        const std::shared_ptr<drone_services::srv::ReadyDrone::Response> response)
+    {
+        if (this->failsafe_enabled)
+        {
+            response->success = false;
+            return;
+        }
+        if (!this->has_received_first_lidar_message)
+        {
+            this->enable_failsafe(u"Waiting for LIDAR timed out! Check the LIDAR USB connection and consider restarting the drone.");
+            response->success = false;
+            return;
+        }
+        this->got_ready_drone_request = true;
+        this->vehicle_control_request->control = CONTROL_MODE_ATTITUDE;
+        auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request,
+                                                                                      std::bind(&PositionChanger::vehicle_control_service_callback, this, std::placeholders::_1));
+        response->success = true;        
+    }
+
    /**
     * @brief Get the yaw angle from a specified normalized quaternion.
     * Uses the theory from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
@@ -343,18 +536,24 @@ private:
    rclcpp::Publisher<drone_services::msg::FailsafeMsg>::SharedPtr failsafe_publisher;
    rclcpp::Subscription<drone_services::msg::LidarReading>::SharedPtr lidar_subscription;
    rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr odometry_subscription;
+    rclcpp::Subscription<drone_services::msg::HeightData>::SharedPtr height_subscription;

    rclcpp::Client<drone_services::srv::SetTrajectory>::SharedPtr trajectory_client;
+    rclcpp::Client<drone_services::srv::SetAttitude>::SharedPtr attitude_client;
    rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedPtr vehicle_control_client;
    rclcpp::Client<drone_services::srv::EnableFailsafe>::SharedPtr failsafe_client;
+    rclcpp::Client<drone_services::srv::ArmDrone>::SharedPtr arm_drone_client;

    rclcpp::Service<drone_services::srv::MovePosition>::SharedPtr move_position_service;
-
+    rclcpp::Service<drone_services::srv::ReadyDrone>::SharedPtr ready_drone_service;
+    rclcpp::Service<drone_services::srv::Land>::SharedPtr land_service;
    rclcpp::TimerBase::SharedPtr lidar_health_timer;

    drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request;
+    drone_services::srv::SetAttitude::Request::SharedPtr attitude_request;
    drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request;
    drone_services::srv::EnableFailsafe::Request::SharedPtr failsafe_request;
+    drone_services::srv::ArmDrone::Request::SharedPtr arm_drone_request;

    float lidar_sensor_values[4]{MIN_DISTANCE}; // last distance measured by the lidar sensors
    float lidar_imu_readings[4];                // last imu readings from the lidar sensors
@@ -362,10 +561,17 @@ private:
    float current_speed_x = 0;
    float current_speed_y = 0;
    float current_speed_z = 0;
+    float current_height = 0;
+    float start_height = -1;
+    bool is_landing = false;
+    bool has_landed = false;
    bool move_direction_allowed[4] = {true};     // whether the drone can move in a certain direction
    float collision_prevention_weights[4] = {0}; // the amount to move away from an object in a certain direction if the drone is too close
    bool failsafe_enabled = false;
    bool received_lidar_message = false;
+    int lidar_health_checks = 0;
+    bool has_received_first_lidar_message = false;
+    bool got_ready_drone_request = false;

    /**
     * @brief waits for a service to be available
@@ -374,7 +580,7 @@ private:
     * @param client the client object to wait for the service
     */
    template <class T>
-    void wait_for_service(T client)
+    void wait_for_service(T client, std::string service_name)
    {
        while (!client->wait_for_service(1s))
        {
@@ -383,7 +589,7 @@ private:
                RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
                return;
            }
-            RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
+            RCLCPP_INFO(this->get_logger(), "service not available, waiting again on service %s", service_name.c_str());
        }
    }
 };
--- a/src/drone_controls/test/test_positionchanger.py
+++ b/src/drone_controls/test/test_positionchanger.py
@@ -0,0 +1,164 @@
+import unittest
+
+import launch
+import launch_ros
+import launch_ros.actions
+import launch_testing.actions
+import pytest
+import rclpy
+import time
+
+from drone_services.srv import MovePosition
+from drone_services.msg import FailsafeMsg
+from drone_services.msg import LidarReading
+
+
+@pytest.mark.rostest
+def generate_test_description():
+    positionchanger_node = launch_ros.actions.Node(
+        package='drone_controls', executable='position_changer')
+    failsafe_node = launch_ros.actions.Node(
+        package='failsafe', executable='failsafe')
+    px4_controller_node = launch_ros.actions.Node(
+        package='px4_connection', executable='px4_controller')
+    heartbeat_node = launch_ros.actions.Node(
+        package='px4_connection', executable='heartbeat')
+
+    return (
+        launch.LaunchDescription([
+            positionchanger_node,
+            failsafe_node,
+            px4_controller_node,
+            heartbeat_node,
+            launch_testing.actions.ReadyToTest(),
+        ]),
+        {
+            'positionchanger_node': positionchanger_node,
+            'failsafe_node': failsafe_node,
+            'px4_controller_node': px4_controller_node,
+            'heartbeat_node': heartbeat_node
+        }
+    )
+
+
+class TestPositionChanger(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+
+    def setUp(self):
+        self.node = rclpy.create_node('test_positionchanger')
+        self.called_positionchanger_service = False
+        self.received_failsafe_callback = False
+
+    def tearDown(self):
+        self.node.destroy_node()
+
+    def failsafe_callback(self, msg):
+        self.assertTrue(msg.enabled, "Failsafe was not enabled!")
+        self.received_failsafe_callback = True
+
+    def move_position_callback(self, future):
+        self.assertFalse(future.result(
+        ).success, "MovePosition service call was successful, but should have failed!")
+        self.called_positionchanger_service = True
+
+    def test_positionchanger_no_lidar_data(self, positionchanger_node, proc_output):
+        self.received_failsafe_callback = False
+        self.called_positionchanger_service = False
+        failsafe_subscriber = self.node.create_subscription(
+            FailsafeMsg, '/drone/failsafe', self.failsafe_callback, 10)
+        move_position_client = self.node.create_client(
+            MovePosition, '/drone/move_position')
+        while not move_position_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('move_position service not available, waiting again...')
+
+        request = MovePosition.Request()
+        request.front_back = 1.0
+        request.left_right = 1.0
+        request.up_down = 1.0
+        request.angle = 1.0
+
+        end_time = time.time() + 10.0
+        try:
+            while time.time() < end_time:
+                rclpy.spin_once(self.node)
+                if not self.called_positionchanger_service:
+                    future = move_position_client.call_async(request)
+                    future.add_done_callback(self.move_position_callback)
+                elif not self.received_failsafe_callback:
+                    continue
+                else:
+                    break
+            self.assertTrue(self.received_failsafe_callback,
+                            "Failsafe callback was not received!")
+            self.assertTrue(self.called_positionchanger_service,
+                            "MovePosition service was not called!")
+        finally:
+            self.node.destroy_client(move_position_client)
+            self.node.destroy_subscription(failsafe_subscriber)
+
+    def test_positionchanger_lidar_stops(self, positionchanger_node, proc_output):
+        self.node.get_logger().info("STARTING TEST test_positionchanger_lidar_stops")
+        self.received_failsafe_callback = False
+        self.called_positionchanger_service = False
+        failsafe_subscriber = self.node.create_subscription(
+            FailsafeMsg, '/drone/failsafe', self.failsafe_callback, 10)
+        lidar_publisher = self.node.create_publisher(
+            LidarReading, '/drone/object_detection', 10)
+        move_position_client = self.node.create_client(
+            MovePosition, '/drone/move_position')
+
+        while not move_position_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('move_position service not available, waiting again...')
+
+        request = MovePosition.Request()
+        request.front_back = 1.0
+        request.left_right = 1.0
+        request.up_down = 1.0
+        request.angle = 1.0
+
+        lidar_msg = LidarReading()
+        lidar_msg.sensor_1 = 2.0
+        lidar_msg.sensor_2 = 2.0
+        lidar_msg.sensor_3 = 2.0
+        lidar_msg.sensor_4 = 2.0
+        lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0]
+        sent_lidar_msg = False
+
+        # wait for nodes to become active
+        time.sleep(3)
+
+        # wait 5 seconds for the failsafe to trigger
+        wait_time = time.time() + 5.0
+        end_time = time.time() + 10.0
+        try:
+            self.node.get_logger().info('STARTING WHILE LOOP')
+            while time.time() < end_time:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+                if not sent_lidar_msg:
+                    lidar_publisher.publish(lidar_msg)
+                    sent_lidar_msg = True
+                # wait 5 seconds before sending the move_position request
+                if not self.called_positionchanger_service and time.time() > wait_time:
+                    self.node.get_logger().info('Sending move_position request')
+                    future = move_position_client.call_async(request)
+                    future.add_done_callback(self.move_position_callback)
+                elif not self.received_failsafe_callback:
+                    continue
+            self.node.get_logger().info('END OF WHILE LOOP')
+            self.assertTrue(self.called_positionchanger_service,
+                            "MovePosition service was not called!")
+            self.assertTrue(self.received_failsafe_callback,
+                            "Failsafe was not activated!")
+
+        finally:
+            self.node.get_logger().info('Cleaning up')
+            self.node.destroy_client(move_position_client)
+            self.node.destroy_subscription(failsafe_subscriber)
+            self.node.destroy_publisher(lidar_publisher)
--- a/src/drone_controls/test/test_positionchanger_height.py
+++ b/src/drone_controls/test/test_positionchanger_height.py
@@ -0,0 +1,158 @@
+import os
+import sys
+import unittest
+
+import launch
+import launch_ros
+import launch_ros.actions
+import launch_testing.actions
+import pytest
+import rclpy
+import time
+
+from drone_services.srv import MovePosition
+from drone_services.srv import ArmDrone
+from drone_services.srv import Land
+from drone_services.msg import HeightData
+
+@pytest.mark.rostest
+def generate_test_description():
+    file_path = os.path.dirname(__file__)
+    # device under test
+    positionchanger_node = launch_ros.actions.Node(
+        package='drone_controls', executable='position_changer')
+    failsafe_node = launch_ros.actions.Node(
+        package='failsafe', executable='failsafe')
+    px4_controller_node = launch_ros.actions.Node(
+        package='px4_connection', executable='px4_controller')
+    heartbeat_node = launch_ros.actions.Node(
+        package='px4_connection', executable='heartbeat')
+
+    return (
+        launch.LaunchDescription([
+            positionchanger_node,
+            failsafe_node,
+            px4_controller_node,
+            heartbeat_node,
+            launch_testing.actions.ReadyToTest(),
+        ]),
+        {
+            'positionchanger_node': positionchanger_node,
+            'failsafe_node': failsafe_node,
+            'px4_controller_node': px4_controller_node,
+            'heartbeat_node': heartbeat_node
+        }
+    )
+
+class TestPositionChanger(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+
+    def setUp(self):
+        self.node = rclpy.create_node('test_positionchanger')
+        self.called_arm_service = False
+        self.called_land_service = False
+        self.called_move_service_up = False
+        self.called_move_service_stop = False
+        self.moved_up = False
+
+    def tearDown(self):
+        self.node.destroy_node()
+
+    def move_position_callback_up(self, future):
+        self.node.get_logger().info("Callback called")
+        # self.assertTrue(future.result().success, "Move service failed")
+        self.called_move_service_up = True
+    
+    def move_position_callback_stop(self, future):
+        self.node.get_logger().info("Callback called")
+        # self.assertTrue(future.result().success, "Move service failed")
+        self.called_move_service_stop = True
+    
+    def arm_callback(self, future):
+        self.node.get_logger().info("Arm Callback called")
+        # self.assertTrue(future.result().success, "Arm service failed")
+        self.called_arm_service = True
+        
+    def land_callback(self,future):
+        self.node.get_logger().info("Land Callback called")
+        self.assertTrue(future.result().is_landing, "Drone is not landing")
+        self.called_land_service = True
+
+    def test_positionchanger_land(self, proc_output):
+        self.node.get_logger().info("starting land test")
+        height_data_publisher = self.node.create_publisher(HeightData, '/drone/height', 10)
+        height_msg = HeightData()
+        height_msg.heights = [0.2,0.2,0.2,0.2]
+        height_msg.min_height = 0.2
+        arm_client = self.node.create_client(ArmDrone, '/drone/arm')
+        while not arm_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('arm service not available, waiting again...')
+        land_client = self.node.create_client(Land, '/drone/land')
+        while not land_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('land service not available, waiting again...')
+        move_client = self.node.create_client(MovePosition, '/drone/move_position')
+        while not move_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('move_position service not available, waiting again...')
+
+        arm_request = ArmDrone.Request()
+        land_request = Land.Request()
+        move_request = MovePosition.Request()
+        move_request.front_back = 0.0
+        move_request.left_right = 0.0
+        move_request.up_down = 5.0
+        move_request.angle = 0.0
+
+        start_height_msgs_published = 0
+        moving_height_msgs_published = 0
+        landing_height_msgs_published = 0
+        
+        end_time = time.time() + 20.0
+        try:
+            while time.time() < end_time:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+                if start_height_msgs_published < 10:
+                    height_data_publisher.publish(height_msg)
+                    start_height_msgs_published += 1
+                elif not self.called_arm_service:
+                    arm_future = arm_client.call_async(arm_request)
+                    arm_future.add_done_callback(self.arm_callback)
+                elif not self.called_move_service_up:
+                    move_future = move_client.call_async(move_request)
+                    move_future.add_done_callback(self.move_position_callback_up)
+                elif moving_height_msgs_published < 10:
+                    height_msg.min_height += 0.1
+                    height_data_publisher.publish(height_msg)
+                    moving_height_msgs_published += 1
+                elif not self.called_move_service_stop:
+                    move_request.up_down = 0.0
+                    move_future = move_client.call_async(move_request)
+                    move_future.add_done_callback(self.move_position_callback_stop)
+                elif not self.called_land_service:
+                    land_future = land_client.call_async(land_request)
+                    land_future.add_done_callback(self.land_callback)
+                elif landing_height_msgs_published < 10:
+                    height_msg.min_height -= 0.15
+                    height_data_publisher.publish(height_msg)
+                    landing_height_msgs_published += 1
+            
+            launch_testing.asserts.assertInStderr(proc_output, "Attitude set to 0 for landing, landing done", 'position_changer-1')
+            launch_testing.asserts.assertInStderr(proc_output, "Vehicle Control mode set to attitude for landing", 'position_changer-1')
+        finally:
+            self.node.get_logger().info("shutting down")
+            self.node.destroy_client(arm_client)
+            self.node.destroy_client(land_client)
+            self.node.destroy_client(move_client)
+            self.node.destroy_publisher(height_data_publisher)
+        #publish height data as start value
+        #arm
+        #move up
+        #hang still
+        #land
+        #check if landed
--- a/src/drone_controls/test/test_positionchanger_lidar.py
+++ b/src/drone_controls/test/test_positionchanger_lidar.py
@@ -0,0 +1,240 @@
+import os
+import unittest
+
+import launch
+import launch_ros
+import launch_ros.actions
+import launch_testing.actions
+import pytest
+import rclpy
+import time
+
+from drone_services.srv import MovePosition
+from drone_services.msg import LidarReading
+
+@pytest.mark.rostest
+def generate_test_description():
+    file_path = os.path.dirname(__file__)
+    # device under test
+    positionchanger_node = launch_ros.actions.Node(
+        package='drone_controls', executable='position_changer')
+    failsafe_node = launch_ros.actions.Node(
+        package='failsafe', executable='failsafe')
+    px4_controller_node = launch_ros.actions.Node(
+        package='px4_connection', executable='px4_controller')
+    heartbeat_node = launch_ros.actions.Node(
+        package='px4_connection', executable='heartbeat')
+
+    return (
+        launch.LaunchDescription([
+            positionchanger_node,
+            failsafe_node,
+            px4_controller_node,
+            heartbeat_node,
+            launch_testing.actions.ReadyToTest(),
+        ]),
+        {
+            'positionchanger_node': positionchanger_node,
+            'failsafe_node': failsafe_node,
+            'px4_controller_node': px4_controller_node,
+            'heartbeat_node': heartbeat_node
+        }
+    )
+
+class TestPositionChanger(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+
+    def setUp(self):
+        self.node = rclpy.create_node('test_positionchanger')
+        self.called_positionchanger_service = False
+        self.received_failsafe_callback = False
+        self.lidar_publisher = self.node.create_publisher(
+            LidarReading, '/drone/object_detection', 10)
+        self.move_position_client = self.node.create_client(
+            MovePosition, '/drone/move_position')
+        while not self.move_position_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('move_position service not available, waiting again...')
+        self.request = MovePosition.Request()
+
+    def tearDown(self):
+        self.node.destroy_client(self.move_position_client)
+        self.node.destroy_publisher(self.lidar_publisher)
+        self.node.destroy_node()
+
+    def toRadians(self, degrees) -> float:
+        return degrees * 3.14159265359 / 180
+
+
+    def move_position_callback(self, future):
+        self.node.get_logger().info("Callback called")
+        self.called_positionchanger_service = True
+
+    def test_positionchanger_lidar_moves_away_front(self, proc_output):
+        self.node.get_logger().info("starting front test")
+        self.request.front_back = 1.0
+        self.request.left_right = 0.0
+        self.request.up_down = 0.0
+        self.request.angle = 0.0
+
+        lidar_msgs_sent = 0
+
+        lidar_msg = LidarReading()
+        lidar_msg.sensor_1 = 0.5 # front right
+        lidar_msg.sensor_2 = 2.0 # front left
+        lidar_msg.sensor_3 = 2.0 # rear left
+        lidar_msg.sensor_4 = 2.0 # rear right
+        lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0]
+        end_time = time.time() + 10.0
+        
+        while time.time() < end_time:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            self.lidar_publisher.publish(lidar_msg)
+            lidar_msgs_sent += 1
+            if (lidar_msgs_sent == 10):
+                lidar_msg.sensor_1 = 1.0
+                lidar_msg.sensor_2 = 0.3
+            elif (lidar_msgs_sent == 20):
+                break
+            if not self.called_positionchanger_service:
+                future = self.move_position_client.call_async(self.request)
+                future.add_done_callback(self.move_position_callback)
+        launch_testing.asserts.assertInStderr(proc_output, "Collision prevention front: -0.5", 'position_changer-1')
+        launch_testing.asserts.assertInStderr(proc_output, "Collision prevention front: -0.7", 'position_changer-1')
+
+    def test_positionchanger_lidar_moves_away_right(self, proc_output):
+        self.node.get_logger().info("starting right test")
+        self.request.front_back = 0.0
+        self.request.left_right = 1.0
+        self.request.up_down = 0.0
+        self.request.angle = 0.0
+
+        lidar_msgs_sent = 0
+
+        lidar_msg = LidarReading()
+        lidar_msg.sensor_1 = 0.4 # front right
+        lidar_msg.sensor_2 = 2.0 # front left
+        lidar_msg.sensor_3 = 2.0 # rear left
+        lidar_msg.sensor_4 = 2.0 # rear right
+        lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0]
+        end_time = time.time() + 10.0
+        
+        while time.time() < end_time:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            self.lidar_publisher.publish(lidar_msg)
+            lidar_msgs_sent += 1
+            if (lidar_msgs_sent == 10):
+                lidar_msg.sensor_1 = 2.0
+                lidar_msg.sensor_4 = 0.29
+            elif (lidar_msgs_sent == 20):
+                break
+            if not self.called_positionchanger_service:
+                future = self.move_position_client.call_async(self.request)
+                future.add_done_callback(self.move_position_callback)
+        launch_testing.asserts.assertInStderr(proc_output, "Collision prevention right: -0.6", 'position_changer-1')
+        launch_testing.asserts.assertInStderr(proc_output, "Collision prevention right: -0.71", 'position_changer-1')
+
+    def test_positionchanger_lidar_moves_away_left(self, proc_output):
+        self.node.get_logger().info("starting left test")
+        self.request.front_back = 0.0
+        self.request.left_right = -1.0
+        self.request.up_down = 0.0
+        self.request.angle = 0.0
+
+        lidar_msgs_sent = 0
+
+        lidar_msg = LidarReading()
+        lidar_msg.sensor_1 = 2.0 # front right
+        lidar_msg.sensor_2 = 0.11 # front left
+        lidar_msg.sensor_3 = 2.0 # rear left
+        lidar_msg.sensor_4 = 2.0 # rear right
+        lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0]
+        end_time = time.time() + 10.0
+        
+        while time.time() < end_time:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            self.lidar_publisher.publish(lidar_msg)
+            lidar_msgs_sent += 1
+            if (lidar_msgs_sent == 10):
+                lidar_msg.sensor_2 = 2.0
+                lidar_msg.sensor_3 = 0.78
+            elif (lidar_msgs_sent == 20):
+                break
+            if not self.called_positionchanger_service:
+                future = self.move_position_client.call_async(self.request)
+                future.add_done_callback(self.move_position_callback)
+        launch_testing.asserts.assertInStderr(proc_output, "Collision prevention left: 0.89", 'position_changer-1')
+        launch_testing.asserts.assertInStderr(proc_output, "Collision prevention left: 0.22", 'position_changer-1')
+
+    def test_positionchanger_lidar_moves_away_back(self, proc_output):
+        self.node.get_logger().info("starting back test")
+        self.request.front_back = -1.0
+        self.request.left_right = 0.0
+        self.request.up_down = 0.0
+        self.request.angle = 0.0
+
+        lidar_msgs_sent = 0
+
+        lidar_msg = LidarReading()
+        lidar_msg.sensor_1 = 2.0 # front right
+        lidar_msg.sensor_2 = 2.0 # front left
+        lidar_msg.sensor_3 = 0.36 # rear left
+        lidar_msg.sensor_4 = 2.0 # rear right
+        lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0]
+        #this test is carried out first, so wait for nodes to start
+        time.sleep(5)
+        end_time = time.time() + 20.0
+        
+        while time.time() < end_time:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            self.lidar_publisher.publish(lidar_msg)
+            lidar_msgs_sent += 1
+            if (lidar_msgs_sent == 10):
+                lidar_msg.sensor_3 = 2.0
+                lidar_msg.sensor_4 = 0.12
+            elif (lidar_msgs_sent == 20):
+                break
+            if not self.called_positionchanger_service:
+                future = self.move_position_client.call_async(self.request)
+                future.add_done_callback(self.move_position_callback)
+        launch_testing.asserts.assertInStderr(proc_output, "Collision prevention back: 0.64", 'position_changer-1')
+        launch_testing.asserts.assertInStderr(proc_output, "Collision prevention back: 0.88", 'position_changer-1')
+
+    def test_positionchanger_lidar_moves_away_still(self, proc_output):
+        self.node.get_logger().info("starting back test")
+        self.request.front_back = 0.0
+        self.request.left_right = 0.0
+        self.request.up_down = 0.0
+        self.request.angle = 0.0
+
+        lidar_msgs_sent = 0
+
+        lidar_msg = LidarReading()
+        lidar_msg.sensor_1 = 2.0 # front right
+        lidar_msg.sensor_2 = 2.0 # front left
+        lidar_msg.sensor_3 = 2.0 # rear left
+        lidar_msg.sensor_4 = 0.12 # rear right
+        lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0]
+        end_time = time.time() + 10.0
+        
+        while time.time() < end_time:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            self.lidar_publisher.publish(lidar_msg)
+            lidar_msgs_sent += 1
+            if (lidar_msgs_sent == 10):
+                lidar_msg.sensor_4 = 2.0
+                lidar_msg.sensor_3 = 0.36
+            elif (lidar_msgs_sent == 20):
+                break
+            if not self.called_positionchanger_service:
+                future = self.move_position_client.call_async(self.request)
+                future.add_done_callback(self.move_position_callback)
+        launch_testing.asserts.assertInStderr(proc_output, "Collision prevention back: 0.64", 'position_changer-1')
+        launch_testing.asserts.assertInStderr(proc_output, "Collision prevention back: 0.88", 'position_changer-1')
+    
--- a/src/drone_services/CMakeLists.txt
+++ b/src/drone_services/CMakeLists.txt
@@ -31,6 +31,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/ControlRelais.srv"
  "srv/MovePosition.srv"
  "srv/EnableFailsafe.srv"
+  "srv/ReadyDrone.srv"
+  "srv/Land.srv"
  "msg/DroneControlMode.msg"
  "msg/DroneArmStatus.msg"
  "msg/DroneRouteStatus.msg"
@@ -38,6 +40,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/HeightData.msg"
  "msg/LidarReading.msg"
  "msg/MultiflexReading.msg"
+  "msg/FailsafeMsg.msg"
 )

 if(BUILD_TESTING)
--- a/src/drone_services/msg/DroneStatus.msg
+++ b/src/drone_services/msg/DroneStatus.msg
@@ -2,4 +2,8 @@ float32 battery_percentage
 float32 cpu_usage
 int32 route_setpoint # -1 if no route
 wstring control_mode
-bool armed
+bool armed
+float32[3] velocity
+float32[3] position
+float32 height
+bool failsafe
--- a/src/drone_services/srv/Land.srv
+++ b/src/drone_services/srv/Land.srv
@@ -0,0 +1,2 @@
+---
+bool is_landing
--- a/src/drone_services/srv/ReadyDrone.srv
+++ b/src/drone_services/srv/ReadyDrone.srv
@@ -0,0 +1,2 @@
+---
+bool success
--- a/src/drone_status/drone_status/drone_status.py
+++ b/src/drone_status/drone_status/drone_status.py
@@ -6,8 +6,10 @@ 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 drone_services.msg import HeightData
 from px4_msgs.msg import BatteryStatus
 from px4_msgs.msg import Cpuload
+from px4_msgs.msg import VehicleOdometry

 CONTROL_MODE_ATTITUDE = 1
 CONTROL_MODE_VELOCITY = 2
@@ -32,17 +34,23 @@ class DroneStatusNode(Node):
            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.height_data_subscriber = self.create_subscription(HeightData, '/drone/height', self.height_data_callback, 10)
        self.battery_status_subscriber = self.create_subscription(
            BatteryStatus, '/fmu/out/battery_status', self.battery_status_callback, qos_profile=qos_profile)
        self.cpu_load_subscriber = self.create_subscription(
            Cpuload, '/fmu/out/cpuload', self.cpu_load_callback, qos_profile=qos_profile)
+        self.vehicle_odometry_subscriber = self.create_subscription(
+            VehicleOdometry, "/fmu/out/vehicle_odometry", self.vehicle_odometry_callback, qos_profile=qos_profile)
        # publish every 0.5 seconds
        self.timer = self.create_timer(0.5, self.publish_status)
        self.armed = False
+        self.height = 0.0
        self.control_mode = "attitude"
        self.battery_percentage = 100.0
        self.cpu_usage = 0.0
        self.route_setpoint = 0
+        self.position = []
+        self.velocity = []

    def publish_status(self):
        msg = DroneStatus()
@@ -51,10 +59,14 @@ class DroneStatusNode(Node):
        msg.battery_percentage = self.battery_percentage
        msg.cpu_usage = self.cpu_usage
        msg.route_setpoint = self.route_setpoint
+        msg.position = self.position
+        msg.velocity = self.velocity
+        msg.height = self.height
        self.publisher.publish(msg)
-        self.get_logger().info('Publishing status message')
+        # self.get_logger().info('Publishing status message')

    def control_mode_callback(self, msg):
+        self.get_logger().info('Got control mode callback!')
        if msg.control_mode == CONTROL_MODE_ATTITUDE:
            self.control_mode = "attitude"
        elif msg.control_mode == CONTROL_MODE_VELOCITY:
@@ -64,7 +76,13 @@ class DroneStatusNode(Node):
        else:
            self.control_mode = "unknown"

+    def height_data_callback(self, msg):
+        self.height = msg.min_height
+
    def arm_status_callback(self, msg):
+        self.get_logger().info("Got arm status callback!")
+        if msg.armed:
+            self.get_logger().info("DRONE IS ARMED")
        self.armed = msg.armed

    def route_status_callback(self, msg):
@@ -76,6 +94,11 @@ class DroneStatusNode(Node):
    def cpu_load_callback(self, msg):
        self.cpu_usage = msg.load

+    def vehicle_odometry_callback(self, msg):
+        self.position = msg.position
+        self.velocity = msg.velocity
+        
+

 def main(args=None):
    rclpy.init(args=args)
--- a/src/failsafe/failsafe/failsafe.py
+++ b/src/failsafe/failsafe/failsafe.py
@@ -5,7 +5,7 @@ from drone_services.srv import EnableFailsafe
 from drone_services.msg import FailsafeMsg
 class FailSafe(Node):
    def __init__(self):
-        super().init("failsafe")
+        super().__init__("failsafe")
        self.failsafe_enabled = False
        self.failsafe_msg = ""
        self.get_logger().info("Failsafe node started")
@@ -18,14 +18,14 @@ class FailSafe(Node):
        self.failsafe_msg = request.message
        response.enabled = self.failsafe_enabled
        response.message = self.failsafe_msg
-        self.get_logger().info("Failsafe triggered")
+        self.get_logger().info("Failsafe triggered! Message: " + self.failsafe_msg)
        self.publish_failsafe()
        return response
    
    def publish_failsafe(self):
        msg = FailsafeMsg()
        msg.enabled = self.failsafe_enabled
-        msg.message = self.failsafe_msg
+        msg.msg = self.failsafe_msg
        self.failsafe_publisher.publish(msg)
        self.get_logger().info("Publishing failsafe message")

@@ -33,7 +33,7 @@ class FailSafe(Node):
 def main(args=None):
    rclpy.init(args=args)
    failsafe_node = FailSafe()
-    failsafe_node.spin()
+    rclpy.spin(failsafe_node)
    failsafe_node.destroy_node()
    rclpy.shutdown()

--- a/src/failsafe/package.xml
+++ b/src/failsafe/package.xml
@@ -8,10 +8,6 @@
  <license>Apache License 2.0</license>
  <depend>rclpy</depend>
  <depend>drone_services</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>
--- a/src/failsafe/test/test_copyright.py
+++ b/src/failsafe/test/test_copyright.py
@@ -1,23 +0,0 @@
-# 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'
--- a/src/failsafe/test/test_failsafe.py
+++ b/src/failsafe/test/test_failsafe.py
@@ -0,0 +1,86 @@
+import os
+import sys
+import unittest
+import time
+
+import launch
+import launch_ros
+import launch_ros.actions
+import launch_testing.actions
+import pytest
+import rclpy
+
+from drone_services.srv import EnableFailsafe
+from drone_services.msg import FailsafeMsg
+
+# launch node
+@pytest.mark.rostest
+def generate_test_description():
+    file_path = os.path.dirname(__file__)
+    failsafe_node = launch_ros.actions.Node(
+        executable=sys.executable,
+        arguments=[os.path.join(file_path, '..', 'failsafe', 'failsafe.py')],
+        additional_env={'PYTHONUNBUFFERED': '1'}
+    )
+
+    return (
+        launch.LaunchDescription([
+            failsafe_node,
+            launch_testing.actions.ReadyToTest(),
+        ]),
+        {
+            'failsafe_node': failsafe_node,
+        }
+    )
+
+class FailsafeUnitTest(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+    
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+    
+    def setUp(self):
+        self.node = rclpy.create_node('test_failsafe')
+        self.service_called = False
+
+    def tearDown(self):
+        self.node.destroy_node()
+
+    def service_call_callback(self,future):
+        self.assertIsNotNone(future.result())
+        self.assertTrue(future.result().enabled)
+        self.assertEqual(future.result().message, "test")
+        self.service_called = True
+    
+    def test_failsafe_node_enables(self,failsafe_node,proc_output):
+        failsafe_msgs = []
+        failsafe_subscription = self.node.create_subscription(FailsafeMsg, "/drone/failsafe", lambda msg: failsafe_msgs.append(msg), 10)
+        failsafe_client = self.node.create_client(EnableFailsafe, "/drone/enable_failsafe")
+        while not failsafe_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info("service not available, waiting again...")
+        request = EnableFailsafe.Request()
+        request.message = "test"
+
+        end_time = time.time() + 10.0
+
+        try:
+            while time.time() < end_time:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+                if (not self.service_called):
+                    future = failsafe_client.call_async(request)
+                    future.add_done_callback(self.service_call_callback)
+                else:
+                    break
+            self.assertTrue(failsafe_msgs[0].enabled)
+            self.assertEqual(failsafe_msgs[0].msg, "test")
+            self.assertTrue(self.service_called)
+        finally:
+            self.node.destroy_subscription(failsafe_subscription)
+            self.node.destroy_client(failsafe_client)
+                
+
+
--- a/src/failsafe/test/test_flake8.py
+++ b/src/failsafe/test/test_flake8.py
@@ -1,25 +0,0 @@
-# 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)
--- a/src/failsafe/test/test_pep257.py
+++ b/src/failsafe/test/test_pep257.py
@@ -1,23 +0,0 @@
-# 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'
--- a/src/px4_connection/CMakeLists.txt
+++ b/src/px4_connection/CMakeLists.txt
@@ -60,8 +60,12 @@ install(
    DESTINATION share/${PROJECT_NAME}
 )

+install(FILES
+    test/test_failsafe_enabled.py
+    DESTINATION lib/${PROJECT_NAME})
+
 if(BUILD_TESTING)
-    find_package(ament_lint_auto REQUIRED)
+    # 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
@@ -69,7 +73,9 @@ if(BUILD_TESTING)
    # 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()
+    # ament_lint_auto_find_test_dependencies()
+    find_package(launch_testing_ament_cmake REQUIRED)
+    add_launch_test(test/test_failsafe_enabled.py)
 endif()

 ament_package()
--- a/src/px4_connection/package.xml
+++ b/src/px4_connection/package.xml
@@ -15,9 +15,6 @@
    <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>
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -54,7 +54,7 @@ public:
        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));
        // subscription on receiving a new control mode
        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));
-        failsafe_subscription = this->create_subscription<drone_services::msg::FailsafeMsg>("/drone/failsafe", qos, std::bind(&PX4Controller::on_failsafe_receive, this, std::placeholders::_1));
+        failsafe_subscription_ = this->create_subscription<drone_services::msg::FailsafeMsg>("/drone/failsafe", qos, std::bind(&PX4Controller::on_failsafe_receive, this, std::placeholders::_1));
        // services for controlling the drone
        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));
@@ -102,17 +102,17 @@ private:

    char current_control_mode = CONTROL_MODE_ATTITUDE; // start with attitude control

-    const float omega = 0.3;       // angular speed of the POLAR trajectory
-    const float K = 0;             // [m] gain that regulates the spiral pitch
+    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;                                // initial rho of polar coordinate
-    float theta_0 = 0;                              // initial theta of polar coordinate
-    float p0_z = -0.0;                        // initial height
+    float rho_0 = 0;                              // initial rho of polar coordinate
+    float theta_0 = 0;                            // initial theta of polar coordinate
+    float p0_z = -0.0;                            // initial height
    float des_x = 0.0, des_y = 0.0, des_z = p0_z; // desired position
-    float gamma = M_PI_4;                           // desired heading direction
+    float gamma = M_PI_4;                         // desired heading direction

-    float local_x = 0;  // local position x
-    float local_y = 0;  // local position y
+    float local_x = 0; // local position x
+    float local_y = 0; // local position y

    bool failsafe_enabled = false;

@@ -130,6 +130,12 @@ private:
        const std::shared_ptr<drone_services::srv::SetAttitude::Request> request,
        const std::shared_ptr<drone_services::srv::SetAttitude::Response> response)
    {
+        if (this->failsafe_enabled)
+        {
+            RCLCPP_INFO(this->get_logger(), "Failsafe enabled, ignoring attitude setpoint");
+            response->success = false;
+            return;
+        }
        if (armed)
        {
            if (request->yaw == 0 && request->pitch == 0 && request->roll == 0 && request->thrust == 0)
@@ -179,6 +185,12 @@ private:
        const std::shared_ptr<drone_services::srv::SetTrajectory::Request> request,
        const std::shared_ptr<drone_services::srv::SetTrajectory::Response> response)
    {
+        if (this->failsafe_enabled)
+        {
+            RCLCPP_INFO(this->get_logger(), "Failsafe enabled, ignoring trajectory setpoint");
+            response->success = false;
+            return;
+        }
        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);
@@ -231,6 +243,7 @@ private:
            user_in_control = false;
            publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0);

+            RCLCPP_INFO(this->get_logger(), "Publishing disarm status message");
            auto msg = drone_services::msg::DroneArmStatus();
            msg.armed = false;
            arm_status_publisher_->publish(msg);
@@ -273,6 +286,7 @@ private:
            this->last_thrust = -0.1; // spin motors at 10% thrust
            armed = true;

+            RCLCPP_INFO(this->get_logger(), "Publishing arm status message");
            auto msg = drone_services::msg::DroneArmStatus();
            msg.armed = true;
            arm_status_publisher_->publish(msg);
@@ -382,9 +396,6 @@ private:
            send_velocity_setpoint();
            return;
        }
-        // 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;
        float rho = rho_0 + K * theta;

@@ -395,7 +406,6 @@ private:

        if (!user_in_control)
        {
-            // RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint");
            send_attitude_setpoint();
        }
        else
@@ -488,10 +498,10 @@ private:

    /**
     * @brief Callback function for receiving failsafe messages
-     * 
+     *
     * @param msg the message indicating that the failsafe was enabled
     */
-    void on_failsafe_received(const drone_services::msg::FailsafeMsg::SharedPtr msg)
+    void on_failsafe_receive(const drone_services::msg::FailsafeMsg::SharedPtr msg)
    {
        if (msg->enabled)
        {
--- a/src/px4_connection/test/test_failsafe_enabled.py
+++ b/src/px4_connection/test/test_failsafe_enabled.py
@@ -0,0 +1,172 @@
+import os
+import unittest
+
+import launch
+import launch_ros
+import launch_ros.actions
+import launch_testing.actions
+import pytest
+import rclpy
+import time
+
+from drone_services.srv import EnableFailsafe
+from drone_services.srv import SetTrajectory
+from drone_services.srv import SetAttitude
+from drone_services.srv import ArmDrone
+
+
+@pytest.mark.rostest
+def generate_test_description():
+    file_path = os.path.dirname(__file__)
+    px4_controller_node = launch_ros.actions.Node(
+        package='px4_connection', executable='px4_controller')
+    failsafe_node = launch_ros.actions.Node(
+        package='failsafe', executable='failsafe')
+
+    return (
+        launch.LaunchDescription([
+            px4_controller_node,
+            failsafe_node,
+            launch_testing.actions.ReadyToTest(),
+        ]),
+        {
+            'px4_controller_node': px4_controller_node,
+            'failsafe_node': failsafe_node,
+        }
+    )
+
+
+class TestPx4Failsafe(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+
+    def setUp(self):
+        self.node = rclpy.create_node('test_px4_failsafe')
+        self.called_failsafe_service = False
+        self.called_trajectory_service = False
+        self.called_arm_service = False
+        self.called_attitude_service = False
+
+    def tearDown(self):
+        self.node.destroy_node()
+
+    def failsafe_service_callback(self,future):
+        self.called_failsafe_service = True
+    
+    def trajectory_service_callback(self,future):
+        self.called_trajectory_service = True
+        self.assertFalse(future.result().success)
+    
+    def attitude_service_callback(self,future):
+        self.called_attitude_service = True
+        self.assertFalse(future.result().success)
+    
+    def arm_service_callback(self,future):
+        self.called_arm_service = True
+        self.assertFalse(future.result().success)
+
+    def test_failsafe_enabled_set_trajectory_returns_false(self, px4_controller_node, proc_output):
+        self.called_failsafe_service = False
+        failsafe_client = self.node.create_client(EnableFailsafe, '/drone/enable_failsafe')
+        set_trajectory_client = self.node.create_client(SetTrajectory, '/drone/set_trajectory')
+        while not failsafe_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('failsafe service not available, waiting again...')
+        while not set_trajectory_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('set_trajectory service not available, waiting again...')
+        failsafe_request = EnableFailsafe.Request()
+        failsafe_request.message = "test"
+        set_trajectory_request = SetTrajectory.Request()
+        set_trajectory_request.control_mode = 2
+        set_trajectory_request.values = [1.0,1.0,1.0]
+        set_trajectory_request.yaw = 0.0
+
+        end_time = time.time() + 10.0
+        try:
+            while time.time() < end_time:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+                if not self.called_failsafe_service:
+                    failsafe_future = failsafe_client.call_async(failsafe_request)
+                    failsafe_future.add_done_callback(self.failsafe_service_callback)
+                elif not self.called_trajectory_service:
+                    trajectory_future = set_trajectory_client.call_async(set_trajectory_request)
+                    trajectory_future.add_done_callback(self.trajectory_service_callback)
+                else:
+                    break
+            self.assertTrue(self.called_failsafe_service, "Failsafe service was not called")
+            self.assertTrue(self.called_trajectory_service, "Trajectory service was not called")
+
+        finally:
+            self.node.destroy_client(failsafe_client)
+            self.node.destroy_client(set_trajectory_client)
+
+
+    def test_failsafe_enabled_set_attitude_returns_false(self, px4_controller_node, proc_output):
+        self.called_failsafe_service = False
+        failsafe_client = self.node.create_client(EnableFailsafe, '/drone/enable_failsafe')
+        set_attitude_client = self.node.create_client(SetAttitude, '/drone/set_attitude')
+        while not failsafe_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('failsafe not available, waiting again...')
+        while not set_attitude_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('attitude not available, waiting again...')
+        failsafe_request = EnableFailsafe.Request()
+        failsafe_request.message = "test"
+        set_attitude_request = SetAttitude.Request()
+        set_attitude_request.pitch = 1.0
+        set_attitude_request.yaw = 1.0
+        set_attitude_request.roll = 1.0
+        set_attitude_request.thrust = 0.5
+
+        end_time = time.time() + 10.0
+        try:
+            while time.time() < end_time:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+                if not self.called_failsafe_service:
+                    failsafe_future = failsafe_client.call_async(failsafe_request)
+                    failsafe_future.add_done_callback(self.failsafe_service_callback)
+                elif not self.called_attitude_service:
+                    attitude_future = set_attitude_client.call_async(set_attitude_request)
+                    attitude_future.add_done_callback(self.attitude_service_callback)
+                else:
+                    break
+            self.assertTrue(self.called_failsafe_service, "Failsafe service was not called")
+            self.assertTrue(self.called_attitude_service, "Attitude service was not called")
+
+        finally:
+            self.node.destroy_client(failsafe_client)
+            self.node.destroy_client(set_attitude_client)
+
+    def test_failsafe_enabled_arm_returns_false(self, px4_controller_node, proc_output):
+        self.called_failsafe_service = False
+        failsafe_client = self.node.create_client(EnableFailsafe, '/drone/enable_failsafe')
+        arm_client = self.node.create_client(ArmDrone, '/drone/arm')
+        while not failsafe_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('failsafe not available, waiting again...')
+        while not arm_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('arm not available, waiting again...')
+        failsafe_request = EnableFailsafe.Request()
+        failsafe_request.message = "test"
+        arm_request = ArmDrone.Request()
+
+        end_time = time.time() + 10.0
+        try:
+            while time.time() < end_time:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+                if not self.called_failsafe_service:
+                    failsafe_future = failsafe_client.call_async(failsafe_request)
+                    failsafe_future.add_done_callback(self.failsafe_service_callback)
+                elif not self.called_arm_service:
+                    arm_future = arm_client.call_async(arm_request)
+                    arm_future.add_done_callback(self.arm_service_callback)
+                else:
+                    break
+            self.assertTrue(self.called_failsafe_service, "Failsafe service was not called")
+            self.assertTrue(self.called_arm_service, "Arm service was not called")
+        finally:
+            self.node.destroy_client(failsafe_client)
+            self.node.destroy_client(arm_client)
--- a/src/px4_connection/test/test_heartbeat_control_mode.py
+++ b/src/px4_connection/test/test_heartbeat_control_mode.py
@@ -0,0 +1,103 @@
+import os
+import sys
+import unittest
+
+import launch
+import launch_ros
+import launch_ros.actions
+import launch_testing.actions
+import pytest
+import rclpy
+
+from drone_services.srv import SetVehicleControl
+from drone_services.msg import DroneControlMode
+
+
+@pytest.mark.rostest
+def generate_test_description():
+    heartbeat_node = launch_ros.actions.Node(
+        package='px4_connection', executable='heartbeat')
+
+    return (
+        launch.LaunchDescription([
+            heartbeat_node,
+            launch_testing.actions.ReadyToTest(),
+        ]),
+        {
+            'heartbeat_node': heartbeat_node,
+        }
+    )
+
+class TestHeartbeatControlMode(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+
+    def setUp(self):
+        self.node = rclpy.create_node('test_heartbeat_control_mode')
+        self.called_control_mode_service = False
+        self.received_control_mode = False
+
+    def tearDown(self):
+        self.node.destroy_node()
+    
+    def control_mode_callback(self,msg):
+        self.received_control_mode = True
+        self.assertEqual(msg.control_mode,1)
+
+    def heartbeat_service_callback_correct(self,future):
+        self.called_control_mode_service = True
+        self.assertIsNotNone(future.result())
+        self.assertTrue(future.result().success)
+    
+    def heartbeat_service_callback_wrong(self,future):
+        self.called_control_mode_service = True
+        self.assertIsNotNone(future.result())
+        self.assertFalse(future.result().success)
+    
+    def test_set_vehicle_control_mode_proper_value(self,heartbeat_node,proc_output):
+        self.called_control_mode_service = False
+        heartbeat_client = self.node.create_client(SetVehicleControl, '/drone/set_vehicle_control')
+        while not heartbeat_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('heartbeat service not available, waiting again...')
+        heartbeat_sub = self.node.create_subscription(DroneControlMode,"/drone/control_mode",self.control_mode_callback,10)
+        request = SetVehicleControl.Request()
+        request.control= 4 # attitude control
+
+        try:
+            while True:
+                rclpy.spin_once(self.node,timeout_sec=0.1)
+                if not self.called_control_mode_service:
+                    vehicle_control_future = heartbeat_client.call_async(request)
+                    vehicle_control_future.add_done_callback(self.heartbeat_service_callback_correct)
+                elif not self.received_control_mode:
+                    continue
+                else:
+                    break
+                
+        finally:
+            self.node.destroy_client(heartbeat_client)
+    
+    def test_set_vehicle_control_mode_wrong_value(self,heartbeat_node,proc_output):
+        self.called_control_mode_service = False
+        heartbeat_client = self.node.create_client(SetVehicleControl, '/drone/set_vehicle_control')
+        while not heartbeat_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('heartbeat service not available, waiting again...')
+        request = SetVehicleControl.Request()
+        request.control = 33 # wrong control mode
+
+        try:
+            while True:
+                rclpy.spin_once(self.node,timeout_sec=0.1)
+                if not self.called_control_mode_service:
+                    vehicle_control_future = heartbeat_client.call_async(request)
+                    vehicle_control_future.add_done_callback(self.heartbeat_service_callback_wrong)
+                else:
+                    break
+        finally:
+            self.node.destroy_client(heartbeat_client)
--- a/src/px4_msgs
+++ b/src/px4_msgs
--- a/test_stream.py
+++ b/test_stream.py
@@ -0,0 +1,46 @@
+import cv2
+import requests
+
+video_url = "http://10.1.1.41:8080/video"
+# Set the headers for the POST request
+headers = {'Content-Type': 'application/octet-stream'}
+
+vid = cv2.VideoCapture(0)
+
+# vid.set(cv2.CAP_PROP_FRAME_WIDTH, RES_4K_W)
+# vid.set(cv2.CAP_PROP_FRAME_HEIGHT, RES_4K_H)
+while True:
+    try:
+        while vid.isOpened():
+            pass
+            ret, frame = vid.read()
+
+            if not ret:
+                # If reading the frame failed, break the loop
+                break
+
+            # Convert the frame to bytes
+            _, img_encoded = cv2.imencode('.jpg', frame)
+            frame_data = img_encoded.tobytes()
+
+            # Send the frame data as the request body
+            response = requests.post(video_url, data=frame_data, headers=headers)
+
+            # Check the response status
+            if response.status_code == 200:
+                print('Frame sent successfully.')
+            else:
+                print('Failed to send frame.')
+            # if self.websocket is not None:
+            #     img,frame = vid.read()
+            #     frame = cv2.resize(frame,(640,480))
+            #     encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 65]
+            #     man = cv2.imencode('.jpg', frame, encode_param)[1]
+            #     self.get_logger().info('Sending video')
+            #     asyncio.ensure_future(self.websocket.send(man.tobytes()),loop=self.event_loop)
+            #     await asyncio.sleep(1)
+            # else:
+            #     self.get_logger().info('No websocket connection')
+
+    except Exception as e:
+        print('Something went wrong while reading and sending video: ' + str(e))