safety_master_plugin/safety_master_plugin/ros_master_wrapper.py

#!/usr/bin/env python
import rclpy
from rclpy.node import Node, NodeNameNonExistentError
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSLivelinessPolicy, QoSDurabilityPolicy

from std_msgs.msg import Bool
from ros2aria_msgs.msg import RobotInfoMsg
from ros2aria_msgs.msg import RestrictionsMsg

from safety_master_plugin.robot_info import RobotInfo
from safety_master_plugin.master_restrictions import Restrictions

from safety_master_plugin.stop_state import StopState as SS

class ROSWrapper(Node):

    def __init__(self):
        super().__init__('safety_master_plugin')
        self.robots_list_update_callback = None
        self.robot_info_update_callback = None

        self.robot_info_subscribers = {}
        self.master_stop_publisher = None
        self.restrictions_publisher = None

        self.setup_subscribers_and_publishers()

        self._qos = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
            history=QoSHistoryPolicy.SYSTEM_DEFAULT,
            liveliness=QoSLivelinessPolicy.AUTOMATIC,
            depth=10
        )
        self.subs = []


    def setup_subscribers_and_publishers(self):
        self.master_stop_publisher = self.create_publisher(Bool, '/pioneers/master_stop', 10)
        self.restrictions_publisher = self.create_publisher(RestrictionsMsg, '/pioneers/restrictions', 10)

    def add_robot_subscriber(self,robot_id):
        if robot_id not in self.robot_info_subscribers:
            topic_name = '/pioneer{0}/robot_info'.format(robot_id)
            self.robot_info_subscribers[robot_id] = self.create_subscription(RobotInfoMsg, topic_name, self.handle_robot_info_update, 10)
        else:
            raise RuntimeError('Subscriber for pioneer{0} already in dictionary'.format(robot_id))

    # def remove_robot_subscriber(self,robot_id):
    #     if robot_id in self.robot_info_subscribers:
    #         self.unsubscribe(self.robot_info_subscribers[robot_id])
    #         del self.robot_info_subscribers[robot_id]
    #     else:
    #         raise RuntimeError('Subscriber for PIONIER{0} not in dictionary. Cannot be removed'.format(robot_id))

    # def unsubscribe(self,subscriber):
    #     if subscriber != None:
    #         subscriber.unregister()

    # def unregister_publisher(self,publisher):
    #     if publisher != None:
    #         publisher.unregister()


    def publish_periodic_update(self):
        restrictions = self.get_restrictions()
        restrictions_msg = RestrictionsMsg()
        restrictions_msg.distance.data = restrictions.distance
        restrictions_msg.linear_velocity.data = restrictions.linear_velocity
        restrictions_msg.angular_velocity.data = restrictions.angular_velocity

        stop_state = self.get_master_stop_state()
        stop_msg = Bool()

        if stop_state == SS.RUNNING:
            stop_msg.data = True
        elif stop_state == SS.STOPPED:
            stop_msg.data = False
        else:
            stop_msg.data = False
            raise ValueError('stop_state UNKNOWN when attempting to publish periodic update')

        self.restrictions_publisher.publish(restrictions_msg)
        self.master_stop_publisher.publish(stop_msg)

    def cancel_subscribers_and_publishers(self):
        self.unregister_publisher(self.master_stop_publisher)
        self.unregister_publisher(self.restrictions_publisher)

        for key, value in self.robot_info_subscribers.iteritems():
            self.unsubscribe(value)

    def unregister_node(self):
        self.cancel_subscribers_and_publishers()
        rospy.signal_shutdown('Closing safety master plugin')

    def setup_get_master_stop_state_function(self,function):
        self.get_master_stop_state = function

    def setup_get_restrictions_function(self,function):
        self.get_restrictions = function

    # ROSWrapper Callbacks
    def get_robots_list(self):
        robots_id_list = []

        # Można to zrobić lepiej np. czekać na publikowaną wiadomość na robot_info
        for pioneer_id in range(6):
            published_topics_list = []
            try:
                published_topics_list = self.get_publisher_names_and_types_by_node('ros2aria', '/pioneer'+str(pioneer_id))
            except NodeNameNonExistentError:
                pass

            if len(published_topics_list):
                robots_id_list.append(pioneer_id)

        self.robots_list_update_callback(robots_id_list)

    def handle_robot_info_update(self, msg):
        _robot_info = RobotInfo(0)
        _robot_info.update_robot_info(msg)
        self.robot_info_update_callback(_robot_info)


    # MasterPlugin Callbacks
    def set_robots_list_update_callback(self,callback_function):
        self.robots_list_update_callback = callback_function

    def set_robot_info_update_callback(self,callback_function):
        self.robot_info_update_callback = callback_function

    def master_started(self):
        msg = Bool()
        msg.data = True
        self.master_stop_publisher.publish(msg)

    def master_stopped(self):
        msg = Bool()
        msg.data = False
        self.master_stop_publisher.publish(msg)

    def restrictions_changed(self,restrictions):
        msg = RestrictionsMsg()

        msg.distance.data = restrictions.distance
        msg.linear_velocity.data = restrictions.linear_velocity
        msg.angular_velocity.data = restrictions.angular_velocity

        self.restrictions_publisher.publish(msg)

    def timer_callback(self):
        if self.publish_periodic_update is not None:
            self.publish_periodic_update()
        
        if self.get_robots_list is not None:
            self.get_robots_list()

        rclpy.spin_once(self, timeout_sec=0.001)