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

mission executor added to subscriber

parent 1395fbb6
...@@ -2,13 +2,13 @@ ...@@ -2,13 +2,13 @@
import rclpy import rclpy
from rclpy.node import Node from rclpy.node import Node
from rclpy.executors import SingleThreadedExecutor
from sensor_msgs.msg import CompressedImage from sensor_msgs.msg import CompressedImage
import cv2 import cv2
import numpy as np import numpy as np
from datetime import datetime from datetime import datetime
import matplotlib.pyplot as plt
from rclpy.qos import ( from rclpy.qos import (
QoSProfile, QoSProfile,
...@@ -17,6 +17,9 @@ from rclpy.qos import ( ...@@ -17,6 +17,9 @@ from rclpy.qos import (
QoSReliabilityPolicy, QoSReliabilityPolicy,
) )
from mission_manager.client import MissionExecutor, MissionClient
from infdist.optimization.agent import EstimatingAgent
CUSTOM_QOS = QoSProfile( CUSTOM_QOS = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
reliability=QoSReliabilityPolicy. reliability=QoSReliabilityPolicy.
...@@ -27,9 +30,9 @@ CUSTOM_QOS = QoSProfile( ...@@ -27,9 +30,9 @@ CUSTOM_QOS = QoSProfile(
) )
class CameraSubscriber(): class CameraSubscriber(Node, MissionExecutor):
def __init__(self): def __init__(self):
self.node = Node('camera_subscriber') super().__init__('camera_subscriber')
self.hosts = { self.hosts = {
'fdrones_t90': 0, 'fdrones_t90': 0,
'fdrones_t91': 1, 'fdrones_t91': 1,
...@@ -63,7 +66,7 @@ class CameraSubscriber(): ...@@ -63,7 +66,7 @@ class CameraSubscriber():
self.images = np.zeros(self.shape + self.resolution) self.images = np.zeros(self.shape + self.resolution)
self.camera_subscriptions = [ self.camera_subscriptions = [
self.node.create_subscription( self.create_subscription(
CompressedImage, CompressedImage,
'/{}/camera'.format(host), '/{}/camera'.format(host),
self.gen_camera_callback(host), self.gen_camera_callback(host),
...@@ -73,6 +76,15 @@ class CameraSubscriber(): ...@@ -73,6 +76,15 @@ class CameraSubscriber():
# self.figure, self.display = plt.subplots(2, 4) # self.figure, self.display = plt.subplots(2, 4)
# plt.show(block=False) # 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): def check_for_old_images(self):
for host in self.hosts.keys(): for host in self.hosts.keys():
now = datetime.now() now = datetime.now()
...@@ -161,10 +173,16 @@ def main(): ...@@ -161,10 +173,16 @@ def main():
rclpy.init() rclpy.init()
camera_subscriber = CameraSubscriber() 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() rclpy.shutdown()
......
...@@ -31,6 +31,7 @@ setup( ...@@ -31,6 +31,7 @@ setup(
entry_points={ entry_points={
'console_scripts': [ 'console_scripts': [
'camera_streaming = camera_streaming.camera_streaming:main', '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