mission executor added to subscriber

camera_streaming/camera_subscriber.py

@@ -2,13 +2,13 @@
 
 import rclpy
 from rclpy.node import Node
+from rclpy.executors import SingleThreadedExecutor
 
 from sensor_msgs.msg import CompressedImage
 
 import cv2
 import numpy as np
 from datetime import datetime
-import matplotlib.pyplot as plt
 
 from rclpy.qos import (
     QoSProfile,
@@ -17,6 +17,9 @@ from rclpy.qos import (
     QoSReliabilityPolicy,
 )
 
+from mission_manager.client import MissionExecutor, MissionClient
+from infdist.optimization.agent import EstimatingAgent
+
 CUSTOM_QOS = QoSProfile(
     history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
     reliability=QoSReliabilityPolicy.
@@ -27,9 +30,9 @@ CUSTOM_QOS = QoSProfile(
 )
 
 
-class CameraSubscriber():
+class CameraSubscriber(Node, MissionExecutor):
     def __init__(self):
-        self.node = Node('camera_subscriber')
+        super().__init__('camera_subscriber')
         self.hosts = {
             'fdrones_t90': 0,
             'fdrones_t91': 1,
@@ -63,7 +66,7 @@ class CameraSubscriber():
         self.images = np.zeros(self.shape + self.resolution)
 
         self.camera_subscriptions = [
-            self.node.create_subscription(
+            self.create_subscription(
                 CompressedImage,
                 '/{}/camera'.format(host),
                 self.gen_camera_callback(host),
@@ -73,6 +76,15 @@ class CameraSubscriber():
         # self.figure, self.display = plt.subplots(2, 4)
         # plt.show(block=False)
 
+    def start_mission(self, timestamp):
+        self.get_logger().info('Mission started')
+
+    def end_mission(self, timestamp):
+        self.get_logger().info('Mission ended')
+
+    def change_params(self, params, timestamp):
+        self.get_logger().info('Params change received')
+
     def check_for_old_images(self):
         for host in self.hosts.keys():
             now = datetime.now()
@@ -161,10 +173,16 @@ def main():
     rclpy.init()
 
     camera_subscriber = CameraSubscriber()
+    mission_client = MissionClient()
+    mission_client.add_mission_executor(camera_subscriber)
+    executor = SingleThreadedExecutor()
+    executor.add_node(camera_subscriber)
+    executor.add_node(mission_client)
+    mission_client.get_logger().info(
+        'Node initialized, waiting for events.'
+    )
+    executor.spin()
 
-    rclpy.spin(camera_subscriber.node)
-
-    camera_subscriber.node.destroy_node()
     rclpy.shutdown()
 
 

setup.py

@@ -31,6 +31,7 @@ setup(
     entry_points={
         'console_scripts': [
             'camera_streaming = camera_streaming.camera_streaming:main',
+            'camera_subscriber = camera_streaming.camera_subscriber:main',
         ],
     },
 )