From fe4ab53c87d4067f4b8d68d195623c1c437239e5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 26 May 2023 23:44:49 +0200 Subject: [PATCH 001/332] change heartbeat hz to 100 --- src/drone_controls/src/PositionChanger.cpp | 7 ++++++- src/failsafe/failsafe/failsafe.py | 2 -- src/px4_connection/src/heartbeat.cpp | 2 +- src/px4_connection/src/px4_controller.cpp | 22 +++++++++++----------- 4 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 8a2a5cd3..ddb36f1d 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -119,7 +119,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 { @@ -143,6 +143,11 @@ public: auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request, std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1)); } + /** + * @brief Enables the failsafe with the specified message + * + * @param message the message indicating the cause of the failsafe + */ void enable_failsafe(std::string message) { this->failsafe_enabled = true; diff --git a/src/failsafe/failsafe/failsafe.py b/src/failsafe/failsafe/failsafe.py index 483f2fcb..b5fa2bcf 100644 --- a/src/failsafe/failsafe/failsafe.py +++ b/src/failsafe/failsafe/failsafe.py @@ -9,7 +9,6 @@ class FailSafe(Node): 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) @@ -38,6 +37,5 @@ def main(args=None): failsafe_node.destroy_node() rclpy.shutdown() - if __name__ == '__main__': main() diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 8aafbf10..b8f657fc 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -29,7 +29,7 @@ public: // create a publisher on the offboard control mode topic offboard_control_mode_publisher_ = this->create_publisher("/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); } diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 9aae5ee4..6c22edc1 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -108,8 +108,7 @@ private: 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 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 @@ -258,7 +257,11 @@ private: const std::shared_ptr 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); @@ -343,7 +346,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 +393,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,6 +486,11 @@ private: RCLCPP_INFO(this->get_logger(), "Control mode set to %d", current_control_mode); } + /** + * @brief Callback function for receiving failsafe messages + * + * @param msg the message indicating that the failsafe was enabled + */ void on_failsafe_received(const drone_services::msg::FailsafeMsg::SharedPtr msg) { if (msg->enabled) From 38fb51709d37fd76309682be4c3a26d0990d18dd Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 10:16:04 +0200 Subject: [PATCH 002/332] change scripts --- drone_scripts/start_camera.sh | 7 +++++++ drone_scripts/start_relais_controller.sh | 7 +++++++ drone_scripts/start_status.sh | 7 +++++++ src/px4_msgs | 2 +- 4 files changed, 22 insertions(+), 1 deletion(-) create mode 100755 drone_scripts/start_camera.sh create mode 100755 drone_scripts/start_relais_controller.sh create mode 100755 drone_scripts/start_status.sh diff --git a/drone_scripts/start_camera.sh b/drone_scripts/start_camera.sh new file mode 100755 index 00000000..37a661b0 --- /dev/null +++ b/drone_scripts/start_camera.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +. /home/ubuntu/source_ros2.sh + +ros2 run camera camera_controller + + diff --git a/drone_scripts/start_relais_controller.sh b/drone_scripts/start_relais_controller.sh new file mode 100755 index 00000000..3796c790 --- /dev/null +++ b/drone_scripts/start_relais_controller.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +. /home/ubuntu/source_ros2.sh + +ros2 run relais_control relais_controller + + diff --git a/drone_scripts/start_status.sh b/drone_scripts/start_status.sh new file mode 100755 index 00000000..7b53e8b1 --- /dev/null +++ b/drone_scripts/start_status.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +. /home/ubuntu/source_ros2.sh + +ros2 run drone_status drone_status + + diff --git a/src/px4_msgs b/src/px4_msgs index b64ef047..ffc3a4cd 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 +Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 From 581f53735b4562ba03a3b4e5068f187ba26c9243 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 10:39:52 +0200 Subject: [PATCH 003/332] add failsafe to msg interfaces --- src/drone_services/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index 8f126d3b..d76bd77e 100644 --- a/src/drone_services/CMakeLists.txt +++ b/src/drone_services/CMakeLists.txt @@ -38,6 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/HeightData.msg" "msg/LidarReading.msg" "msg/MultiflexReading.msg" + "msg/FailsafeMsg.msg" ) if(BUILD_TESTING) From d5ce727c3d0f21e3fe8254870efe2012ac160a1e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 10:40:03 +0200 Subject: [PATCH 004/332] add failsafe to msg interfaces --- src/px4_msgs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_msgs b/src/px4_msgs index ffc3a4cd..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 From 437ea4f5361f053daf899ecd65b93067139f2fe8 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 10:48:05 +0200 Subject: [PATCH 005/332] change string in enable_failsafe to be same type of string as in failsafe message request --- src/drone_controls/src/PositionChanger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index ddb36f1d..c974228e 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -148,7 +148,7 @@ public: * * @param message the message indicating the cause of the failsafe */ - void enable_failsafe(std::string message) + void enable_failsafe(std::__cxx11::basic_string message) { this->failsafe_enabled = true; this->failsafe_request->message = message; From 641dd746c46501a1c506c0119a9f28fd66e76a8e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 10:56:17 +0200 Subject: [PATCH 006/332] change failsafe wstring (char16) to string (char) --- src/drone_controls/src/PositionChanger.cpp | 2 +- src/drone_services/msg/FailsafeMsg.msg | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index c974228e..ddb36f1d 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -148,7 +148,7 @@ public: * * @param message the message indicating the cause of the failsafe */ - void enable_failsafe(std::__cxx11::basic_string message) + void enable_failsafe(std::string message) { this->failsafe_enabled = true; this->failsafe_request->message = message; diff --git a/src/drone_services/msg/FailsafeMsg.msg b/src/drone_services/msg/FailsafeMsg.msg index 91277660..a69cca44 100644 --- a/src/drone_services/msg/FailsafeMsg.msg +++ b/src/drone_services/msg/FailsafeMsg.msg @@ -1,2 +1,2 @@ bool enabled -wstring msg \ No newline at end of file +string msg \ No newline at end of file From bb8182baa0f0524c0254ce7d79203105e02f8930 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:02:18 +0200 Subject: [PATCH 007/332] add handling of velocity --- .../api_communication/api_listener.py | 29 +++++++++++++++++++ src/drone_controls/src/PositionChanger.cpp | 2 +- src/drone_services/msg/FailsafeMsg.msg | 2 +- 3 files changed, 31 insertions(+), 2 deletions(-) diff --git a/src/api_communication/api_communication/api_listener.py b/src/api_communication/api_communication/api_listener.py index 1ba12ca1..84ee5eff 100644 --- a/src/api_communication/api_communication/api_listener.py +++ b/src/api_communication/api_communication/api_listener.py @@ -3,6 +3,7 @@ from rclpy.node import Node from drone_services.msg import DroneStatus from drone_services.srv import TakePicture +from drone_services.srv import MovePosition import asyncio import websockets.server @@ -27,6 +28,7 @@ class ResponseMessage(Enum): ALL_REQUESTS_RESPONSES = -1 STATUS = 0 IMAGE = 1 + MOVE_DIRECTION_RESULT = 2 class ApiListener(Node): @@ -41,6 +43,10 @@ class ApiListener(Node): while not self.take_picture_client.wait_for_service(timeout_sec=1.0): self.get_logger().info('Take picture service not available, waiting again...') self.take_picture_request = TakePicture.Request() + self.move_position_client = self.create_client(MovePosition, '/drone/move_position') + while not self.move_position_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Move position service not available, waiting again...') + self.move_position_request = MovePosition.Request() self.status_data = {} self.status_data_received = False @@ -116,6 +122,28 @@ class ApiListener(Node): result['response_messages'] = messagetypes self.message_queue.append(json.dumps( {'type': ResponseMessage.ALL_REQUESTS_RESPONSES.name, 'data': result})) + + def handle_direction_message(self,message): + self.move_position_request.up_down = message['up_down'] + self.move_position_request.left_right = message['left_right'] + self.move_position_request.front_back = message['forward_backward'] + self.move_position_request.angle = message['yaw'] + self.get_logger().info(f'Calling move position service with request: {str(self.move_position_request)}') + + + self.future = self.move_position_client.call_async(self.move_position_request) + rclpy.spin_until_future_complete(self, self.future) + result = self.future.result() + message_result = {} + message_result['type'] = ResponseMessage.MOVE_DIRECTION_RESULT.name + 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)) + def consume_message(self, message): self.get_logger().info(f'Consuming message: {message}') @@ -137,6 +165,7 @@ class ApiListener(Node): self.get_logger().info('Move position command received') elif message_json['command'] == RequestCommand.MOVE_DIRECTION.value: self.get_logger().info('Move direction command received') + self.handle_direction_message(message_json) elif message_json['command'] == RequestCommand.TAKE_PICTURE.value: self.process_image_request(message_json) elif message_json['command'] == RequestCommand.GET.value: diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index ddb36f1d..71dd48a9 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -148,7 +148,7 @@ public: * * @param message the message indicating the cause of the failsafe */ - void enable_failsafe(std::string message) + void enable_failsafe(std::u16string message) { this->failsafe_enabled = true; this->failsafe_request->message = message; diff --git a/src/drone_services/msg/FailsafeMsg.msg b/src/drone_services/msg/FailsafeMsg.msg index a69cca44..91277660 100644 --- a/src/drone_services/msg/FailsafeMsg.msg +++ b/src/drone_services/msg/FailsafeMsg.msg @@ -1,2 +1,2 @@ bool enabled -string msg \ No newline at end of file +wstring msg \ No newline at end of file From 34e4748737165b7dfd5e481ad2e23b820ba2a0fe Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:07:21 +0200 Subject: [PATCH 008/332] add u16 [prefix --- src/drone_controls/src/PositionChanger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 71dd48a9..06bd860f 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -259,7 +259,7 @@ public: if (!this->received_lidar_message) { RCLCPP_WARN(this->get_logger(), "Lidar not sending messages, enabling failsafe"); - enable_failsafe("No healthy connection to LIDAR!"); + enable_failsafe(u"No healthy connection to LIDAR!"); } this->received_lidar_message = false; } From 4b18669bb3ecc48df6ee08e59916cb2ae182f149 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:25:29 +0200 Subject: [PATCH 009/332] update api --- api/index.js | 6 ++++-- api/views/index.ejs | 1 + 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/api/index.js b/api/index.js index 70f863ba..a46a1d54 100644 --- a/api/index.js +++ b/api/index.js @@ -3,9 +3,11 @@ var app = express(); const WebSocket = require("ws"); var ws = new WebSocket("ws://10.100.0.40:9001/"); +var api_connected = false; ws.on('open', function open() { - console.log("connected"); + console.log("connected with websockets to API!"); + api_connected = true; }); ws.on("message", function message(message) { @@ -22,7 +24,7 @@ app.set("view engine", "ejs"); // index page app.get("/", function (req, res) { - res.render("index"); + res.render("index", { api_connected: api_connected }); }); app.listen(8080); diff --git a/api/views/index.ejs b/api/views/index.ejs index 1b7be72b..88abb1e2 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -5,6 +5,7 @@ 5G drone API +

<%- api_connected %>

Hello World!
From 78421ccf6f9e3ed5c4f4c921a3075b49479a16ee Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:29:18 +0200 Subject: [PATCH 010/332] update style --- api/index.js | 2 ++ api/public/css/stylesheet.css | 8 ++++++++ api/views/index.ejs | 4 +++- 3 files changed, 13 insertions(+), 1 deletion(-) create mode 100644 api/public/css/stylesheet.css diff --git a/api/index.js b/api/index.js index a46a1d54..d17e95f9 100644 --- a/api/index.js +++ b/api/index.js @@ -2,6 +2,8 @@ var express = require("express"); var app = express(); const WebSocket = require("ws"); +app.use(express.static('public')) + var ws = new WebSocket("ws://10.100.0.40:9001/"); var api_connected = false; diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css new file mode 100644 index 00000000..62051349 --- /dev/null +++ b/api/public/css/stylesheet.css @@ -0,0 +1,8 @@ +body { + background-color: azure; +} + +.header { + color: black; + text-align: center; +} \ No newline at end of file diff --git a/api/views/index.ejs b/api/views/index.ejs index 88abb1e2..2fa64c08 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -3,11 +3,13 @@ + 5G drone API -

<%- api_connected %>

+

5G Drone API

+

<%- api_connected %>

Hello World!
From a1643f9a5cea70efb16a572d8b2978cf1b8d1499 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:32:51 +0200 Subject: [PATCH 011/332] video elements --- api/public/css/stylesheet.css | 17 ++++++++++++++++ api/views/index.ejs | 37 +++++++++++++++++++++++------------ 2 files changed, 41 insertions(+), 13 deletions(-) diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css index 62051349..cf68897b 100644 --- a/api/public/css/stylesheet.css +++ b/api/public/css/stylesheet.css @@ -1,8 +1,25 @@ body { background-color: azure; + font-family: 'Segoe UI', Tahoma, Geneva, Verdana, sans-serif; } .header { color: black; text-align: center; +} + +.video { + width: 100%; +} + +.mainvideo { + width: 60%; + float: left; + border-color: 1px sold blue; +} + +.lastpicture { + width: 40%; + float: right; + border-color: 1px solid red; } \ No newline at end of file diff --git a/api/views/index.ejs b/api/views/index.ejs index 2fa64c08..9e275069 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -1,15 +1,26 @@ - - - - - - 5G drone API - - -

5G Drone API

-

<%- api_connected %>

-
Hello World!
- - + + + + + + + 5G drone API + + + +

5G Drone API

+
+
+
+
+ +
+ +
+

<%- api_connected %>

+
Hello World!
+ + + \ No newline at end of file From 0c3f561c381f0009a6c71c577c8395a10f99b8fe Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:35:21 +0200 Subject: [PATCH 012/332] change heights --- api/public/css/stylesheet.css | 2 ++ 1 file changed, 2 insertions(+) diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css index cf68897b..23ec8a75 100644 --- a/api/public/css/stylesheet.css +++ b/api/public/css/stylesheet.css @@ -15,11 +15,13 @@ body { .mainvideo { width: 60%; float: left; + height: 500px; border-color: 1px sold blue; } .lastpicture { width: 40%; float: right; + height: 400px; border-color: 1px solid red; } \ No newline at end of file From 37437f9a53bc9c9d888bdc9ea3140348447099f5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:35:58 +0200 Subject: [PATCH 013/332] border --- api/public/css/stylesheet.css | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css index 23ec8a75..6c4e7e21 100644 --- a/api/public/css/stylesheet.css +++ b/api/public/css/stylesheet.css @@ -16,12 +16,12 @@ body { width: 60%; float: left; height: 500px; - border-color: 1px sold blue; + border: 1px sold blue; } .lastpicture { width: 40%; float: right; height: 400px; - border-color: 1px solid red; + border: 1px solid red; } \ No newline at end of file From 133c2a04ab7422b0236164e7058e8586e713e2ff Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:38:04 +0200 Subject: [PATCH 014/332] how to position two divs nexto to eachoter --- api/public/css/stylesheet.css | 3 ++- api/views/index.ejs | 1 - 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css index 6c4e7e21..d5e5e9de 100644 --- a/api/public/css/stylesheet.css +++ b/api/public/css/stylesheet.css @@ -10,6 +10,7 @@ body { .video { width: 100%; + overflow: hidden; } .mainvideo { @@ -21,7 +22,7 @@ body { .lastpicture { width: 40%; - float: right; + float: left; height: 400px; border: 1px solid red; } \ No newline at end of file diff --git a/api/views/index.ejs b/api/views/index.ejs index 9e275069..7fe431cf 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -17,7 +17,6 @@
-

<%- api_connected %>

Hello World!
From a539dff7ea37c05eb0a3d303fa52b341f58b1222 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:39:16 +0200 Subject: [PATCH 015/332] how to position two divs nexto to eachoter --- api/public/css/stylesheet.css | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css index d5e5e9de..37ea43a7 100644 --- a/api/public/css/stylesheet.css +++ b/api/public/css/stylesheet.css @@ -10,11 +10,11 @@ body { .video { width: 100%; - overflow: hidden; + overflow: auto; } .mainvideo { - width: 60%; + width: 50%; float: left; height: 500px; border: 1px sold blue; From 8d021678da6c16848a26c769b5ca421b696492a1 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:40:02 +0200 Subject: [PATCH 016/332] how to position two divs nexto to eachoter --- api/public/css/stylesheet.css | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css index 37ea43a7..f3fd71a2 100644 --- a/api/public/css/stylesheet.css +++ b/api/public/css/stylesheet.css @@ -17,7 +17,7 @@ body { width: 50%; float: left; height: 500px; - border: 1px sold blue; + border: 1px solid blue; } .lastpicture { From 0ac76f788a951b886f57e78ce37a608662ee7e32 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:41:12 +0200 Subject: [PATCH 017/332] add labels --- api/public/css/stylesheet.css | 1 + api/views/index.ejs | 2 ++ 2 files changed, 3 insertions(+) diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css index f3fd71a2..ec0edc1c 100644 --- a/api/public/css/stylesheet.css +++ b/api/public/css/stylesheet.css @@ -18,6 +18,7 @@ body { float: left; height: 500px; border: 1px solid blue; + margin-right: 10px; } .lastpicture { diff --git a/api/views/index.ejs b/api/views/index.ejs index 7fe431cf..ec37140d 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -13,8 +13,10 @@

5G Drone API

+

Camera view:

+

Last picture:

From c67d4ac73ad076d455ae612b6d92afdc384ca84b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:41:57 +0200 Subject: [PATCH 018/332] add labels --- api/public/css/stylesheet.css | 2 ++ 1 file changed, 2 insertions(+) diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css index ec0edc1c..d3b99251 100644 --- a/api/public/css/stylesheet.css +++ b/api/public/css/stylesheet.css @@ -19,6 +19,7 @@ body { height: 500px; border: 1px solid blue; margin-right: 10px; + padding: 10px; } .lastpicture { @@ -26,4 +27,5 @@ body { float: left; height: 400px; border: 1px solid red; + padding: 10px; } \ No newline at end of file From 3b97c44ed806524d50f957973e6ebedb7ca072ee Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:47:40 +0200 Subject: [PATCH 019/332] publish status? --- api/views/index.ejs | 3 +-- src/api_communication/api_communication/api_listener.py | 4 +++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/api/views/index.ejs b/api/views/index.ejs index ec37140d..08cd4642 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -14,14 +14,13 @@

Camera view:

+

<%- api_connected %>

Last picture:

-

<%- api_connected %>

-
Hello World!
\ No newline at end of file diff --git a/src/api_communication/api_communication/api_listener.py b/src/api_communication/api_communication/api_listener.py index 84ee5eff..13e26fa3 100644 --- a/src/api_communication/api_communication/api_listener.py +++ b/src/api_communication/api_communication/api_listener.py @@ -77,6 +77,7 @@ class ApiListener(Node): def publish_status(self): 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})) @@ -84,6 +85,7 @@ class ApiListener(Node): def handle_responses(self): while True: if len(self.message_queue) > 0: + self.get_logger().info("sending message") self.publish_message(self.message_queue.pop(0)) def start_api_thread(self): @@ -130,7 +132,7 @@ class ApiListener(Node): self.move_position_request.angle = message['yaw'] self.get_logger().info(f'Calling move position service with request: {str(self.move_position_request)}') - + self.future = self.move_position_client.call_async(self.move_position_request) rclpy.spin_until_future_complete(self, self.future) result = self.future.result() From 340419a1874dc5df1a0fc6ba5789171d2973c863 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 12:57:23 +0200 Subject: [PATCH 020/332] update gui --- api/public/css/stylesheet.css | 19 +++++++++++++++++++ api/views/index.ejs | 16 ++++++++++++++++ 2 files changed, 35 insertions(+) diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css index d3b99251..61a5b626 100644 --- a/api/public/css/stylesheet.css +++ b/api/public/css/stylesheet.css @@ -28,4 +28,23 @@ body { height: 400px; border: 1px solid red; padding: 10px; +} + +#connectedbuttons { + overflow: auto; +} + +#buttons { + float: left; +} + +#connectedstatus { + float:left; +} + +#take_picture { + float:right; +} +#arm_disarm { + float: right; } \ No newline at end of file diff --git a/api/views/index.ejs b/api/views/index.ejs index 08cd4642..5f7c74aa 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -15,10 +15,26 @@

Camera view:

<%- api_connected %>

+
+
+ + +
+
+

Connected: <%- api_connected %>

+
+

Last picture:

+

Drone status

+

Battery percentage

+

CPU load

+

Current speed

+

Current position

+
+

Failsafe status

From 7c1bb248e7865de771241df018be4605e10201b4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 13:01:02 +0200 Subject: [PATCH 021/332] typo --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 6c22edc1..6ae04d53 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -54,7 +54,7 @@ public: vehicle_local_position_subscription_ = this->create_subscription("/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/control_mode", qos, std::bind(&PX4Controller::on_control_mode_receive, this, std::placeholders::_1)); - failsafe_subscription = this->create_subscription("/drone/failsafe", qos, std::bind(&PX4Controller::on_failsafe_receive, this, std::placeholders::_1)); + failsafe_subscription_ = this->create_subscription("/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/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/set_trajectory", std::bind(&PX4Controller::handle_trajectory_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); From 98133c88154226af5be165939fc054d8f0f6e3bd Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 13:05:10 +0200 Subject: [PATCH 022/332] css? --- api/public/css/stylesheet.css | 1 + api/views/index.ejs | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css index 61a5b626..314a3660 100644 --- a/api/public/css/stylesheet.css +++ b/api/public/css/stylesheet.css @@ -40,6 +40,7 @@ body { #connectedstatus { float:left; + width: 80%; } #take_picture { diff --git a/api/views/index.ejs b/api/views/index.ejs index 5f7c74aa..cca296d5 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -14,7 +14,6 @@

Camera view:

-

<%- api_connected %>

From d656338993a0fe0d48c03a98c41565bbbe2ddd95 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 13:07:16 +0200 Subject: [PATCH 023/332] typo --- api/views/index.ejs | 1 + src/px4_connection/src/px4_controller.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/api/views/index.ejs b/api/views/index.ejs index cca296d5..5146e3d3 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -17,6 +17,7 @@
+
diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 6ae04d53..d7f99a56 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -491,7 +491,7 @@ private: * * @param msg the message indicating that the failsafe was enabled */ - void on_failsafe_received(const drone_services::msg::FailsafeMsg::SharedPtr msg) + void on_failsafe_receive(const drone_services::msg::FailsafeMsg::SharedPtr msg) { if (msg->enabled) { From 369a4c2b75e9b280f01df7cef8ed84ef32e9fb34 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 13:19:56 +0200 Subject: [PATCH 024/332] add buttons --- api/public/css/stylesheet.css | 5 +++++ api/views/index.ejs | 19 +++++++++++++++++-- 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/api/public/css/stylesheet.css b/api/public/css/stylesheet.css index 314a3660..ecb9792f 100644 --- a/api/public/css/stylesheet.css +++ b/api/public/css/stylesheet.css @@ -48,4 +48,9 @@ body { } #arm_disarm { float: right; +} + +#button_stop { + background-color: red; + color: white; } \ No newline at end of file diff --git a/api/views/index.ejs b/api/views/index.ejs index 5146e3d3..b275f848 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -15,13 +15,28 @@

Camera view:

+
+

Connected: <%- api_connected %>

+

-
-

Connected: <%- api_connected %>

+
+
+

Controls

+
+ + + + + + + + + +
From 1f4fbb224612e923b3e062de7807162078a97f69 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 13:33:16 +0200 Subject: [PATCH 025/332] button callbacks --- api/views/index.ejs | 67 +++++++++++++++++++++++++++++++++++++-------- 1 file changed, 56 insertions(+), 11 deletions(-) diff --git a/api/views/index.ejs b/api/views/index.ejs index b275f848..7c9a8866 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -19,23 +19,23 @@

Connected: <%- api_connected %>

- +
- +

Controls

- - - - - - - - - + + + + + + + + +
@@ -54,4 +54,49 @@
+ + \ No newline at end of file From 73709a257f7b5979b8c88648a23eae7904e57c24 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 14:08:06 +0200 Subject: [PATCH 026/332] add up request --- api/index.js | 8 ++++++++ api/views/index.ejs | 6 ++++++ 2 files changed, 14 insertions(+) diff --git a/api/index.js b/api/index.js index d17e95f9..b2fb31e0 100644 --- a/api/index.js +++ b/api/index.js @@ -29,5 +29,13 @@ app.get("/", function (req, res) { res.render("index", { api_connected: api_connected }); }); +app.post("/move_up", function (req, res) { + console.log("got move up request") + var speed = req.data.speed; + var request = JSON.stringify({ command: 3, "up_down": speed, "left_right": 0 , "forward_backward": 0, "yaw": 0}); + ws.send(request); +} +); + app.listen(8080); console.log("Server is listening on port 8080"); diff --git a/api/views/index.ejs b/api/views/index.ejs index 7c9a8866..cb44f01d 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -67,6 +67,12 @@ } function up() { console.log("up"); + var xhr = new XMLHttpRequest(); + xhr.setRequestHeader("Content-Type", "application/json"); + xhr.open("POST", "/move_up", true); + var data = JSON.stringify({"speed": 1}); + xhr.send(data); + } function down() { console.log("down"); From f98b44917c9c913c03eb220a17493ee0c62fb1a8 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 14:14:55 +0200 Subject: [PATCH 027/332] add more websocket connected checks --- src/api_communication/api_communication/api_listener.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/api_communication/api_communication/api_listener.py b/src/api_communication/api_communication/api_listener.py index 13e26fa3..c0896e99 100644 --- a/src/api_communication/api_communication/api_listener.py +++ b/src/api_communication/api_communication/api_listener.py @@ -73,7 +73,10 @@ class ApiListener(Node): def publish_message(self, message): self.get_logger().info(f'Publishing message: {message}') - asyncio.run(self.websocket.send(message)) + if self.websocket is not None: + asyncio.run(self.websocket.send(message)) + else: + self.get_logger().error('Trying to publish message but no websocket connection') def publish_status(self): if self.status_data_received: @@ -84,7 +87,7 @@ class ApiListener(Node): def handle_responses(self): while True: - if len(self.message_queue) > 0: + if len(self.message_queue) > 0 and self.websocket is not None: self.get_logger().info("sending message") self.publish_message(self.message_queue.pop(0)) From 8e4e42cf0a5f45df4a531274e1a58efb0a334e6b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 14:17:34 +0200 Subject: [PATCH 028/332] log type and data --- api/index.js | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/api/index.js b/api/index.js index b2fb31e0..8213e70a 100644 --- a/api/index.js +++ b/api/index.js @@ -2,19 +2,20 @@ var express = require("express"); var app = express(); const WebSocket = require("ws"); -app.use(express.static('public')) +app.use(express.static("public")); var ws = new WebSocket("ws://10.100.0.40:9001/"); var api_connected = false; -ws.on('open', function open() { - console.log("connected with websockets to API!"); - api_connected = true; - }); +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); + console.log("got type: " + msg.type) + console.log("RECEIVED: " + msg.data); }); ws.on("error", console.error); @@ -30,12 +31,17 @@ app.get("/", function (req, res) { }); app.post("/move_up", function (req, res) { - console.log("got move up request") - var speed = req.data.speed; - var request = JSON.stringify({ command: 3, "up_down": speed, "left_right": 0 , "forward_backward": 0, "yaw": 0}); - ws.send(request); -} -); + console.log("got move up request"); + var speed = req.data.speed; + var request = JSON.stringify({ + command: 3, + up_down: speed, + left_right: 0, + forward_backward: 0, + yaw: 0, + }); + ws.send(request); +}); app.listen(8080); console.log("Server is listening on port 8080"); From b69bc88b6626b5afaf862a16c755edbab6711cc0 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 29 May 2023 14:27:24 +0200 Subject: [PATCH 029/332] receive status --- api/index.js | 12 +++++++++++- api/views/index.ejs | 16 +++++++++++++++- 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/api/index.js b/api/index.js index 8213e70a..abcee652 100644 --- a/api/index.js +++ b/api/index.js @@ -2,6 +2,8 @@ var express = require("express"); var app = express(); const WebSocket = require("ws"); +var last_status = {}; + app.use(express.static("public")); var ws = new WebSocket("ws://10.100.0.40:9001/"); @@ -14,7 +16,11 @@ ws.on("open", function open() { ws.on("message", function message(message) { var msg = JSON.parse(message); - console.log("got type: " + msg.type) + if (msg.type == "STATUS") { + last_status = msg.data; + } + + console.log("got type: " + msg.type); console.log("RECEIVED: " + msg.data); }); @@ -30,6 +36,10 @@ app.get("/", function (req, res) { res.render("index", { api_connected: api_connected }); }); +app.get("/status", function (req, res) { + res.json(last_status); +}); + app.post("/move_up", function (req, res) { console.log("got move up request"); var speed = req.data.speed; diff --git a/api/views/index.ejs b/api/views/index.ejs index cb44f01d..a4250834 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -55,8 +55,22 @@ + + \ No newline at end of file diff --git a/src/api_communication/api_communication/api_listener.py b/src/api_communication/api_communication/api_listener.py index 4b692191..f39b916a 100644 --- a/src/api_communication/api_communication/api_listener.py +++ b/src/api_communication/api_communication/api_listener.py @@ -2,6 +2,7 @@ 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 @@ -39,6 +40,7 @@ class ResponseMessage(Enum): STATUS = 0 IMAGE = 1 MOVE_DIRECTION_RESULT = 2 + FAILSAFE = 3 class ApiListener(Node): @@ -47,6 +49,8 @@ class ApiListener(Node): 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) waiting = 0 self.take_picture_client = self.create_client( @@ -95,7 +99,14 @@ class ApiListener(Node): self.status_data['armed'] = msg.armed self.status_data['control_mode'] = msg.control_mode self.status_data['route_setpoint'] = msg.route_setpoint + self.status_data['velocity'] = msg.velocity + self.status_data['position'] = msg.position + self.status_data['failsafe'] = msg.failsafe + def failsafe_callback(self, msg): + self.status_data['failsafe'] = msg.enabled + self.message_queue.append(json.dumps( + {'type': ResponseMessage.FAILSAFE.name, 'message': msg.msg})) # def send_video(self): # self.get_logger().info('Starting video thread') # vid = cv2.VideoCapture(0) diff --git a/src/camera/camera/camera_controller.py b/src/camera/camera/camera_controller.py index 6f723ac6..abed0785 100644 --- a/src/camera/camera/camera_controller.py +++ b/src/camera/camera/camera_controller.py @@ -58,7 +58,7 @@ class CameraController(Node): def handle_video_connection(self): self.get_logger().info('Starting sending video') - vid = cv2.VideoCapture(0, cv2.CAP_V4L) + 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) diff --git a/src/drone_services/msg/DroneStatus.msg b/src/drone_services/msg/DroneStatus.msg index 0489be52..5c4700d7 100644 --- a/src/drone_services/msg/DroneStatus.msg +++ b/src/drone_services/msg/DroneStatus.msg @@ -2,4 +2,7 @@ float32 battery_percentage float32 cpu_usage int32 route_setpoint # -1 if no route wstring control_mode -bool armed \ No newline at end of file +bool armed +float32 velocity[3] +float32 position[3] +bool failsafe \ No newline at end of file From 2c9b12cd8dc97a2713155dd669064217e4d885e4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Sat, 3 Jun 2023 14:19:03 +0200 Subject: [PATCH 141/332] try streaming with websockets --- api/views/index.ejs | 71 +++++++++++++++++++++++++++------------------ 1 file changed, 43 insertions(+), 28 deletions(-) diff --git a/api/views/index.ejs b/api/views/index.ejs index 8bff468e..0e5ca9e8 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -9,39 +9,39 @@ 5G drone API - +

5G Drone API

Camera view:

- -
-
-

Connected: <%- api_connected %>

- + +
+
+

Connected: <%- api_connected %>

+ +
+
+ +
+ +
-
- -
- -
-
-
-

Controls

-
- - - - - - - - - - +
+

Controls

+
+ + + + + + + + + + +
-

Last picture:

@@ -60,7 +60,22 @@