add height to drone status

src/drone_services/msg/DroneStatus.msg

@@ -5,4 +5,5 @@ wstring control_mode
 bool armed
 float32[3] velocity
 float32[3] position
+float32 height
 bool failsafe

src/drone_status/drone_status/drone_status.py

@@ -6,6 +6,7 @@ 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
@@ -33,6 +34,7 @@ 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(
@@ -42,6 +44,7 @@ class DroneStatusNode(Node):
         # 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
@@ -58,6 +61,7 @@ class DroneStatusNode(Node):
         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')
 
@@ -72,6 +76,9 @@ 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: