Fix/latched topics (#36)

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

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)
 

udp_bridge/sender.py

@@ -91,6 +91,7 @@ def __message_callback(self, data, latched=False):
  "data": data,
  "topic": self.topic,
  "hostname": HOSTNAME,
+ "latched": latched,
  }
  )