sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Merge pull request #12 from SemvdH/api

Browse Source 
Merge API branch into main
This commit is contained in:
SemvdH
2023-06-13 21:54:43 +02:00
committed by GitHub
parent 14b94a2ec4 93ac0ad866
commit 9a81018c1d
51 changed files with 2733 additions and 378 deletions
Download Patch File Download Diff File Expand all files Collapse all files

180
api/index.js
View File

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

66
api/public/css/stylesheet.css Normal file
View File

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

BIN
api/public/img/droneimage_2023-05-30_14-55-57.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 7.1 MiB

426
api/views/index.ejs
View File

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

37
api/views/test.ejs Normal file
View File

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

14
drone_scripts/restart_services.sh Executable file
View File

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

7
drone_scripts/start_api.sh Executable file
View File

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

7
drone_scripts/start_camera.sh Executable file
View File

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

7
drone_scripts/start_failsafe.sh Executable file
View File

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

7
drone_scripts/start_positionchanger.sh Executable file
View File

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

7
drone_scripts/start_px4_connection.sh Executable file
View File

@@ -0,0 +1,7 @@
#!/bin/bash
. /home/ubuntu/source_ros2.sh
ros2 launch px4_connection px4_controller_heardbeat_launch.py

7
drone_scripts/start_relais_controller.sh Executable file
View File

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

7
drone_scripts/start_status.sh Executable file
View File

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

10
drone_scripts/stop_services.sh Executable file
View File

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

BIN
img.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 143 KiB

10
restart_services.sh Executable file
View File

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

384
src/api_communication/api_communication/api_listener.py
View File

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

4
src/api_communication/package.xml
View File

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

110
src/api_communication/test/test_api_listener.py Normal file
View File

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

23
src/api_communication/test/test_copyright.py
View File

@@ -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'

25
src/api_communication/test/test_flake8.py
View File

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

23
src/api_communication/test/test_pep257.py
View File

@@ -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'

153
src/camera/camera/camera_controller.py
View File

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

3
src/camera/package.xml
View File

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

72
src/camera/test/test_camera.py Normal file
View File

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

23
src/camera/test/test_copyright.py
View File

@@ -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'

25
src/camera/test/test_flake8.py
View File

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

23
src/camera/test/test_pep257.py
View File

@@ -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'

24
src/drone_controls/CMakeLists.txt
View File

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

257
src/drone_controls/src/PositionChanger.cpp
View File

@@ -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
{
@@ -119,7 +178,7 @@ public:
auto status = future.wait_for(1s);
if (status == std::future_status::ready)
{
RCLCPP_INFO(this->get_logger(), "Set failsafe enabled: %d, message: ", future.get()->enabled, future.get()->message);
RCLCPP_INFO(this->get_logger(), "Set failsafe enabled: %d, message: %s", future.get()->enabled, future.get()->message);
}
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;
@@ -143,7 +201,12 @@ public:
auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request, std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1));
}
void enable_failsafe(std::string message)
/**
* @brief Enables the failsafe with the specified message
*
* @param message the message indicating the cause of the failsafe
*/
void enable_failsafe(std::u16string message)
{
this->failsafe_enabled = true;
this->failsafe_request->message = message;
@@ -157,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);
}
// }
}
/**
@@ -207,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
*
@@ -218,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)
@@ -247,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;
}
@@ -286,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)
{
@@ -293,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
@@ -338,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
@@ -357,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
@@ -369,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))
{
@@ -378,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());
}
}
};

168
src/drone_controls/test/test_positionchanger.py Normal file
View File

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

158
src/drone_controls/test/test_positionchanger_height.py Normal file
View File

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

242
src/drone_controls/test/test_positionchanger_lidar.py Normal file
View File

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

3
src/drone_services/CMakeLists.txt
View File

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

6
src/drone_services/msg/DroneStatus.msg
View File

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

2
src/drone_services/srv/Land.srv Normal file
View File

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

2
src/drone_services/srv/ReadyDrone.srv Normal file
View File

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

25
src/drone_status/drone_status/drone_status.py
View File

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

10
src/failsafe/failsafe/failsafe.py
View File

@@ -5,11 +5,10 @@ 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")
# create service on /drone/failsafe topic
self.service = self.create_service(
EnableFailsafe, "/drone/enable_failsafe", self.failsafe_callback)
self.failsafe_publisher = self.create_publisher(FailsafeMsg, "/drone/failsafe", 10)
@@ -19,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")
@@ -34,10 +33,9 @@ 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()
if __name__ == '__main__':
main()

4
src/failsafe/package.xml
View File

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

23
src/failsafe/test/test_copyright.py
View File

@@ -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'

86
src/failsafe/test/test_failsafe.py Normal file
View File

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

25
src/failsafe/test/test_flake8.py
View File

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

23
src/failsafe/test/test_pep257.py
View File

@@ -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'

10
src/px4_connection/CMakeLists.txt
View File

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

3
src/px4_connection/package.xml
View File

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

2
src/px4_connection/src/heartbeat.cpp
View File

@@ -29,7 +29,7 @@ public:
// create a publisher on the offboard control mode topic
offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
// create timer to send heartbeat messages (offboard control) every 100ms
timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this));
timer_ = this->create_wall_timer(10ms, std::bind(&HeartBeat::send_heartbeat, this));
start_time = this->get_clock()->now().seconds();
RCLCPP_INFO(this->get_logger(), "done initializing at %d seconds. Sending heartbeat...", start_time);
}

56
src/px4_connection/src/px4_controller.cpp
View File

@@ -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,18 +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 des_x = p0_x, des_y = p0_y, des_z = p0_z; // desired position
float dot_des_x = 0.0, dot_des_y = 0.0; // desired velocity
float gamma = M_PI_4; // desired heading direction
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 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;
@@ -131,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)
@@ -180,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);
@@ -232,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);
@@ -258,7 +270,11 @@ private:
const std::shared_ptr<drone_services::srv::ArmDrone::Response> response)
{
RCLCPP_INFO(this->get_logger(), "Got arm request...");
if (this->failsafe_enabled)
{
response->success = false;
return;
}
if (!armed)
{
this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
@@ -270,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);
@@ -343,7 +360,6 @@ private:
RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z);
RCLCPP_INFO(this->get_logger(), "local position: %f %f", local_x, local_y);
msg.position = {local_x, local_y, des_z};
msg.velocity = {dot_des_x, dot_des_y, 0.0};
msg.yaw = gamma; //-3.14; // [-PI:PI]
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
@@ -391,13 +407,6 @@ private:
des_y = rho * sin(theta);
des_z = position[2]; // the z position can be set to the received height
// velocity computation
float dot_rho = K * omega;
dot_des_x = dot_rho * cos(theta) - rho * sin(theta) * omega;
dot_des_y = dot_rho * sin(theta) + rho * cos(theta) * omega;
// desired heading direction
gamma = atan2(dot_des_y, dot_des_x);
if (!user_in_control)
{
// RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint");
@@ -491,7 +500,12 @@ private:
RCLCPP_INFO(this->get_logger(), "Control mode set to %d", current_control_mode);
}
void on_failsafe_received(const drone_services::msg::FailsafeMsg::SharedPtr msg)
/**
* @brief Callback function for receiving failsafe messages
*
* @param msg the message indicating that the failsafe was enabled
*/
void on_failsafe_receive(const drone_services::msg::FailsafeMsg::SharedPtr msg)
{
if (msg->enabled)
{

173
src/px4_connection/test/test_failsafe_enabled.py Normal file
View File

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

103
src/px4_connection/test/test_heartbeat_control_mode.py Normal file
View File

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

46
test_stream.py Normal file
View File

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