add land service

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("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] + " y: " + data.data.position[1] + " z: " + data.data.position[2];
+            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 () {

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(
+            self.status_data['velocity'] = [float(msg.velocity[0]), float(
-                msg.velocity[1]), int(msg.velocity[2])]
+                msg.velocity[1]), float(msg.velocity[2])]
-            self.status_data['position'] = [int(msg.position[0]), int(
+            self.status_data['position'] = [float(msg.position[0]), float(
-                msg.position[1]), int(msg.position[2])]
+            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}')

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;

src/drone_services/srv/Land.srv

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