diff --git a/foreman/foreman/adapters/__init__.py b/foreman/foreman/adapters/__init__.py index bbbb1eb..cb588cc 100644 --- a/foreman/foreman/adapters/__init__.py +++ b/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", ] diff --git a/foreman/foreman/adapters/ros_status_publisher.py b/foreman/foreman/adapters/ros_status_publisher.py new file mode 100644 index 0000000..4bfa361 --- /dev/null +++ b/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) diff --git a/foreman/foreman/node.py b/foreman/foreman/node.py index 5b8e28a..0822247 100644 --- a/foreman/foreman/node.py +++ b/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: diff --git a/foreman/test/test_ros_status_publisher.py b/foreman/test/test_ros_status_publisher.py new file mode 100644 index 0000000..4658eea --- /dev/null +++ b/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() diff --git a/foreman_msgs/CMakeLists.txt b/foreman_msgs/CMakeLists.txt index eaf35a1..de921f7 100644 --- a/foreman_msgs/CMakeLists.txt +++ b/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} ) diff --git a/foreman_msgs/msg/ComponentState.msg b/foreman_msgs/msg/ComponentState.msg new file mode 100644 index 0000000..1345699 --- /dev/null +++ b/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 diff --git a/foreman_msgs/msg/ForemanErrorState.msg b/foreman_msgs/msg/ForemanErrorState.msg new file mode 100644 index 0000000..beb1bb3 --- /dev/null +++ b/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 diff --git a/foreman_msgs/msg/ForemanStatus.msg b/foreman_msgs/msg/ForemanStatus.msg new file mode 100644 index 0000000..738452c --- /dev/null +++ b/foreman_msgs/msg/ForemanStatus.msg @@ -0,0 +1,6 @@ +# Current Foreman engine status. + +string goal +bool ready +bool at_goal +ForemanErrorState error