fix: Auto-populate header.stamp when header omitted (backport #1220)

ROSBRIDGE_PROTOCOL.md

@@ -312,7 +312,8 @@ The operation fails if the `msg` does not conform to the type of the topic.
 Special cases for how the server handles the `msg` field:
 
 - If the `msg` does not contain all fields for the topic type, then the unspecified fields are filled in with defaults.
-- If the topic type has a `header` field of type `std_msgs/Header` and the client omits the `header.stamp` field, then the server will automatically populate it with the current ROS time.
+- If the topic type has a root `header` field of type `std_msgs/Header` and the client omits the `header.stamp` field, then the server will automatically populate it with the current ROS time.
+- If the topic type has any field of type `builtin_interfaces/Time` and the client sets that field to `"now"` string, then the server will automatically populate it with the current ROS time.
 
 **Server → Client**
 

rosbridge_library/src/rosbridge_library/internal/message_conversion.py

@@ -99,7 +99,6 @@
     "double",
     "string",
 )
-ros_header_types = ("Header", "std_msgs/Header", "roslib/Header")
 ros_binary_types = ("uint8[]", "char[]", "sequence<uint8>", "sequence<char>")
 # Remove the list type wrapper, and length specifier, from rostypes i.e. sequence<double, 3>
 list_tokens = re.compile(r"<(.+?)(, \d+)?>")
@@ -206,6 +205,19 @@ def populate_instance(
 
     inst_type = msg_instance_type_repr(inst)
 
+    # Auto-populate header.stamp when the root message has a "header" field of type std_msgs/Header
+    # and the client omitted the header entirely or omitted only the stamp.
+    inst_fields = inst.get_fields_and_field_types()
+    if inst_fields.get("header") == "std_msgs/Header":
+        header_msg = msg.get("header")
+        if header_msg is not None and not isinstance(header_msg, dict):
+            raise FieldTypeMismatchException(inst_type, ["header"], "dict", type(header_msg))
+        if header_msg is None or "stamp" not in header_msg:
+            assert hasattr(inst, "header")
+            header_inst = inst.header  # type: ignore[attr-defined]
+            if isinstance(header_inst, HeaderMsg):
+                header_inst.stamp = clock.now().to_msg()
+
     return _to_object_inst(msg, inst_type, inst_type, clock, inst, [])
 
 
@@ -497,13 +509,6 @@ def _to_object_inst(
     if not isinstance(msg, dict):
         raise FieldTypeMismatchException(roottype, stack, rostype, type(msg))
 
-    # Substitute the correct time if we're an std_msgs/Header
-    if rostype in ros_header_types:
-        if not isinstance(inst, HeaderMsg):
-            err_msg = f"inst is not a HeaderMsg, but a {type(inst)}"
-            raise TypeError(err_msg)
-        inst.stamp = clock.now().to_msg()
-
     inst_fields: dict[str, str] = inst.get_fields_and_field_types()
     for field_name, field_value in msg.items():
         # Add this field to the field stack

rosbridge_library/test/capabilities/test_publish.py

@@ -8,6 +8,7 @@
 from typing import Any
 
 import rclpy
+from geometry_msgs.msg import PoseStamped
 from rclpy.executors import SingleThreadedExecutor
 from rclpy.node import Node
 from rclpy.qos import DurabilityPolicy, QoSProfile
@@ -17,6 +18,7 @@
     MissingArgumentException,
 )
 from rosbridge_library.protocol import Protocol
+from sensor_msgs.msg import TimeReference
 from std_msgs.msg import String
 
 
@@ -74,6 +76,143 @@ def cb(msg: String) -> None:
         time.sleep(0.5)
         self.assertEqual(received["msg"].data, msg["data"])
 
+    def test_publish_header_stamp_auto_populated_when_stamp_omitted(self) -> None:
+        proto = Protocol("hello", self.node)
+        pub = Publish(proto)
+        topic = "/test_header_stamp_auto_populated"
+
+        received: dict[str, Any] = {"msg": None}
+
+        def cb(msg: PoseStamped) -> None:
+            received["msg"] = msg
+
+        subscriber_qos = QoSProfile(
+            depth=10,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+        )
+        self.node.create_subscription(PoseStamped, topic, cb, subscriber_qos)
+
+        # Publish with header present but stamp omitted
+        pub_msg = {
+            "op": "publish",
+            "topic": topic,
+            "type": "geometry_msgs/msg/PoseStamped",
+            "msg": {"header": {}},
+        }
+        pub.publish(pub_msg)
+        time.sleep(0.5)
+
+        self.assertIsNotNone(received["msg"])
+        stamp = received["msg"].header.stamp
+        self.assertGreater(
+            stamp.sec + stamp.nanosec,
+            0,
+            "Expected server to auto-populate header.stamp with current ROS time, but stamp is zero",
+        )
+
+    def test_publish_header_stamp_preserved_when_provided_by_client(self) -> None:
+        proto = Protocol("hello", self.node)
+        pub = Publish(proto)
+        topic = "/test_header_stamp_preserved"
+
+        received: dict[str, Any] = {"msg": None}
+
+        def cb(msg: PoseStamped) -> None:
+            received["msg"] = msg
+
+        subscriber_qos = QoSProfile(
+            depth=10,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+        )
+        self.node.create_subscription(PoseStamped, topic, cb, subscriber_qos)
+
+        expected_sec = 12345
+        expected_nanosec = 67890
+        pub_msg = {
+            "op": "publish",
+            "topic": topic,
+            "type": "geometry_msgs/msg/PoseStamped",
+            "msg": {
+                "header": {
+                    "stamp": {"sec": expected_sec, "nanosec": expected_nanosec},
+                }
+            },
+        }
+        pub.publish(pub_msg)
+        time.sleep(0.5)
+
+        self.assertIsNotNone(received["msg"])
+        stamp = received["msg"].header.stamp
+        self.assertEqual(stamp.sec, expected_sec)
+        self.assertEqual(stamp.nanosec, expected_nanosec)
+
+    def test_publish_header_stamp_auto_populated_when_header_omitted(self) -> None:
+        proto = Protocol("hello", self.node)
+        pub = Publish(proto)
+        topic = "/test_header_stamp_auto_populated_no_header"
+
+        received: dict[str, Any] = {"msg": None}
+
+        def cb(msg: PoseStamped) -> None:
+            received["msg"] = msg
+
+        subscriber_qos = QoSProfile(
+            depth=10,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+        )
+        self.node.create_subscription(PoseStamped, topic, cb, subscriber_qos)
+
+        # Publish with no header at all (the entire header field is omitted)
+        pub_msg = {
+            "op": "publish",
+            "topic": topic,
+            "type": "geometry_msgs/msg/PoseStamped",
+            "msg": {},
+        }
+        pub.publish(pub_msg)
+        time.sleep(0.5)
+
+        self.assertIsNotNone(received["msg"])
+        stamp = received["msg"].header.stamp
+        self.assertGreater(
+            stamp.sec + stamp.nanosec,
+            0,
+            "Expected server to auto-populate header.stamp even when header is entirely omitted",
+        )
+
+    def test_publish_time_field_now_string_populated(self) -> None:
+        proto = Protocol("hello", self.node)
+        pub = Publish(proto)
+        topic = "/test_time_field_now"
+
+        received: dict[str, Any] = {"msg": None}
+
+        def cb(msg: TimeReference) -> None:
+            received["msg"] = msg
+
+        subscriber_qos = QoSProfile(
+            depth=10,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+        )
+        self.node.create_subscription(TimeReference, topic, cb, subscriber_qos)
+
+        pub_msg = {
+            "op": "publish",
+            "topic": topic,
+            "type": "sensor_msgs/msg/TimeReference",
+            "msg": {"time_ref": "now"},
+        }
+        pub.publish(pub_msg)
+        time.sleep(0.5)
+
+        self.assertIsNotNone(received["msg"])
+        time_ref = received["msg"].time_ref
+        self.assertGreater(
+            time_ref.sec + time_ref.nanosec,
+            0,
+            'Expected server to populate time_ref with current ROS time when set to "now"',
+        )
+
 
 if __name__ == "__main__":
     unittest.main()