sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Merge branch 'api' of github.com:SemvdH/5g_drone_ROS2 into api

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-24 15:40:31 +02:00
parent 62a5b276fb d180392666
commit 9ea338f37f
3 changed files with 102 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
src/api_communication/api_communication/api_listener.py
View File

@@ -2,19 +2,45 @@ import rclpy
from rclpy.node import Node
from drone_services.msg import DroneStatus
from drone_services.srv import TakePicture
import asyncio
import websockets.server
import threading
import json
from enum import Enum
# communication: client always sends commands that have a command id.
# server always sends messages back that have a message type
class RequestCommand(Enum):
GET_COMMANDS_TYPES = -1 # to get the available commands and types
TAKEOFF = 0
LAND = 1
MOVE_POSITION = 2
MOVE_DIRECTION = 3
TAKE_PICTURE = 5
class ResponseMessage(Enum):
ALL_REQUESTS_RESPONSES = -1
STATUS = 0
IMAGE = 1
class ApiListener(Node):
def __init__(self):
super().__init__('api_listener')
self.get_logger().info('ApiListener node started')
self.drone_status_subscriber = self.create_subscription(DroneStatus, '/drone/status', self.drone_status_callback, 10)
self.drone_status_subscriber = self.create_subscription(
DroneStatus, '/drone/status', self.drone_status_callback, 10)
self.timer = self.create_timer(1, self.publish_status)
self.take_picture_client = self.create_client(
TakePicture, 'drone/picture')
while not self.take_picture_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('Take picture service not available, waiting again...')
self.take_picture_request = TakePicture.Request()
self.status_data = {}
@@ -24,21 +50,34 @@ class ApiListener(Node):
self.websocket = None
self.server = None
self.server_thread = threading.Thread(target=self.start_api_thread,daemon=True)
self.server_thread = threading.Thread(
target=self.start_api_thread, daemon=True)
self.server_thread.start()
self.response_thread = threading.Thread(
target=self.handle_responses, daemon=True)
self.response_thread.start()
def drone_status_callback(self, msg):
self.status_data['battery_percentage'] = msg.battery_percentage
self.status_data['cpu_usage'] = msg.cpu_usage
def publish_message(self,message):
self.status_data['armed'] = msg.armed
self.status_data['control_mode'] = msg.control_mode
self.status_data['route_setpoint'] = msg.route_setpoint
def publish_message(self, message):
self.get_logger().info(f'Publishing message: {message}')
asyncio.run(self.websocket.send(message))
def publish_status(self):
if self.websocket is not None:
self.get_logger().info(f"Publishing status: battery: {self.status_data['battery_percentage']}, cpu: {self.status_data['cpu_usage']}")
self.publish_message(json.dumps(self.status_data))
self.message_queue.append(json.dumps(
{'type': ResponseMessage.STATUS, 'data': self.status_data}))
def handle_responses(self):
while True:
if len(self.message_queue) > 0:
self.publish_message(self.message_queue.pop(0))
def start_api_thread(self):
asyncio.run(self.handle_api())
@@ -47,27 +86,65 @@ class ApiListener(Node):
self.server = await websockets.serve(self.api_handler, '0.0.0.0', 9001)
self.get_logger().info('API started')
await self.server.wait_closed()
async def handle_message_receive(self,websocket):
self.get_logger().info(f"Received message callback: {self.last_message}")
self.last_message = await websocket.recv()
def message_received_callback(self):
self.get_logger().info(f"Received message callback: {self.last_message}")
self.message_queue.append(self.last_message)
self.checking_for_message = False
def process_image_request(self, message_json):
self.get_logger().info('Take picture command received')
if message_json['filename']:
self.get_logger().info(
f'Filename: {message_json["filename"]}')
self.take_picture_request.input_name = message_json['filename']
self.future = self.cli.call_async(self.take_picture_request)
rclpy.spin_until_future_complete(self, self.future)
result_filename = self.future.result()
with open(result_filename, 'rb') as f:
image_data = f.read()
self.message_queue.append(json.dumps(
{'type': ResponseMessage.IMAGE, 'image': image_data}))
def send_available_commands(self):
result = {}
for command in RequestCommand:
result[command.name] = command.value
for message_type in ResponseMessage:
result[message_type.name] = message_type.value
self.message_queue.append(json.dumps(
{'type': ResponseMessage.ALL_REQUESTS_RESPONSES, 'data': result}))
def consume_message(self, message):
message_json = json.loads(message)
if not message_json['command']:
self.get_logger().error('Received message without command')
self.send_available_commands()
else:
self.get_logger().info(
f'Received command: {message_json["command"]}')
if message_json['command'] == RequestCommand.TAKEOFF:
self.get_logger().info('Takeoff command received')
elif message_json['command'] == RequestCommand.LAND:
self.get_logger().info('Land command received')
elif message_json['command'] == RequestCommand.MOVE_POSITION:
self.get_logger().info('Move position command received')
elif message_json['command'] == RequestCommand.MOVE_DIRECTION:
self.get_logger().info('Move direction command received')
elif message_json['command'] == RequestCommand.TAKE_PICTURE:
self.process_image_request(message_json)
elif message_json['command'] == RequestCommand.GET:
self.get_logger().info('Get command received')
self.send_available_commands()
else:
self.get_logger().error('Received unknown command')
self.send_available_commands()
async def api_handler(self, websocket):
self.get_logger().info('New connection')
if self.websocket is not None:
self.get_logger().error('Got a new websocket connection but I am already connected!')
return
self.websocket = websocket
try:
async for message in websocket:
self.get_logger().info(f"Received message: {message}")
await websocket.send(f"battery: {self.last_battery_percentage}, cpu: {self.last_cpu_usage}")
except websockets.exceptions.ConnectionClosed:
self.get_logger().info('Connection closed')
@@ -83,5 +160,6 @@ def main(args=None):
api_listener.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
main()

1
src/camera/camera/camera_controller.py
View File

@@ -2,6 +2,7 @@ import rclpy
from rclpy.node import Node
from drone_services.srv import TakePicture
from sensor_msgs.msg import Image
import os
from datetime import datetime

1
src/camera/package.xml
View File

@@ -9,6 +9,7 @@
<exec_depend>rclpy</exec_depend>
<exec_depend>drone_services</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>