From 4daf79cdc021ddee11f6fa3e82c1114a93db3d69 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Thu, 8 Jun 2023 21:21:12 +0200
Subject: [PATCH] add land service

---
 api/views/index.ejs                                     | 6 ++++--
 src/api_communication/api_communication/api_listener.py | 9 +++++----
 src/drone_controls/src/PositionChanger.cpp              | 8 ++++++++
 src/drone_services/srv/Land.srv                         | 2 ++
 4 files changed, 19 insertions(+), 6 deletions(-)
 create mode 100644 src/drone_services/srv/Land.srv

diff --git a/api/views/index.ejs b/api/views/index.ejs
index 16cc2e65..0d3724b7 100644
--- a/api/views/index.ejs
+++ b/api/views/index.ejs
@@ -65,6 +65,7 @@
         <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"
@@ -139,6 +140,7 @@
                     document.getElementById("control_mode").innerHTML = "Control mode: " + data.data.control_mode;
                     document.getElementById("speed").innerHTML = "Current speed (m/s): x: " + data.data.velocity[0] + " y: " + data.data.velocity[1] + " z: " + data.data.velocity[2];
                     document.getElementById("position").innerHTML = "Current position (m): x: " + data.data.position[0] + " y: " + data.data.position[1] + " z: " + data.data.position[2];
+                    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";
@@ -313,8 +315,8 @@
             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] + " y: " + data.data.velocity[1] + " z: " + data.data.velocity[2];
-            document.getElementById("position").innerHTML = "Current position (m): x: " + data.data.position[0] + " y: " + data.data.position[1] + " z: " + data.data.position[2];
+            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 () {
diff --git a/src/api_communication/api_communication/api_listener.py b/src/api_communication/api_communication/api_listener.py
index c8eee072..855d97dc 100644
--- a/src/api_communication/api_communication/api_listener.py
+++ b/src/api_communication/api_communication/api_listener.py
@@ -153,11 +153,12 @@ class ApiListener(Node):
             self.armed = msg.armed
             self.status_data['control_mode'] = msg.control_mode
             self.status_data['route_setpoint'] = msg.route_setpoint
-            self.status_data['velocity'] = [int(msg.velocity[0]), int(
-                msg.velocity[1]), int(msg.velocity[2])]
-            self.status_data['position'] = [int(msg.position[0]), int(
-                msg.position[1]), int(msg.position[2])]
+            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}')
diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp
index bc2e9cb4..667f0eb7 100644
--- a/src/drone_controls/src/PositionChanger.cpp
+++ b/src/drone_controls/src/PositionChanger.cpp
@@ -49,6 +49,7 @@ 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));
@@ -270,6 +271,11 @@ public:
         apply_collision_weights();
     }
 
+    void handle_height_message(const drone_services::msg::HeightData::SharedPtr message)
+    {
+        this->current_height = message->height;
+    }
+
     /**
      * @brief Callback function for receiving new LIDAR data
      *
@@ -462,6 +468,8 @@ private:
     float current_speed_x = 0;
     float current_speed_y = 0;
     float current_speed_z = 0;
+    float current_height = 0;
+    bool is_landing = 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;
diff --git a/src/drone_services/srv/Land.srv b/src/drone_services/srv/Land.srv
new file mode 100644
index 00000000..bdc6893e
--- /dev/null
+++ b/src/drone_services/srv/Land.srv
@@ -0,0 +1,2 @@
+---
+bool is_landing
\ No newline at end of file