add remaining functionality and comments

api/index.js

@@ -82,7 +82,6 @@ var connect_to_api = function () {
 
 function send_image_data_to_clients(videoData) {
   sse_clients.forEach((client) => {
-
     // Set the SSE event name as 'message'
     client.response.write("event: message\n");
 
@@ -98,7 +97,7 @@ function send_image_data_to_clients(videoData) {
 
 // Define the endpoint to receive video data
 app.post("/video", (req, res) => {
-    // console.log("got video endpoint")
+  // console.log("got video endpoint")
   let videoData = Buffer.from("");
 
   req.on("data", (chunk) => {
@@ -109,7 +108,7 @@ app.post("/video", (req, res) => {
   req.on("end", () => {
     // Process the received video data
     //   console.log("Received video data:" + videoData.length);
-      send_image_data_to_clients(videoData); 
+    send_image_data_to_clients(videoData);
 
     // Send a response indicating successful receipt
     res.sendStatus(200);
@@ -150,6 +149,26 @@ app.post("/move", function (req, res) {
   ws.send(request);
 });
 
+app.post("/estop", function (req, res) {
+  console.log("got estop request");
+  var request = JSON.stringify({
+    command: 6,
+  });
+  ws.send(request);
+});
+
+app.post("/land", function (req, res) {
+  console.log("got land request");
+  var request = JSON.stringify({ command: 0 });
+  ws.send(request);
+});
+
+app.post("/arm_disarm", function (req, res) {
+    console.log("got arm/disarm request");
+    var request = JSON.stringify({ command: 1 });
+    ws.send(request);
+});
+
 app.get("/connect", function (req, res) {
   console.log("got connect request");
   connect_to_api();
@@ -164,7 +183,7 @@ app.get("/connect", function (req, res) {
 });
 
 app.get("/test", function (req, res) {
-    res.render("test");
+  res.render("test");
 });
 
 app.listen(8080);

api/views/index.ejs

@@ -37,7 +37,7 @@
                     <button class="movebutton" id="button_backward">Backward</button>
                     <button class="movebutton" id="button_left">Left</button>
                     <button class="movebutton" id="button_right">Right</button>
-                    <button id="button_stop" onclick="stop()">Stop</button>
+                    <button id="button_land" onclick="land()">Land</button>
                     <button id="button_estop" onclick="estop()"><strong>Emergency Stop</strong></button>
 
                 </div>
@@ -112,6 +112,9 @@
                     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 if (data.type == "FAILSAFE") {
+                    document.getElementById("failsafe").innerHTML = "Failsafe: ENABLED";
+                    alert("Failsafe enabled! Drone is landing. The failsafe message is: " + data.message);
                 } else {
                     // decodeBase64Image(data.image, document.getElementById("result-video"));
                 }
@@ -208,9 +211,19 @@
         console.log("stop");
         send_move_request(JSON.stringify({ "up_down": 0.0, "forward_backward": 0.0, "left_right": 0.0, "turn_left_right": 0.0 }));
     }
+    function land_takeoff() {
+        console.log("land");
+        var xhr = new XMLHttpRequest();
+        xhr.open("POST", "/land", true);
+        xhr.send();
+
+    }
 
     function estop() {
         console.log("estop");
+        var xhr = new XMLHttpRequest();
+        xhr.open("POST", "/estop", true);
+        xhr.send();
     }
 
     function take_picture() {
@@ -224,6 +237,9 @@
 
     function arm_disarm() {
         console.log("arm/disarm");
+        var xhr = new XMLHttpRequest();
+        xhr.open("POST", "/arm_disarm", true);
+        xhr.send();
     }
 
     function connect_to_video_stream() {

src/api_communication/api_communication/api_listener.py

@@ -5,6 +5,9 @@ from drone_services.msg import DroneStatus
 from drone_services.msg import FailsafeMsg
 from drone_services.srv import TakePicture
 from drone_services.srv import MovePosition
+from drone_services.srv import EnableFailsafe
+from drone_services.srv import ArmDrone
+from drone_services.srv import DisarmDrone
 
 import asyncio
 import websockets.server
@@ -26,9 +29,12 @@ RES_4K_W = 4656
 # TODO change video to be handled by camera controller through websocket with different port
 
 class RequestCommand(Enum):
+    """
+    Enum for the commands that can be sent to the API
+    """
     GET_COMMANDS_TYPES = -1  # to get the available commands and types
-    TAKEOFF = 0
+    LAND = 0
-    LAND = 1
+    ARM_DISARM = 1
     MOVE_POSITION = 2
     MOVE_DIRECTION = 3
     TAKE_PICTURE = 5
@@ -36,6 +42,9 @@ class RequestCommand(Enum):
 
 
 class ResponseMessage(Enum):
+    """
+    Enum for the messages that can be sent to the client
+    """
     ALL_REQUESTS_RESPONSES = -1
     STATUS = 0
     IMAGE = 1
@@ -44,6 +53,9 @@ class ResponseMessage(Enum):
 
 
 class ApiListener(Node):
+    """
+    Node that listens to the client and sends the commands to the drone
+    """
     def __init__(self):
         super().__init__('api_listener')
         self.get_logger().info('ApiListener node started')
@@ -52,29 +64,29 @@ class ApiListener(Node):
         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(
             TakePicture, '/drone/picture')
-        while not self.take_picture_client.wait_for_service(timeout_sec=1.0):
+        self.wait_for_service(self.take_picture_client, "Take picture")
-            if (waiting > 10):
-                self.get_logger().error(
-                    'Take picture service not available, exiting...')
-                exit(-1)
-            self.get_logger().info('Take picture service not available, waiting again...')
-            waiting = waiting + 1
         self.take_picture_request = TakePicture.Request()
+
         self.move_position_client = self.create_client(
             MovePosition, '/drone/move_position')
-        waiting = 0
+        self.wait_for_service(self.move_position_client, "Move position")
-        while not self.move_position_client.wait_for_service(timeout_sec=1.0):
-            if (waiting > 10):
-                self.get_logger().error(
-                    'Move position service not available, exiting...')
-                exit(-1)
-            self.get_logger().info('Move position service not available, waiting again...')
-            waiting = waiting + 1
         self.move_position_request = MovePosition.Request()
 
+        self.enable_failsafe_client = self.create_client(EnableFailsafe, "/drone/enable_failsafe")
+        self.wait_for_service(self.enable_failsafe_client, "Enable failsafe")
+        self.enable_failsafe_request = EnableFailsafe.Request()
+
+        self.arm_drone_client = self.create_client(ArmDrone, "/drone/arm")
+        self.wait_for_service(self.arm_drone_client, "Arm drone")
+        self.arm_drone_request = ArmDrone.Request()
+
+        self.disarm_drone_client = self.create_client(DisarmDrone, "/drone/disarm")
+        self.wait_for_service(self.disarm_drone_client, "Disarm drone")
+        self.disarm_drone_request = DisarmDrone.Request()
+
         self.status_data = {}
         self.status_data_received = False
         self.last_message = ""
@@ -91,12 +103,36 @@ class ApiListener(Node):
         self.response_thread.start()
 
         self.event_loop = None
+        self.armed = False
+    
+    def wait_for_service(self,client,service_name):
+        """Waits for a client service to be available
+
+        Args:
+            client (ROS2 service client): The client to use to wait fo the service
+            service_name (str): The client name for logging
+        """
+        waiting = 0
+        while not client.wait_for_service(timeout_sec=1.0):
+            if (waiting > 10):
+                self.get_logger().error(
+                    service_name + ' service not available, exiting...')
+                exit(-1)
+            self.get_logger().info(service_name + 'service not available, waiting again...')
+            waiting = waiting + 1
+
 
     def drone_status_callback(self, msg):
+        """Callback for when a drone_status message is received
+
+        Args:
+            msg (DroneStatus): The received message 
+        """
         self.status_data_received = True
         self.status_data['battery_percentage'] = msg.battery_percentage
         self.status_data['cpu_usage'] = msg.cpu_usage
         self.status_data['armed'] = msg.armed
+        self.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
@@ -106,11 +142,21 @@ class ApiListener(Node):
         self.status_data['position'] = msg.position
 
     def failsafe_callback(self, msg):
+        """Callback for when the failsafe gets enabled. Queues a FAILSAFE message to the client
+
+        Args:
+            msg (FailSAfe): The message that was received
+        """
         self.status_data['failsafe'] = msg.enabled
         self.message_queue.append(json.dumps(
                     {'type': ResponseMessage.FAILSAFE.name, 'message': msg.msg}))
 
     async def publish_message(self, message):
+        """publishes a message to the NodeJS client
+
+        Args:
+            message (JSON object): the message to send
+        """
         # self.get_logger().info(f'Publishing message: {message}')
         if self.websocket is not None:
             try:
@@ -122,6 +168,8 @@ class ApiListener(Node):
             self.get_logger().error('Trying to publish message but no websocket connection')
 
     def publish_status(self):
+        """publishes the current status to the client by queueing a STATUS message
+        """
         if self.status_data_received:
             self.status_data_received = False
             if self.websocket is not None:
@@ -129,15 +177,19 @@ class ApiListener(Node):
                     {'type': ResponseMessage.STATUS.name, 'data': self.status_data}))
 
     def handle_responses(self):
+        """Thread to handle responses to send to the client
+        """
         while True:
             if len(self.message_queue) > 0 and self.websocket is not None and self.event_loop is not None:
                 # self.get_logger().info("sending message")
                 asyncio.run(self.publish_message(self.message_queue.pop(0)))
 
     def start_api_thread(self):
+        """Starts the API thread"""
         asyncio.run(self.handle_api())
 
     async def handle_api(self):
+        """Handles the API requests and starts the websockets server"""
         self.get_logger().info('Starting API')
         self.event_loop = asyncio.get_event_loop()
         self.server = await websockets.serve(self.api_handler, '0.0.0.0', 9001)
@@ -145,6 +197,11 @@ class ApiListener(Node):
         await self.server.wait_closed()
 
     def process_image_request(self, message_json):
+        """Processes an image request from the client
+
+        Args:
+            message_json (str): The JSON message received, containing an optional filename for the result image
+        """
         self.get_logger().info('Processing image request')
         if 'filename' in message_json:
             self.get_logger().info(
@@ -156,6 +213,11 @@ class ApiListener(Node):
         
     
     def image_request_callback(self, future):
+        """Callback for when an image request is made from the client"
+
+        Args:
+            future (Future): Future object that holds the result
+        """
         try:
             result_filename = future.result().filename
             self.get_logger().info("Received result filename: " + result_filename)
@@ -217,6 +279,17 @@ class ApiListener(Node):
         except Exception as e:
             self.get_logger().error(
                 'Something went wrong while sending a move position request!\n' + str(e))
+            
+    def emergency_stop(self):
+        try:
+            self.enable_failsafe_request.message = "Emergency stop activated"
+            future = self.enable_failsafe_client.call_async(self.enable_failsafe_request)
+            rclpy.spin_until_future_complete(self, future)
+            result = future.result()
+            if (result.enabled == True):
+                self.get_logger().info("Emergency stop activated")
+        except Exception as e:
+            self.get_logger().error("Something went wrong while trying to enable failsafe!\n" + str(e))
 
     def takeoff(self):
         self.get_logger().info('Takeoff command received')
@@ -234,6 +307,22 @@ class ApiListener(Node):
         self.move_position_request.angle = 0.0
         self.send_move_position_request()
 
+    def arm_disarm(self):
+        if self.armed:
+            self.get_logger().info('Disarm command received')
+            future = self.disarm_drone_client.call_async(self.disarm_drone_request)
+            rclpy.spin_until_future_complete(self, future)
+            self.get_logger().info('publishing message arm msg on service')
+            if future.result().success:
+                self.get_logger().info('Disarm service call success')
+        else:
+            self.get_logger().info('Arm command received')
+            future = self.arm_drone_client.call_async(self.arm_drone_request)
+            rclpy.spin_until_future_complete(self, future)
+            self.get_logger().info('publishing message arm msg on service')
+            if future.result().success:
+                self.get_logger().info('Arm service call success')
+
     def consume_message(self, message):
         self.get_logger().info(f'Consuming message: {message}')
         try:
@@ -246,12 +335,12 @@ class ApiListener(Node):
             else:
                 self.get_logger().info(
                     f'Received command: {message_json["command"]}')
-                if message_json['command'] == RequestCommand.TAKEOFF.value:
+                if message_json['command'] == RequestCommand.LAND.value:
-                    self.get_logger().info('Takeoff command received')
-                    self.takeoff()
-                elif message_json['command'] == RequestCommand.LAND.value:
                     self.get_logger().info('Land command received')
                     self.land()
+                elif message_json['command'] == RequestCommand.ARM_DISARM.value:
+                    self.get_logger().info('Arm/disarm command received')
+                    self.arm_disarm()
                 elif message_json['command'] == RequestCommand.MOVE_POSITION.value:
                     self.get_logger().info('Move position command received')
                 elif message_json['command'] == RequestCommand.MOVE_DIRECTION.value:
@@ -265,7 +354,7 @@ class ApiListener(Node):
                     self.send_available_commands()
                 elif message_json['command'] == RequestCommand.EMERGENCY_STOP.value:
                     self.get_logger().info('Emergency stop command received')
-                    # emergency stop: set to attitude mode and stop, also disarm
+                    self.emergency_stop()
                 else:
                     self.get_logger().error('Received unknown command ' + str(message_json['command']) )
                     self.send_available_commands()