diff --git a/config/udp_bridge.yaml b/config/udp_bridge.yaml index 1e8315b..884a61c 100644 --- a/config/udp_bridge.yaml +++ b/config/udp_bridge.yaml @@ -21,13 +21,8 @@ - /debug/dsd/localization/dsd_tree - /debug/dsd/localization/dsd_stack - /team_data - - /balls_relative - - /debug/viz_ball - - /robots_relative + - /ball_position_relative_filtered - /gamestate - - /animation - /diagnostics_agg - - /imu/data - - /buttons - /system_workload - /field/map diff --git a/udp_bridge/receiver.py b/udp_bridge/receiver.py index 2434ea2..004970f 100755 --- a/udp_bridge/receiver.py +++ b/udp_bridge/receiver.py @@ -5,6 +5,7 @@ import rclpy from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile from udp_bridge.message_handler import MessageHandler @@ -49,15 +50,16 @@ def handle_message(self, msg: bytes): data = deserialized_msg.get("data") topic: str = deserialized_msg.get("topic") hostname: str = deserialized_msg.get("hostname") + latched: bool = deserialized_msg.get("latched") if hostname not in self.known_senders: self.known_senders.append(hostname) - self.publish(topic, data, hostname) + self.publish(topic, data, hostname, latched) except Exception as e: self.node.get_logger().error(f"Could not deserialize received message with error {e}") - def publish(self, topic: str, msg, hostname: str): + def publish(self, topic: str, msg, hostname: str, latched: bool): """ Publish a message into ROS @@ -72,7 +74,11 @@ def publish(self, topic: str, msg, hostname: str): # create a publisher object if we don't have one already if namespaced_topic not in self.publishers.keys(): self.node.get_logger().info(f"Publishing new topic {namespaced_topic}") - self.publishers[namespaced_topic] = self.node.create_publisher(type(msg), namespaced_topic, 1) + self.publishers[namespaced_topic] = self.node.create_publisher( + type(msg), + namespaced_topic, + qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL) if latched else 10, + ) self.publishers[namespaced_topic].publish(msg) diff --git a/udp_bridge/sender.py b/udp_bridge/sender.py index 67ad2c0..03702ba 100755 --- a/udp_bridge/sender.py +++ b/udp_bridge/sender.py @@ -91,6 +91,7 @@ def __message_callback(self, data, latched=False): "data": data, "topic": self.topic, "hostname": HOSTNAME, + "latched": latched, } )