From 273d979beb3088a28db94c0a6eb6bc7b43f8df36 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 5 Jun 2023 13:36:44 +0200 Subject: [PATCH] add remaining functionality and comments --- api/index.js | 27 +++- api/views/index.ejs | 18 ++- .../api_communication/api_listener.py | 135 +++++++++++++++--- 3 files changed, 152 insertions(+), 28 deletions(-) diff --git a/api/index.js b/api/index.js index 949b6e81..22cdbcb2 100644 --- a/api/index.js +++ b/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); diff --git a/api/views/index.ejs b/api/views/index.ejs index d918e4d7..b5aa0e27 100644 --- a/api/views/index.ejs +++ b/api/views/index.ejs @@ -37,7 +37,7 @@ - + @@ -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() { diff --git a/src/api_communication/api_communication/api_listener.py b/src/api_communication/api_communication/api_listener.py index 5f3cef05..fa80309b 100644 --- a/src/api_communication/api_communication/api_listener.py +++ b/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 = 1 + LAND = 0 + 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): - 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.wait_for_service(self.take_picture_client, "Take picture") self.take_picture_request = TakePicture.Request() + self.move_position_client = self.create_client( MovePosition, '/drone/move_position') - waiting = 0 - 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.wait_for_service(self.move_position_client, "Move position") 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: - self.get_logger().info('Takeoff command received') - self.takeoff() - elif message_json['command'] == RequestCommand.LAND.value: + if 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()