Refactor component predicates

.github/common/build-test.sh

@@ -26,8 +26,8 @@ build_and_test() {
 source /opt/ros/"${ROS_DISTRO}"/setup.bash
 cd /home/ros2/ros2_ws
 
-build_and_test modulo_utils
 build_and_test modulo_component_interfaces
+build_and_test modulo_utils
 build_and_test modulo_core
 build_and_test modulo_components
 

CHANGELOG.md

@@ -7,6 +7,10 @@ Release Versions:
 - [2.1.1](#211)
 - [2.1.0](#210)
 
+## Upcoming changes (in development)
+
+- Refactor component predicates with a single Predicate publisher (#26)
+
 ## 2.3.0
 
 ### July 25, 2023

VERSION

@@ -1 +1 @@
-2.3.0
+2.3.3

doxygen/doxygen.conf

@@ -38,7 +38,7 @@ PROJECT_NAME           = "Modulo"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER = 2.3.0
+PROJECT_NUMBER = 2.3.3
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

source/modulo_component_interfaces/CMakeLists.txt

@@ -3,7 +3,8 @@ project(modulo_component_interfaces)
 
 find_package(rosidl_default_generators REQUIRED)
 
+file(GLOB MSGS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} msg/*.msg)
 file(GLOB SRVS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} srv/*.srv)
-rosidl_generate_interfaces(${PROJECT_NAME} ${SRVS})
+rosidl_generate_interfaces(${PROJECT_NAME} ${MSGS} ${SRVS})
 
 ament_package()

source/modulo_component_interfaces/msg/Predicate.msg

@@ -0,0 +1,3 @@
+string component
+string predicate
+bool value

source/modulo_component_interfaces/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>modulo_component_interfaces</name>
-  <version>2.3.0</version>
+  <version>2.3.3</version>
   <description>Interface package for communicating with modulo components through the ROS framework</description>
   <maintainer email="enrico@aica.tech">Enrico Eberhard</maintainer>
   <license>GPLv3</license>

source/modulo_components/include/modulo_components/ComponentInterface.hpp

@@ -27,6 +27,7 @@
 
 #include <modulo_component_interfaces/srv/empty_trigger.hpp>
 #include <modulo_component_interfaces/srv/string_trigger.hpp>
+#include <modulo_component_interfaces/msg/predicate.hpp>
 
 #include "modulo_components/exceptions/AddServiceException.hpp"
 #include "modulo_components/exceptions/AddSignalException.hpp"
@@ -559,8 +560,8 @@ class ComponentInterface : public NodeT {
       publisher_type_; ///< Type of the output publishers (one of PUBLISHER, LIFECYCLE_PUBLISHER)
 
   std::map<std::string, utilities::PredicateVariant> predicates_; ///< Map of predicates
-  std::map<std::string, std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Bool>>>
-      predicate_publishers_; ///< Map of predicate publishers
+  std::shared_ptr<rclcpp::Publisher<modulo_component_interfaces::msg::Predicate>>
+  predicate_publisher_; ///< Predicate publisher
   std::map<std::string, bool> triggers_; ///< Map of triggers
 
   std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_component_interfaces::srv::EmptyTrigger>>>
@@ -596,6 +597,8 @@ ComponentInterface<NodeT>::ComponentInterface(
       });
   this->add_parameter("period", 0.1, "The time interval in seconds for all periodic callbacks", true);
 
+  this->predicate_publisher_ =
+      rclcpp::create_publisher<modulo_component_interfaces::msg::Predicate>(*this, "/predicates", this->qos_);
   this->add_predicate("in_error_state", false);
 
   this->step_timer_ = this->create_wall_timer(
@@ -773,9 +776,6 @@ inline void ComponentInterface<NodeT>::add_variant_predicate(
     RCLCPP_WARN_STREAM(this->get_logger(), "Predicate with name '" << name << "' already exists, overwriting.");
   } else {
     RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding predicate '" << name << "'.");
-    auto publisher = rclcpp::create_publisher<std_msgs::msg::Bool>(
-        *this, utilities::generate_predicate_topic(this->get_name(), name), this->qos_);
-    this->predicate_publishers_.insert_or_assign(name, publisher);
   }
   this->predicates_.insert_or_assign(name, predicate);
 }
@@ -1258,14 +1258,11 @@ inline state_representation::CartesianPose ComponentInterface<NodeT>::lookup_tra
 
 template<class NodeT>
 inline void ComponentInterface<NodeT>::publish_predicate(const std::string& name) {
-  std_msgs::msg::Bool message;
-  message.data = this->get_predicate(name);
-  if (this->predicate_publishers_.find(name) == this->predicate_publishers_.end()) {
-    RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
-                                 "No publisher for predicate '" << name << "' found.");
-    return;
-  }
-  predicate_publishers_.at(name)->publish(message);
+  modulo_component_interfaces::msg::Predicate message;
+  message.component = this->get_node_base_interface()->get_fully_qualified_name();
+  message.predicate = name;
+  message.value = this->get_predicate(name);
+  this->predicate_publisher_->publish(message);
 }
 
 template<class NodeT>

source/modulo_components/include/modulo_components/utilities/utilities.hpp

@@ -61,15 +61,4 @@ parse_node_name(const rclcpp::NodeOptions& options, const std::string& fallback
   }
   return output;
 }
-
-/**
- * @brief Generate the topic name for a predicate from component name and predicate name.
- * @param component_name The name of the component which the predicate belongs to
- * @param predicate_name The name of the predicate
- * @return The generated predicate topic as /predicates/component_name/predicate_name
- */
-[[maybe_unused]] static std::string
-generate_predicate_topic(const std::string& component_name, const std::string& predicate_name) {
-  return "/predicates/" + component_name + "/" + predicate_name;
-}
 }// namespace modulo_components::utilities

source/modulo_components/modulo_components/component_interface.py

@@ -8,18 +8,18 @@
 import modulo_core.translators.message_writers as modulo_writers
 import state_representation as sr
 from geometry_msgs.msg import TransformStamped
+from modulo_component_interfaces.msg import Predicate
 from modulo_component_interfaces.srv import EmptyTrigger, StringTrigger
 from modulo_components.exceptions import AddServiceError, AddSignalError, ComponentError, ComponentParameterError, \
     LookupTransformError
-from modulo_components.utilities import generate_predicate_topic, parse_topic_name
+from modulo_components.utilities import parse_topic_name
 from modulo_core import EncodedState
 from modulo_core.exceptions import MessageTranslationError, ParameterTranslationError
 from modulo_core.translators.parameter_translators import get_ros_parameter_type, read_parameter_const, write_parameter
 from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult
 from rclpy.duration import Duration
 from rclpy.node import Node
 from rclpy.parameter import Parameter
-from rclpy.publisher import Publisher
 from rclpy.qos import QoSProfile
 from rclpy.service import Service
 from rclpy.time import Time
@@ -45,7 +45,6 @@ def __init__(self, node_name: str, *args, **kwargs):
         super().__init__(node_name, *args, **node_kwargs)
         self._parameter_dict: Dict[str, Union[str, sr.Parameter]] = {}
         self._predicates: Dict[str, Union[bool, Callable[[], bool]]] = {}
-        self._predicate_publishers: Dict[str, Publisher] = {}
         self._triggers: Dict[str, bool] = {}
         self._periodic_callbacks: Dict[str, Callable[[], None]] = {}
         self._inputs = {}
@@ -63,6 +62,7 @@ def __init__(self, node_name: str, *args, **kwargs):
         self.add_parameter(sr.Parameter("period", 0.1, sr.ParameterType.DOUBLE),
                            "Period (in s) between step function calls.")
 
+        self._predicate_publisher = self.create_publisher(Predicate, "/predicates", self._qos)
         self.add_predicate("in_error_state", False)
 
         self.create_timer(self.get_parameter_value("period"), self._step)
@@ -251,9 +251,6 @@ def add_predicate(self, name: str, value: Union[bool, Callable[[], bool]]):
             self.get_logger().warn(f"Predicate with name '{name}' already exists, overwriting.")
         else:
             self.get_logger().debug(f"Adding predicate '{name}'.")
-            self._predicate_publishers[name] = self.create_publisher(Bool,
-                                                                     generate_predicate_topic(self.get_name(), name),
-                                                                     10)
         self._predicates[name] = value
 
     def get_predicate(self, name: str) -> bool:
@@ -707,18 +704,17 @@ def _publish_predicate(self, name):
 
         :param name: The name of the predicate to publish
         """
-        message = Bool()
+        message = Predicate()
         value = self.get_predicate(name)
         try:
-            message.data = value
+            message.value = value
         except AssertionError:
             self.get_logger().error(f"Predicate '{name}' has invalid type: expected 'bool', got '{type(value)}'.",
                                     throttle_duration_sec=1.0)
             return
-        if name not in self._predicate_publishers.keys():
-            self.get_logger().error(f"No publisher for predicate '{name}' found.", throttle_duration_sec=1.0)
-            return
-        self._predicate_publishers[name].publish(message)
+        message.component = self.get_fully_qualified_name()
+        message.predicate = name
+        self._predicate_publisher.publish(message)
 
     def _publish_predicates(self):
         """

source/modulo_components/modulo_components/utilities/__init__.py

@@ -1 +1 @@
-from modulo_components.utilities.utilities import generate_predicate_topic, parse_topic_name
+from modulo_components.utilities.utilities import parse_topic_name

source/modulo_components/modulo_components/utilities/utilities.py

@@ -1,17 +1,6 @@
 import re
 
 
-def generate_predicate_topic(node_name: str, predicate_name: str) -> str:
-    """
-    Generate a topic name from the name of node and the predicate.
-
-    :param node_name: The name of the node
-    :param predicate_name: The name of the associated predicate
-    :return: The name of the topic
-    """
-    return f'/predicates/{node_name}/{predicate_name}'
-
-
 def parse_topic_name(topic_name: str) -> str:
     """
     Parse a string topic name from a user-provided input.

source/modulo_components/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>modulo_components</name>
-  <version>2.3.0</version>
+  <version>2.3.3</version>
   <description>Modulo base classes that wrap ROS2 Nodes as modular components for the AICA application framework</description>
   <maintainer email="baptiste@aica.tech">Baptiste Busch</maintainer>
   <maintainer email="enrico@aica.tech">Enrico Eberhard</maintainer>

source/modulo_components/test/cpp/test_component_communication.cpp

@@ -64,8 +64,8 @@ TEST_F(ComponentCommunicationTest, InputOutputManual) {
 
 TEST_F(ComponentCommunicationTest, Trigger) {
   auto trigger = std::make_shared<Trigger>(rclcpp::NodeOptions());
-  auto listener = std::make_shared<modulo_utils::testutils::PredicatesListener>(
-      rclcpp::NodeOptions(), "trigger", std::vector<std::string>{"test"});
+  auto listener =
+      std::make_shared<modulo_utils::testutils::PredicatesListener>("/trigger", std::vector<std::string>{"test"});
   this->exec_->add_node(listener);
   this->exec_->add_node(trigger);
   auto result_code = this->exec_->spin_until_future_complete(listener->get_predicate_future(), 500ms);

source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp

@@ -68,8 +68,8 @@ TEST_F(LifecycleComponentCommunicationTest, InputOutputManual) {
 
 TEST_F(LifecycleComponentCommunicationTest, Trigger) {
   auto trigger = std::make_shared<LifecycleTrigger>(rclcpp::NodeOptions());
-  auto listener = std::make_shared<modulo_utils::testutils::PredicatesListener>(
-      rclcpp::NodeOptions(), "trigger", std::vector<std::string>{"test"});
+  auto listener =
+      std::make_shared<modulo_utils::testutils::PredicatesListener>("/trigger", std::vector<std::string>{"test"});
   this->exec_->add_node(trigger->get_node_base_interface());
   trigger->configure();
   this->exec_->add_node(listener);

source/modulo_components/test/python/test_component_communication.py

@@ -50,7 +50,7 @@ def test_input_output_invalid(ros_exec, make_minimal_invalid_encoded_state_publi
 
 def test_trigger(ros_exec, make_predicates_listener):
     trigger = Trigger()
-    listener = make_predicates_listener("trigger", ["test"])
+    listener = make_predicates_listener("/trigger", ["test"])
     ros_exec.add_node(listener)
     ros_exec.add_node(trigger)
     ros_exec.spin_until_future_complete(listener.predicates_future, timeout_sec=0.5)

source/modulo_components/test/python/test_lifecycle_component_communication.py

@@ -77,7 +77,7 @@ def test_input_output_invalid(ros_exec, make_lifecycle_change_client, make_minim
 
 def test_trigger(ros_exec, make_lifecycle_change_client, make_predicates_listener):
     trigger = Trigger()
-    listener = make_predicates_listener("trigger", ["test"])
+    listener = make_predicates_listener("/trigger", ["test"])
     client = make_lifecycle_change_client("trigger")
     ros_exec.add_node(trigger)
     ros_exec.add_node(listener)

source/modulo_core/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>modulo_core</name>
-  <version>2.3.0</version>
+  <version>2.3.3</version>
   <description>Modulo Core communication and translation utilities for interoperability with AICA Control Libraries</description>
   <maintainer email="baptiste@aica.tech">Baptiste Busch</maintainer>
   <maintainer email="enrico@aica.tech">Enrico Eberhard</maintainer>

source/modulo_utils/include/modulo_utils/testutils/PredicatesListener.hpp

@@ -4,7 +4,7 @@
 #include <map>
 
 #include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/bool.hpp>
+#include <modulo_component_interfaces/msg/predicate.hpp>
 
 namespace modulo_utils::testutils {
 
@@ -13,8 +13,8 @@ using namespace std::chrono_literals;
 class PredicatesListener : public rclcpp::Node {
 public:
   PredicatesListener(
-      const rclcpp::NodeOptions& node_options, const std::string& ns, const std::vector<std::string>& predicates
-  );
+      const std::string& component, const std::vector<std::string>& predicates,
+      const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions());
 
   void reset_future();
 
@@ -23,7 +23,7 @@ class PredicatesListener : public rclcpp::Node {
   [[nodiscard]] const std::map<std::string, bool>& get_predicate_values() const;
 
 private:
-  std::map<std::string, std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Bool>>> subscriptions_;
+  std::shared_ptr<rclcpp::Subscription<modulo_component_interfaces::msg::Predicate>> subscription_;
   std::map<std::string, bool> predicates_;
   std::shared_future<void> received_future_;
   std::promise<void> received_;

source/modulo_utils/modulo_utils/testutils/predicates_listener.py

@@ -1,30 +1,28 @@
-from functools import partial
 from typing import List
 
 import pytest
 from rclpy import Future
 from rclpy.node import Node
-from std_msgs.msg import Bool
+
+from modulo_component_interfaces.msg import Predicate
 
 
 class PredicatesListener(Node):
-    def __init__(self, namespace: str, predicates: List[str]):
+    def __init__(self, component: str, predicates: List[str]):
         """
         Listener node subscribing to predicate topics and setting its Future to done once at least one of the predicates
         is True
 
-        :param namespace: Namespace of the publishing component (usually '<component_name>')
+        :param component: Name of the publishing component
         :param predicates: A list of predicates to subscribe to
         """
         super().__init__('predicates_listener')
         self.__future = Future()
+        self.__component = component
         self.__predicates = {}
-        self.__subscriptions = {}
         for predicate in predicates:
             self.__predicates[predicate] = None
-            self.__subscriptions[predicate] = self.create_subscription(Bool, f'/predicates/{namespace}/{predicate}',
-                                                                       partial(self.__callback,
-                                                                               predicate_name=predicate), 10)
+        self.__subscription = self.create_subscription(Predicate, "/predicates", self.__callback, 10)
 
     @property
     def predicates_future(self) -> Future:
@@ -40,22 +38,25 @@ def predicate_values(self) -> dict:
         """
         return self.__predicates
 
-    def __callback(self, msg, predicate_name):
-        self.__predicates[predicate_name] = msg.data
-        if msg.data:
-            self.__future.set_result(True)
+    def __callback(self, message):
+        if message.component == self.__component:
+            for predicate in self.__predicates.keys():
+                if message.predicate == predicate:
+                    self.__predicates[predicate] = message.value
+                    if message.value:
+                        self.__future.set_result(True)
 
 
 @pytest.fixture
 def make_predicates_listener():
     """
     Create a listener node subscribing to predicate topics and setting its Future to done once at least one of the
     predicates is True. Provide\n
-    namespace (str): Namespace of the publishing component (usually '<component_name>')\n
+    component (str): Name of the publishing component\n
     predicates (List[str]): A list of predicates to subscribe to
     """
 
-    def _make_predicates_listener(namespace: str, predicates: List[str]):
-        return PredicatesListener(namespace, predicates)
+    def _make_predicates_listener(component: str, predicates: List[str]):
+        return PredicatesListener(component, predicates)
 
     return _make_predicates_listener