diff --git a/.github/common/build-test.sh b/.github/common/build-test.sh index 8744389c..13d7d786 100755 --- a/.github/common/build-test.sh +++ b/.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 diff --git a/CHANGELOG.md b/CHANGELOG.md index 91f417ab..4fe5344f 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/VERSION b/VERSION index 276cbf9e..0bee604d 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.3.0 +2.3.3 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 2f90b598..eaac19cb 100644 --- a/doxygen/doxygen.conf +++ b/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 diff --git a/source/modulo_component_interfaces/CMakeLists.txt b/source/modulo_component_interfaces/CMakeLists.txt index 76cc0ad9..0263d97c 100644 --- a/source/modulo_component_interfaces/CMakeLists.txt +++ b/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() \ No newline at end of file diff --git a/source/modulo_component_interfaces/msg/Predicate.msg b/source/modulo_component_interfaces/msg/Predicate.msg new file mode 100644 index 00000000..1befa3c2 --- /dev/null +++ b/source/modulo_component_interfaces/msg/Predicate.msg @@ -0,0 +1,3 @@ +string component +string predicate +bool value \ No newline at end of file diff --git a/source/modulo_component_interfaces/package.xml b/source/modulo_component_interfaces/package.xml index cff76cef..a8926d3e 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.3.0 + 2.3.3 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard GPLv3 diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index bf747b13..151aef7f 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -27,6 +27,7 @@ #include #include +#include #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 predicates_; ///< Map of predicates - std::map>> - predicate_publishers_; ///< Map of predicate publishers + std::shared_ptr> + predicate_publisher_; ///< Predicate publisher std::map triggers_; ///< Map of triggers std::map>> @@ -596,6 +597,8 @@ ComponentInterface::ComponentInterface( }); this->add_parameter("period", 0.1, "The time interval in seconds for all periodic callbacks", true); + this->predicate_publisher_ = + rclcpp::create_publisher(*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::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( - *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::lookup_tra template inline void ComponentInterface::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 diff --git a/source/modulo_components/include/modulo_components/utilities/utilities.hpp b/source/modulo_components/include/modulo_components/utilities/utilities.hpp index 91ded6ac..a28754dc 100644 --- a/source/modulo_components/include/modulo_components/utilities/utilities.hpp +++ b/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 diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 21db409f..02bc0253 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -8,10 +8,11 @@ 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 @@ -19,7 +20,6 @@ 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): """ diff --git a/source/modulo_components/modulo_components/utilities/__init__.py b/source/modulo_components/modulo_components/utilities/__init__.py index d661061c..4ad06aca 100644 --- a/source/modulo_components/modulo_components/utilities/__init__.py +++ b/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 diff --git a/source/modulo_components/modulo_components/utilities/utilities.py b/source/modulo_components/modulo_components/utilities/utilities.py index 2e90ee54..bcd28a5d 100644 --- a/source/modulo_components/modulo_components/utilities/utilities.py +++ b/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. diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 21cff5b2..1611f633 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.3.0 + 2.3.3 Modulo base classes that wrap ROS2 Nodes as modular components for the AICA application framework Baptiste Busch Enrico Eberhard diff --git a/source/modulo_components/test/cpp/test_component_communication.cpp b/source/modulo_components/test/cpp/test_component_communication.cpp index b905c71e..05628a00 100644 --- a/source/modulo_components/test/cpp/test_component_communication.cpp +++ b/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(rclcpp::NodeOptions()); - auto listener = std::make_shared( - rclcpp::NodeOptions(), "trigger", std::vector{"test"}); + auto listener = + std::make_shared("/trigger", std::vector{"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); diff --git a/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp b/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp index bd85546d..94786c3c 100644 --- a/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp +++ b/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(rclcpp::NodeOptions()); - auto listener = std::make_shared( - rclcpp::NodeOptions(), "trigger", std::vector{"test"}); + auto listener = + std::make_shared("/trigger", std::vector{"test"}); this->exec_->add_node(trigger->get_node_base_interface()); trigger->configure(); this->exec_->add_node(listener); diff --git a/source/modulo_components/test/python/test_component_communication.py b/source/modulo_components/test/python/test_component_communication.py index 409d1c5e..61b9e9c6 100644 --- a/source/modulo_components/test/python/test_component_communication.py +++ b/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) diff --git a/source/modulo_components/test/python/test_lifecycle_component_communication.py b/source/modulo_components/test/python/test_lifecycle_component_communication.py index 025e0d7b..28e2ba84 100644 --- a/source/modulo_components/test/python/test_lifecycle_component_communication.py +++ b/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) diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 490800fc..1277908f 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.3.0 + 2.3.3 Modulo Core communication and translation utilities for interoperability with AICA Control Libraries Baptiste Busch Enrico Eberhard diff --git a/source/modulo_utils/include/modulo_utils/testutils/PredicatesListener.hpp b/source/modulo_utils/include/modulo_utils/testutils/PredicatesListener.hpp index b662ff10..bc4de03b 100644 --- a/source/modulo_utils/include/modulo_utils/testutils/PredicatesListener.hpp +++ b/source/modulo_utils/include/modulo_utils/testutils/PredicatesListener.hpp @@ -4,7 +4,7 @@ #include #include -#include +#include 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& predicates - ); + const std::string& component, const std::vector& predicates, + const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()); void reset_future(); @@ -23,7 +23,7 @@ class PredicatesListener : public rclcpp::Node { [[nodiscard]] const std::map& get_predicate_values() const; private: - std::map>> subscriptions_; + std::shared_ptr> subscription_; std::map predicates_; std::shared_future received_future_; std::promise received_; diff --git a/source/modulo_utils/modulo_utils/testutils/predicates_listener.py b/source/modulo_utils/modulo_utils/testutils/predicates_listener.py index 17e072f8..4aab3327 100644 --- a/source/modulo_utils/modulo_utils/testutils/predicates_listener.py +++ b/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 '') + :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,10 +38,13 @@ 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 @@ -51,11 +52,11 @@ 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 '')\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 diff --git a/source/modulo_utils/package.xml b/source/modulo_utils/package.xml index 9c86241f..3b043e0b 100644 --- a/source/modulo_utils/package.xml +++ b/source/modulo_utils/package.xml @@ -2,7 +2,7 @@ modulo_utils - 2.3.0 + 2.3.3 Modulo utils package for shared test fixtures Dominic Reber GPLv3 @@ -12,9 +12,10 @@ rclcpp rclpy - std_msgs std_srvs + modulo_component_interfaces + ament_lint_auto ament_lint_common diff --git a/source/modulo_utils/src/testutils/PredicatesListener.cpp b/source/modulo_utils/src/testutils/PredicatesListener.cpp index c8ee8539..84d125c0 100644 --- a/source/modulo_utils/src/testutils/PredicatesListener.cpp +++ b/source/modulo_utils/src/testutils/PredicatesListener.cpp @@ -3,21 +3,25 @@ namespace modulo_utils::testutils { PredicatesListener::PredicatesListener( - const rclcpp::NodeOptions& node_options, const std::string& ns, const std::vector& predicates + const std::string& component, const std::vector& predicates, const rclcpp::NodeOptions& node_options ) : rclcpp::Node("predicates_listener", node_options) { this->received_future_ = this->received_.get_future(); for (const auto& predicate : predicates) { this->predicates_.insert_or_assign(predicate, false); - auto subscription = this->create_subscription( - "/predicates/" + ns + "/" + predicate, 10, - [this, predicate](const std::shared_ptr message) { - this->predicates_.at(predicate) = message->data; - if (message->data) { - this->received_.set_value(); - } - }); - this->subscriptions_.insert_or_assign(predicate, subscription); } + this->subscription_ = this->create_subscription( + "/predicates", 10, [this, component](const std::shared_ptr message) { + if (message->component == component) { + for (auto& predicate : this->predicates_) { + if (message->predicate == predicate.first) { + predicate.second = message->value; + if (message->value) { + this->received_.set_value(); + } + } + } + } + }); } void PredicatesListener::reset_future() { diff --git a/source/modulo_utils/test/cpp/testutils/test_available_fixtures.cpp b/source/modulo_utils/test/cpp/testutils/test_available_fixtures.cpp index fea31ef9..deb8cb75 100644 --- a/source/modulo_utils/test/cpp/testutils/test_available_fixtures.cpp +++ b/source/modulo_utils/test/cpp/testutils/test_available_fixtures.cpp @@ -32,8 +32,7 @@ TEST_F(FixturesTest, ServiceClient) { } TEST_F(FixturesTest, PredicatesListener) { - auto listener = std::make_shared( - rclcpp::NodeOptions(), "test", std::vector{"in_error_state"}); + auto listener = std::make_shared("test", std::vector{"in_error_state"}); exec_->add_node(listener->get_node_base_interface()); auto result = exec_->spin_until_future_complete(listener->get_predicate_future(), 1s); EXPECT_EQ(result, rclcpp::FutureReturnCode::TIMEOUT);