Commit 629f1fc4 authored by Barciś, Agata's avatar Barciś, Agata
Browse files

mission executor added to subscriber

parent 1395fbb6
......@@ -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()
......
......@@ -31,6 +31,7 @@ setup(
entry_points={
'console_scripts': [
'camera_streaming = camera_streaming.camera_streaming:main',
'camera_subscriber = camera_streaming.camera_subscriber:main',
],
},
)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment