merge with api

src/api_communication/api_communication/api_listener.py

@@ -0,0 +1,191 @@
+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.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 = {}
+        self.status_data_received = False
+        self.last_message = ""
+        self.message_queue = []
+        self.checking_for_message = False
+
+        self.websocket = None
+        self.server = None
+        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_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.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.status_data_received:
+            if self.websocket is not None:
+                self.message_queue.append(json.dumps(
+                    {'type': ResponseMessage.STATUS.name, '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())
+
+    async def handle_api(self):
+        self.get_logger().info('Starting API')
+        self.server = await websockets.serve(self.api_handler, '0.0.0.0', 9001)
+        self.get_logger().info('API started on port 9001')
+        await self.server.wait_closed()
+
+    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.name, 'image': image_data}))
+
+    def send_available_commands(self):
+        print('Sending available commands')
+        requestcommands = {}
+        messagetypes = {}
+        result = {}
+        for command in RequestCommand:
+            requestcommands[command.name] = command.value
+        for message_type in ResponseMessage:
+            messagetypes[message_type.name] = message_type.value
+        result['request_commands'] = requestcommands
+        result['response_messages'] = messagetypes
+        self.message_queue.append(json.dumps(
+            {'type': ResponseMessage.ALL_REQUESTS_RESPONSES.name, 'data': result}))
+
+    def consume_message(self, message):
+        self.get_logger().info(f'Consuming message: {message}')
+        try:
+            message_json = json.loads(str(message))
+            # self.get_logger().info(f'JSON: {str(message_json)}, type:{type(message_json)}')
+            # self.get_logger().info(f'JSON CMD: {str(message_json["command"])}, type:{type(message_json["command"])}')
+            if not "command" in message_json:
+                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.value:
+                    self.get_logger().info('Takeoff command received')
+                elif message_json['command'] == RequestCommand.LAND.value:
+                    self.get_logger().info('Land command received')
+                elif message_json['command'] == RequestCommand.MOVE_POSITION.value:
+                    self.get_logger().info('Move position command received')
+                elif message_json['command'] == RequestCommand.MOVE_DIRECTION.value:
+                    self.get_logger().info('Move direction command received')
+                elif message_json['command'] == RequestCommand.TAKE_PICTURE.value:
+                    self.process_image_request(message_json)
+                elif message_json['command'] == RequestCommand.GET.value:
+                    self.get_logger().info('Get command received')
+                    self.send_available_commands()
+                else:
+                    self.get_logger().error('Received unknown command')
+                    self.send_available_commands()
+        except TypeError:
+            self.get_logger().error('Received unknown command')
+            self.send_available_commands()
+        except json.JSONDecodeError:
+            self.get_logger().error('Received invalid JSON')
+            self.send_available_commands()
+        except Exception as e:
+            self.get_logger().error('Something went wrong!')
+            self.get_logger().error(str(e))
+
+    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}")
+                self.consume_message(message)
+
+        except websockets.exceptions.ConnectionClosed:
+            self.get_logger().info('Connection closed')
+            self.websocket = None
+        except Exception as e:
+            self.get_logger().error('Something went wrong!')
+            self.get_logger().error(str(e))
+            self.websocket = None
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    api_listener = ApiListener()
+
+    rclpy.spin(api_listener)
+
+    api_listener.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/api_communication/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>api_communication</name>
+  <version>0.0.0</version>
+  <description>package for communicating with edge computer through 5G</description>
+  <maintainer email="semmer99@gmail.com">sem</maintainer>
+  <license>Apache License 2.0</license>
+  <depend>rclpy</depend>
+  <depend>drone_services</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/api_communication/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/api_communication
+[install]
+install-scripts=$base/lib/api_communication

src/api_communication/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'api_communication'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='ubuntu',
+    maintainer_email='semmer99@gmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+                    'api_listener = api_communication.api_listener:main'
+        ],
+    },
+)

src/api_communication/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/api_communication/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/api_communication/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/camera/camera/camera_controller.py

@@ -1,6 +1,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
 

src/camera/package.xml

@@ -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>

src/drone_status/drone_status/drone_status.py

@@ -1,5 +1,6 @@
 import rclpy
 from rclpy.node import Node
+from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy
 
 from drone_services.msg import DroneStatus
 from drone_services.msg import DroneControlMode
@@ -12,24 +13,37 @@ CONTROL_MODE_ATTITUDE = 1
 CONTROL_MODE_VELOCITY = 2
 CONTROL_MODE_POSITION = 3
 
-class DroneStatus(Node):
+
+class DroneStatusNode(Node):
     def __init__(self):
         super().__init__('drone_status')
-        #publish to drone/status topic
-        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)
-        self.arm_status_subscriber = self.create_subscription(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.battery_status_subscriber = self.create_subscription(BatteryStatus, '/fmu/out/battery_status', self.battery_status_callback, 10)
-        self.cpu_load_subscriber = self.create_subscription(Cpuload, '/fmu/out/cpuload', self.cpu_load_callback, 10)
-        #publish every 0.5 seconds
+        qos_profile = QoSProfile(
+            reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+            durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
+            history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+            depth=1
+        )
+        # publish to drone/status topic
+        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)
+        self.arm_status_subscriber = self.create_subscription(
+            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.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)
+        # publish every 0.5 seconds
         self.timer = self.create_timer(0.5, self.publish_status)
         self.armed = False
         self.control_mode = "attitude"
         self.battery_percentage = 100.0
         self.cpu_usage = 0.0
         self.route_setpoint = 0
-    
+
     def publish_status(self):
         msg = DroneStatus()
         msg.armed = self.armed
@@ -38,9 +52,9 @@ class DroneStatus(Node):
         msg.cpu_usage = self.cpu_usage
         msg.route_setpoint = self.route_setpoint
         self.publisher.publish(msg)
-        self.get_logger().info('Publishing: "%s"' % msg.status)
+        self.get_logger().info('Publishing status message')
 
-    def control_mode_callback(self,msg):
+    def control_mode_callback(self, msg):
         if msg.control_mode == CONTROL_MODE_ATTITUDE:
             self.control_mode = "attitude"
         elif msg.control_mode == CONTROL_MODE_VELOCITY:
@@ -50,14 +64,29 @@ class DroneStatus(Node):
         else:
             self.control_mode = "unknown"
 
-    def arm_status_callback(self,msg):
+    def arm_status_callback(self, msg):
         self.armed = msg.armed
-    
-    def route_status_callback(self,msg):
+
+    def route_status_callback(self, msg):
         self.route_setpoint = msg.current_setpoint_index
 
     def battery_status_callback(self, msg):
         self.battery_percentage = msg.remaining * 100.0
-    
+
     def cpu_load_callback(self, msg):
-        self.cpu_usage = msg.load
+        self.cpu_usage = msg.load
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    drone_status = DroneStatusNode()
+
+    rclpy.spin(drone_status)
+
+    drone_status.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/drone_status/setup.py

@@ -20,6 +20,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+                    'drone_status = drone_status.drone_status:main'
         ],
     },
 )