diff --git a/flyguard/flyguard_ros2_node.py b/flyguard/flyguard_ros2_node.py index 9b3b6f5..d3b9127 100644 --- a/flyguard/flyguard_ros2_node.py +++ b/flyguard/flyguard_ros2_node.py @@ -20,7 +20,8 @@ if str(ROOT_DIR) not in sys.path: import rclpy from rclpy.executors import ExternalShutdownException from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data +from rclpy.qos import (QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, + QoSReliabilityPolicy) from geometry_msgs.msg import Vector3 from sensor_msgs.msg import PointCloud2 as RosPointCloud2 @@ -36,6 +37,17 @@ from flyguard.mushroom_body import MushroomBody from flyguard.pipeline import FlyGuard, Params +# Matches the publisher profile recorded in the supplied rosbag: RELIABLE, +# VOLATILE, KEEP_LAST(10). PointCloud2 dimensions and payload size are read +# from every message, not configured in QoS. +LIDAR_QOS = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE, +) + + class FlyGuardNode(Node): """Consume lidar clouds and publish final FlyGuard decisions.""" @@ -59,11 +71,10 @@ class FlyGuardNode(Node): self.fg = FlyGuard(Params(fov_deg=float(fov_deg)), memory=memory, readout=readout) - # Pandar and most lidar drivers offer BEST_EFFORT sensor QoS. A - # default RELIABLE subscriber is incompatible and would receive no data. + # Match the supplied lidar publisher exactly; it is RELIABLE. self.sub_cloud = self.create_subscription( RosPointCloud2, self.lidar_topic, self.pointcloud_callback, - qos_profile_sensor_data, + LIDAR_QOS, ) self.pub_threat = self.create_publisher(String, "flyguard/threat_level", 10) self.pub_boxes = self.create_publisher(