From a2206e66d74d3775d412ffc7b89979d7c0647a70 Mon Sep 17 00:00:00 2001 From: Hitoshi-Hub Date: Sat, 26 Sep 2026 22:08:29 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9D=D0=B5=D0=B1=D0=BE=D0=BB=D1=8C=D1=88?= =?UTF-8?q?=D0=B8=D0=B5=20=D0=BF=D1=80=D0=B0=D0=B2=D0=BA=D0=B8=20ros2=20?= =?UTF-8?q?=D1=81=D0=B2=D1=8F=D0=B7=D0=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- flyguard/flyguard_ros2_node.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) 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(