update code for report

api/index.js

@@ -2,18 +2,118 @@ var express = require("express");
 var app = express();
 const WebSocket = require("ws");
 
-var ws = new WebSocket("ws://10.100.0.40:9001/");
+//# TODO SSE https://www.digitalocean.com/community/tutorials/nodejs-server-sent-events-build-realtime-app
 
-ws.on('open', function open() {
-    console.log("connected");
+var last_status = {};
+var last_image;
+var received_picture = false;
+var received_error = false;
+
+let sse_clients = [];
+
+app.use(express.static("public"));
+app.use(express.json());
+
+var ws;
+var api_connected = false;
+
+function send_events_to_clients(data) {
+    // console.log("sending events to clients");
+  sse_clients.forEach((client) => {
+    client.response.write("event: message\n");
+    client.response.write("data:" + JSON.stringify(data) + "\n\n");
+  });
+}
+
+function handle_sse_client(request, response, next) {
+  console.log("handling sse client");
+  const headers = {
+    "Content-Type": "text/event-stream",
+    Connection: "keep-alive",
+    "Cache-Control": "no-cache",
+  };
+
+  response.writeHead(200, headers);
+  response.write(JSON.stringify("yeet") + "\n\n");
+  const clientID = Date.now();
+  const newClient = {
+    id: clientID,
+    response,
+  };
+
+  sse_clients.push(newClient);
+
+  request.on("close", () => {
+    console.log(`${clientID} Connection closed`);
+    sse_clients = sse_clients.filter((client) => client.id !== clientID);
+  });
+}
+
+var connect_to_api = function () {
+  console.log("Connecting to API");
+  ws = new WebSocket("ws://10.100.0.40:9001/");
+
+  ws.on("open", function open() {
+    console.log("connected with websockets to API!");
+    api_connected = true;
   });
 
-ws.on("message", function message(message) {
-  var msg = JSON.parse(message);
-  console.log("RECEIVED: " + msg);
-});
+  ws.on("message", function message(message) {
+    try {
+      var msg = JSON.parse(message);
+      if (msg.type != "IMAGE") {
+        //   console.log("got message");
+        send_events_to_clients(msg);
+      } else {
+        console.log("got image");
+      }
+    } catch (error) {
+      console.log("could not parse as json");
+      //   send_image_data_to_clients(message);
+    }
+  });
 
-ws.on("error", console.error);
+  ws.on("error", function error(err) {
+    console.log("there was an error");
+    console.error("error: " + err);
+    received_error = true;
+  });
+};
+
+function send_image_data_to_clients(videoData) {
+  sse_clients.forEach((client) => {
+    // Set the SSE event name as 'message'
+    client.response.write("event: message\n");
+
+    // Convert the Buffer to a base64-encoded string
+    const base64Data = videoData.toString("base64");
+
+    // Set the SSE event data as the base64-encoded string
+    client.response.write(
+      "data: " + JSON.stringify({ image: base64Data }) + "\n\n"
+    );
+  });
+}
+
+// Define the endpoint to receive video data
+app.post("/video", (req, res) => {
+  // console.log("got video endpoint")
+  let videoData = Buffer.from("");
+
+  req.on("data", (chunk) => {
+    // Accumulate the received video data
+    videoData = Buffer.concat([videoData, chunk]);
+  });
+
+  req.on("end", () => {
+    // Process the received video data
+    //   console.log("Received video data:" + videoData.length);
+    send_image_data_to_clients(videoData);
+
+    // Send a response indicating successful receipt
+    res.sendStatus(200);
+  });
+});
 
 // set the view engine to ejs
 app.set("view engine", "ejs");
@@ -22,7 +122,69 @@ 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.get("/image", function (req, res) {
+  console.log("got picture request");
+  var request = JSON.stringify({
+    command: 5,
+  });
+  console.log("sending picture request");
+  ws.send(request);
+  res.status(200).send(last_image);
+});
+
+app.post("/move", function (req, res) {
+  console.log("got move request");
+  var request = JSON.stringify({
+    command: 3,
+    up_down: req.body.up_down,
+    left_right: req.body.left_right,
+    forward_backward: req.body.forward_backward,
+    yaw: req.body.turn_left_right,
+  });
+  ws.send(request);
+});
+
+app.post("/estop", function (req, res) {
+  console.log("got estop request");
+  var request = JSON.stringify({
+    command: 6,
+  });
+  ws.send(request);
+});
+
+app.post("/land", function (req, res) {
+  console.log("got land request");
+  var request = JSON.stringify({ command: 0 });
+  ws.send(request);
+});
+
+app.post("/arm_disarm", function (req, res) {
+  console.log("got arm/disarm request");
+  var request = JSON.stringify({ command: 1 });
+  ws.send(request);
+});
+
+app.get("/connect", function (req, res) {
+  console.log("got connect request");
+  connect_to_api();
+  setTimeout(function () {
+    if (api_connected) {
+      console.log("Connected to API");
+      res.status(200).json({ connected: true });
+    } else {
+      received_error = false;
+      res.status(400).json({ connected: false });
+    }
+  }, 1000);
+});
+
+app.get("/test", function (req, res) {
+  res.render("test");
 });
 
 app.listen(8080);

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

api/views/index.ejs

@@ -1,12 +1,418 @@
 <!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="video"> -->
+    <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>
+                <!-- <br>
+                <label for="control_mode">Control mode:</label>
+                <br>
+                <select name="control_mode" id="control_mode">
+                    <option value="attitude">Attitude</option>
+                    <option value="velocity">Velocity</option>
+                    <option value="position">Position</option>
+                </select> -->
+            </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>
+    <!-- </div> -->
+</body>
+
+<script>
+    var ws;
+    var api_timout;
+    var checked_for_connection = false;
+    var connected_to_api = false;
+    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";
+        });
+    }
+
+    // Helper function to decode base64 image and set it as source of <img> element
+    function decodeBase64Image(base64Data, imgElement) {
+        const img = new Image();
+
+        img.onload = function () {
+            // Once the image has loaded, set it as the source of the <img> element
+            imgElement.src = this.src;
+            // console.log("set image data src")
+        };
+
+        // Set the base64-encoded image data as the source of the image
+        img.src = 'data:image/jpeg;base64,' + base64Data;
+    }
+
+    window.onload = function () {
+        const events = new EventSource("/events");
+
+        events.onopen = () => {
+            console.log("OPENED EVENT");
+        }
+
+        events.onmessage = (event) => {
+            console.log("MESSAGE RECEIVED")
+            try {
+                const data = JSON.parse(event.data);
+                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);
+                    document.getElementById("height").innerHTML = "Height (m): " + data.data.height;
+                } 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 {
+                    // decodeBase64Image(data.image, document.getElementById("result-video"));
+                }
+            } catch (error) {
+                console.error("Received an error")
+                console.error(error);
+            }
+        }
+    };
+
+    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 update_status() {
+        // {"battery_percentage": 100.0, "cpu_usage": 0.0, "armed": false, "control_mode": "attitude", "route_setpoint": 0}}
+        // console.log("updating status")
+        var xhr = new XMLHttpRequest();
+        xhr.open("GET", "/status", true);
+        xhr.onreadystatechange = function () {
+            if (this.status == 200) {
+                // console.log(this.responseText);
+                if (this.responseText.length > 0) {
+                    var status = JSON.parse(this.responseText);
+                    // console.log(status)
+                    document.getElementById("batterypercentage").innerHTML = "Battery percentage: " + status.battery_percentage;
+                    document.getElementById("cpuload").innerHTML = "CPU load: " + status.cpu_usage;
+                    document.getElementById("armed").innerHTML = "Armed: " + status.armed;
+                    document.getElementById("control_mode").innerHTML = "Control mode: " + status.control_mode;
+                }
+            }
+        };
+        xhr.send();
+    }
+
+    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 connect_to_video_stream() {
+        console.log("Connecting to websocket")
+        const video_holder = document.getElementById("result-video");
+        const socket = new WebSocket('ws://10.100.0.40:9002/');
+
+        socket.addEventListener('open', (event) => {
+            console.log('Connected to WebSocket server');
+        });
+
+        socket.addEventListener('message', (event) => {
+            // Assuming the received data is an ArrayBuffer or Uint8Array
+            const imageData = event.data;
+
+            // Convert the image data to a Blob
+            const blob = new Blob([imageData], { type: 'image/jpeg' });
+
+            // Generate a temporary URL for the Blob
+            const imageURL = URL.createObjectURL(blob);
+
+            // Set the src attribute of the <img> element
+            video_holder.src = imageURL;
+        });
+
+        // Connection closed
+        socket.addEventListener('close', (event) => {
+            console.log('Disconnected from WebSocket server');
+        });
+
+        // Error occurred
+        socket.addEventListener('error', (event) => {
+            console.error('WebSocket error:', event.error);
+        });
+
+    }
+
+    // var set_timout = false;
+
+    function handle_ws_message(data) {
+        // console.log("Handling message " + data);
+        // clearTimeout(api_timout);
+        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);
+            // TODO fix
+            // if (set_timout == false) {
+            //     api_timeout = setTimeout(function () {
+            //         set_timout = true;
+            //         console.log("API timed out")
+            //         alert("Connection to API timed out!");
+            //         document.getElementById("connectedlabel").innerHTML = "Not connected to drone";
+            //         document.getElementById("connectbutton").disabled = false;
+            //     }, 5000);
+            // }
+        } 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);
+        } else {
+            // decodeBase64Image(data.image, document.getElementById("result-video"));
+        }
+    }
+
+    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;
+            connected_to_api = true;
+            openSocket();
+        });
+
+        ws.addEventListener("message", function message(message) {
+            try {
+                // console.log(message.data)
+                var msg = JSON.parse(message.data);
+                handle_ws_message(msg);
+
+            } catch (error) {
+                console.log("could not parse as json");
+                console.error(error)
+                //   send_image_data_to_clients(message);
+            }
+        });
+
+        ws.addEventListener("error", function error(err) {
+            console.log("there was an error! " + err);
+            // console.error("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);
+                    // console.log(status)
+                    document.getElementById("connectedlabel").innerHTML = "Connected to drone";
+                    document.getElementById("connectbutton").disabled = true;
+                    // connect_to_video_stream();
+
+                }
+            } else {
+                console.log("error");
+                document.getElementById("connectedlabel").innerHTML = "Not connected to drone";
+                if (!received) {
+                    alert("Could not connect to API!");
+                    received = true;
+
+                }
+            }
+        };
+        xhr.send();
+    }
+    // window onload function die elke seconde een request doet om te kijken of er al nieuwe foto is
+    // function die elke 100 ms een request doet om de status te updaten
+    // button callbacks
+</script>
+
+</html>

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>

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

drone_scripts/start_api.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+. /home/ubuntu/source_ros2.sh
+
+ros2 run api_communication api_listener
+
+

drone_scripts/start_failsafe.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+. /home/ubuntu/source_ros2.sh
+
+ros2 run failsafe failsafe
+
+

drone_scripts/start_positionchanger.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+. /home/ubuntu/source_ros2.sh
+
+ros2 run drone_controls position_changer
+
+

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

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

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

src/api_communication/api_communication/api_listener.py

@@ -2,46 +2,103 @@ 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
+from functools import partial
+import base64
+import cv2
+from functools import partial
 
 # communication: client always sends commands that have a command id.
 # server always sends messages back that have a message type
 
+# TODO send video https://github.com/Jatin1o1/Python-Javascript-Websocket-Video-Streaming-/tree/main
+
+RES_4K_H = 3496
+RES_4K_W = 4656
+
+# TODO change video to be handled by camera controller through websocket with different port
+
 
 class RequestCommand(Enum):
-    GET_COMMANDS_TYPES = -1  # to get the available commands and types
-    TAKEOFF = 0
-    LAND = 1
-    MOVE_POSITION = 2
+    """
+    Enum for the commands that can be sent to the API
+    """
+    GET_COMMANDS_TYPES = -1 
+    LAND = 0
+    ARM_DISARM = 1
     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.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.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 +114,121 @@ 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 = {}
@@ -117,7 +242,151 @@ class ApiListener(Node):
         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.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_drone_client.call_async(self.land_drone_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 +398,44 @@ 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(str(e.with_traceback()))
 
     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:
@@ -177,11 +453,13 @@ class ApiListener(Node):
 
 
 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()

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>

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)

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'

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)

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'

src/camera/camera/camera_controller.py

@@ -5,17 +5,51 @@ 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
 
+video_url = "http://10.1.1.41:8080/video"
+# Set the headers for the POST request
+headers = {'Content-Type': 'application/octet-stream'}
+
+#TODO change to serve video stream through websockets connection
+
 class CameraController(Node):
     def __init__(self):
         super().__init__('camera_controller')
 
         self.get_logger().info("Camera controller started. Waiting for service call...")
         self.srv = self.create_service(
-            TakePicture, 'drone/picture', self.take_picture_callback)
+            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.websocket_thread = threading.Thread(target=self.start_listening)
+        # self.websocket_thread.start()
+
+        self.video_thread = threading.Thread(target=self.setup_websocket)
+        self.video_thread.start()
+
+
+
+    def timer_callback(self):
+        if self.should_exit:
+            self.get_logger().info("Shutting down...")
+            self.destroy_node()
+            sys.exit(-1)
 
     def take_picture_callback(self, request, response):
         if (request.input_name == "default"):
@@ -25,11 +59,126 @@ 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):
+        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):
+        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()
+                    # self.get_logger().info("frame before: " + str(frame.shape))
+                    #frame = cv2.resize(frame,(RES_4K_W,RES_4K_H))
+                    #print("frame after: " + str(frame.shape))
+                    encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 100]
+                    man = cv2.imencode('.jpg', frame)[1]
+                    #sender(man)
+                    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 handle_video_connection(self):
+        self.get_logger().info('Starting sending video')
+        vid = cv2.VideoCapture(0, cv2.CAP_DSHOW)
+
+        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:
+                self.get_logger().error('Something went wrong while reading and sending video: ' + str(e))
+
+    def start_listening(self):
+        self.get_logger().info('Starting listening for websocket connections')
+        asyncio.run(self.start_websocket_server())
+
+    async def start_websocket_server(self):
+        self.get_logger().info('Starting websocket server for video')
+        self.event_loop = asyncio.get_event_loop()
+        self.server = await websockets.server.serve(self.websocket_handler, '0.0.0.0', 9002)
+        await self.server.wait_closed()
+    
+    async def websocket_handler(self,websocket):
+        self.get_logger().info('New connection')
+
+        self.websocket = websocket
+        try:
+            async for message in websocket:
+                self.get_logger().info(f"Received message: {message}")
+
+        except websockets.exceptions.ConnectionClosed:
+            self.get_logger().info('Connection closed')
+            self.websocket = None
+        except Exception as e:
+            self.get_logger().error('Something went wrong!')
+            self.get_logger().error(str(e))
+            self.websocket = None
+
+       
 def main(args=None):
     rclpy.init(args=args)
 

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>

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)

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'

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)

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'

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()

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,37 @@ 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));
+        this->height_subscription = this->create_subscription<drone_services::msg::HeightData>("/drone/height", qos, std::bind(&PositionChanger::handle_height_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->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 +102,47 @@ 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...");
+        }
+    }
+
+    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...");
+        }
+    }
+
+    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 +192,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 +203,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,34 +220,39 @@ public:
      */
     void apply_collision_weights()
     {
-        if (this->current_speed_x > 0) // if moving forward
+
+        // 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
+        // }
+        // 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->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
+        // }
+        // 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 +275,65 @@ 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();
     }
 
+    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...");
+        }
+    }
+
+    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...");
+        }
+    }
+
+    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 +341,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 +371,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 +413,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 +425,59 @@ 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;
         }
     }
 
+    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;
+    }
+
+    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));
+        // TODO set motors to spin at 30%
+        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 +512,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 +537,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 +556,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 +565,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());
         }
     }
 };

src/drone_controls/test/test_positionchanger.py

@@ -0,0 +1,168 @@
+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.msg import FailsafeMsg
+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
+
+    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)

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

src/drone_controls/test/test_positionchanger_lidar.py

@@ -0,0 +1,242 @@
+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.msg import FailsafeMsg
+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')
+    

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)

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

src/drone_services/srv/Land.srv

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

src/drone_services/srv/ReadyDrone.srv

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

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)

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()
 

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>

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'

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)
+                
+
+

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)

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'

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()

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>

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);
@@ -488,10 +502,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)
         {

src/px4_connection/test/test_failsafe_enabled.py

@@ -0,0 +1,173 @@
+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 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)

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)

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))