Publish Foreman status over ROS topic

foreman/foreman/adapters/__init__.py

@@ -4,12 +4,14 @@
 from .lifecycle_node_service_caller import LifecycleNodeServiceCaller
 from .ros_node_parameters import RosNodeParameters
 from .ros_set_goal_server import RosSetGoalServer
+from .ros_status_publisher import RosStatusPublisher
 
 __all__ = [
     "ComponentStateMonitor",
     "ControllerManagerServiceCaller",
     "LifecycleNodeServiceCaller",
     "RosSetGoalServer",
+    "RosStatusPublisher",
     "RosNodeParameters",
     "AutostartAdapter",
 ]

foreman/foreman/adapters/ros_status_publisher.py

@@ -0,0 +1,62 @@
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy
+from rclpy.qos import HistoryPolicy
+from rclpy.qos import QoSProfile
+from rclpy.qos import ReliabilityPolicy
+
+from foreman.types import ForemanSnapshot
+from foreman_msgs.msg import ComponentState
+from foreman_msgs.msg import ForemanStatus
+
+
+class RosStatusPublisher:
+    """Publish the current Foreman engine status as a ROS 2 topic."""
+
+    def __init__(self, node: Node):
+        self.logger_prefix = "Adapters.RosStatusPublisher:"
+
+        qos_profile = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1
+        )
+        self._publisher = node.create_publisher(
+            ForemanStatus,
+            '~/status',
+            qos_profile
+        )
+        self._last_published = None
+
+        node.get_logger().info(
+            f"{self.logger_prefix} Topic {self._publisher.topic_name} is ready.")
+
+    def publish_status(self, snapshot: ForemanSnapshot):
+        """Publish the given Foreman status snapshot if it changed."""
+        observed = {component.name: component for component in snapshot.components}
+
+        msg = ForemanStatus()
+        msg.goal = snapshot.goal
+        msg.ready = snapshot.ready
+        msg.at_goal = snapshot.at_goal
+        msg.error.is_error = snapshot.error.is_error
+        msg.error.category = snapshot.error.category
+        msg.error.message = snapshot.error.message
+
+        # The error names the blamed components.
+        # A component that is no longer observed is still
+        # named, with its state left empty.
+        for name in snapshot.error.components or []:
+            component_msg = ComponentState()
+            component_msg.name = name
+            component = observed.get(name)
+            if component:
+                component_msg.component_type = component.component_type.value
+                component_msg.lifecycle_state = component.lifecycle_state.name
+            msg.error.components.append(component_msg)
+
+        if msg == self._last_published:
+            return
+
+        self._last_published = msg
+        self._publisher.publish(msg)

foreman/foreman/node.py

@@ -60,6 +60,7 @@ def __init__(self):
             node=self,
             engine=self.foreman_engine
         )
+        self.ros_status_publisher = adapters.RosStatusPublisher(node=self)
 
         self.autostart_adapter = adapters.AutostartAdapter(
             node=self,
@@ -87,6 +88,8 @@ def callback_main_loop(self):
         if bool(self.foreman_config.autostart_goal_state) and not self.autostart_adapter.is_done:
             self.autostart_adapter.autostart()
 
+        self.ros_status_publisher.publish_status(self.foreman_engine.get_engine_snapshot())
+
         # do we have an active transition running?
         if self._service_call_active_future and self._service_call_active_future.done():
             try:

foreman/test/test_ros_status_publisher.py

@@ -0,0 +1,140 @@
+import unittest
+from unittest.mock import MagicMock
+
+import rclpy
+from rclpy.qos import DurabilityPolicy
+from rclpy.qos import HistoryPolicy
+from rclpy.qos import ReliabilityPolicy
+
+from foreman.adapters.ros_status_publisher import RosStatusPublisher
+from foreman.types import Component
+from foreman.types import ComponentType
+from foreman.types import ErrorSnapshot
+from foreman.types import ForemanErrorCategory
+from foreman.types import ForemanSnapshot
+from foreman.types import LifecycleState
+
+
+def _component(name="joint_trajectory_controller", state=LifecycleState.INACTIVE):
+    return Component(
+        name=name,
+        component_type=ComponentType.CONTROLLER,
+        lifecycle_state=state
+    )
+
+
+def _snapshot(components=None):
+    return ForemanSnapshot(
+        goal="running",
+        ready=True,
+        at_goal=False,
+        error=ErrorSnapshot(
+            is_error=True,
+            category=ForemanErrorCategory.EXECUTION.value,
+            message="boom",
+            components=["joint_trajectory_controller"],
+        ),
+        components=components if components is not None else [_component()]
+    )
+
+
+class TestRosStatusPublisher(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+
+    def setUp(self):
+        self.node = rclpy.create_node("test_status_publisher")
+        self.addCleanup(self.node.destroy_node)
+
+    def test_status_topic_is_advertised_in_node_namespace(self):
+        RosStatusPublisher(self.node)
+        expected = f"/{self.node.get_name()}/status"
+        advertised = dict(self.node.get_topic_names_and_types())
+        self.assertIn(expected, advertised)
+        self.assertEqual(advertised[expected], ["foreman_msgs/msg/ForemanStatus"])
+
+    def test_publish_status_maps_every_snapshot_field(self):
+        publisher = RosStatusPublisher(self.node)
+        publisher._publisher = MagicMock()
+
+        publisher.publish_status(_snapshot())
+
+        published = publisher._publisher.publish.call_args[0][0]
+        self.assertEqual(published.goal, "running")
+        self.assertTrue(published.ready)
+        self.assertFalse(published.at_goal)
+        self.assertTrue(published.error.is_error)
+        self.assertEqual(published.error.category, ForemanErrorCategory.EXECUTION.value)
+        self.assertEqual(published.error.message, "boom")
+        self.assertEqual(
+            [c.name for c in published.error.components], ["joint_trajectory_controller"])
+        self.assertEqual(published.error.components[0].component_type,
+                         ComponentType.CONTROLLER.value)
+        self.assertEqual(published.error.components[0].lifecycle_state, "INACTIVE")
+
+    def test_status_topic_is_transient_local(self):
+        publisher = RosStatusPublisher(self.node)
+
+        qos = publisher._publisher.qos_profile
+        self.assertEqual(qos.durability, DurabilityPolicy.TRANSIENT_LOCAL)
+        self.assertEqual(qos.reliability, ReliabilityPolicy.RELIABLE)
+        self.assertEqual(qos.history, HistoryPolicy.KEEP_LAST)
+        self.assertEqual(qos.depth, 1)
+
+    def test_unchanged_status_is_not_republished(self):
+        publisher = RosStatusPublisher(self.node)
+        publisher._publisher = MagicMock()
+
+        publisher.publish_status(_snapshot())
+        publisher.publish_status(_snapshot())
+        publisher.publish_status(_snapshot())
+
+        self.assertEqual(publisher._publisher.publish.call_count, 1)
+
+    def test_changed_status_is_republished(self):
+        publisher = RosStatusPublisher(self.node)
+        publisher._publisher = MagicMock()
+
+        publisher.publish_status(_snapshot())
+
+        changed = _snapshot()
+        changed.at_goal = True
+        publisher.publish_status(changed)
+
+        self.assertEqual(publisher._publisher.publish.call_count, 2)
+        self.assertTrue(publisher._publisher.publish.call_args[0][0].at_goal)
+
+    def test_publish_status_handles_missing_error_components(self):
+        publisher = RosStatusPublisher(self.node)
+        publisher._publisher = MagicMock()
+
+        snapshot = _snapshot()
+        snapshot.error.components = None
+
+        publisher.publish_status(snapshot)
+
+        published = publisher._publisher.publish.call_args[0][0]
+        self.assertEqual(list(published.error.components), [])
+
+    def test_vanished_component_is_named_without_a_state(self):
+        # A component that dropped out of /activity is no longer observed, so
+        # only its name is known.
+        publisher = RosStatusPublisher(self.node)
+        publisher._publisher = MagicMock()
+
+        publisher.publish_status(_snapshot(components=[_component("something_else")]))
+
+        published = publisher._publisher.publish.call_args[0][0]
+        self.assertEqual(len(published.error.components), 1)
+        self.assertEqual(published.error.components[0].name, "joint_trajectory_controller")
+        self.assertEqual(published.error.components[0].component_type, "")
+        self.assertEqual(published.error.components[0].lifecycle_state, "")
+
+
+if __name__ == "__main__":
+    unittest.main()

foreman_msgs/CMakeLists.txt

@@ -13,11 +13,18 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
+set(msg_files
+  "msg/ComponentState.msg"
+  "msg/ForemanErrorState.msg"
+  "msg/ForemanStatus.msg"
+)
+
 set(srv_files
   "srv/SetGoal.srv"
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  ${msg_files}
   ${srv_files}
 )
 

foreman_msgs/msg/ComponentState.msg

@@ -0,0 +1,8 @@
+# A component and the state it was observed in.
+
+string name
+# ComponentType value: hardware, controller or lifecycle_node.
+# Empty when the component is no longer observed, e.g. it vanished from
+# /activity, so the name is still reported without a known state.
+string component_type
+string lifecycle_state

foreman_msgs/msg/ForemanErrorState.msg

@@ -0,0 +1,9 @@
+# Domain error reported by the Foreman engine.
+# Mirrors foreman.types.ErrorSnapshot.
+
+bool is_error
+# ForemanErrorCategory value: TransportError, ExecutionError,
+# UnexpectedStateError, PlannerError, or None.
+string category
+string message
+ComponentState[] components

foreman_msgs/msg/ForemanStatus.msg

@@ -0,0 +1,6 @@
+# Current Foreman engine status.
+
+string goal
+bool ready
+bool at_goal
+ForemanErrorState error