62 lines
2.3 KiB
Python
62 lines
2.3 KiB
Python
|
import rclpy
|
||
|
|
from rclpy.node import Node
|
||
|
|
|
||
|
|
from std_msgs.msg import Bool
|
||
|
|
from ros2aria_msgs.msg import RestrictionsMsg
|
||
|
|
from rclpy.qos import QoSDurabilityPolicy
|
||
|
|
from rclpy.qos import QoSHistoryPolicy
|
||
|
|
from rclpy.qos import QoSLivelinessPolicy
|
||
|
|
from rclpy.qos import QoSProfile
|
||
|
|
from rclpy.qos import QoSReliabilityPolicy
|
||
|
|
import sys
|
||
|
|
|
||
|
|
class RestrictionRepublisher(Node):
|
||
|
|
|
||
|
|
def __init__(self):
|
||
|
|
|
||
|
|
profile = QoSProfile(
|
||
|
|
depth=10,
|
||
|
|
history=QoSHistoryPolicy.KEEP_ALL,
|
||
|
|
reliability=QoSReliabilityPolicy.RELIABLE,
|
||
|
|
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
|
||
|
|
super().__init__('restriction_republisher')
|
||
|
|
self.restriction_pub_ = self.create_publisher(RestrictionsMsg, '/pioneers/restrictions', qos_profile=profile)
|
||
|
|
self.master_stop_pub_ = self.create_publisher(Bool, '/pioneers/master_stop', qos_profile=profile)
|
||
|
|
self.restriction_sub_ = self.create_subscription(RestrictionsMsg, '/pioneers/restrictions', self.restriction_callback, qos_profile=profile)
|
||
|
|
self.master_stop_sub_ = self.create_subscription(Bool, '/pioneers/master_stop', self.master_stop_callback, qos_profile=profile)
|
||
|
|
timer_period = 1.0
|
||
|
|
self.timer = self.create_timer(timer_period, self.timer_callback)
|
||
|
|
|
||
|
|
self.master_stop_msg = Bool()
|
||
|
|
self.restriction_msg = RestrictionsMsg()
|
||
|
|
self.master_stop_msg. data = True
|
||
|
|
self.restriction_msg.distance.data = sys.float_info.max
|
||
|
|
self.restriction_msg.linear_velocity.data = 0.0
|
||
|
|
self.restriction_msg.angular_velocity.data = 0.0
|
||
|
|
|
||
|
|
def timer_callback(self):
|
||
|
|
self.restriction_pub_.publish(self.restriction_msg)
|
||
|
|
self.master_stop_pub_.publish(self.master_stop_msg)
|
||
|
|
|
||
|
|
def restriction_callback(self, msg):
|
||
|
|
if msg != self.restriction_msg:
|
||
|
|
self.get_logger().info('New restricitons.')
|
||
|
|
self.restriction_msg = msg
|
||
|
|
|
||
|
|
def master_stop_callback(self, msg):
|
||
|
|
if msg != self.master_stop_msg:
|
||
|
|
self.get_logger().info('New master stop.')
|
||
|
|
self.master_stop_msg = msg
|
||
|
|
|
||
|
|
def main(args=None):
|
||
|
|
rclpy.init(args=args)
|
||
|
|
|
||
|
|
restriction_republisher = RestrictionRepublisher()
|
||
|
|
|
||
|
|
rclpy.spin(restriction_republisher)
|
||
|
|
restriction_republisher.destroy_node()
|
||
|
|
rclpy.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
main()
|