Небольшие правки ros2 связи

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
+        # Match the supplied lidar publisher exactly; it is RELIABLE.
-        # default RELIABLE subscriber is incompatible and would receive no data.
         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(