add velocity and position display

api/views/index.ejs

@@ -110,6 +110,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.speed[0] + " y: " + data.data.speed[1] + " z: " + data.data.speed[2];
+                    document.getElementById("position").innerHTML = "Current position (m): x: " + data.data.position[0] + " y: " + data.data.position[1] + " z: " + data.data.position[2];
                 } else {
                     // decodeBase64Image(data.image, document.getElementById("result-video"));
                 }

src/api_communication/api_communication/api_listener.py

@@ -102,29 +102,13 @@ class ApiListener(Node):
         self.status_data['velocity'] = msg.velocity
         self.status_data['position'] = msg.position
         self.status_data['failsafe'] = msg.failsafe
+        self.status_data['velocity'] = msg.velocity
+        self.status_data['position'] = msg.position
 
     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)
-
-    #     # 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():
-    #                 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')
-    #                 self.message_queue.append(man.tobytes())
-
-    #         except Exception as e:
-    #             self.get_logger().error('Something went wrong while reading video: ' + str(e))
 
     async def publish_message(self, message):
         # self.get_logger().info(f'Publishing message: {message}')

src/drone_status/drone_status/drone_status.py

@@ -8,6 +8,7 @@ from drone_services.msg import DroneArmStatus
 from drone_services.msg import DroneRouteStatus
 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
@@ -27,15 +28,17 @@ class DroneStatusNode(Node):
         self.publisher = self.create_publisher(
             DroneStatus, '/drone/status', 10)
         self.control_mode_subscriber = self.create_subscription(
-            DroneControlMode, '/drone/control_mode', self.control_mode_callback, 10)
+            DroneControlMode, '/drone/control_mode', self.control_mode_callback, qos_profile=qos_profile)
         self.arm_status_subscriber = self.create_subscription(
-            DroneArmStatus, '/drone/arm_status', self.arm_status_callback, 10)
+            DroneArmStatus, '/drone/arm_status', self.arm_status_callback, qos_profile=qos_profile)
         self.route_status_subscriber = self.create_subscription(
-            DroneRouteStatus, '/drone/route_status', self.route_status_callback, 10)
+            DroneRouteStatus, '/drone/route_status', self.route_status_callback, qos_profile=qos_profile)
         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
@@ -43,6 +46,8 @@ class DroneStatusNode(Node):
         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,6 +56,8 @@ 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
         self.publisher.publish(msg)
         self.get_logger().info('Publishing status message')
 
@@ -76,6 +83,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)