diff --git a/ros-control-py/udp_teleop_bridge/package.xml b/ros-control-py/udp_teleop_bridge/package.xml index 28262bd..fc70b79 100644 --- a/ros-control-py/udp_teleop_bridge/package.xml +++ b/ros-control-py/udp_teleop_bridge/package.xml @@ -15,6 +15,7 @@ launch launch_ros rclpy + rosidl_runtime_py teleop_twist_joy teleop_twist_keyboard diff --git a/ros-control-py/udp_teleop_bridge/setup.py b/ros-control-py/udp_teleop_bridge/setup.py index 4957d6d..9d10851 100644 --- a/ros-control-py/udp_teleop_bridge/setup.py +++ b/ros-control-py/udp_teleop_bridge/setup.py @@ -29,6 +29,7 @@ setup( 'console_scripts': [ 'cmd_vel_udp_sender = udp_teleop_bridge.cmd_vel_udp_sender:main', 'udp_cmd_vel_receiver = udp_teleop_bridge.udp_cmd_vel_receiver:main', + 'topic_status_reader = udp_teleop_bridge.topic_status_reader:main', ], }, ) diff --git a/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/topic_status_reader.py b/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/topic_status_reader.py new file mode 100644 index 0000000..bedfee6 --- /dev/null +++ b/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/topic_status_reader.py @@ -0,0 +1,122 @@ +"""Subscribe to a ROS 2 topic with runtime type discovery and print messages.""" + +from __future__ import annotations + +import json +import time +from typing import Any + +import rclpy +from rclpy.node import Node +from rosidl_runtime_py.convert import message_to_ordereddict +from rosidl_runtime_py.utilities import get_message + + +WAIT_LOG_INTERVAL_SEC = 5.0 + + +class TopicStatusReader(Node): + """Wait for a topic to appear, subscribe to it, and print each message.""" + + def __init__(self) -> None: + super().__init__('topic_status_reader') + + self.declare_parameter('topic', '/hrc/robot/cmd_vel_status') + self.declare_parameter('qos_depth', 10) + self.declare_parameter('poll_interval_sec', 0.5) + + self._topic = str(self.get_parameter('topic').value).strip() + self._qos_depth = int(self.get_parameter('qos_depth').value) + self._poll_interval_sec = float(self.get_parameter('poll_interval_sec').value) + + if not self._topic: + raise ValueError('topic must not be empty') + if self._qos_depth <= 0: + raise ValueError('qos_depth must be > 0') + if self._poll_interval_sec <= 0.0: + raise ValueError('poll_interval_sec must be > 0') + + self._topic_type: str | None = None + self._subscription = None + self._message_count = 0 + self._last_wait_log_monotonic = 0.0 + + self._poll_timer = self.create_timer(self._poll_interval_sec, self._ensure_subscription) + self._ensure_subscription() + + def _discover_topic_types(self) -> list[str]: + for topic_name, topic_types in self.get_topic_names_and_types(): + if topic_name == self._topic: + return list(topic_types) + return [] + + def _log_waiting(self) -> None: + now = time.monotonic() + if (now - self._last_wait_log_monotonic) < WAIT_LOG_INTERVAL_SEC: + return + self._last_wait_log_monotonic = now + self.get_logger().info(f'Waiting for topic {self._topic} to appear...') + + def _ensure_subscription(self) -> None: + if self._subscription is not None: + return + + topic_types = self._discover_topic_types() + if not topic_types: + self._log_waiting() + return + + if len(topic_types) > 1: + joined = ', '.join(topic_types) + self.get_logger().warning( + f'Topic {self._topic} reports multiple types ({joined}); using {topic_types[0]}' + ) + + self._topic_type = topic_types[0] + try: + message_type = get_message(self._topic_type) + except Exception as exc: + self.get_logger().error( + f'Failed to import message type {self._topic_type} for {self._topic}: {exc}' + ) + return + + self._subscription = self.create_subscription( + message_type, + self._topic, + self._handle_message, + self._qos_depth, + ) + self._poll_timer.cancel() + self.get_logger().info( + f'Subscribed to {self._topic} with type {self._topic_type} (qos_depth={self._qos_depth})' + ) + + def _format_message(self, msg: Any) -> str: + try: + payload = message_to_ordereddict(msg) + except Exception: + return str(msg) + return json.dumps(payload, ensure_ascii=False, indent=2) + + def _handle_message(self, msg: Any) -> None: + self._message_count += 1 + received_at = time.strftime('%Y-%m-%d %H:%M:%S') + topic_type = self._topic_type or type(msg).__name__ + rendered = self._format_message(msg) + print( + f'[{received_at}] #{self._message_count} {self._topic} ({topic_type})\n{rendered}\n', + flush=True, + ) + + +def main(args: list[str] | None = None) -> None: + rclpy.init(args=args) + node = TopicStatusReader() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown()