From d5c62c55232d080a5a04ab8568326b19a8772c0a Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Fri, 5 Aug 2022 17:12:05 +0200 Subject: [PATCH 01/20] Release version 2.0.0 (#116) * Set version number * Update CHANGELOG * Set default base tag for the build-server script to `galactic` --- CHANGELOG.md | 19 +++++++++++++++++-- VERSION | 2 +- build-server.sh | 2 +- doxygen/doxygen.conf | 2 +- .../modulo_component_interfaces/package.xml | 2 +- source/modulo_components/package.xml | 2 +- source/modulo_core/package.xml | 2 +- 7 files changed, 23 insertions(+), 8 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d406a69ca..98e7b944d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,11 +1,26 @@ # CHANGELOG Release Versions: +- [2.0.0](#200) - [1.1.0](#110) - [1.0.0](#100) - [Pre-release versions](#pre-release-versions) -## Upcoming changes (in development) +## 2.0.0 +### August 05, 2022 + +Version 2.0.0 constitutes a major refactor of the Modulo project. It now provides stand-alone classes and helpers +for handling and translating messages and parameters in modulo_core, with new and improved component classes +for dynamic application composition in modulo_components. + +The old concepts of Cell and Component have been fully redeveloped into modulo_components::Component and +modulo_components::LifecycleComponent, which provide a simple framework for creating stateless (unmanaged) or +state-based (managed) custom components. + +For more information, see the new description of each package in the respective README.md files and view +the full generated documentation on [epfl-lasa.github.io/modulo](epfl-lasa.github.io/modulo). + +### Changes - Fix develop after control libraries StateType refactor (#55) - Fix examples after DS factory refactor (#56) @@ -74,7 +89,7 @@ Release Versions: - Improve State message reading (#108) - Catch core and component exceptions (#111) - Modulo components documentation (#112) -- Versioning (#113) +- Better versioning and changelog (#113) - Fix const reading of State parameters (#114) ## 1.1.0 diff --git a/VERSION b/VERSION index 45a1b3f44..227cea215 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -1.1.2 +2.0.0 diff --git a/build-server.sh b/build-server.sh index ed9a726d8..bc89003e1 100755 --- a/build-server.sh +++ b/build-server.sh @@ -1,5 +1,5 @@ #!/bin/bash -BASE_TAG=galactic-devel +BASE_TAG=galactic IMAGE_NAME=epfl-lasa/modulo IMAGE_TAG=latest diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 8ea40b3ed..a1acd16d0 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 = 1.1.2 +PROJECT_NUMBER = 2.0.0 # 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/package.xml b/source/modulo_component_interfaces/package.xml index adf46041e..b7ac68d24 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 1.1.2 + 2.0.0 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index bb7f57f0d..3deae551c 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 1.1.2 + 2.0.0 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 2fad5f5e5..e353e7b20 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 1.1.2 + 2.0.0 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From 38d0b62a02e584bdfc8dd7554cf3b6dc7d1668cb Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Mon, 8 Aug 2022 10:20:13 +0200 Subject: [PATCH 02/20] Support multiple build test actions (#117) * Move build-test entrypoint script to a common directory * Remove local Dockerfile from action and instead directly reference the base ghcr.io image and common build-test entrypoint script * Add build test action variant for galactic devel image * Invoke build test action job for galactic devel * 2.0.0 -> 2.0.1 * Changelog --- .../actions/build-test-galactic-devel/action.yml | 6 ++++++ .github/actions/build-test-galactic/Dockerfile | 7 ------- .github/actions/build-test-galactic/action.yml | 5 +++-- .../entrypoint.sh => common/build-test.sh} | 0 .github/common/entrypoint.sh | 3 +++ .github/workflows/build-test.yml | 15 ++++++++++++++- CHANGELOG.md | 4 ++++ VERSION | 2 +- doxygen/doxygen.conf | 2 +- source/modulo_component_interfaces/package.xml | 2 +- source/modulo_components/package.xml | 2 +- source/modulo_core/package.xml | 2 +- 12 files changed, 35 insertions(+), 15 deletions(-) create mode 100644 .github/actions/build-test-galactic-devel/action.yml delete mode 100644 .github/actions/build-test-galactic/Dockerfile rename .github/{actions/build-test-galactic/entrypoint.sh => common/build-test.sh} (100%) create mode 100755 .github/common/entrypoint.sh diff --git a/.github/actions/build-test-galactic-devel/action.yml b/.github/actions/build-test-galactic-devel/action.yml new file mode 100644 index 000000000..467a434bb --- /dev/null +++ b/.github/actions/build-test-galactic-devel/action.yml @@ -0,0 +1,6 @@ +name: 'Build and Test (galactic-devel)' +description: 'Build the source packages and run all unit tests' +runs: + using: 'docker' + image: 'ghcr.io/aica-technology/ros2-control-libraries:galactic-devel' + entrypoint: '/github/workspace/.github/common/entrypoint.sh' diff --git a/.github/actions/build-test-galactic/Dockerfile b/.github/actions/build-test-galactic/Dockerfile deleted file mode 100644 index 731001a7d..000000000 --- a/.github/actions/build-test-galactic/Dockerfile +++ /dev/null @@ -1,7 +0,0 @@ -# https://docs.github.com/en/actions/creating-actions/dockerfile-support-for-github-actions -FROM ghcr.io/aica-technology/ros2-control-libraries:galactic-devel - -# Copy and set the entrypoint commands to execute when the container starts, -# explicilty invoking as ros2 user and with bash interpreter. -COPY entrypoint.sh /entrypoint.sh -ENTRYPOINT ["sudo", "su", "-", "ros2", "-c", "/bin/bash", "-c", "/entrypoint.sh"] diff --git a/.github/actions/build-test-galactic/action.yml b/.github/actions/build-test-galactic/action.yml index 69747bc9f..ddba096a9 100644 --- a/.github/actions/build-test-galactic/action.yml +++ b/.github/actions/build-test-galactic/action.yml @@ -1,5 +1,6 @@ -name: 'Build and Test (Galactic)' +name: 'Build and Test (galactic)' description: 'Build the source packages and run all unit tests' runs: using: 'docker' - image: 'Dockerfile' \ No newline at end of file + image: 'ghcr.io/aica-technology/ros2-control-libraries:galactic' + entrypoint: '/github/workspace/.github/common/entrypoint.sh' diff --git a/.github/actions/build-test-galactic/entrypoint.sh b/.github/common/build-test.sh similarity index 100% rename from .github/actions/build-test-galactic/entrypoint.sh rename to .github/common/build-test.sh diff --git a/.github/common/entrypoint.sh b/.github/common/entrypoint.sh new file mode 100755 index 000000000..a2d20c182 --- /dev/null +++ b/.github/common/entrypoint.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +sudo su - ros2 -c /bin/bash -c /github/workspace/.github/common/build-test.sh diff --git a/.github/workflows/build-test.yml b/.github/workflows/build-test.yml index c5f433d49..731562e79 100644 --- a/.github/workflows/build-test.yml +++ b/.github/workflows/build-test.yml @@ -60,4 +60,17 @@ jobs: uses: actions/checkout@v2 # Load the repository build-test action - name: Build and Test - uses: ./.github/actions/build-test-galactic \ No newline at end of file + uses: ./.github/actions/build-test-galactic + + build-test-galactic-devel: + needs: check-skippable-changes + if: ${{ needs.check-skippable-changes.outputs.skip != 'true' }} + runs-on: ubuntu-latest + name: Galactic development build and test + steps: + # First check out the repository + - name: Checkout + uses: actions/checkout@v2 + # Load the repository build-test action + - name: Build and Test + uses: ./.github/actions/build-test-galactic-devel \ No newline at end of file diff --git a/CHANGELOG.md b/CHANGELOG.md index 98e7b944d..e3f763158 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,10 @@ Release Versions: - [1.0.0](#100) - [Pre-release versions](#pre-release-versions) +## Upcoming changes (in development) + +- Support multiple build test actions (#117) + ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 227cea215..38f77a65b 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.0 +2.0.1 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index a1acd16d0..f842cde86 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.0.0 +PROJECT_NUMBER = 2.0.1 # 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/package.xml b/source/modulo_component_interfaces/package.xml index b7ac68d24..0894cb31a 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.0 + 2.0.1 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 3deae551c..da3e056d3 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.0 + 2.0.1 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index e353e7b20..a503a9f49 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.0 + 2.0.1 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From 788882eba32129f95371213c11f280887511ac4e Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 11 Aug 2022 12:17:33 +0200 Subject: [PATCH 03/20] Manually export modulo_core dependencies (#118) * Try exporting dependencies correctly * Add component interfaces * Format CMakeLists * Ament manual not needed for modulo_components * Update CHANGELOG * 2.0.1 -> 2.0.2 * Remove auto from package depend tag --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- .../modulo_component_interfaces/package.xml | 2 +- source/modulo_components/package.xml | 2 +- source/modulo_core/CMakeLists.txt | 56 ++++++++++++++++--- source/modulo_core/package.xml | 4 +- 7 files changed, 55 insertions(+), 14 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e3f763158..a45832a16 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ Release Versions: ## Upcoming changes (in development) - Support multiple build test actions (#117) +- Manually export modulo_core dependencies (#118) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 38f77a65b..e9307ca57 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.1 +2.0.2 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index f842cde86..007ed3e8f 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.0.1 +PROJECT_NUMBER = 2.0.2 # 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/package.xml b/source/modulo_component_interfaces/package.xml index 0894cb31a..ce7197ab5 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.1 + 2.0.2 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index da3e056d3..0c8576ec9 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.1 + 2.0.2 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_core/CMakeLists.txt b/source/modulo_core/CMakeLists.txt index bff22707b..c3dadef02 100644 --- a/source/modulo_core/CMakeLists.txt +++ b/source/modulo_core/CMakeLists.txt @@ -16,17 +16,19 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() # find dependencies -find_package(ament_cmake_auto REQUIRED) +find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) -ament_auto_find_build_dependencies() +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_msgs REQUIRED) find_package(control_libraries 6.1.0 REQUIRED COMPONENTS state_representation) find_package(clproto 6.1.0 REQUIRED) -include_directories(include) - -ament_auto_add_library(modulo_core SHARED +add_library(${PROJECT_NAME} SHARED ${PROJECT_SOURCE_DIR}/src/communication/MessagePair.cpp ${PROJECT_SOURCE_DIR}/src/communication/MessagePairInterface.cpp ${PROJECT_SOURCE_DIR}/src/communication/PublisherInterface.cpp @@ -36,7 +38,26 @@ ament_auto_add_library(modulo_core SHARED ${PROJECT_SOURCE_DIR}/src/translators/message_readers.cpp ${PROJECT_SOURCE_DIR}/src/translators/message_writers.cpp) -target_link_libraries(${PROJECT_NAME} clproto state_representation) +target_include_directories( + ${PROJECT_NAME} PRIVATE + include +) + +ament_target_dependencies( + ${PROJECT_NAME} + clproto + rclcpp +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) # install Python modules ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) @@ -51,7 +72,8 @@ if(BUILD_TESTING) ament_add_gtest(test_modulo_core ${TEST_CPP_SOURCES}) target_include_directories(test_modulo_core PRIVATE include) - target_link_libraries(test_modulo_core modulo_core state_representation clproto) + target_link_libraries(test_modulo_core ${PROJECT_NAME} clproto state_representation) + ament_target_dependencies(test_modulo_core geometry_msgs sensor_msgs std_msgs rclcpp tf2_msgs) # prevent pluginlib from using boost target_compile_definitions(test_modulo_core PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") @@ -66,4 +88,22 @@ if(BUILD_TESTING) endif() -ament_auto_package() +ament_export_dependencies( + geometry_msgs + sensor_msgs + std_msgs + rclcpp + tf2_msgs +) + +ament_export_include_directories( + include +) + +ament_export_libraries( + ${PROJECT_NAME} + state_representation + clproto +) + +ament_package() diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index a503a9f49..0ee694589 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,14 +2,14 @@ modulo_core - 2.0.1 + 2.0.2 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard Dominic Reber TODO: License declaration - ament_cmake_auto + ament_cmake ament_cmake_python rclcpp From d13ef50342962beafd38a07a325de0b2d4029673 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Mon, 15 Aug 2022 16:39:26 +0200 Subject: [PATCH 04/20] Ensure compatibility with humble (#119) * Fix package.xml and CMakeLists for humble * Back to galactic tag by default * 2.0.2 -> 2.0.3 * Update CHANGELOG * Keep rclcpp as dependency * Rename services to services_dict * Rename kargs to args --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- source/modulo_component_interfaces/package.xml | 2 +- .../modulo_components/component.py | 4 ++-- .../modulo_components/component_interface.py | 18 ++++++++++-------- .../modulo_components/lifecycle_component.py | 4 ++-- source/modulo_components/package.xml | 4 ++-- .../test/python/test_component_interface.py | 16 ++++++++-------- source/modulo_core/CMakeLists.txt | 9 ++++++++- source/modulo_core/package.xml | 3 ++- 11 files changed, 38 insertions(+), 27 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a45832a16..6261f8fa8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,7 @@ Release Versions: - Support multiple build test actions (#117) - Manually export modulo_core dependencies (#118) +- Ensure compatibility with humble (#119) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index e9307ca57..50ffc5aa7 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.2 +2.0.3 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 007ed3e8f..2b45f0a73 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.0.2 +PROJECT_NUMBER = 2.0.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/package.xml b/source/modulo_component_interfaces/package.xml index ce7197ab5..d0a48e188 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.2 + 2.0.3 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/modulo_components/component.py b/source/modulo_components/modulo_components/component.py index e59d62530..fe85768f8 100644 --- a/source/modulo_components/modulo_components/component.py +++ b/source/modulo_components/modulo_components/component.py @@ -13,13 +13,13 @@ class Component(ComponentInterface): as the C++ modulo_components::Component class. """ - def __init__(self, node_name: str, *kargs, **kwargs): + def __init__(self, node_name: str, *args, **kwargs): """ Constructs all the necessary attributes and declare all the parameters. :param node_name: The name of the node to be passed to the base Node class """ - super().__init__(node_name, *kargs, **kwargs) + super().__init__(node_name, *args, **kwargs) self.__started = False self.__execute_thread = None self.add_predicate("is_finished", False) diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index faf8ad85b..b978417e1 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -1,5 +1,5 @@ -import sys import inspect +import sys from functools import partial from typing import Callable, Dict, List, Optional, TypeVar, Union @@ -20,6 +20,7 @@ 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 from std_msgs.msg import Bool, Int32, Float64, Float64MultiArray, String from tf2_py import TransformException @@ -35,8 +36,8 @@ class ComponentInterface(Node): following the same logic pattern as the C++ modulo_components::ComponentInterface class. """ - def __init__(self, node_name: str, *kargs, **kwargs): - super().__init__(node_name, *kargs, **kwargs) + def __init__(self, node_name: str, *args, **kwargs): + super().__init__(node_name, *args, **kwargs) self._parameter_dict: Dict[str, Union[str, sr.Parameter]] = {} self._predicates: Dict[str, Union[bool, Callable[[], bool]]] = {} self._predicate_publishers: Dict[str, Publisher] = {} @@ -44,7 +45,7 @@ def __init__(self, node_name: str, *kargs, **kwargs): self._periodic_callbacks: Dict[str, Callable[[], None]] = {} self._inputs = {} self._outputs = {} - self._services = {} + self._services_dict: Dict[str, Service] = {} self.__tf_buffer: Optional[Buffer] = None self.__tf_listener: Optional[TransformListener] = None self.__tf_broadcaster: Optional[TransformBroadcaster] = None @@ -370,7 +371,7 @@ def __subscription_callback(self, message: MsgT, attribute_name: str, reader: Ca self.__setattr__(attribute_name, reader(message)) except (AttributeError, MessageTranslationError) as e: self.get_logger().warn(f"Failed to read message for attribute {attribute_name}: {e}", - throttle_duration_sec=1.0) + throttle_duration_sec=1.0) def add_input(self, signal_name: str, subscription: Union[str, Callable], message_type: MsgT, default_topic="", fixed_topic=False): @@ -437,6 +438,7 @@ def add_service(self, service_name: str, callback: Union[Callable[[], dict], Cal :return: A dict with the outcome of the service call, containing "success" and "message" fields in the format {"success": [True | False], "message": "..."} """ + def callback_wrapper(request, response, cb): try: if hasattr(request, "payload"): @@ -463,16 +465,16 @@ def callback_wrapper(request, response, cb): parsed_service_name = parse_topic_name(service_name) if not parsed_service_name: raise AddServiceError(f"Failed to add service '{service_name}': Parsed service name is empty.") - if parsed_service_name in self._services.keys(): + if parsed_service_name in self._services_dict.keys(): raise AddServiceError(f"Failed to add service '{service_name}': Service already exists") signature = inspect.signature(callback) if len(signature.parameters) == 0: - self.get_logger().debug(f"Adding empty service '{parsed_service_name}'") + self.get_logger().error(f"Adding empty service '{parsed_service_name}'") service_type = EmptyTrigger else: self.get_logger().debug(f"Adding string service '{parsed_service_name}'") service_type = StringTrigger - self._services[parsed_service_name] = \ + self._services_dict[parsed_service_name] = \ self.create_service(service_type, "~/" + parsed_service_name, lambda request, response: callback_wrapper(request, response, callback)) except Exception as e: diff --git a/source/modulo_components/modulo_components/lifecycle_component.py b/source/modulo_components/modulo_components/lifecycle_component.py index c959559cb..43fb14ad7 100644 --- a/source/modulo_components/modulo_components/lifecycle_component.py +++ b/source/modulo_components/modulo_components/lifecycle_component.py @@ -16,13 +16,13 @@ class LifecycleComponent(ComponentInterface): as the C++ modulo_components::LifecycleComponent class. """ - def __init__(self, node_name: str, *kargs, **kwargs): + def __init__(self, node_name: str, *args, **kwargs): """ Constructs all the necessary attributes and declare all the parameters. :param node_name: The name of the node to be passed to the base Node class """ - super().__init__(node_name, *kargs, **kwargs) + super().__init__(node_name, *args, **kwargs) self.__state = State.PRIMARY_STATE_UNCONFIGURED # add the service to mimic the lifecycle paradigm diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 0c8576ec9..b922f01a2 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.2 + 2.0.3 TODO Baptiste Busch Enrico Eberhard @@ -12,9 +12,9 @@ ament_cmake_auto ament_cmake_python + console_bridge modulo_core modulo_component_interfaces - rclcpp_lifecycle tf2_ros ament_lint_auto diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index 14b58b3f0..72c1ce5ba 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -69,28 +69,28 @@ def test_add_service(component_interface): def empty_callback(): return {"success": True, "message": "test"} component_interface.add_service("empty", empty_callback) - assert len(component_interface._services) == 1 - assert "empty" in component_interface._services.keys() + assert len(component_interface._services_dict) == 1 + assert "empty" in component_interface._services_dict.keys() def string_callback(): return {"success": True, "message": "test"} component_interface.add_service("string", string_callback) - assert len(component_interface._services) == 2 - assert "string" in component_interface._services.keys() + assert len(component_interface._services_dict) == 2 + assert "string" in component_interface._services_dict.keys() # adding a service under an existing name should fail for either callback type, but is exception safe component_interface.add_service("empty", empty_callback) component_interface.add_service("empty", string_callback) - assert len(component_interface._services) == 2 + assert len(component_interface._services_dict) == 2 component_interface.add_service("string", empty_callback) component_interface.add_service("string", string_callback) - assert len(component_interface._services) == 2 + assert len(component_interface._services_dict) == 2 # adding a mangled service name should succeed under a sanitized name component_interface.add_service("_tEsT_#1@3", empty_callback) - assert len(component_interface._services) == 3 - assert "test_13" in component_interface._services.keys() + assert len(component_interface._services_dict) == 3 + assert "test_13" in component_interface._services_dict.keys() # TODO: use a service client to trigger the service and test the behaviour diff --git a/source/modulo_core/CMakeLists.txt b/source/modulo_core/CMakeLists.txt index c3dadef02..d1383289a 100644 --- a/source/modulo_core/CMakeLists.txt +++ b/source/modulo_core/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(tf2_msgs REQUIRED) find_package(control_libraries 6.1.0 REQUIRED COMPONENTS state_representation) @@ -47,6 +48,11 @@ ament_target_dependencies( ${PROJECT_NAME} clproto rclcpp + rclcpp_lifecycle + geometry_msgs + sensor_msgs + std_msgs + tf2_msgs ) install(DIRECTORY include/ @@ -73,7 +79,7 @@ if(BUILD_TESTING) ament_add_gtest(test_modulo_core ${TEST_CPP_SOURCES}) target_include_directories(test_modulo_core PRIVATE include) target_link_libraries(test_modulo_core ${PROJECT_NAME} clproto state_representation) - ament_target_dependencies(test_modulo_core geometry_msgs sensor_msgs std_msgs rclcpp tf2_msgs) + ament_target_dependencies(test_modulo_core geometry_msgs sensor_msgs std_msgs rclcpp rclcpp_lifecycle tf2_msgs) # prevent pluginlib from using boost target_compile_definitions(test_modulo_core PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") @@ -93,6 +99,7 @@ ament_export_dependencies( sensor_msgs std_msgs rclcpp + rclcpp_lifecycle tf2_msgs ) diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 0ee694589..36ac8c73a 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.2 + 2.0.3 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard @@ -13,6 +13,7 @@ ament_cmake_python rclcpp + rclcpp_lifecycle rclpy geometry_msgs sensor_msgs From 51d7a46deaf77102e9f6fb1a8875ab97da4d6eb7 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Tue, 16 Aug 2022 14:24:32 +0200 Subject: [PATCH 05/20] Add static tf broadcaster (#120) * Add static tf broadcaster * Add static tf broadcaster as class property, with methods to add static broadcaster and send static tf * In C++, pass default publisher options to static transform broadcaster constructor, instead of the default-constructed qos override options * Add helper function to publish the transform in a generic way to support static and non-static broadcasters, and thereby reduce code duplication in the user-facing send transforms * Add tests * 2.0.3 -> 2.0.4 --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- .../modulo_component_interfaces/package.xml | 2 +- .../modulo_components/ComponentInterface.hpp | 65 +++++++++++++++++-- .../modulo_components/component_interface.py | 52 +++++++++++---- source/modulo_components/package.xml | 2 +- .../component_public_interfaces.hpp | 2 + .../test/cpp/test_component_interface.cpp | 11 +++- .../test/python/test_component_interface.py | 8 +++ source/modulo_core/package.xml | 2 +- 11 files changed, 126 insertions(+), 23 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6261f8fa8..f932af131 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,7 @@ Release Versions: - Support multiple build test actions (#117) - Manually export modulo_core dependencies (#118) - Ensure compatibility with humble (#119) +- Add static tf broadcaster (#120) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 50ffc5aa7..2165f8f9b 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.3 +2.0.4 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 2b45f0a73..ce3eff2b1 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.0.3 +PROJECT_NUMBER = 2.0.4 # 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/package.xml b/source/modulo_component_interfaces/package.xml index d0a48e188..e64dc753d 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.3 + 2.0.4 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 8f2662a76..de1f74776 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -282,6 +283,11 @@ class ComponentInterface : public NodeT { */ void add_tf_broadcaster(); + /** + * @brief Configure a static transform broadcaster. + */ + void add_static_tf_broadcaster(); + /** * @brief Configure a transform buffer and listener. */ @@ -322,6 +328,12 @@ class ComponentInterface : public NodeT { */ void send_transform(const state_representation::CartesianPose& transform); + /** + * @brief Send a static transform to TF. + * @param transform The transform to send + */ + void send_static_transform(const state_representation::CartesianPose& transform); + /** * @brief Look up a transform from TF. * @param frame_name The desired frame of the transform @@ -358,6 +370,19 @@ class ComponentInterface : public NodeT { */ void evaluate_periodic_callbacks(); + /** + * @brief Helper function to send a transform through a transform broadcaster + * @tparam T The type of the broadcaster (tf2_ros::TransformBroadcaster or tf2_ros::StaticTransformBroadcaster) + * @param transform The transform to send + * @param tf_broadcaster A pointer to a configured transform broadcaster object + * @param is_static If true, treat the broadcaster as a static frame broadcaster for the sake of log messages + */ + template + void publish_transform( + const state_representation::CartesianPose& transform, const std::shared_ptr& tf_broadcaster, + bool is_static = false + ); + /** * @brief Put the component in error state by setting the 'in_error_state' predicate to true. */ @@ -431,7 +456,7 @@ class ComponentInterface : public NodeT { std::shared_ptr tf_buffer_; ///< TF buffer std::shared_ptr tf_listener_; ///< TF listener std::shared_ptr tf_broadcaster_; ///< TF broadcaster - // TODO maybe add a static tf broadcaster + std::shared_ptr static_tf_broadcaster_; ///< TF static broadcaster bool set_parameter_callback_called_ = false; ///< Flag to indicate if on_set_parameter_callback was called }; @@ -920,6 +945,19 @@ inline void ComponentInterface::add_tf_broadcaster() { } } +template +inline void ComponentInterface::add_static_tf_broadcaster() { + if (this->static_tf_broadcaster_ == nullptr) { + RCLCPP_DEBUG(this->get_logger(), "Adding static TF broadcaster."); + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); + tf2_ros::StaticBroadcasterQoS qos; + rclcpp::PublisherOptionsWithAllocator> options; + this->static_tf_broadcaster_ = std::make_shared(*this, qos, options); + } else { + RCLCPP_DEBUG(this->get_logger(), "Static TF broadcaster already exists."); + } +} + template inline void ComponentInterface::add_tf_listener() { if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { @@ -934,18 +972,33 @@ inline void ComponentInterface::add_tf_listener() { template inline void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { - if (this->tf_broadcaster_ == nullptr) { - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Failed to send transform: No TF broadcaster configured."); + this->template publish_transform(transform, this->tf_broadcaster_); +} + +template +inline void ComponentInterface::send_static_transform(const state_representation::CartesianPose& transform) { + this->template publish_transform(transform, this->static_tf_broadcaster_); +} + +template +template +inline void ComponentInterface::publish_transform( + const state_representation::CartesianPose& transform, const std::shared_ptr& tf_broadcaster, bool is_static +) { + std::string modifier = is_static ? "static " : ""; + if (tf_broadcaster == nullptr) { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "Failed to send " << modifier << "transform: No " << modifier << "TF broadcaster configured."); return; } try { geometry_msgs::msg::TransformStamped transform_message; modulo_core::translators::write_message(transform_message, transform, this->get_clock()->now()); - this->tf_broadcaster_->sendTransform(transform_message); + tf_broadcaster->sendTransform(transform_message); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Failed to send transform: " << ex.what()); + "Failed to send " << modifier << "transform: " << ex.what()); } } diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index b978417e1..eb130db91 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -24,7 +24,7 @@ from rclpy.time import Time from std_msgs.msg import Bool, Int32, Float64, Float64MultiArray, String from tf2_py import TransformException -from tf2_ros import Buffer, TransformBroadcaster, TransformListener +from tf2_ros import Buffer, StaticTransformBroadcaster, TransformBroadcaster, TransformListener MsgT = TypeVar('MsgT') T = TypeVar('T') @@ -49,6 +49,7 @@ def __init__(self, node_name: str, *args, **kwargs): self.__tf_buffer: Optional[Buffer] = None self.__tf_listener: Optional[TransformListener] = None self.__tf_broadcaster: Optional[TransformBroadcaster] = None + self.__static_tf_broadcaster: Optional[StaticTransformBroadcaster] = None self._qos = QoSProfile(depth=10) @@ -490,6 +491,16 @@ def add_tf_broadcaster(self): else: self.get_logger().error("TF broadcaster already exists.") + def add_static_tf_broadcaster(self): + """ + Configure a static transform broadcaster. + """ + if not self.__static_tf_broadcaster: + self.get_logger().debug("Adding static TF broadcaster.") + self.__static_tf_broadcaster = StaticTransformBroadcaster(self) + else: + self.get_logger().error("TF broadcaster already exists.") + def add_tf_listener(self): """ Configure a transform buffer and listener. @@ -507,16 +518,15 @@ def send_transform(self, transform: sr.CartesianPose): :param transform: The transform to send """ - if not self.__tf_broadcaster: - self.get_logger().error("Failed to send transform: No TF broadcaster configured.", - throttle_duration_sec=1.0) - return - try: - transform_message = TransformStamped() - modulo_writers.write_stamped_message(transform_message, transform, self.get_clock().now()) - self.__tf_broadcaster.sendTransform(transform_message) - except (MessageTranslationError, TransformException) as e: - self.get_logger().error(f"Failed to send transform: {e}", throttle_duration_sec=1.0) + self.__publish_transform(transform) + + def send_static_transform(self, transform: sr.CartesianPose): + """ + Send a static transform to TF. + + :param transform: The transform to send + """ + self.__publish_transform(transform, static=True) def lookup_transform(self, frame_name: str, reference_frame_name="world", time_point=Time(), duration=Duration(nanoseconds=1e4)) -> sr.CartesianPose: @@ -618,6 +628,26 @@ def _evaluate_periodic_callbacks(self): self.get_logger().error(f"Failed to evaluate periodic function callback '{name}': {e}", throttle_duration_sec=1.0) + def __publish_transform(self, transform: sr.CartesianPose, static: Bool=False): + """ + Send a transform to TF using the normal or static tf broadcaster + + :param transform: The transform to send + :param static: If true, use the static tf broadcaster instead of the normal one + """ + tf_broadcaster = self.__static_tf_broadcaster if static else self.__tf_broadcaster + modifier = 'static ' if static else '' + if not tf_broadcaster: + self.get_logger().error(f"Failed to send {modifier}transform: No {modifier}TF broadcaster configured.", + throttle_duration_sec=1.0) + return + try: + transform_message = TransformStamped() + modulo_writers.write_stamped_message(transform_message, transform, self.get_clock().now()) + tf_broadcaster.sendTransform(transform_message) + except (MessageTranslationError, TransformException) as e: + self.get_logger().error(f"Failed to send {modifier}transform: {e}", throttle_duration_sec=1.0) + def raise_error(self): """ Put the component in error state by setting the 'in_error_state' predicate to true. diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index b922f01a2..3730e2241 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.3 + 2.0.4 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 7622acc2e..9ef43a332 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -35,8 +35,10 @@ class ComponentInterfacePublicInterface : public ComponentInterface { using ComponentInterface::empty_services_; using ComponentInterface::string_services_; using ComponentInterface::add_tf_broadcaster; + using ComponentInterface::add_static_tf_broadcaster; using ComponentInterface::add_tf_listener; using ComponentInterface::send_transform; + using ComponentInterface::send_static_transform; using ComponentInterface::lookup_transform; using ComponentInterface::raise_error; using ComponentInterface::get_qos; diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index b4a36c7c7..ae9d96c9c 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -161,15 +161,24 @@ TYPED_TEST(ComponentInterfaceTest, CreateOutput) { TYPED_TEST(ComponentInterfaceTest, TF) { this->component_->add_tf_broadcaster(); + this->component_->add_static_tf_broadcaster(); this->component_->add_tf_listener(); auto send_tf = state_representation::CartesianPose::Random("test", "world"); EXPECT_NO_THROW(this->component_->send_transform(send_tf)); + auto send_static_tf = state_representation::CartesianPose::Random("static_test", "world"); + EXPECT_NO_THROW(this->component_->send_static_transform(send_static_tf)); EXPECT_THROW(auto throw_tf = this->component_->lookup_transform("dummy", "world"), exceptions::LookupTransformException); - auto lookup_tf = this->component_->lookup_transform("test", "world"); + state_representation::CartesianPose lookup_tf; + EXPECT_NO_THROW(lookup_tf = this->component_->lookup_transform("test", "world")); auto identity = send_tf * lookup_tf.inverse(); EXPECT_FLOAT_EQ(identity.data().norm(), 1.); EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.); + + EXPECT_NO_THROW(lookup_tf = this->component_->lookup_transform("static_test", "world")); + identity = send_static_tf * lookup_tf.inverse(); + EXPECT_FLOAT_EQ(identity.data().norm(), 1.); + EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.); } TYPED_TEST(ComponentInterfaceTest, GetSetQoS) { diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index 72c1ce5ba..00263f87c 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -97,9 +97,12 @@ def string_callback(): def test_tf(component_interface): component_interface.add_tf_broadcaster() + component_interface.add_static_tf_broadcaster() component_interface.add_tf_listener() send_tf = sr.CartesianPose().Random("test", "world") + send_static_tf = sr.CartesianPose().Random("static_test", "world") component_interface.send_transform(send_tf) + component_interface.send_static_transform(send_static_tf) for i in range(10): rclpy.spin_once(component_interface) with pytest.raises(LookupTransformError): @@ -109,6 +112,11 @@ def test_tf(component_interface): assert np.linalg.norm(identity.data()) - 1 < 1e-3 assert abs(identity.get_orientation().w) - 1 < 1e-3 + lookup_tf = component_interface.lookup_transform("static_test", "world") + identity = send_static_tf * lookup_tf.inverse() + assert np.linalg.norm(identity.data()) - 1 < 1e-3 + assert abs(identity.get_orientation().w) - 1 < 1e-3 + def test_add_trigger(component_interface): component_interface.add_trigger("trigger") diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 36ac8c73a..2a02fcc15 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.3 + 2.0.4 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From cb76e6ce37e3c5dfa954fc0d34257bc7c4c632bb Mon Sep 17 00:00:00 2001 From: Baptiste Busch Date: Wed, 17 Aug 2022 11:49:16 +0200 Subject: [PATCH 06/20] Accept list of transforms in send_transform (#122) * feat: accept list of transforms as input of send_transform * feat: accept both list of transform and single transform as input of send_transform * update version number * fix: change changelog * fix: accept any Iterable in type hints * fix: use only Iterable from typing * fix: rename helper function to publish_transforms * fix: add tests for list of transforms * fix: correct typos Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> * fix: use the plural version for the list functions Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- .../modulo_component_interfaces/package.xml | 2 +- .../modulo_components/ComponentInterface.hpp | 52 +++++++++++++++---- .../modulo_components/component_interface.py | 37 +++++++++---- source/modulo_components/package.xml | 2 +- .../component_public_interfaces.hpp | 2 + .../test/cpp/test_component_interface.cpp | 27 ++++++++++ .../test/python/test_component_interface.py | 24 +++++++++ source/modulo_core/package.xml | 2 +- 11 files changed, 128 insertions(+), 25 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f932af131..d60c582a5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ Release Versions: - Manually export modulo_core dependencies (#118) - Ensure compatibility with humble (#119) - Add static tf broadcaster (#120) +- Accept list of transforms in send_transform (#122) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 2165f8f9b..e01025862 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.4 +2.0.5 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index ce3eff2b1..b855585f3 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.0.4 +PROJECT_NUMBER = 2.0.5 # 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/package.xml b/source/modulo_component_interfaces/package.xml index e64dc753d..675596555 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.4 + 2.0.5 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index de1f74776..ebc032e39 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -328,12 +328,24 @@ class ComponentInterface : public NodeT { */ void send_transform(const state_representation::CartesianPose& transform); + /** + * @brief Send a vector of transforms to TF. + * @param transforms The vector of transforms to send + */ + void send_transforms(const std::vector& transforms); + /** * @brief Send a static transform to TF. * @param transform The transform to send */ void send_static_transform(const state_representation::CartesianPose& transform); + /** + * @brief Send a vector of static transforms to TF. + * @param transforms The vector of transforms to send + */ + void send_static_transforms(const std::vector& transforms); + /** * @brief Look up a transform from TF. * @param frame_name The desired frame of the transform @@ -371,15 +383,15 @@ class ComponentInterface : public NodeT { void evaluate_periodic_callbacks(); /** - * @brief Helper function to send a transform through a transform broadcaster + * @brief Helper function to send a vector of transforms through a transform broadcaster * @tparam T The type of the broadcaster (tf2_ros::TransformBroadcaster or tf2_ros::StaticTransformBroadcaster) - * @param transform The transform to send + * @param transforms The transforms to send * @param tf_broadcaster A pointer to a configured transform broadcaster object * @param is_static If true, treat the broadcaster as a static frame broadcaster for the sake of log messages */ template - void publish_transform( - const state_representation::CartesianPose& transform, const std::shared_ptr& tf_broadcaster, + void publish_transforms( + const std::vector& transforms, const std::shared_ptr& tf_broadcaster, bool is_static = false ); @@ -972,18 +984,31 @@ inline void ComponentInterface::add_tf_listener() { template inline void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { - this->template publish_transform(transform, this->tf_broadcaster_); + this->send_transforms(std::vector{transform}); +} + +template +inline void +ComponentInterface::send_transforms(const std::vector& transforms) { + this->template publish_transforms(transforms, this->tf_broadcaster_); } template inline void ComponentInterface::send_static_transform(const state_representation::CartesianPose& transform) { - this->template publish_transform(transform, this->static_tf_broadcaster_); + this->send_static_transforms(std::vector{transform}); +} + +template +inline void +ComponentInterface::send_static_transforms(const std::vector& transforms) { + this->template publish_transforms(transforms, this->static_tf_broadcaster_); } template template -inline void ComponentInterface::publish_transform( - const state_representation::CartesianPose& transform, const std::shared_ptr& tf_broadcaster, bool is_static +inline void ComponentInterface::publish_transforms( + const std::vector& transforms, const std::shared_ptr& tf_broadcaster, + bool is_static ) { std::string modifier = is_static ? "static " : ""; if (tf_broadcaster == nullptr) { @@ -993,9 +1018,14 @@ inline void ComponentInterface::publish_transform( return; } try { - geometry_msgs::msg::TransformStamped transform_message; - modulo_core::translators::write_message(transform_message, transform, this->get_clock()->now()); - tf_broadcaster->sendTransform(transform_message); + std::vector transform_messages; + transform_messages.reserve(transforms.size()); + for (const auto& tf: transforms) { + geometry_msgs::msg::TransformStamped transform_message; + modulo_core::translators::write_message(transform_message, tf, this->get_clock()->now()); + transform_messages.emplace_back(transform_message); + } + tf_broadcaster->sendTransform(transform_messages); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Failed to send " << modifier << "transform: " << ex.what()); diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index eb130db91..8ce20440f 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -1,7 +1,7 @@ import inspect import sys from functools import partial -from typing import Callable, Dict, List, Optional, TypeVar, Union +from typing import Callable, Dict, List, Optional, TypeVar, Union, Iterable import clproto import modulo_core.translators.message_readers as modulo_readers @@ -512,13 +512,29 @@ def add_tf_listener(self): else: self.get_logger().error("TF buffer and listener already exist.") + def send_transforms(self, transforms: Iterable[sr.CartesianPose]): + """ + Send a list of transforms to TF. + + :param transforms: The list of transforms to send + """ + self.__publish_transforms(transforms) + def send_transform(self, transform: sr.CartesianPose): """ Send a transform to TF. :param transform: The transform to send """ - self.__publish_transform(transform) + self.send_transforms([transform]) + + def send_static_transforms(self, transforms: Iterable[sr.CartesianPose]): + """ + Send a list of static transforms to TF. + + :param transforms: The list of transforms to send + """ + self.__publish_transforms(transforms, static=True) def send_static_transform(self, transform: sr.CartesianPose): """ @@ -526,7 +542,7 @@ def send_static_transform(self, transform: sr.CartesianPose): :param transform: The transform to send """ - self.__publish_transform(transform, static=True) + self.send_static_transforms([transform]) def lookup_transform(self, frame_name: str, reference_frame_name="world", time_point=Time(), duration=Duration(nanoseconds=1e4)) -> sr.CartesianPose: @@ -628,11 +644,11 @@ def _evaluate_periodic_callbacks(self): self.get_logger().error(f"Failed to evaluate periodic function callback '{name}': {e}", throttle_duration_sec=1.0) - def __publish_transform(self, transform: sr.CartesianPose, static: Bool=False): + def __publish_transforms(self, transforms: Iterable[sr.CartesianPose], static: Bool=False): """ - Send a transform to TF using the normal or static tf broadcaster + Send a list of transforms to TF using the normal or static tf broadcaster - :param transform: The transform to send + :param transforms: The list of transforms to send :param static: If true, use the static tf broadcaster instead of the normal one """ tf_broadcaster = self.__static_tf_broadcaster if static else self.__tf_broadcaster @@ -642,9 +658,12 @@ def __publish_transform(self, transform: sr.CartesianPose, static: Bool=False): throttle_duration_sec=1.0) return try: - transform_message = TransformStamped() - modulo_writers.write_stamped_message(transform_message, transform, self.get_clock().now()) - tf_broadcaster.sendTransform(transform_message) + transform_messages = [] + for tf in transforms: + transform_message = TransformStamped() + modulo_writers.write_stamped_message(transform_message, tf, self.get_clock().now()) + transform_messages.append(transform_message) + tf_broadcaster.sendTransform(transform_messages) except (MessageTranslationError, TransformException) as e: self.get_logger().error(f"Failed to send {modifier}transform: {e}", throttle_duration_sec=1.0) diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 3730e2241..4d341e9cd 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.4 + 2.0.5 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 9ef43a332..0fb676e75 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -38,7 +38,9 @@ class ComponentInterfacePublicInterface : public ComponentInterface { using ComponentInterface::add_static_tf_broadcaster; using ComponentInterface::add_tf_listener; using ComponentInterface::send_transform; + using ComponentInterface::send_transforms; using ComponentInterface::send_static_transform; + using ComponentInterface::send_static_transforms; using ComponentInterface::lookup_transform; using ComponentInterface::raise_error; using ComponentInterface::get_qos; diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index ae9d96c9c..de107edf1 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -179,6 +179,33 @@ TYPED_TEST(ComponentInterfaceTest, TF) { identity = send_static_tf * lookup_tf.inverse(); EXPECT_FLOAT_EQ(identity.data().norm(), 1.); EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.); + + std::vector send_tfs; + send_tfs.reserve(3); + for (std::size_t idx = 0; idx < 3; ++idx) { + send_tfs.emplace_back(state_representation::CartesianPose::Random("test_" + std::to_string(idx), "world")); + } + EXPECT_NO_THROW(this->component_->send_transforms(send_tfs)); + for (const auto& tf: send_tfs) { + lookup_tf = this->component_->lookup_transform(tf.get_name(), tf.get_reference_frame()); + identity = tf * lookup_tf.inverse(); + EXPECT_FLOAT_EQ(identity.data().norm(), 1.); + EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.); + } + + std::vector send_static_tfs; + send_static_tfs.reserve(3); + for (std::size_t idx = 0; idx < 3; ++idx) { + send_static_tfs.emplace_back( + state_representation::CartesianPose::Random("test_static_" + std::to_string(idx), "world")); + } + EXPECT_NO_THROW(this->component_->send_static_transforms(send_static_tfs)); + for (const auto& tf: send_static_tfs) { + lookup_tf = this->component_->lookup_transform(tf.get_name(), tf.get_reference_frame()); + identity = tf * lookup_tf.inverse(); + EXPECT_FLOAT_EQ(identity.data().norm(), 1.); + EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.); + } } TYPED_TEST(ComponentInterfaceTest, GetSetQoS) { diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index 00263f87c..a73346807 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -117,6 +117,30 @@ def test_tf(component_interface): assert np.linalg.norm(identity.data()) - 1 < 1e-3 assert abs(identity.get_orientation().w) - 1 < 1e-3 + send_tfs = [] + for idx in range(3): + send_tfs.append(sr.CartesianPose.Random("test_" + str(idx), "world")) + component_interface.send_transforms(send_tfs) + for i in range(10): + rclpy.spin_once(component_interface) + for tf in send_tfs: + lookup_tf = component_interface.lookup_transform(tf.get_name(), tf.get_reference_frame()) + identity = tf * lookup_tf.inverse() + assert np.linalg.norm(identity.data()) - 1 < 1e-3 + assert abs(identity.get_orientation().w) - 1 < 1e-3 + + send_static_tfs = [] + for idx in range(3): + send_static_tfs.append(sr.CartesianPose.Random("test_static_" + str(idx), "world")) + component_interface.send_static_transforms(send_static_tfs) + for i in range(10): + rclpy.spin_once(component_interface) + for tf in send_static_tfs: + lookup_tf = component_interface.lookup_transform(tf.get_name(), tf.get_reference_frame()) + identity = tf * lookup_tf.inverse() + assert np.linalg.norm(identity.data()) - 1 < 1e-3 + assert abs(identity.get_orientation().w) - 1 < 1e-3 + def test_add_trigger(component_interface): component_interface.add_trigger("trigger") diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 2a02fcc15..223bcb2bb 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.4 + 2.0.5 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From 66274f0e1826c1fb386e98ce5110d05dac1ce6cb Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 18 Aug 2022 14:05:05 +0200 Subject: [PATCH 07/20] Use prefix for all exceptions (#121) * Use errors with prefix in Python * Use exceptions with prefix in modulo_components C++ * Update exception messages to avoid duplication of prefix * Update CHANGELOG * 2.0.5 -> 2.0.6 Co-authored-by: Enrico Eberhard --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- source/modulo_component_interfaces/package.xml | 2 +- .../modulo_components/ComponentInterface.hpp | 18 ++++++++---------- .../exceptions/AddServiceException.hpp | 2 +- .../exceptions/AddSignalException.hpp | 2 +- .../exceptions/ComponentException.hpp | 4 +++- .../exceptions/ComponentParameterException.hpp | 3 ++- .../exceptions/LookupTransformException.hpp | 2 +- .../modulo_components/component_interface.py | 14 ++++++++------ .../exceptions/component_exceptions.py | 12 ++++++------ source/modulo_components/package.xml | 2 +- .../modulo_core/exceptions/core_exceptions.py | 8 ++++---- .../modulo_core/translators/message_readers.py | 4 +++- .../modulo_core/translators/message_writers.py | 6 ++++++ source/modulo_core/package.xml | 2 +- 17 files changed, 49 insertions(+), 37 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d60c582a5..74b36f09e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ Release Versions: - Manually export modulo_core dependencies (#118) - Ensure compatibility with humble (#119) - Add static tf broadcaster (#120) +- Use prefix for all exceptions (#121) - Accept list of transforms in send_transform (#122) ## 2.0.0 diff --git a/VERSION b/VERSION index e01025862..157e54f3e 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.5 +2.0.6 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index b855585f3..942879896 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.0.5 +PROJECT_NUMBER = 2.0.6 # 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/package.xml b/source/modulo_component_interfaces/package.xml index 675596555..a66654813 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.5 + 2.0.6 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index ebc032e39..30623f675 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -681,8 +681,8 @@ inline bool ComponentInterface::get_predicate(const std::string& predicat value = (callback_function)(); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Failed to evaluate callback of predicate'" << predicate_name << "', returning false:" - << ex.what()); + "Failed to evaluate callback of predicate '" << predicate_name + << "', returning false: " << ex.what()); } return value; } @@ -750,12 +750,10 @@ template inline std::string ComponentInterface::validate_input_signal_name(const std::string& signal_name) { std::string parsed_signal_name = utilities::parse_topic_name(signal_name); if (parsed_signal_name.empty()) { - throw exceptions::AddSignalException( - "Failed to add input '" + signal_name + "': Parsed signal name is empty." - ); + throw exceptions::AddSignalException("Parsed signal name is empty."); } if (this->inputs_.find(parsed_signal_name) != this->inputs_.cend()) { - throw exceptions::AddSignalException("Failed to add input '" + signal_name + "': Input already exists"); + throw exceptions::AddSignalException("Input already exists"); } return parsed_signal_name; } @@ -864,13 +862,11 @@ template inline std::string ComponentInterface::validate_service_name(const std::string& service_name) { std::string parsed_service_name = utilities::parse_topic_name(service_name); if (parsed_service_name.empty()) { - throw exceptions::AddServiceException( - "Failed to add service '" + service_name + "': Parsed service name is empty." - ); + throw exceptions::AddServiceException("Parsed service name is empty."); } if (this->empty_services_.find(parsed_service_name) != this->empty_services_.cend() || this->string_services_.find(parsed_service_name) != this->string_services_.cend()) { - throw exceptions::AddServiceException("Failed to add service '" + service_name + "': Service already exists"); + throw exceptions::AddServiceException("Service already exists"); } return parsed_service_name; } @@ -1127,6 +1123,8 @@ inline std::string ComponentInterface::create_output( ); } return parsed_signal_name; + } catch (const exceptions::AddSignalException&) { + throw; } catch (const std::exception& ex) { throw exceptions::AddSignalException(ex.what()); } diff --git a/source/modulo_components/include/modulo_components/exceptions/AddServiceException.hpp b/source/modulo_components/include/modulo_components/exceptions/AddServiceException.hpp index 7f2a861a2..ac3c56939 100644 --- a/source/modulo_components/include/modulo_components/exceptions/AddServiceException.hpp +++ b/source/modulo_components/include/modulo_components/exceptions/AddServiceException.hpp @@ -11,6 +11,6 @@ namespace modulo_components::exceptions { */ class AddServiceException : public ComponentException { public: - explicit AddServiceException(const std::string& msg) : ComponentException(msg) {}; + explicit AddServiceException(const std::string& msg) : ComponentException("AddServiceException", msg) {} }; }// namespace modulo_components::exceptions diff --git a/source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp b/source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp index 5f3ec9b43..a4329181e 100644 --- a/source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp +++ b/source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp @@ -11,6 +11,6 @@ namespace modulo_components::exceptions { */ class AddSignalException : public ComponentException { public: - explicit AddSignalException(const std::string& msg) : ComponentException(msg) {}; + explicit AddSignalException(const std::string& msg) : ComponentException("AddSignalException", msg) {} }; }// namespace modulo_components::exceptions diff --git a/source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp b/source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp index d4d208185..5eda405d1 100644 --- a/source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp +++ b/source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp @@ -16,6 +16,8 @@ namespace modulo_components::exceptions { */ class ComponentException : public std::runtime_error { public: - explicit ComponentException(const std::string& msg) : std::runtime_error(msg) {}; + explicit ComponentException(const std::string& msg) : ComponentException("ComponentException", msg) {}; +protected: + ComponentException(const std::string& prefix, const std::string& msg) : std::runtime_error(prefix + ": " + msg) {} }; }// namespace modulo_components::exceptions diff --git a/source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp b/source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp index fd6bd0a87..19d127d6b 100644 --- a/source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp +++ b/source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp @@ -12,6 +12,7 @@ namespace modulo_components::exceptions { */ class ComponentParameterException : public ComponentException { public: - explicit ComponentParameterException(const std::string& msg) : ComponentException(msg) {}; + explicit ComponentParameterException(const std::string& msg) : + ComponentException("ComponentParameterException", msg) {} }; }// namespace modulo_components::exceptions diff --git a/source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp b/source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp index 2ea95ffa3..7c1cd19c3 100644 --- a/source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp +++ b/source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp @@ -12,6 +12,6 @@ namespace modulo_components::exceptions { */ class LookupTransformException : public ComponentException { public: - explicit LookupTransformException(const std::string& msg) : ComponentException(msg) {}; + explicit LookupTransformException(const std::string& msg) : ComponentException("LookupTransformException", msg) {} }; }// namespace modulo_components::exceptions diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 8ce20440f..3a78fe113 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -353,10 +353,12 @@ def _create_output(self, signal_name: str, data: str, message_type: MsgT, clprot translator = partial(modulo_writers.write_clproto_message, clproto_message_type=clproto_message_type) else: - raise AddSignalError("The provided message type is not supported to create a component output") + raise AddSignalError("The provided message type is not supported to create a component output.") self._outputs[parsed_signal_name] = {"attribute": data, "message_type": message_type, "translator": translator} return parsed_signal_name + except AddSignalError: + raise except Exception as e: raise AddSignalError(f"{e}") @@ -390,9 +392,9 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag try: parsed_signal_name = parse_topic_name(signal_name) if not parsed_signal_name: - raise AddSignalError(f"Failed to add input '{signal_name}': Parsed signal name is empty.") + raise AddSignalError(f"Parsed signal name is empty.") if parsed_signal_name in self._inputs.keys(): - raise AddSignalError(f"Failed to add input '{parsed_signal_name}': Input already exists") + raise AddSignalError(f"Input already exists.") topic_name = default_topic if default_topic else "~/" + parsed_signal_name parameter_name = parsed_signal_name + "_topic" if self.has_parameter(parameter_name) and self.get_parameter(parameter_name).is_empty(): @@ -420,7 +422,7 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag reader=modulo_readers.read_clproto_message), self._qos) else: - raise TypeError("The provided message type is not supported to create a component input") + raise TypeError("The provided message type is not supported to create a component input.") else: raise TypeError("Provide either a string containing the name of an attribute or a callable.") except Exception as e: @@ -465,9 +467,9 @@ def callback_wrapper(request, response, cb): try: parsed_service_name = parse_topic_name(service_name) if not parsed_service_name: - raise AddServiceError(f"Failed to add service '{service_name}': Parsed service name is empty.") + raise AddServiceError(f"Parsed service name is empty.") if parsed_service_name in self._services_dict.keys(): - raise AddServiceError(f"Failed to add service '{service_name}': Service already exists") + raise AddServiceError(f"Service already exists.") signature = inspect.signature(callback) if len(signature.parameters) == 0: self.get_logger().error(f"Adding empty service '{parsed_service_name}'") diff --git a/source/modulo_components/modulo_components/exceptions/component_exceptions.py b/source/modulo_components/modulo_components/exceptions/component_exceptions.py index 5aafd2f42..d70d35567 100644 --- a/source/modulo_components/modulo_components/exceptions/component_exceptions.py +++ b/source/modulo_components/modulo_components/exceptions/component_exceptions.py @@ -3,8 +3,8 @@ class ComponentError(Exception): A base class for all component exceptions. """ - def __init__(self, message: str): - super().__init__(message) + def __init__(self, message: str, prefix="ComponentError"): + super().__init__(f"{prefix}: {message}") class AddServiceError(ComponentError): @@ -14,7 +14,7 @@ class AddServiceError(ComponentError): """ def __init__(self, message: str): - super().__init__(message) + super().__init__(message, "AddServiceError") class AddSignalError(ComponentError): @@ -24,7 +24,7 @@ class AddSignalError(ComponentError): """ def __init__(self, message: str): - super().__init__(message) + super().__init__(message, "AddSignalError") class ComponentParameterError(ComponentError): @@ -34,7 +34,7 @@ class ComponentParameterError(ComponentError): """ def __init__(self, message: str): - super().__init__(message) + super().__init__(message, "ComponentParameterError") class LookupTransformError(ComponentError): @@ -44,4 +44,4 @@ class LookupTransformError(ComponentError): """ def __init__(self, message: str): - super().__init__(message) + super().__init__(message, "LookupTransformError") diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 4d341e9cd..72c5e825f 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.5 + 2.0.6 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_core/modulo_core/exceptions/core_exceptions.py b/source/modulo_core/modulo_core/exceptions/core_exceptions.py index b6fb3155f..e481bcd23 100644 --- a/source/modulo_core/modulo_core/exceptions/core_exceptions.py +++ b/source/modulo_core/modulo_core/exceptions/core_exceptions.py @@ -3,8 +3,8 @@ class CoreError(Exception): A base class for all core exceptions. """ - def __init__(self, message: str): - super().__init__(message) + def __init__(self, message: str, prefix="CoreError"): + super().__init__(f"{prefix}: {message}") class MessageTranslationError(CoreError): @@ -13,7 +13,7 @@ class MessageTranslationError(CoreError): """ def __init__(self, message: str): - super().__init__(message) + super().__init__(message, "MessageTranslationError") class ParameterTranslationError(CoreError): @@ -24,4 +24,4 @@ class ParameterTranslationError(CoreError): """ def __init__(self, message: str): - super().__init__(message) + super().__init__(message, "ParameterTranslationError") diff --git a/source/modulo_core/modulo_core/translators/message_readers.py b/source/modulo_core/modulo_core/translators/message_readers.py index 4308faeea..44bca70c9 100644 --- a/source/modulo_core/modulo_core/translators/message_readers.py +++ b/source/modulo_core/modulo_core/translators/message_readers.py @@ -75,6 +75,8 @@ def read_message(state: StateT, message: MsgT) -> StateT: raise MessageTranslationError(f"{e}") else: raise MessageTranslationError("The provided combination of state type and message type is not supported") + except MessageTranslationError: + raise except Exception as e: raise MessageTranslationError(f"{e}") return state @@ -104,7 +106,7 @@ def read_stamped_message(state: StateT, message: MsgT) -> StateT: else: raise MessageTranslationError("The provided combination of state type and message type is not supported") except MessageTranslationError as e: - raise MessageTranslationError(f"{e}") + raise state.set_reference_frame(message.header.frame_id) return state diff --git a/source/modulo_core/modulo_core/translators/message_writers.py b/source/modulo_core/modulo_core/translators/message_writers.py index 119dbf851..eda25fd9b 100644 --- a/source/modulo_core/modulo_core/translators/message_writers.py +++ b/source/modulo_core/modulo_core/translators/message_writers.py @@ -83,6 +83,8 @@ def write_message(message: MsgT, state: StateT): message.effort = state.get_torques().tolist() else: raise MessageTranslationError("The provided combination of state type and message type is not supported") + except MessageTranslationError: + raise except Exception as e: raise MessageTranslationError(f"{e}") @@ -112,6 +114,8 @@ def write_stamped_message(message: MsgT, state: StateT, time: rclpy.time.Time): raise MessageTranslationError("The provided combination of state type and message type is not supported") message.header.stamp = time.to_msg() message.header.frame_id = state.get_reference_frame() + except MessageTranslationError: + raise except Exception as e: raise MessageTranslationError(f"{e}") @@ -139,5 +143,7 @@ def write_clproto_message(message: EncodedState, state: StateT, clproto_message_ raise MessageTranslationError("This state type is not supported.") try: message.data = clproto.encode(state, clproto_message_type) + except MessageTranslationError: + raise except Exception as e: raise MessageTranslationError(f"{e}") diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 223bcb2bb..7ac05056a 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.5 + 2.0.6 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From c7d4d8c7fb87d2195e64a1344790c65263505c6a Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Tue, 23 Aug 2022 16:36:33 +0200 Subject: [PATCH 08/20] Rename all frame_name parameters to frame (#123) * Rename all frame_name parameters to frame * All parameter names that were previously a derivative of `frame_name` (e.g. 'reference_frame_name`) have the `_name` suffix removed for brevity. * 2.0.6 -> 2.0.7 --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- source/modulo_component_interfaces/package.xml | 2 +- .../include/modulo_components/ComponentInterface.hpp | 12 ++++++------ .../modulo_components/component_interface.py | 10 +++++----- source/modulo_components/package.xml | 2 +- source/modulo_core/package.xml | 2 +- 8 files changed, 17 insertions(+), 16 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 74b36f09e..16609ee62 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,7 @@ Release Versions: - Add static tf broadcaster (#120) - Use prefix for all exceptions (#121) - Accept list of transforms in send_transform (#122) +- Rename all frame_name parameters to frame (#123) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 157e54f3e..f1547e6d1 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.6 +2.0.7 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 942879896..84a796d80 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.0.6 +PROJECT_NUMBER = 2.0.7 # 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/package.xml b/source/modulo_component_interfaces/package.xml index a66654813..e85f348c7 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.6 + 2.0.7 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 30623f675..967212689 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -348,8 +348,8 @@ class ComponentInterface : public NodeT { /** * @brief Look up a transform from TF. - * @param frame_name The desired frame of the transform - * @param reference_frame_name The desired reference frame of the transform + * @param frame The desired frame of the transform + * @param reference_frame The desired reference frame of the transform * @param time_point The time at which the value of the transform is desired (default: 0, will get the latest) * @param duration How long to block the lookup call before failing * @throws modulo_components::exceptions::LookupTransformException if TF buffer/listener are unconfigured or @@ -357,7 +357,7 @@ class ComponentInterface : public NodeT { * @return If it exists, the requested transform */ [[nodiscard]] state_representation::CartesianPose lookup_transform( - const std::string& frame_name, const std::string& reference_frame_name = "world", + const std::string& frame, const std::string& reference_frame = "world", const tf2::TimePoint& time_point = tf2::TimePoint(std::chrono::microseconds(0)), const tf2::Duration& duration = tf2::Duration(std::chrono::microseconds(10))); @@ -1030,15 +1030,15 @@ inline void ComponentInterface::publish_transforms( template inline state_representation::CartesianPose ComponentInterface::lookup_transform( - const std::string& frame_name, const std::string& reference_frame_name, const tf2::TimePoint& time_point, + const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point, const tf2::Duration& duration ) { if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { throw exceptions::LookupTransformException("Failed to lookup transform: To TF buffer / listener configured."); } try { - state_representation::CartesianPose result(frame_name, reference_frame_name); - auto transform = this->tf_buffer_->lookupTransform(reference_frame_name, frame_name, time_point, duration); + state_representation::CartesianPose result(frame, reference_frame); + auto transform = this->tf_buffer_->lookupTransform(reference_frame, frame, time_point, duration); modulo_core::translators::read_message(result, transform); return result; } catch (const tf2::TransformException& ex) { diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 3a78fe113..c71c3410f 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -546,13 +546,13 @@ def send_static_transform(self, transform: sr.CartesianPose): """ self.send_static_transforms([transform]) - def lookup_transform(self, frame_name: str, reference_frame_name="world", time_point=Time(), + def lookup_transform(self, frame: str, reference_frame="world", time_point=Time(), duration=Duration(nanoseconds=1e4)) -> sr.CartesianPose: """ Look up a transform from TF. - :param frame_name: The desired frame of the transform - :param reference_frame_name: The desired reference frame of the transform + :param frame: The desired frame of the transform + :param reference_frame: The desired reference frame of the transform :param time_point: The time at which the value of the transform is desired (default: 0, will get the latest) :param duration: How long to block the lookup call before failing :return: If it exists, the requested transform @@ -560,8 +560,8 @@ def lookup_transform(self, frame_name: str, reference_frame_name="world", time_p if not self.__tf_buffer or not self.__tf_listener: raise LookupTransformError("Failed to lookup transform: To TF buffer / listener configured.") try: - result = sr.CartesianPose(frame_name, reference_frame_name) - transform = self.__tf_buffer.lookup_transform(reference_frame_name, frame_name, time_point, duration) + result = sr.CartesianPose(frame, reference_frame) + transform = self.__tf_buffer.lookup_transform(reference_frame, frame, time_point, duration) modulo_readers.read_stamped_message(result, transform) return result except TransformException as e: diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 72c5e825f..623f20eb9 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.6 + 2.0.7 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 7ac05056a..03b35b550 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.6 + 2.0.7 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From 0f74e8c45d4fefa0a2dd25e546c625392334ebec Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Thu, 25 Aug 2022 18:30:28 +0200 Subject: [PATCH 09/20] Check for nullptr when adding signal (#128) * Assert that the data pointer given to a signal is not null - if it is null, the MessagePair read / write would fail and the signal would be unusable * Improve consistency between input and output creating functions * 2.0.7 -> 2.0.8 --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- .../modulo_component_interfaces/package.xml | 2 +- .../modulo_components/ComponentInterface.hpp | 45 ++++++++++++------- source/modulo_components/package.xml | 2 +- source/modulo_core/package.xml | 2 +- 7 files changed, 36 insertions(+), 20 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 16609ee62..a44edbcc2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,7 @@ Release Versions: - Use prefix for all exceptions (#121) - Accept list of transforms in send_transform (#122) - Rename all frame_name parameters to frame (#123) +- Check that the data pointer is not null when adding a signal (#128) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index f1547e6d1..815e68dd2 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.7 +2.0.8 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 84a796d80..2e0c3a4ba 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.0.7 +PROJECT_NUMBER = 2.0.8 # 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/package.xml b/source/modulo_component_interfaces/package.xml index e85f348c7..01414706f 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.7 + 2.0.8 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 967212689..84e1ba6ef 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -750,10 +750,12 @@ template inline std::string ComponentInterface::validate_input_signal_name(const std::string& signal_name) { std::string parsed_signal_name = utilities::parse_topic_name(signal_name); if (parsed_signal_name.empty()) { - throw exceptions::AddSignalException("Parsed signal name is empty."); + throw exceptions::AddSignalException( + "The parsed signal name for input '" + signal_name + + "' is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_])."); } if (this->inputs_.find(parsed_signal_name) != this->inputs_.cend()) { - throw exceptions::AddSignalException("Input already exists"); + throw exceptions::AddSignalException("Input with name '" + parsed_signal_name + "' already exists."); } return parsed_signal_name; } @@ -767,18 +769,22 @@ inline void ComponentInterface::add_input( using namespace modulo_core::communication; try { std::string parsed_signal_name = this->validate_input_signal_name(signal_name); + if (data == nullptr) { + throw modulo_core::exceptions::NullPointerException( + "Invalid data pointer for input '" + parsed_signal_name + "'."); + } std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; auto parameter_name = parsed_signal_name + "_topic"; if (NodeT::has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { this->set_parameter_value(parameter_name, topic_name); } else { this->add_parameter( - parameter_name, topic_name, "Input topic name of signal '" + parsed_signal_name + "'", fixed_topic + parameter_name, topic_name, "Signal topic name of input '" + parsed_signal_name + "'", fixed_topic ); } - topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); + topic_name = this->get_parameter_value(parameter_name); RCLCPP_DEBUG_STREAM(this->get_logger(), - "Adding input '" << signal_name << "' with topic name '" << topic_name << "'."); + "Adding input '" << parsed_signal_name << "' with topic name '" << topic_name << "'."); auto message_pair = make_shared_message_pair(data, this->get_clock()); std::shared_ptr subscription_interface; switch (message_pair->get_type()) { @@ -842,13 +848,17 @@ inline void ComponentInterface::add_input( try { std::string parsed_signal_name = this->validate_input_signal_name(signal_name); std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; - this->add_parameter( - parsed_signal_name + "_topic", topic_name, "Output topic name of signal '" + parsed_signal_name + "'", - fixed_topic - ); - topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); + auto parameter_name = parsed_signal_name + "_topic"; + if (NodeT::has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { + this->set_parameter_value(parameter_name, topic_name); + } else { + this->add_parameter( + parameter_name, topic_name, "Signal topic name of input '" + parsed_signal_name + "'", fixed_topic + ); + } + topic_name = this->get_parameter_value(parameter_name); RCLCPP_DEBUG_STREAM(this->get_logger(), - "Adding input '" << signal_name << "' with topic name '" << topic_name << "'."); + "Adding input '" << parsed_signal_name << "' with topic name '" << topic_name << "'."); auto subscription = NodeT::template create_subscription(topic_name, this->qos_, callback); auto subscription_interface = std::make_shared>()->create_subscription_interface(subscription); @@ -1100,26 +1110,31 @@ inline std::string ComponentInterface::create_output( auto parsed_signal_name = utilities::parse_topic_name(signal_name); if (parsed_signal_name.empty()) { throw exceptions::AddSignalException( - "The parsed signal name for signal '" + signal_name + "The parsed signal name for output '" + signal_name + "'is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_])." ); } + if (data == nullptr) { + throw modulo_core::exceptions::NullPointerException( + "Invalid data pointer for output '" + parsed_signal_name + "'."); + } RCLCPP_DEBUG_STREAM(this->get_logger(), "Creating output '" << parsed_signal_name << "' (provided signal name was '" << signal_name - << "'."); + << "')."); if (this->outputs_.find(parsed_signal_name) != this->outputs_.end()) { - throw exceptions::AddSignalException("Output with name '" + signal_name + "' already exists."); + throw exceptions::AddSignalException("Output with name '" + parsed_signal_name + "' already exists."); } auto message_pair = make_shared_message_pair(data, this->get_clock()); this->outputs_.insert_or_assign( parsed_signal_name, std::make_shared(this->publisher_type_, message_pair)); + std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; auto parameter_name = parsed_signal_name + "_topic"; if (NodeT::has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { this->set_parameter_value(parameter_name, topic_name); } else { this->add_parameter( - parameter_name, topic_name, "Output topic name of signal '" + parsed_signal_name + "'", fixed_topic + parameter_name, topic_name, "Signal topic name of output '" + parsed_signal_name + "'", fixed_topic ); } return parsed_signal_name; diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 623f20eb9..611a31314 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.7 + 2.0.8 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 03b35b550..3bcf86dfe 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.7 + 2.0.8 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From ef81f5ff127337bb4a9b6ca0c0f591f65799c5e0 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Sun, 28 Aug 2022 19:40:00 +0200 Subject: [PATCH 10/20] Rename validate parameter function (#130) * Rename validate parameter callback and validate period value (#126) * Wrap validate parameter function to have additional check for period * Small fixes * 2.0.7 -> 2.0.8 * Update CHANGELOG * Refactor validate_parameter to on_validate_parameter_callback * Replace validate_parameter with on_validate_parameter_callback * Fix period value checks * Rename PR * Add double underscore to private function (#127) * Remove underscore from validate parameter function * Update CHANGELOG * Update PR title * 2.0.8 -> 2.0.9 --- CHANGELOG.md | 2 ++ VERSION | 2 +- doxygen/doxygen.conf | 2 +- .../modulo_component_interfaces/package.xml | 2 +- source/modulo_components/README.md | 9 +++--- .../include/modulo_components/Component.hpp | 5 +-- .../modulo_components/ComponentInterface.hpp | 26 +++++++++++++++- .../modulo_components/LifecycleComponent.hpp | 4 +-- .../modulo_components/component_interface.py | 31 ++++++++++++++----- source/modulo_components/package.xml | 2 +- .../component_public_interfaces.hpp | 2 +- ...t_component_interface_empty_parameters.cpp | 3 +- ...st_component_interface_empty_parameters.py | 2 +- .../test_component_interface_parameters.py | 2 +- source/modulo_core/package.xml | 2 +- 15 files changed, 70 insertions(+), 26 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a44edbcc2..a4c1a4f4f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ Release Versions: - Accept list of transforms in send_transform (#122) - Rename all frame_name parameters to frame (#123) - Check that the data pointer is not null when adding a signal (#128) +- Rename validate parameter callback and validate period value (#126) +- Add double underscore to private function (#127) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 815e68dd2..09843e3be 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.8 +2.0.9 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 2e0c3a4ba..3355ad137 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.0.8 +PROJECT_NUMBER = 2.0.9 # 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/package.xml b/source/modulo_component_interfaces/package.xml index 01414706f..ddfb908e2 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.8 + 2.0.9 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/README.md b/source/modulo_components/README.md index 0e6ab709b..48bc2b0f2 100644 --- a/source/modulo_components/README.md +++ b/source/modulo_components/README.md @@ -13,8 +13,9 @@ externally triggered operations. Examples of triggered behavior include providin or publishing outputs on a periodic timer. One-shot behaviors may include interacting with the filesystem or publishing a predefined sequence of outputs. -Developers should override `validate_parameter()` if any parameters are added and `on_execute_callback()` to implement -any one-shot behavior. In the latter case, `execute()` should be invoked at the end of the derived constructor. +Developers should override `on_validate_parameter_callback()` if any parameters are added and `on_execute_callback()` +to implement any one-shot behavior. In the latter case, `execute()` should be invoked at the end of the derived +constructor. ## LifecycleComponent @@ -26,8 +27,8 @@ different behaviors based on their state and on state transitions. An example of signal component that requires configuration steps to determine which inputs to register and subsequently should publish outputs only when the component is activated. -Developers should override `validate_parameter()` if any parameters are added. In addition, the following state -transition callbacks should be overridden whenever custom transition behavior is needed: +Developers should override `on_validate_parameter_callback()` if any parameters are added. In addition, the following +state transition callbacks should be overridden whenever custom transition behavior is needed: - `on_configure_callback()` - `on_activate_callback()` - `on_deactivate_callback()` diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 9139f9f69..3966a8609 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -18,8 +18,9 @@ namespace modulo_components { * externally triggered operations. Examples of triggered behavior include providing a service, processing signals * or publishing outputs on a periodic timer. One-shot behaviors may include interacting with the filesystem or * publishing a predefined sequence of outputs. - * Developers should override validate_parameter() if any parameters are added and on_execute_callback() to implement - * any one-shot behavior. In the latter case, execute() should be invoked at the end of the derived constructor. + * Developers should override on_validate_parameter_callback() if any parameters are added and on_execute_callback() + * to implement any one-shot behavior. In the latter case, execute() should be invoked at the end of the derived + * constructor. * @see LifecycleComponent for a state-based composition alternative */ class Component : public ComponentInterface { diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 84e1ba6ef..c1a1edcfd 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -165,7 +165,8 @@ class ComponentInterface : public NodeT { * @param parameter A ParameterInterface pointer to a Parameter instance * @return The validation result */ - virtual bool validate_parameter(const std::shared_ptr& parameter); + virtual bool + on_validate_parameter_callback(const std::shared_ptr& parameter); /** * @brief Add a predicate to the map of predicates. @@ -413,6 +414,15 @@ class ComponentInterface : public NodeT { */ rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(const std::vector& parameters); + /** + * @brief Parameter validation function + * @details This validates the period and calls the on_validate_parameter_callback function of the derived Component + * classes. + * @param parameter A ParameterInterface pointer to a Parameter instance + * @return The validation result + */ + bool validate_parameter(const std::shared_ptr& parameter); + /** * @brief Add a predicate to the map of predicates. * @param name The name of the predicate @@ -591,6 +601,20 @@ inline void ComponentInterface::set_parameter_value(const std::string& na template inline bool ComponentInterface::validate_parameter( + const std::shared_ptr& parameter +) { + if (parameter->get_name() == "period") { + auto value = parameter->get_parameter_value(); + if (value <= 0.0 || !std::isfinite(value)) { + RCLCPP_ERROR(this->get_logger(), "Value for parameter 'period' has to be a positive finite number."); + return false; + } + } + return this->on_validate_parameter_callback(parameter); +} + +template +inline bool ComponentInterface::on_validate_parameter_callback( const std::shared_ptr& ) { return true; diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index f16d5ba18..c39a77a69 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -16,8 +16,8 @@ namespace modulo_components { * different behaviors based on their state and on state transitions. An example of state-based behaviour is a * signal component that requires configuration steps to determine which inputs to register and subsequently should * publish outputs only when the component is activated. - * Developers should override validate_parameter() if any parameters are added. In addition, the following state - * transition callbacks should be overridden whenever custom transition behavior is needed: + * Developers should override on_validate_parameter_callback() if any parameters are added. In addition, the following + * state transition callbacks should be overridden whenever custom transition behavior is needed: * - on_configure_callback() * - on_activate_callback() * - on_deactivate_callback() diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index c71c3410f..211b2cf66 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -129,11 +129,11 @@ def get_parameter(self, name: str) -> Union[sr.Parameter, Parameter]: if "rclpy" in co_filename: return Node.get_parameter(self, name) else: - return self._get_component_parameter(name) + return self.__get_component_parameter(name) except Exception as e: raise ComponentParameterError(f"Failed to get parameter '{name}': {e}") - def _get_component_parameter(self, name: str) -> sr.Parameter: + def __get_component_parameter(self, name: str) -> sr.Parameter: """ Get the parameter from the parameter dictionary by its name. @@ -159,7 +159,7 @@ def get_parameter_value(self, name: str) -> T: :raises ComponentParameterError if the parameter does not exist :return: The value of the parameter, if the parameter exists """ - return self._get_component_parameter(name).get_value() + return self.__get_component_parameter(name).get_value() def set_parameter_value(self, name: str, value: T, parameter_type: sr.ParameterType) -> None: """ @@ -180,7 +180,22 @@ def set_parameter_value(self, name: str, value: T, parameter_type: sr.ParameterT self.get_logger().error(f"Failed to set parameter value of parameter '{name}': {e}", throttle_duration_sec=1.0) - def _validate_parameter(self, parameter: sr.Parameter) -> bool: + def __validate_parameter(self, parameter: sr.Parameter) -> bool: + """ + Parameter validation wrapper that validates the period and calls the on_validate_parameter_callback function of + the derived Component classes. + + :param parameter: The parameter to be validated + :return: The validation result + """ + if parameter.get_name() == "period": + value = parameter.get_value() + if value <= 0.0 or value > sys.float_info.max: + self.get_logger().error("Value for parameter 'period' has to be a positive finite number.") + return False + return self.on_validate_parameter_callback(parameter) + + def on_validate_parameter_callback(self, parameter: sr.Parameter) -> bool: """ Parameter validation function to be redefined by derived Component classes. This method is automatically invoked whenever the ROS interface tried to modify a parameter. If the validation returns True, the updated parameter @@ -202,9 +217,9 @@ def __on_set_parameters_callback(self, ros_parameters: List[Parameter]) -> SetPa result = SetParametersResult(successful=True) for ros_param in ros_parameters: try: - parameter = self._get_component_parameter(ros_param.name) + parameter = self.__get_component_parameter(ros_param.name) new_parameter = read_parameter_const(ros_param, parameter) - if not self._validate_parameter(new_parameter): + if not self.__validate_parameter(new_parameter): result.successful = False result.reason = f"Validation of parameter '{ros_param.name}' returned false!" else: @@ -309,7 +324,7 @@ def trigger(self, trigger_name: str): :param trigger_name: The name of the trigger """ - if not trigger_name in self._triggers.keys(): + if trigger_name not in self._triggers.keys(): self.get_logger().error(f"Failed to trigger: could not find trigger with name '{trigger_name}'.") return self._triggers[trigger_name] = True @@ -646,7 +661,7 @@ def _evaluate_periodic_callbacks(self): self.get_logger().error(f"Failed to evaluate periodic function callback '{name}': {e}", throttle_duration_sec=1.0) - def __publish_transforms(self, transforms: Iterable[sr.CartesianPose], static: Bool=False): + def __publish_transforms(self, transforms: Iterable[sr.CartesianPose], static=False): """ Send a list of transforms to TF using the normal or static tf broadcaster diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 611a31314..ffbc066dd 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.8 + 2.0.9 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 0fb676e75..98ff6323c 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -46,7 +46,7 @@ class ComponentInterfacePublicInterface : public ComponentInterface { using ComponentInterface::get_qos; using ComponentInterface::set_qos; - bool validate_parameter(const std::shared_ptr&) override { + bool on_validate_parameter_callback(const std::shared_ptr&) override { validate_parameter_was_called = true; return validate_parameter_return_value; } diff --git a/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp index ca456e589..f168e6d13 100644 --- a/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp @@ -24,7 +24,8 @@ class EmptyParameterInterface : public ComponentInterfacePublicInterface }; private: - bool validate_parameter(const std::shared_ptr& parameter) override { + bool + on_validate_parameter_callback(const std::shared_ptr& parameter) override { if (parameter->get_name() == "name") { if (parameter->is_empty()) { return this->allow_empty_; diff --git a/source/modulo_components/test/python/test_component_interface_empty_parameters.py b/source/modulo_components/test/python/test_component_interface_empty_parameters.py index 0dc77785f..c1cf9bf52 100644 --- a/source/modulo_components/test/python/test_component_interface_empty_parameters.py +++ b/source/modulo_components/test/python/test_component_interface_empty_parameters.py @@ -25,7 +25,7 @@ def get_ros_parameter(self, name: str) -> rclpy.Parameter: def set_ros_parameter(self, param: rclpy.Parameter) -> SetParametersResult: return rclpy.node.Node.set_parameters(self, [param])[0] - def _validate_parameter(self, parameter: sr.Parameter) -> bool: + def on_validate_parameter_callback(self, parameter: sr.Parameter) -> bool: if parameter.get_name() == "name": if parameter.is_empty(): return self._allow_empty diff --git a/source/modulo_components/test/python/test_component_interface_parameters.py b/source/modulo_components/test/python/test_component_interface_parameters.py index ad038901f..93fadc10e 100644 --- a/source/modulo_components/test/python/test_component_interface_parameters.py +++ b/source/modulo_components/test/python/test_component_interface_parameters.py @@ -20,7 +20,7 @@ def get_ros_parameter(self, name: str) -> rclpy.Parameter: def set_ros_parameter(self, param: rclpy.Parameter) -> SetParametersResult: return rclpy.node.Node.set_parameters(self, [param])[0] - def _validate_parameter(self, parameter: sr.Parameter) -> bool: + def on_validate_parameter_callback(self, parameter: sr.Parameter) -> bool: self.validate_was_called = True return self.validation_return_value diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 3bcf86dfe..df6a535eb 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.8 + 2.0.9 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From 74c107b2c76f9d4a16f8ce174e32de3b83e40ea9 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Mon, 29 Aug 2022 13:49:09 +0200 Subject: [PATCH 11/20] Improve error messages and logs further (#131) * Improve error messages and logs further * New file in gitignore from CLion * Update CHANGELOG * 2.0.9 -> 2.0.10 * Change log level for existing parameter warning --- .gitignore | 1 + CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- .../modulo_component_interfaces/package.xml | 2 +- .../modulo_components/ComponentInterface.hpp | 10 ++++--- .../modulo_components/component.py | 2 +- .../modulo_components/component_interface.py | 26 ++++++++++--------- .../modulo_components/lifecycle_component.py | 3 ++- source/modulo_components/package.xml | 2 +- source/modulo_core/package.xml | 2 +- 11 files changed, 30 insertions(+), 23 deletions(-) diff --git a/.gitignore b/.gitignore index cb2c31758..603364d8b 100644 --- a/.gitignore +++ b/.gitignore @@ -4,5 +4,6 @@ .idea cmake-build-* +./fileList.txt doxygen/docs/html diff --git a/CHANGELOG.md b/CHANGELOG.md index a4c1a4f4f..9bea594b1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,7 @@ Release Versions: - Check that the data pointer is not null when adding a signal (#128) - Rename validate parameter callback and validate period value (#126) - Add double underscore to private function (#127) +- Improve error messages and logs further (#131) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 09843e3be..0a692060f 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.9 +2.0.10 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 3355ad137..795b2a060 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.0.9 +PROJECT_NUMBER = 2.0.10 # 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/package.xml b/source/modulo_component_interfaces/package.xml index ddfb908e2..967793de5 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.9 + 2.0.10 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index c1a1edcfd..76ec80e48 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -569,7 +569,7 @@ inline void ComponentInterface::add_parameter( throw exceptions::ComponentParameterException("Failed to add parameter: " + std::string(ex.what())); } } else { - RCLCPP_WARN_STREAM(this->get_logger(), "Parameter '" << parameter->get_name() << "' already exists."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Parameter '" << parameter->get_name() << "' already exists."); } } @@ -672,7 +672,7 @@ inline void ComponentInterface::add_variant_predicate( return; } if (this->predicates_.find(name) != this->predicates_.end()) { - RCLCPP_WARN_STREAM(this->get_logger(), "Predicate '" << name << "' already exists, overwriting."); + 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( @@ -896,11 +896,13 @@ template inline std::string ComponentInterface::validate_service_name(const std::string& service_name) { std::string parsed_service_name = utilities::parse_topic_name(service_name); if (parsed_service_name.empty()) { - throw exceptions::AddServiceException("Parsed service name is empty."); + throw exceptions::AddServiceException( + "The parsed service name for service '" + service_name + + "' is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_])."); } if (this->empty_services_.find(parsed_service_name) != this->empty_services_.cend() || this->string_services_.find(parsed_service_name) != this->string_services_.cend()) { - throw exceptions::AddServiceException("Service already exists"); + throw exceptions::AddServiceException("Service with name '" + parsed_service_name + "' already exists."); } return parsed_service_name; } diff --git a/source/modulo_components/modulo_components/component.py b/source/modulo_components/modulo_components/component.py index fe85768f8..1dbfb9b06 100644 --- a/source/modulo_components/modulo_components/component.py +++ b/source/modulo_components/modulo_components/component.py @@ -90,4 +90,4 @@ def add_output(self, signal_name: str, data: str, message_type: MsgT, publisher = self.create_publisher(message_type, topic_name, self._qos) self._outputs[parsed_signal_name]["publisher"] = publisher except Exception as e: - self.get_logger().error(f"Failed to add output '{signal_name}: {e}") + self.get_logger().error(f"Failed to add output '{signal_name}': {e}") diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 211b2cf66..f6dd2dc6e 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -111,7 +111,7 @@ def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, r del self._parameter_dict[sr_parameter.get_name()] raise ComponentParameterError(f"Failed to add parameter: {e}") else: - self.get_logger().warn(f"Parameter '{sr_parameter.get_name()}' already exists.") + self.get_logger().debug(f"Parameter '{sr_parameter.get_name()}' already exists.") def get_parameter(self, name: str) -> Union[sr.Parameter, Parameter]: """ @@ -242,7 +242,7 @@ def add_predicate(self, name: str, value: Union[bool, Callable[[], bool]]): if not name: self.get_logger().error("Failed to add predicate: Provide a non empty string as a name.") if name in self._predicates.keys(): - self.get_logger().warn(f"Predicate {name} already exists, overwriting.") + 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, @@ -349,8 +349,8 @@ def _create_output(self, signal_name: str, data: str, message_type: MsgT, clprot raise AddSignalError(f"Provide a valid clproto message type for outputs of type EncodedState.") parsed_signal_name = parse_topic_name(signal_name) if not parsed_signal_name: - raise AddSignalError("The parsed signal name is empty. Provide a string with valid " - "characters for the signal name ([a-zA-Z0-9_]).") + raise AddSignalError(f"The parsed signal name for output '{signal_name}' is empty. Provide a " + f"string with valid characters for the signal name ([a-zA-Z0-9_]).") if parsed_signal_name in self._outputs.keys(): raise AddSignalError(f"Output with parsed name '{parsed_signal_name}' already exists.") topic_name = default_topic if default_topic else "~/" + parsed_signal_name @@ -359,7 +359,7 @@ def _create_output(self, signal_name: str, data: str, message_type: MsgT, clprot self.set_parameter_value(parameter_name, topic_name) else: self.add_parameter(sr.Parameter(parameter_name, topic_name, sr.ParameterType.STRING), - f"Output topic name of signal '{parsed_signal_name}'", fixed_topic) + f"Signal topic name of output '{parsed_signal_name}'", fixed_topic) translator = None if message_type == Bool or message_type == Float64 or \ message_type == Float64MultiArray or message_type == Int32 or message_type == String: @@ -407,16 +407,17 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag try: parsed_signal_name = parse_topic_name(signal_name) if not parsed_signal_name: - raise AddSignalError(f"Parsed signal name is empty.") + raise AddSignalError(f"The parsed signal name for input '{signal_name}' is empty. Provide a " + f"string with valid characters for the signal name ([a-zA-Z0-9_]).") if parsed_signal_name in self._inputs.keys(): - raise AddSignalError(f"Input already exists.") + raise AddSignalError(f"Input with name '{parsed_signal_name}' already exists.") topic_name = default_topic if default_topic else "~/" + parsed_signal_name parameter_name = parsed_signal_name + "_topic" if self.has_parameter(parameter_name) and self.get_parameter(parameter_name).is_empty(): self.set_parameter_value(parameter_name, topic_name) else: self.add_parameter(sr.Parameter(parameter_name, topic_name, sr.ParameterType.STRING), - f"Input topic name of signal '{parsed_signal_name}'", fixed_topic) + f"Signal topic name of input '{parsed_signal_name}'", fixed_topic) topic_name = self.get_parameter_value(parsed_signal_name + "_topic") self.get_logger().debug(f"Adding input '{parsed_signal_name}' with topic name '{topic_name}'.") if isinstance(subscription, Callable): @@ -482,15 +483,16 @@ def callback_wrapper(request, response, cb): try: parsed_service_name = parse_topic_name(service_name) if not parsed_service_name: - raise AddServiceError(f"Parsed service name is empty.") + raise AddServiceError(f"The parsed signal name for service {service_name} is empty. Provide a " + f"string with valid characters for the service name ([a-zA-Z0-9_]).") if parsed_service_name in self._services_dict.keys(): - raise AddServiceError(f"Service already exists.") + raise AddServiceError(f"Service with name '{parsed_service_name}' already exists.") signature = inspect.signature(callback) if len(signature.parameters) == 0: - self.get_logger().error(f"Adding empty service '{parsed_service_name}'") + self.get_logger().error(f"Adding empty service '{parsed_service_name}'.") service_type = EmptyTrigger else: - self.get_logger().debug(f"Adding string service '{parsed_service_name}'") + self.get_logger().debug(f"Adding string service '{parsed_service_name}'.") service_type = StringTrigger self._services_dict[parsed_service_name] = \ self.create_service(service_type, "~/" + parsed_service_name, diff --git a/source/modulo_components/modulo_components/lifecycle_component.py b/source/modulo_components/modulo_components/lifecycle_component.py index 43fb14ad7..9670e76c7 100644 --- a/source/modulo_components/modulo_components/lifecycle_component.py +++ b/source/modulo_components/modulo_components/lifecycle_component.py @@ -246,6 +246,7 @@ def add_output(self, signal_name: str, data: str, message_type: MsgT, try: parsed_signal_name = self._create_output(signal_name, data, message_type, clproto_message_type, default_topic, fixed_topic) - self.get_logger().debug(f"Adding output '{parsed_signal_name}.") + topic_name = self.get_parameter_value(parsed_signal_name + "_topic") + self.get_logger().debug(f"Adding output '{parsed_signal_name}' with topic name '{topic_name}'.") except AddSignalError as e: self.get_logger().error(f"Failed to add output '{signal_name}': {e}") diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index ffbc066dd..461fc9155 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.9 + 2.0.10 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index df6a535eb..11d349480 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.9 + 2.0.10 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From 8920f57176ef102a5ea3afd2be5a586ff257afb8 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Sun, 28 Aug 2022 18:25:53 +0200 Subject: [PATCH 12/20] User-defined callback on inputs (C++) (#124) * Insert a user callback into SubscriptionHandler * Add private class property and setter for user_callback_, a void function * Invoke user callback within the subscription callback, after reading the message * Move callback error handling to helper function to simplify implementation and reduce code duplication * Allow setting user callback directly when getting the subscription callback with a function overload * Implement user callback in component add_input * Resolve TODO by overloading add_input with a user_callback argument after the shared data pointer. This allows a void function to be executed after the message is read into the data (as opposed to the generic input callback which takes a raw message). The default add_input with a data pointer passes an empty user callback instead. * Update tests for adding inputs with user callback * Add tests on component level * Adjust docstrings Co-authored-by: Dominic Reber --- .../modulo_components/ComponentInterface.hpp | 38 +++++++++++--- .../communication_components.hpp | 50 +++++++++++++++++++ .../component_public_interfaces.hpp | 1 + .../test/cpp/test_component_communication.cpp | 36 +++++++++++++ .../test/cpp/test_component_interface.cpp | 33 ++++++++++-- ...test_lifecycle_component_communication.cpp | 36 +++++++++++++ .../communication/SubscriptionHandler.hpp | 49 +++++++++++++++++- .../src/communication/SubscriptionHandler.cpp | 36 ++++++------- .../test_modulo_core/communication_nodes.hpp | 9 ++-- 9 files changed, 251 insertions(+), 37 deletions(-) create mode 100644 source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp create mode 100644 source/modulo_components/test/cpp/test_component_communication.cpp create mode 100644 source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 76ec80e48..19ef398ba 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -229,14 +229,27 @@ class ComponentInterface : public NodeT { * @param default_topic If set, the default value for the topic name to use * @param fixed_topic If true, the topic name of the input signal is fixed */ - // TODO could be nice to add an optional callback here that would be executed from within the subscription callback - // in order to manipulate the data pointer upon reception of a message template void add_input( const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic = "", bool fixed_topic = false ); + /** + * @brief Add and configure an input signal of the component. + * @tparam DataT Type of the data pointer + * @param signal_name Name of the input signal + * @param data Data to receive on the input signal + * @param callback Callback function to trigger after receiving the input signal + * @param default_topic If set, the default value for the topic name to use + * @param fixed_topic If true, the topic name of the input signal is fixed + */ + template + void add_input( + const std::string& signal_name, const std::shared_ptr& data, const std::function& callback, + const std::string& default_topic = "", bool fixed_topic = false + ); + /** * @brief Add and configure an input signal of the component. * @tparam MsgT The ROS message type of the subscription @@ -789,6 +802,15 @@ template inline void ComponentInterface::add_input( const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic, bool fixed_topic +) { + this->add_input(signal_name, data, []{}, default_topic, fixed_topic); +} + +template +template +inline void ComponentInterface::add_input( + const std::string& signal_name, const std::shared_ptr& data, const std::function& user_callback, + const std::string& default_topic, bool fixed_topic ) { using namespace modulo_core::communication; try { @@ -815,14 +837,14 @@ inline void ComponentInterface::add_input( case MessageType::BOOL: { auto subscription_handler = std::make_shared>(message_pair); auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback()); + topic_name, this->qos_, subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } case MessageType::FLOAT64: { auto subscription_handler = std::make_shared>(message_pair); auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback()); + topic_name, this->qos_, subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } @@ -830,28 +852,28 @@ inline void ComponentInterface::add_input( auto subscription_handler = std::make_shared>(message_pair); auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback()); + topic_name, this->qos_, subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } case MessageType::INT32: { auto subscription_handler = std::make_shared>(message_pair); auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback()); + topic_name, this->qos_, subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } case MessageType::STRING: { auto subscription_handler = std::make_shared>(message_pair); auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback()); + topic_name, this->qos_, subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } case MessageType::ENCODED_STATE: { auto subscription_handler = std::make_shared>(message_pair); auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback()); + topic_name, this->qos_, subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp new file mode 100644 index 000000000..bc24cb0c7 --- /dev/null +++ b/source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include + +#include "modulo_components/LifecycleComponent.hpp" + +using namespace state_representation; +using namespace std::chrono_literals; + +namespace modulo_components { + +inline void add_configure_activate( + const std::shared_ptr& exec, + const std::shared_ptr& component +) { + exec->add_node(component->get_node_base_interface()); + component->configure(); + component->activate(); +} + +template +class MinimalOutput : public ComponentT { +public: + MinimalOutput( + const rclcpp::NodeOptions& node_options, const std::string& topic, const CartesianState& cartesian_state + ) : ComponentT(node_options, "minimal_output"), output_(std::make_shared(cartesian_state)) { + this->add_output("cartesian_state", this->output_, topic); + } + +private: + std::shared_ptr output_; +}; + +template +class MinimalInput : public ComponentT { +public: + MinimalInput(const rclcpp::NodeOptions& node_options, const std::string& topic) : + ComponentT(node_options, "minimal_input"), input(std::make_shared()) { + this->received_future = this->received_.get_future(); + this->add_input("cartesian_state", this->input, [this]() { this->received_.set_value(); }, topic); + } + + std::shared_ptr input; + std::shared_future received_future; + +private: + std::promise received_; +}; +}// namespace modulo_components diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 98ff6323c..7a24997f6 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -45,6 +45,7 @@ class ComponentInterfacePublicInterface : public ComponentInterface { using ComponentInterface::raise_error; using ComponentInterface::get_qos; using ComponentInterface::set_qos; + using ComponentInterface::get_clock; bool on_validate_parameter_callback(const std::shared_ptr&) override { validate_parameter_was_called = true; diff --git a/source/modulo_components/test/cpp/test_component_communication.cpp b/source/modulo_components/test/cpp/test_component_communication.cpp new file mode 100644 index 000000000..10973e920 --- /dev/null +++ b/source/modulo_components/test/cpp/test_component_communication.cpp @@ -0,0 +1,36 @@ +#include + +#include "modulo_components/Component.hpp" +#include "test_modulo_components/communication_components.hpp" + +using namespace modulo_components; + +class ComponentCommunicationTest : public ::testing::Test { +protected: + void SetUp() override { + rclcpp::init(0, nullptr); + exec_ = std::make_shared(); + } + + void TearDown() override { + rclcpp::shutdown(); + } + + std::shared_ptr exec_; +}; + +TEST_F(ComponentCommunicationTest, InputOutput) { + auto cartesian_state = state_representation::CartesianState::Random("test"); + auto input_node = std::make_shared>( + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.1)}), "/topic" + ); + auto output_node = std::make_shared>( + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.1)}), "/topic", cartesian_state + ); + this->exec_->add_node(input_node); + this->exec_->add_node(output_node); + this->exec_->template spin_until_future_complete(input_node->received_future, 500ms); + EXPECT_EQ(cartesian_state.get_name(), input_node->input->get_name()); + EXPECT_TRUE(cartesian_state.data().isApprox(input_node->input->data())); + this->exec_.reset(); +} diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index de107edf1..56f6e6eab 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -1,6 +1,7 @@ #include #include "modulo_core/EncodedState.hpp" +#include "modulo_core/translators/message_writers.hpp" #include "test_modulo_components/component_public_interfaces.hpp" namespace modulo_components { @@ -87,18 +88,42 @@ TYPED_TEST(ComponentInterfaceTest, AddInput) { EXPECT_EQ(this->component_->template get_parameter_value("test_13_topic"), "~/test_13"); EXPECT_NO_THROW(this->component_->template add_input( - "_tEsT_#1@5", [](const std::shared_ptr) {}, "/topic", true - )); + "_tEsT_#1@5", [](const std::shared_ptr) {}, "/topic", true)); EXPECT_FALSE(this->component_->inputs_.find("test_15") == this->component_->inputs_.end()); EXPECT_EQ(this->component_->template get_parameter_value("test_15_topic"), "/topic"); + // trying to add an input that already exists should fail this->component_->template add_input( - "test_13", [](const std::shared_ptr) {} - ); + "test_13", [](const std::shared_ptr) {}); EXPECT_EQ(this->component_->inputs_.at("test_13")->get_message_pair()->get_type(), modulo_core::communication::MessageType::BOOL); } +TYPED_TEST(ComponentInterfaceTest, AddInputWithUserCallback) { + auto state_data = std::make_shared("A"); + bool callback_triggered = false; + auto user_callback = [&callback_triggered] { + callback_triggered = true; + }; + + EXPECT_NO_THROW(this->component_->add_input("state", state_data, user_callback)); + EXPECT_FALSE(this->component_->inputs_.find("state") == this->component_->inputs_.end()); + auto callback = this->component_->inputs_.at("state")->template get_handler()->get_callback(); + state_representation::CartesianState new_state("B"); + auto message = std::make_shared(); + modulo_core::translators::write_message(*message, new_state, this->component_->get_clock()->now()); + EXPECT_STREQ(state_data->get_name().c_str(), "A"); + EXPECT_NO_THROW(callback(message)); + EXPECT_TRUE(callback_triggered); + EXPECT_STREQ(state_data->get_name().c_str(), "B"); + + // if the subscription callback fails (for example, the message is invalid), the user callback should be skipped + message->data.clear(); + callback_triggered = false; + EXPECT_NO_THROW(callback(message)); + EXPECT_FALSE(callback_triggered); +} + TYPED_TEST(ComponentInterfaceTest, AddService) { EXPECT_EQ(static_cast(this->component_->empty_services_.size()), 0); EXPECT_EQ(static_cast(this->component_->string_services_.size()), 0); diff --git a/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp b/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp new file mode 100644 index 000000000..78c6f8f63 --- /dev/null +++ b/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp @@ -0,0 +1,36 @@ +#include + +#include "modulo_components/LifecycleComponent.hpp" +#include "test_modulo_components/communication_components.hpp" + +using namespace modulo_components; + +class LifecycleComponentCommunicationTest : public ::testing::Test { +protected: + void SetUp() override { + rclcpp::init(0, nullptr); + exec_ = std::make_shared(); + } + + void TearDown() override { + rclcpp::shutdown(); + } + + std::shared_ptr exec_; +}; + +TEST_F(LifecycleComponentCommunicationTest, InputOutput) { + auto cartesian_state = state_representation::CartesianState::Random("test"); + auto input_node = std::make_shared>( + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.1)}), "/topic" + ); + auto output_node = std::make_shared>( + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.1)}), "/topic", cartesian_state + ); + add_configure_activate(this->exec_, input_node); + add_configure_activate(this->exec_, output_node); + this->exec_->template spin_until_future_complete(input_node->received_future, 500ms); + EXPECT_EQ(cartesian_state.get_name(), input_node->input->get_name()); + EXPECT_TRUE(cartesian_state.data().isApprox(input_node->input->data())); + this->exec_.reset(); +} diff --git a/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp b/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp index 80481ae07..5edb270b2 100644 --- a/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp @@ -26,17 +26,31 @@ class SubscriptionHandler : public SubscriptionInterface { /** * @brief Setter of the ROS subscription. + * @param subscription The ROS subscription * @throws modulo_core::exceptions::NullPointerException if the provided subscription pointer is null */ void set_subscription(const std::shared_ptr>& subscription); + /** + * @brief Setter of a user callback function to be executed after the subscription callback + * @param user_callback The ser callback function + */ + void set_user_callback(const std::function& user_callback); + /** * @brief Get a callback function that will be associated with the ROS subscription to receive and translate messages. */ std::function)> get_callback(); /** - * @brief Create an SubscriptionInterface pointer through an instance of a SubscriptionHandler by providing a ROS + * @brief Get a callback function that will be associated with the ROS subscription to receive and translate messages. + * @details This variant also takes a user callback function to execute after the message is received and translated. + * @param user_callback Void callback function for additional logic after the message is received and translated. + */ + std::function)> get_callback(const std::function& user_callback); + + /** + * @brief Create a SubscriptionInterface pointer through an instance of a SubscriptionHandler by providing a ROS * subscription. * @details This throws a NullPointerException if the ROS subscription is null. * @see SubscriptionHandler::set_subscription @@ -47,8 +61,14 @@ class SubscriptionHandler : public SubscriptionInterface { create_subscription_interface(const std::shared_ptr>& subscription); private: + /** + * @brief Handle exceptions caught during callback evaluation. + */ + void handle_callback_exceptions(); + std::shared_ptr> subscription_; ///< The pointer referring to the ROS subscription std::shared_ptr clock_; ///< ROS clock for throttling log + std::function user_callback_ = []{}; ///< User callback to be executed after the subscription callback }; template @@ -64,6 +84,18 @@ void SubscriptionHandler::set_subscription(const std::shared_ptrsubscription_ = subscription; } +template +void SubscriptionHandler::set_user_callback(const std::function& user_callback) { + this->user_callback_ = user_callback; +} + +template +std::function)> +SubscriptionHandler::get_callback(const std::function& user_callback) { + this->set_user_callback(user_callback); + return this->get_callback(); +} + template std::shared_ptr SubscriptionHandler::create_subscription_interface( const std::shared_ptr>& subscription @@ -71,4 +103,19 @@ std::shared_ptr SubscriptionHandler::create_subscri this->set_subscription(subscription); return std::shared_ptr(this->shared_from_this()); } + +template +void SubscriptionHandler::handle_callback_exceptions() { + try { + // re-throw the original exception + throw; + } catch (const exceptions::CoreException& ex) { + RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, + "Exception in subscription callback: " << ex.what()); + } catch (const std::exception& ex) { + RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, + "Unhandled exception in subscription user callback: " << ex.what()); + } +} + }// namespace modulo_core::communication diff --git a/source/modulo_core/src/communication/SubscriptionHandler.cpp b/source/modulo_core/src/communication/SubscriptionHandler.cpp index fcee696d8..4d73c5b1b 100644 --- a/source/modulo_core/src/communication/SubscriptionHandler.cpp +++ b/source/modulo_core/src/communication/SubscriptionHandler.cpp @@ -35,9 +35,9 @@ SubscriptionHandler::get_callback() { return [this](const std::shared_ptr message) { try { this->get_message_pair()->template read(*message); - } catch (const exceptions::CoreException& ex) { - RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, - "Exception in subscription callback: " << ex.what()); + this->user_callback_(); + } catch (...) { + this->handle_callback_exceptions(); } }; } @@ -48,9 +48,9 @@ SubscriptionHandler::get_callback() { return [this](const std::shared_ptr message) { try { this->get_message_pair()->template read(*message); - } catch (const exceptions::CoreException& ex) { - RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, - "Exception in subscription callback: " << ex.what()); + this->user_callback_(); + } catch (...) { + this->handle_callback_exceptions(); } }; } @@ -61,9 +61,9 @@ SubscriptionHandler::get_callback() { return [this](const std::shared_ptr message) { try { this->get_message_pair()->template read>(*message); - } catch (const exceptions::CoreException& ex) { - RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, - "Exception in subscription callback: " << ex.what()); + this->user_callback_(); + } catch (...) { + this->handle_callback_exceptions(); } }; } @@ -74,9 +74,9 @@ SubscriptionHandler::get_callback() { return [this](const std::shared_ptr message) { try { this->get_message_pair()->template read(*message); - } catch (const exceptions::CoreException& ex) { - RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, - "Exception in subscription callback: " << ex.what()); + this->user_callback_(); + } catch (...) { + this->handle_callback_exceptions(); } }; } @@ -87,9 +87,9 @@ SubscriptionHandler::get_callback() { return [this](const std::shared_ptr message) { try { this->get_message_pair()->template read(*message); - } catch (const exceptions::CoreException& ex) { - RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, - "Exception in subscription callback: " << ex.what()); + this->user_callback_(); + } catch (...) { + this->handle_callback_exceptions(); } }; } @@ -99,9 +99,9 @@ std::function)> SubscriptionHandler message) { try { this->get_message_pair()->template read(*message); - } catch (const exceptions::CoreException& ex) { - RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, - "Exception in subscription callback: " << ex.what()); + this->user_callback_(); + } catch (...) { + this->handle_callback_exceptions(); } }; } diff --git a/source/modulo_core/test/cpp/include/test_modulo_core/communication_nodes.hpp b/source/modulo_core/test/cpp/include/test_modulo_core/communication_nodes.hpp index 8a9378a28..c43c6457d 100644 --- a/source/modulo_core/test/cpp/include/test_modulo_core/communication_nodes.hpp +++ b/source/modulo_core/test/cpp/include/test_modulo_core/communication_nodes.hpp @@ -34,12 +34,9 @@ class MinimalSubscriber : public rclcpp::Node { Node("minimal_subscriber") { this->received_future = this->received_.get_future(); this->subscription_interface_ = std::make_shared>(message_pair); - auto subscription = this->create_subscription( - topic_name, 10, [this](const std::shared_ptr message) { - this->subscription_interface_->template get_handler()->get_callback()(message); - this->received_.set_value(); - } - ); + auto handler = this->subscription_interface_->template get_handler(); + handler->set_user_callback([this] { this->received_.set_value(); }); + auto subscription = this->create_subscription(topic_name, 10, handler->get_callback()); this->subscription_interface_ = this->subscription_interface_->template get_handler()->create_subscription_interface(subscription); } From ede8707a4d12c056b1e75fc236e2004342c8d34c Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 1 Sep 2022 15:25:17 +0200 Subject: [PATCH 13/20] Implement input callback in Python (#132) * Rename test classes for C++ tests * Add callback in Python component interface * Add communication tests in Python * Set default period to 0.1 and remove parameter overrides * Let EncodedState reader raise if message is None * Return early if message reading failed * Update tests * Fix log level --- .../modulo_components/ComponentInterface.hpp | 2 +- .../modulo_components/component_interface.py | 28 +++-- .../communication_components.hpp | 12 +- .../test/cpp/test_component_communication.cpp | 9 +- ...test_lifecycle_component_communication.cpp | 9 +- .../modulo_components/test/python/conftest.py | 108 +++++++++++++++++- .../python/test_component_communication.py | 24 ++++ .../test_lifecycle_component_communication.py | 36 ++++++ .../translators/message_readers.py | 10 +- 9 files changed, 208 insertions(+), 30 deletions(-) create mode 100644 source/modulo_components/test/python/test_component_communication.py create mode 100644 source/modulo_components/test/python/test_lifecycle_component_communication.py diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 19ef398ba..bd2c727d0 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -508,7 +508,7 @@ ComponentInterface::ComponentInterface( return this->on_set_parameters_callback(parameters); } ); - this->add_parameter("period", 1.0, "The time interval in seconds for all periodic callbacks", true); + this->add_parameter("period", 0.1, "The time interval in seconds for all periodic callbacks", true); this->add_predicate("in_error_state", false); diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index f6dd2dc6e..5ce97161b 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -377,7 +377,7 @@ def _create_output(self, signal_name: str, data: str, message_type: MsgT, clprot except Exception as e: raise AddSignalError(f"{e}") - def __subscription_callback(self, message: MsgT, attribute_name: str, reader: Callable): + def __subscription_callback(self, message: MsgT, attribute_name: str, reader: Callable, user_callback: Callable): """ Subscription callback for the ROS subscriptions. @@ -390,11 +390,15 @@ def __subscription_callback(self, message: MsgT, attribute_name: str, reader: Ca except (AttributeError, MessageTranslationError) as e: self.get_logger().warn(f"Failed to read message for attribute {attribute_name}: {e}", throttle_duration_sec=1.0) + return + try: + user_callback() + except Exception as e: + self.get_logger().error(f"Failed to execute user callback in subscription for attribute" + f" '{attribute_name}': {e}", throttle_duration_sec=1.0) def add_input(self, signal_name: str, subscription: Union[str, Callable], message_type: MsgT, default_topic="", - fixed_topic=False): - # TODO could be nice to add an optional callback here that would be executed from within the subscription - # callback in order to manipulate the data pointer upon reception of a message + fixed_topic=False, user_callback=lambda: None): """ Add and configure an input signal of the component. @@ -403,6 +407,7 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag :param message_type: ROS message type of the subscription :param default_topic: If set, the default value for the topic name to use :param fixed_topic: If true, the topic name of the output signal is fixed + :param user_callback: Callback function to trigger after receiving the input signal """ try: parsed_signal_name = parse_topic_name(signal_name) @@ -421,21 +426,30 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag topic_name = self.get_parameter_value(parsed_signal_name + "_topic") self.get_logger().debug(f"Adding input '{parsed_signal_name}' with topic name '{topic_name}'.") if isinstance(subscription, Callable): + if user_callback: + self.get_logger().warn("Providing a callable for arguments 'subscription' and 'user_callback' is" + "not supported. The user callback will be ignored.") self._inputs[parsed_signal_name] = self.create_subscription(message_type, topic_name, subscription, self._qos) elif isinstance(subscription, str): + if user_callback: + signature = inspect.signature(user_callback) + if len(signature.parameters) != 0: + raise AddSignalError("Provide a user callback that has no input arguments.") if message_type == Bool or message_type == Float64 or \ message_type == Float64MultiArray or message_type == Int32 or message_type == String: self._inputs[parsed_signal_name] = self.create_subscription(message_type, topic_name, partial(self.__subscription_callback, attribute_name=subscription, - reader=modulo_readers.read_std_message), + reader=modulo_readers.read_std_message, + user_callback=user_callback), self._qos) elif message_type == EncodedState: self._inputs[parsed_signal_name] = self.create_subscription(message_type, topic_name, partial(self.__subscription_callback, attribute_name=subscription, - reader=modulo_readers.read_clproto_message), + reader=modulo_readers.read_clproto_message, + user_callback=user_callback), self._qos) else: raise TypeError("The provided message type is not supported to create a component input.") @@ -489,7 +503,7 @@ def callback_wrapper(request, response, cb): raise AddServiceError(f"Service with name '{parsed_service_name}' already exists.") signature = inspect.signature(callback) if len(signature.parameters) == 0: - self.get_logger().error(f"Adding empty service '{parsed_service_name}'.") + self.get_logger().debug(f"Adding empty service '{parsed_service_name}'.") service_type = EmptyTrigger else: self.get_logger().debug(f"Adding string service '{parsed_service_name}'.") diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp index bc24cb0c7..576befed5 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp @@ -20,11 +20,11 @@ inline void add_configure_activate( } template -class MinimalOutput : public ComponentT { +class MinimalCartesianOutput : public ComponentT { public: - MinimalOutput( + MinimalCartesianOutput( const rclcpp::NodeOptions& node_options, const std::string& topic, const CartesianState& cartesian_state - ) : ComponentT(node_options, "minimal_output"), output_(std::make_shared(cartesian_state)) { + ) : ComponentT(node_options, "minimal_cartesian_output"), output_(std::make_shared(cartesian_state)) { this->add_output("cartesian_state", this->output_, topic); } @@ -33,10 +33,10 @@ class MinimalOutput : public ComponentT { }; template -class MinimalInput : public ComponentT { +class MinimalCartesianInput : public ComponentT { public: - MinimalInput(const rclcpp::NodeOptions& node_options, const std::string& topic) : - ComponentT(node_options, "minimal_input"), input(std::make_shared()) { + MinimalCartesianInput(const rclcpp::NodeOptions& node_options, const std::string& topic) : + ComponentT(node_options, "minimal_cartesian_input"), input(std::make_shared()) { this->received_future = this->received_.get_future(); this->add_input("cartesian_state", this->input, [this]() { this->received_.set_value(); }, topic); } diff --git a/source/modulo_components/test/cpp/test_component_communication.cpp b/source/modulo_components/test/cpp/test_component_communication.cpp index 10973e920..5caa8cf4a 100644 --- a/source/modulo_components/test/cpp/test_component_communication.cpp +++ b/source/modulo_components/test/cpp/test_component_communication.cpp @@ -21,12 +21,9 @@ class ComponentCommunicationTest : public ::testing::Test { TEST_F(ComponentCommunicationTest, InputOutput) { auto cartesian_state = state_representation::CartesianState::Random("test"); - auto input_node = std::make_shared>( - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.1)}), "/topic" - ); - auto output_node = std::make_shared>( - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.1)}), "/topic", cartesian_state - ); + auto input_node = std::make_shared>(rclcpp::NodeOptions(), "/topic"); + auto output_node = + std::make_shared>(rclcpp::NodeOptions(), "/topic", cartesian_state); this->exec_->add_node(input_node); this->exec_->add_node(output_node); this->exec_->template spin_until_future_complete(input_node->received_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 78c6f8f63..17a3a6a97 100644 --- a/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp +++ b/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp @@ -21,12 +21,9 @@ class LifecycleComponentCommunicationTest : public ::testing::Test { TEST_F(LifecycleComponentCommunicationTest, InputOutput) { auto cartesian_state = state_representation::CartesianState::Random("test"); - auto input_node = std::make_shared>( - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.1)}), "/topic" - ); - auto output_node = std::make_shared>( - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.1)}), "/topic", cartesian_state - ); + auto input_node = std::make_shared>(rclcpp::NodeOptions(), "/topic"); + auto output_node = + std::make_shared>(rclcpp::NodeOptions(), "/topic", cartesian_state); add_configure_activate(this->exec_, input_node); add_configure_activate(this->exec_, output_node); this->exec_->template spin_until_future_complete(input_node->received_future, 500ms); diff --git a/source/modulo_components/test/python/conftest.py b/source/modulo_components/test/python/conftest.py index 807010268..a46c7231e 100644 --- a/source/modulo_components/test/python/conftest.py +++ b/source/modulo_components/test/python/conftest.py @@ -1,9 +1,115 @@ +import clproto import pytest import rclpy +import state_representation as sr +from lifecycle_msgs.msg import Transition +from lifecycle_msgs.srv import ChangeState +from modulo_components.component import Component +from modulo_core import EncodedState +from rclpy.node import Node +from rclpy.task import Future + + +@pytest.fixture +def random_state(): + return sr.CartesianPose.Random("test") @pytest.fixture def ros_context(): rclpy.init() yield - rclpy.shutdown() \ No newline at end of file + rclpy.shutdown() + + +@pytest.fixture +def ros_exec(): + rclpy.init() + executor = rclpy.executors.SingleThreadedExecutor() + yield executor + rclpy.shutdown() + + +@pytest.fixture +def minimal_cartesian_output(request, random_state): + def _make_minimal_cartesian_output(component_type, topic): + component = component_type("minimal_cartesian_output") + component._output = random_state + component.add_output("cartesian_state", "_output", EncodedState, clproto.MessageType.CARTESIAN_STATE_MESSAGE, + topic) + return component + + yield _make_minimal_cartesian_output(request.param[0], request.param[1]) + + +class MinimalInvalidEncodedStatePublisher(Component): + def __init__(self, topic, *args, **kwargs): + super().__init__("minimal_invalid_encoded_state_publisher", *args, **kwargs) + self.publisher = self.create_publisher(EncodedState, topic, self.get_qos()) + self.add_periodic_callback("publish", self.__publish) + + def __publish(self): + msg = EncodedState() + data = "hello" + msg.data = bytes(data.encode()) + self.publisher.publish(msg) + + +@pytest.fixture +def make_minimal_invalid_encoded_state_publisher(): + def _make_minimal_invalid_encoded_state_publisher(topic): + return MinimalInvalidEncodedStatePublisher(topic) + + yield _make_minimal_invalid_encoded_state_publisher + + +@pytest.fixture +def minimal_cartesian_input(request): + def _make_minimal_cartesian_input(component_type, topic): + component = component_type("minimal_cartesian_input") + component.received_future = Future() + component.input = sr.CartesianState() + component.add_input("cartesian_state", "input", EncodedState, topic, + user_callback=lambda: component.received_future.set_result(True)) + return component + + yield _make_minimal_cartesian_input(request.param[0], request.param[1]) + + +class LifecycleServiceClient(Node): + def __init__(self, namespace): + super().__init__("lifecycle_service_node") + self.client = self.create_client(ChangeState, namespace + "/change_state") + + +class Helpers: + @staticmethod + def trigger_lifecycle_transition(ros_exec, lifecycle_client, transition_id): + future = lifecycle_client.client.call_async(ChangeState.Request(transition=Transition(id=transition_id))) + ros_exec.spin_until_future_complete(future, timeout_sec=0.5) + assert future.result().success + + @staticmethod + def configure(ros_exec, lifecycle_client): + Helpers.trigger_lifecycle_transition(ros_exec, lifecycle_client, 1) + + @staticmethod + def activate(ros_exec, lifecycle_client): + Helpers.trigger_lifecycle_transition(ros_exec, lifecycle_client, 3) + + @staticmethod + def deactivate(ros_exec, lifecycle_client): + Helpers.trigger_lifecycle_transition(ros_exec, lifecycle_client, 4) + + +@pytest.fixture +def helpers(): + return Helpers + + +@pytest.fixture +def make_lifecycle_service_client(): + def _make_lifecycle_service_client(namespace): + return LifecycleServiceClient(namespace) + + yield _make_lifecycle_service_client diff --git a/source/modulo_components/test/python/test_component_communication.py b/source/modulo_components/test/python/test_component_communication.py new file mode 100644 index 000000000..d4d96318c --- /dev/null +++ b/source/modulo_components/test/python/test_component_communication.py @@ -0,0 +1,24 @@ +import pytest +from modulo_components.component import Component + + +@pytest.mark.parametrize("minimal_cartesian_input", [[Component, "/topic"]], indirect=True) +@pytest.mark.parametrize("minimal_cartesian_output", [[Component, "/topic"]], indirect=True) +def test_input_output(ros_exec, random_state, minimal_cartesian_output, minimal_cartesian_input): + ros_exec.add_node(minimal_cartesian_input) + ros_exec.add_node(minimal_cartesian_output) + ros_exec.spin_until_future_complete(minimal_cartesian_input.received_future, timeout_sec=0.5) + ros_exec.remove_node(minimal_cartesian_input) + ros_exec.remove_node(minimal_cartesian_output) + assert minimal_cartesian_input.received_future.result() + assert random_state.get_name() == minimal_cartesian_input.input.get_name() + assert random_state.dist(minimal_cartesian_input.input) < 1e-3 + + +@pytest.mark.parametrize("minimal_cartesian_input", [[Component, "/topic"]], indirect=True) +def test_input_output_invalid(ros_exec, make_minimal_invalid_encoded_state_publisher, minimal_cartesian_input): + invalid_publisher = make_minimal_invalid_encoded_state_publisher("/topic") + ros_exec.add_node(minimal_cartesian_input) + ros_exec.add_node(invalid_publisher) + ros_exec.spin_until_future_complete(minimal_cartesian_input.received_future, timeout_sec=0.5) + assert not minimal_cartesian_input.received_future.result() diff --git a/source/modulo_components/test/python/test_lifecycle_component_communication.py b/source/modulo_components/test/python/test_lifecycle_component_communication.py new file mode 100644 index 000000000..59d61acbb --- /dev/null +++ b/source/modulo_components/test/python/test_lifecycle_component_communication.py @@ -0,0 +1,36 @@ +import pytest +from modulo_components.lifecycle_component import LifecycleComponent + + +@pytest.mark.parametrize("minimal_cartesian_input", [[LifecycleComponent, "/topic"]], indirect=True) +@pytest.mark.parametrize("minimal_cartesian_output", [[LifecycleComponent, "/topic"]], indirect=True) +def test_input_output(ros_exec, make_lifecycle_service_client, helpers, random_state, minimal_cartesian_output, + minimal_cartesian_input): + input_client = make_lifecycle_service_client("minimal_cartesian_input") + output_client = make_lifecycle_service_client("minimal_cartesian_output") + ros_exec.add_node(input_client) + ros_exec.add_node(output_client) + ros_exec.add_node(minimal_cartesian_input) + ros_exec.add_node(minimal_cartesian_output) + helpers.configure(ros_exec, input_client) + helpers.configure(ros_exec, output_client) + helpers.activate(ros_exec, input_client) + helpers.activate(ros_exec, output_client) + ros_exec.spin_until_future_complete(minimal_cartesian_input.received_future, timeout_sec=0.5) + assert minimal_cartesian_input.received_future.result() + assert random_state.get_name() == minimal_cartesian_input.input.get_name() + assert random_state.dist(minimal_cartesian_input.input) < 1e-3 + + +@pytest.mark.parametrize("minimal_cartesian_input", [[LifecycleComponent, "/topic"]], indirect=True) +def test_input_output_invalid(ros_exec, make_lifecycle_service_client, helpers, + make_minimal_invalid_encoded_state_publisher, minimal_cartesian_input): + input_client = make_lifecycle_service_client("minimal_cartesian_input") + invalid_publisher = make_minimal_invalid_encoded_state_publisher("/topic") + ros_exec.add_node(input_client) + ros_exec.add_node(minimal_cartesian_input) + ros_exec.add_node(invalid_publisher) + helpers.configure(ros_exec, input_client) + helpers.activate(ros_exec, input_client) + ros_exec.spin_until_future_complete(minimal_cartesian_input.received_future, timeout_sec=0.5) + assert not minimal_cartesian_input.received_future.result() diff --git a/source/modulo_core/modulo_core/translators/message_readers.py b/source/modulo_core/modulo_core/translators/message_readers.py index 44bca70c9..30453b694 100644 --- a/source/modulo_core/modulo_core/translators/message_readers.py +++ b/source/modulo_core/modulo_core/translators/message_readers.py @@ -61,7 +61,8 @@ def read_message(state: StateT, message: MsgT) -> StateT: state.set_force(read_xyz(message.force)) state.set_torque(read_xyz(message.torque)) else: - raise MessageTranslationError("The provided combination of state type and message type is not supported") + raise MessageTranslationError( + "The provided combination of state type and message type is not supported") elif isinstance(message, JointState) and isinstance(state, sr.JointState): try: state.set_names(message.name) @@ -130,6 +131,9 @@ def read_clproto_message(message: EncodedState) -> StateT: :return: The decoded clproto message """ try: - return clproto.decode(message.data.tobytes()) + decoded_message = clproto.decode(message.data.tobytes()) + if not decoded_message: + raise RuntimeError("The decoded message is None.") + return decoded_message except Exception as e: - MessageTranslationError(f"{e}") + raise MessageTranslationError(f"{e}") From 78f6e056a92dafd5aae8d03f8c5532aee4f57029 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Thu, 1 Sep 2022 15:28:05 +0200 Subject: [PATCH 14/20] User-defined callback for inputs (#133) 2.0.10 -> 2.0.11 Update CHANGELOG --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- source/modulo_component_interfaces/package.xml | 2 +- source/modulo_components/package.xml | 2 +- source/modulo_core/package.xml | 2 +- 6 files changed, 6 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9bea594b1..b55f608a9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,7 @@ Release Versions: - Rename validate parameter callback and validate period value (#126) - Add double underscore to private function (#127) - Improve error messages and logs further (#131) +- User-defined callback for inputs (#124, #132, #133) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 0a692060f..6cbacdc8d 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.10 +2.0.11 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 795b2a060..6bd4336d7 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.0.10 +PROJECT_NUMBER = 2.0.11 # 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/package.xml b/source/modulo_component_interfaces/package.xml index 967793de5..cbe007c8f 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.10 + 2.0.11 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 461fc9155..94db4b4bb 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.10 + 2.0.11 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 11d349480..82f0f63d4 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.10 + 2.0.11 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From 5249e7bd989b3b78c968765f08b3759ecec45656 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 13 Sep 2022 17:25:18 +0200 Subject: [PATCH 15/20] Add humble-devel build and test workflow (#135) * Add humble-devel build and test workflow * 2.0.11 -> 2.0.12 * Update CHANGELOG --- .../actions/build-test-humble-devel/action.yml | 6 ++++++ .github/workflows/build-test.yml | 15 ++++++++++++++- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- source/modulo_component_interfaces/package.xml | 2 +- source/modulo_components/package.xml | 2 +- source/modulo_core/package.xml | 2 +- 8 files changed, 26 insertions(+), 6 deletions(-) create mode 100644 .github/actions/build-test-humble-devel/action.yml diff --git a/.github/actions/build-test-humble-devel/action.yml b/.github/actions/build-test-humble-devel/action.yml new file mode 100644 index 000000000..25181c7ef --- /dev/null +++ b/.github/actions/build-test-humble-devel/action.yml @@ -0,0 +1,6 @@ +name: 'Build and Test (humble-devel)' +description: 'Build the source packages and run all unit tests' +runs: + using: 'docker' + image: 'ghcr.io/aica-technology/ros2-control-libraries:humble-devel' + entrypoint: '/github/workspace/.github/common/entrypoint.sh' diff --git a/.github/workflows/build-test.yml b/.github/workflows/build-test.yml index 731562e79..7a2e77846 100644 --- a/.github/workflows/build-test.yml +++ b/.github/workflows/build-test.yml @@ -73,4 +73,17 @@ jobs: uses: actions/checkout@v2 # Load the repository build-test action - name: Build and Test - uses: ./.github/actions/build-test-galactic-devel \ No newline at end of file + uses: ./.github/actions/build-test-galactic-devel + + build-test-humble-devel: + needs: check-skippable-changes + if: ${{ needs.check-skippable-changes.outputs.skip != 'true' }} + runs-on: ubuntu-latest + name: Humble development build and test + steps: + # First check out the repository + - name: Checkout + uses: actions/checkout@v2 + # Load the repository build-test action + - name: Build and Test + uses: ./.github/actions/build-test-humble-devel \ No newline at end of file diff --git a/CHANGELOG.md b/CHANGELOG.md index b55f608a9..9377edde1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,7 @@ Release Versions: - Add double underscore to private function (#127) - Improve error messages and logs further (#131) - User-defined callback for inputs (#124, #132, #133) +- Add humble-devel build and test workflow (#135) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 6cbacdc8d..280a1e336 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.11 +2.0.12 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 6bd4336d7..ce7165e9f 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.0.11 +PROJECT_NUMBER = 2.0.12 # 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/package.xml b/source/modulo_component_interfaces/package.xml index cbe007c8f..9d31f87a9 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.11 + 2.0.12 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 94db4b4bb..ea86c2e78 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.11 + 2.0.12 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 82f0f63d4..892e5db77 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.11 + 2.0.12 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From 1608d15a5823a7c50eab1925026cb083b554f6a1 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Wed, 14 Sep 2022 11:34:38 +0200 Subject: [PATCH 16/20] Correctly list rclcpp_components as build dependency (#137) * Build depend on rclcpp_components * 2.0.12 -> 2.0.13 * Update CHANGELOG --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- source/modulo_component_interfaces/package.xml | 2 +- source/modulo_components/package.xml | 5 ++++- source/modulo_core/package.xml | 2 +- 6 files changed, 9 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9377edde1..57a452ebd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,7 @@ Release Versions: - Improve error messages and logs further (#131) - User-defined callback for inputs (#124, #132, #133) - Add humble-devel build and test workflow (#135) +- Correctly list rclcpp_components as build dependency (#137) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 280a1e336..82bd22f9c 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.12 +2.0.13 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index ce7165e9f..095fee44e 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.0.12 +PROJECT_NUMBER = 2.0.13 # 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/package.xml b/source/modulo_component_interfaces/package.xml index 9d31f87a9..3360b37ac 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.12 + 2.0.13 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index ea86c2e78..4b4e1c0da 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.12 + 2.0.13 TODO Baptiste Busch Enrico Eberhard @@ -12,6 +12,9 @@ ament_cmake_auto ament_cmake_python + rclcpp_components + rclcpp_components + console_bridge modulo_core modulo_component_interfaces diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 892e5db77..ae15f419b 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.12 + 2.0.13 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From def34d3a19f642c86dd4bcce3ac7f6fb008159d6 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 15 Sep 2022 08:27:01 +0200 Subject: [PATCH 17/20] Add option to declare signals (#136) * Add the declare_signal methods (C++) * Add the declare_signal methods (Python) * Fixes * Add tests * Update CHANGELOG * 2.0.13 -> 2.0.14 * Remove TODOs --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- .../modulo_component_interfaces/package.xml | 2 +- .../modulo_components/ComponentInterface.hpp | 127 ++++++++++-------- .../modulo_components/component_interface.py | 90 ++++++++----- source/modulo_components/package.xml | 2 +- .../component_public_interfaces.hpp | 2 + .../test/cpp/test_component_interface.cpp | 9 ++ .../test/python/test_component_interface.py | 9 ++ source/modulo_core/package.xml | 2 +- 11 files changed, 159 insertions(+), 89 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 57a452ebd..901b9add1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,7 @@ Release Versions: - User-defined callback for inputs (#124, #132, #133) - Add humble-devel build and test workflow (#135) - Correctly list rclcpp_components as build dependency (#137) +- Add option to declare signals (#136) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 82bd22f9c..3d45b5c65 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.13 +2.0.14 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 095fee44e..5d929d52a 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.0.13 +PROJECT_NUMBER = 2.0.14 # 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/package.xml b/source/modulo_component_interfaces/package.xml index 3360b37ac..85925ba8d 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.13 + 2.0.14 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index bd2c727d0..0b48fe1a8 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -221,6 +221,26 @@ class ComponentInterface : public NodeT { */ void trigger(const std::string& trigger_name); + /** + * @brief Declare an input to create the topic parameter without adding it to the map of inputs yet. + * @param signal_name The signal name of the input + * @param default_topic If set, the default value for the topic name to use + * @param fixed_topic If true, the topic name of the signal is fixed + * @throws modulo_components::exceptions::AddSignalException if the input could not be declared + * (empty name or already created) + */ + void declare_input(const std::string& signal_name, const std::string& default_topic = "", bool fixed_topic = false); + + /** + * @brief Declare an output to create the topic parameter without adding it to the map of outputs yet. + * @param signal_name The signal name of the output + * @param default_topic If set, the default value for the topic name to use + * @param fixed_topic If true, the topic name of the signal is fixed + * @throws modulo_components::exceptions::AddSignalException if the output could not be declared + * (empty name or already created) + */ + void declare_output(const std::string& signal_name, const std::string& default_topic = "", bool fixed_topic = false); + /** * @brief Add and configure an input signal of the component. * @tparam DataT Type of the data pointer @@ -451,13 +471,17 @@ class ComponentInterface : public NodeT { void set_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate); /** - * @brief Validate an add_input request by parsing the signal name and checking the map of registered inputs. - * @param signal_name The name of the input signal - * @throws modulo_components::exceptions::AddSignalException if the input could not be created - * (empty name or already registered) - * @return The parsed signal name + * @brief Declare a signal to create the topic parameter without adding it to the map of signals. + * @param signal_name The name of the signal + * @param type The type of the signal (input or output) + * @param default_topic If set, the default value for the topic name to use + * @param fixed_topic If true, the topic name of the signal is fixed + * @throws modulo_components::exceptions::AddSignalException if the signal could not be declared + * (empty name or already created) */ - std::string validate_input_signal_name(const std::string& signal_name); + void declare_signal( + const std::string& signal_name, const std::string& type, const std::string& default_topic, bool fixed_topic + ); /** * @brief Validate an add_service request by parsing the service name and checking the maps of registered services. @@ -784,17 +808,47 @@ inline void ComponentInterface::set_predicate( } template -inline std::string ComponentInterface::validate_input_signal_name(const std::string& signal_name) { +inline void ComponentInterface::declare_signal( + const std::string& signal_name, const std::string& type, const std::string& default_topic, bool fixed_topic +) { std::string parsed_signal_name = utilities::parse_topic_name(signal_name); if (parsed_signal_name.empty()) { throw exceptions::AddSignalException( - "The parsed signal name for input '" + signal_name + "The parsed signal name for " + type + " '" + signal_name + "' is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_])."); } if (this->inputs_.find(parsed_signal_name) != this->inputs_.cend()) { - throw exceptions::AddSignalException("Input with name '" + parsed_signal_name + "' already exists."); + throw exceptions::AddSignalException("Signal with name '" + parsed_signal_name + "' already exists as input."); + } + if (this->outputs_.find(parsed_signal_name) != this->outputs_.cend()) { + throw exceptions::AddSignalException("Signal with name '" + parsed_signal_name + "' already exists as output."); + } + std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; + auto parameter_name = parsed_signal_name + "_topic"; + if (NodeT::has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { + this->set_parameter_value(parameter_name, topic_name); + } else { + this->add_parameter( + parameter_name, topic_name, "Signal topic name of " + type + " '" + parsed_signal_name + "'", fixed_topic + ); } - return parsed_signal_name; + RCLCPP_DEBUG_STREAM(this->get_logger(), + "Declared signal '" << parsed_signal_name << "' and parameter '" << parameter_name + << "' with value '" << topic_name << "'."); +} + +template +inline void ComponentInterface::declare_input( + const std::string& signal_name, const std::string& default_topic, bool fixed_topic +) { + this->declare_signal(signal_name, "input", default_topic, fixed_topic); +} + +template +inline void ComponentInterface::declare_output( + const std::string& signal_name, const std::string& default_topic, bool fixed_topic +) { + this->declare_signal(signal_name, "output", default_topic, fixed_topic); } template @@ -814,21 +868,14 @@ inline void ComponentInterface::add_input( ) { using namespace modulo_core::communication; try { - std::string parsed_signal_name = this->validate_input_signal_name(signal_name); + std::string parsed_signal_name = utilities::parse_topic_name(signal_name); if (data == nullptr) { throw modulo_core::exceptions::NullPointerException( - "Invalid data pointer for input '" + parsed_signal_name + "'."); - } - std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; - auto parameter_name = parsed_signal_name + "_topic"; - if (NodeT::has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { - this->set_parameter_value(parameter_name, topic_name); - } else { - this->add_parameter( - parameter_name, topic_name, "Signal topic name of input '" + parsed_signal_name + "'", fixed_topic + "Invalid data pointer for input '" + parsed_signal_name + "'." ); } - topic_name = this->get_parameter_value(parameter_name); + this->declare_input(parsed_signal_name, default_topic, fixed_topic); + auto topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding input '" << parsed_signal_name << "' with topic name '" << topic_name << "'."); auto message_pair = make_shared_message_pair(data, this->get_clock()); @@ -892,17 +939,9 @@ inline void ComponentInterface::add_input( ) { using namespace modulo_core::communication; try { - std::string parsed_signal_name = this->validate_input_signal_name(signal_name); - std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; - auto parameter_name = parsed_signal_name + "_topic"; - if (NodeT::has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { - this->set_parameter_value(parameter_name, topic_name); - } else { - this->add_parameter( - parameter_name, topic_name, "Signal topic name of input '" + parsed_signal_name + "'", fixed_topic - ); - } - topic_name = this->get_parameter_value(parameter_name); + std::string parsed_signal_name = utilities::parse_topic_name(signal_name); + this->declare_input(parsed_signal_name, default_topic, fixed_topic); + auto topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding input '" << parsed_signal_name << "' with topic name '" << topic_name << "'."); auto subscription = NodeT::template create_subscription(topic_name, this->qos_, callback); @@ -934,7 +973,7 @@ inline void ComponentInterface::add_service( const std::string& service_name, const std::function& callback ) { try { - std::string parsed_service_name = validate_service_name(service_name); + std::string parsed_service_name = this->validate_service_name(service_name); RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding empty service '" << parsed_service_name << "'."); auto service = NodeT::template create_service( "~/" + parsed_service_name, [callback]( @@ -962,7 +1001,7 @@ inline void ComponentInterface::add_service( const std::string& service_name, const std::function& callback ) { try { - std::string parsed_service_name = validate_service_name(service_name); + std::string parsed_service_name = this->validate_service_name(service_name); RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding string service '" << parsed_service_name << "'."); auto service = NodeT::template create_service( "~/" + parsed_service_name, [callback]( @@ -1156,35 +1195,17 @@ inline std::string ComponentInterface::create_output( using namespace modulo_core::communication; try { auto parsed_signal_name = utilities::parse_topic_name(signal_name); - if (parsed_signal_name.empty()) { - throw exceptions::AddSignalException( - "The parsed signal name for output '" + signal_name - + "'is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_])." - ); - } if (data == nullptr) { throw modulo_core::exceptions::NullPointerException( "Invalid data pointer for output '" + parsed_signal_name + "'."); } + this->declare_output(parsed_signal_name, default_topic, fixed_topic); RCLCPP_DEBUG_STREAM(this->get_logger(), "Creating output '" << parsed_signal_name << "' (provided signal name was '" << signal_name << "')."); - if (this->outputs_.find(parsed_signal_name) != this->outputs_.end()) { - throw exceptions::AddSignalException("Output with name '" + parsed_signal_name + "' already exists."); - } auto message_pair = make_shared_message_pair(data, this->get_clock()); this->outputs_.insert_or_assign( parsed_signal_name, std::make_shared(this->publisher_type_, message_pair)); - - std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; - auto parameter_name = parsed_signal_name + "_topic"; - if (NodeT::has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { - this->set_parameter_value(parameter_name, topic_name); - } else { - this->add_parameter( - parameter_name, topic_name, "Signal topic name of output '" + parsed_signal_name + "'", fixed_topic - ); - } return parsed_signal_name; } catch (const exceptions::AddSignalException&) { throw; diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 5ce97161b..9265ab6ef 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -78,7 +78,7 @@ def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, r :param parameter: Either the name of the parameter attribute or the parameter itself :param description: The parameter description :param read_only: If True, the value of the parameter cannot be changed after declaration - :raises ComponentParameterError if the parameter could not be added + :raises ComponentParameterError: if the parameter could not be added """ try: if isinstance(parameter, sr.Parameter): @@ -120,7 +120,7 @@ def get_parameter(self, name: str) -> Union[sr.Parameter, Parameter]: dictionary. :param name: The name of the parameter - :raises ComponentParameterError if the parameter does not exist + :raises ComponentParameterError: if the parameter does not exist :return: The requested parameter """ try: @@ -138,7 +138,7 @@ def __get_component_parameter(self, name: str) -> sr.Parameter: Get the parameter from the parameter dictionary by its name. :param name: The name of the parameter - :raises ComponentParameterError if the parameter does not exist + :raises ComponentParameterError: if the parameter does not exist :return: The parameter, if it exists """ if name not in self._parameter_dict.keys(): @@ -156,7 +156,7 @@ def get_parameter_value(self, name: str) -> T: Get the parameter value from the parameter dictionary by its name. :param name: The name of the parameter - :raises ComponentParameterError if the parameter does not exist + :raises ComponentParameterError: if the parameter does not exist :return: The value of the parameter, if the parameter exists """ return self.__get_component_parameter(name).get_value() @@ -341,26 +341,14 @@ def _create_output(self, signal_name: str, data: str, message_type: MsgT, clprot :param clproto_message_type: The clproto message type, if applicable :param default_topic: If set, the default value for the topic name to use :param fixed_topic: If true, the topic name of the output signal is fixed - :raises AddSignalError if there is a problem adding the output + :raises AddSignalError: if there is a problem adding the output :return: The parsed signal name """ try: if message_type == EncodedState and clproto_message_type == clproto.MessageType.UNKNOWN_MESSAGE: raise AddSignalError(f"Provide a valid clproto message type for outputs of type EncodedState.") parsed_signal_name = parse_topic_name(signal_name) - if not parsed_signal_name: - raise AddSignalError(f"The parsed signal name for output '{signal_name}' is empty. Provide a " - f"string with valid characters for the signal name ([a-zA-Z0-9_]).") - if parsed_signal_name in self._outputs.keys(): - raise AddSignalError(f"Output with parsed name '{parsed_signal_name}' already exists.") - topic_name = default_topic if default_topic else "~/" + parsed_signal_name - parameter_name = parsed_signal_name + "_topic" - if self.has_parameter(parameter_name) and self.get_parameter(parameter_name).is_empty(): - self.set_parameter_value(parameter_name, topic_name) - else: - self.add_parameter(sr.Parameter(parameter_name, topic_name, sr.ParameterType.STRING), - f"Signal topic name of output '{parsed_signal_name}'", fixed_topic) - translator = None + self.declare_output(parsed_signal_name, default_topic, fixed_topic) if message_type == Bool or message_type == Float64 or \ message_type == Float64MultiArray or message_type == Int32 or message_type == String: translator = modulo_writers.write_std_message @@ -397,6 +385,56 @@ def __subscription_callback(self, message: MsgT, attribute_name: str, reader: Ca self.get_logger().error(f"Failed to execute user callback in subscription for attribute" f" '{attribute_name}': {e}", throttle_duration_sec=1.0) + def declare_signal(self, signal_name: str, signal_type: str, default_topic="", fixed_topic=False): + """ + Declare an input to create the topic parameter without adding it to the map of inputs yet. + + :param signal_name: The signal name of the input + :param signal_type: The type of the signal (input or output) + :param default_topic: If set, the default value for the topic name to use + :param fixed_topic: If true, the topic name of the signal is fixed + :raises AddSignalError: if the signal could not be declared (empty name or already created) + """ + parsed_signal_name = parse_topic_name(signal_name) + if not parsed_signal_name: + raise AddSignalError(f"The parsed signal name for {signal_type} '{signal_name}' is empty. Provide a " + f"string with valid characters for the signal name ([a-zA-Z0-9_]).") + if parsed_signal_name in self._inputs.keys(): + raise AddSignalError(f"Signal with name '{parsed_signal_name}' already exists as input.") + if parsed_signal_name in self._outputs.keys(): + raise AddSignalError(f"Signal with name '{parsed_signal_name}' already exists as output.") + topic_name = default_topic if default_topic else "~/" + parsed_signal_name + parameter_name = parsed_signal_name + "_topic" + if self.has_parameter(parameter_name) and self.get_parameter(parameter_name).is_empty(): + self.set_parameter_value(parameter_name, topic_name) + else: + self.add_parameter(sr.Parameter(parameter_name, topic_name, sr.ParameterType.STRING), + f"Signal topic name of {signal_type} '{parsed_signal_name}'", fixed_topic) + self.get_logger().debug( + f"Declared signal '{parsed_signal_name}' and parameter '{parameter_name}' with value '{topic_name}'.") + + def declare_input(self, signal_name: str, default_topic="", fixed_topic=False): + """ + Declare an input to create the topic parameter without adding it to the map of inputs yet. + + :param signal_name: The signal name of the input + :param default_topic: If set, the default value for the topic name to use + :param fixed_topic: If true, the topic name of the signal is fixed + :raises AddSignalError: if the input could not be declared (empty name or already created) + """ + self.declare_signal(signal_name, "input", default_topic, fixed_topic) + + def declare_output(self, signal_name: str, default_topic="", fixed_topic=False): + """ + Declare an output to create the topic parameter without adding it to the map of outputs yet. + + :param signal_name: The signal name of the output + :param default_topic: If set, the default value for the topic name to use + :param fixed_topic: If true, the topic name of the signal is fixed + :raises AddSignalError: if the output could not be declared (empty name or already created) + """ + self.declare_signal(signal_name, "output", default_topic, fixed_topic) + def add_input(self, signal_name: str, subscription: Union[str, Callable], message_type: MsgT, default_topic="", fixed_topic=False, user_callback=lambda: None): """ @@ -408,21 +446,11 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag :param default_topic: If set, the default value for the topic name to use :param fixed_topic: If true, the topic name of the output signal is fixed :param user_callback: Callback function to trigger after receiving the input signal + :raises AddSignalError: if there is a problem adding the input """ try: parsed_signal_name = parse_topic_name(signal_name) - if not parsed_signal_name: - raise AddSignalError(f"The parsed signal name for input '{signal_name}' is empty. Provide a " - f"string with valid characters for the signal name ([a-zA-Z0-9_]).") - if parsed_signal_name in self._inputs.keys(): - raise AddSignalError(f"Input with name '{parsed_signal_name}' already exists.") - topic_name = default_topic if default_topic else "~/" + parsed_signal_name - parameter_name = parsed_signal_name + "_topic" - if self.has_parameter(parameter_name) and self.get_parameter(parameter_name).is_empty(): - self.set_parameter_value(parameter_name, topic_name) - else: - self.add_parameter(sr.Parameter(parameter_name, topic_name, sr.ParameterType.STRING), - f"Signal topic name of input '{parsed_signal_name}'", fixed_topic) + self.declare_input(parsed_signal_name, default_topic, fixed_topic) topic_name = self.get_parameter_value(parsed_signal_name + "_topic") self.get_logger().debug(f"Adding input '{parsed_signal_name}' with topic name '{topic_name}'.") if isinstance(subscription, Callable): @@ -498,7 +526,7 @@ def callback_wrapper(request, response, cb): parsed_service_name = parse_topic_name(service_name) if not parsed_service_name: raise AddServiceError(f"The parsed signal name for service {service_name} is empty. Provide a " - f"string with valid characters for the service name ([a-zA-Z0-9_]).") + f"string with valid characters for the service name ([a-zA-Z0-9_]).") if parsed_service_name in self._services_dict.keys(): raise AddServiceError(f"Service with name '{parsed_service_name}' already exists.") signature = inspect.signature(callback) diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 4b4e1c0da..1b40a7ff8 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.13 + 2.0.14 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 7a24997f6..436804b34 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -27,6 +27,8 @@ class ComponentInterfacePublicInterface : public ComponentInterface { using ComponentInterface::add_trigger; using ComponentInterface::trigger; using ComponentInterface::triggers_; + using ComponentInterface::declare_input; + using ComponentInterface::declare_output; using ComponentInterface::add_input; using ComponentInterface::add_service; using ComponentInterface::inputs_; diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 56f6e6eab..4cedb9d3e 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -81,6 +81,15 @@ TYPED_TEST(ComponentInterfaceTest, SetPredicateValue) { EXPECT_FALSE(this->component_->get_predicate("bar")); } +TYPED_TEST(ComponentInterfaceTest, DeclareSignal) { + this->component_->declare_input("input", "test"); + EXPECT_EQ(this->component_->template get_parameter_value("input_topic"), "test"); + EXPECT_TRUE(this->component_->inputs_.find("input") == this->component_->inputs_.end()); + this->component_->declare_output("output", "test_again"); + EXPECT_EQ(this->component_->template get_parameter_value("output_topic"), "test_again"); + EXPECT_TRUE(this->component_->outputs_.find("output") == this->component_->outputs_.end()); +} + TYPED_TEST(ComponentInterfaceTest, AddInput) { auto data = std::make_shared(true); EXPECT_NO_THROW(this->component_->add_input("_tEsT_#1@3", data)); diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index a73346807..cb25b2e6a 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -52,6 +52,15 @@ def test_set_predicate(component_interface): assert not component_interface.get_predicate('bar') +def test_declare_signal(component_interface): + component_interface.declare_input("input", "test") + assert component_interface.get_parameter_value("input_topic") == "test" + assert "input" not in component_interface._inputs.keys() + component_interface.declare_output("output", "test_again") + assert component_interface.get_parameter_value("output_topic") == "test_again" + assert "test_again" not in component_interface._outputs.keys() + + def test_add_input(component_interface): component_interface.add_input("_tEsT_#1@3", "test", Bool) assert "test_13" in component_interface._inputs.keys() diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index ae15f419b..1babaa25e 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.13 + 2.0.14 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From 0b3628a7dd18de18eeebe4657ef3ed4de2be3333 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 20 Sep 2022 22:14:46 +0200 Subject: [PATCH 18/20] Correct default value for user callback on add_input (#138) * Default value for user callback should be None * Formatting and fixes * 2.0.14 -> 2.0.15 * Improve log * Update CHANGELOG * Avoid PEP warnings Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- .../modulo_component_interfaces/package.xml | 2 +- .../modulo_components/component_interface.py | 38 +++++++++++-------- source/modulo_components/package.xml | 2 +- source/modulo_core/package.xml | 2 +- 7 files changed, 29 insertions(+), 20 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 901b9add1..2d1001cb7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -23,6 +23,7 @@ Release Versions: - Add humble-devel build and test workflow (#135) - Correctly list rclcpp_components as build dependency (#137) - Add option to declare signals (#136) +- Correct default value for user callback on add_input (#138) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index 3d45b5c65..b8061b50d 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.14 +2.0.15 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 5d929d52a..6c39130ee 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.0.14 +PROJECT_NUMBER = 2.0.15 # 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/package.xml b/source/modulo_component_interfaces/package.xml index 85925ba8d..ae5795cc5 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.14 + 2.0.15 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 9265ab6ef..85622b6bf 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -406,7 +406,7 @@ def declare_signal(self, signal_name: str, signal_type: str, default_topic="", f topic_name = default_topic if default_topic else "~/" + parsed_signal_name parameter_name = parsed_signal_name + "_topic" if self.has_parameter(parameter_name) and self.get_parameter(parameter_name).is_empty(): - self.set_parameter_value(parameter_name, topic_name) + self.set_parameter_value(parameter_name, topic_name, sr.ParameterType.STRING) else: self.add_parameter(sr.Parameter(parameter_name, topic_name, sr.ParameterType.STRING), f"Signal topic name of {signal_type} '{parsed_signal_name}'", fixed_topic) @@ -436,7 +436,7 @@ def declare_output(self, signal_name: str, default_topic="", fixed_topic=False): self.declare_signal(signal_name, "output", default_topic, fixed_topic) def add_input(self, signal_name: str, subscription: Union[str, Callable], message_type: MsgT, default_topic="", - fixed_topic=False, user_callback=lambda: None): + fixed_topic=False, user_callback: Callable = None): """ Add and configure an input signal of the component. @@ -460,25 +460,33 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag self._inputs[parsed_signal_name] = self.create_subscription(message_type, topic_name, subscription, self._qos) elif isinstance(subscription, str): - if user_callback: + if callable(user_callback): signature = inspect.signature(user_callback) if len(signature.parameters) != 0: raise AddSignalError("Provide a user callback that has no input arguments.") + else: + if user_callback: + self.get_logger().warn("Provided user callback is not a callable, ignoring it.") + def default_callback(): + return None + user_callback = default_callback if message_type == Bool or message_type == Float64 or \ message_type == Float64MultiArray or message_type == Int32 or message_type == String: - self._inputs[parsed_signal_name] = self.create_subscription(message_type, topic_name, - partial(self.__subscription_callback, - attribute_name=subscription, - reader=modulo_readers.read_std_message, - user_callback=user_callback), - self._qos) + self._inputs[parsed_signal_name] = \ + self.create_subscription(message_type, topic_name, + partial(self.__subscription_callback, + attribute_name=subscription, + reader=modulo_readers.read_std_message, + user_callback=user_callback), + self._qos) elif message_type == EncodedState: - self._inputs[parsed_signal_name] = self.create_subscription(message_type, topic_name, - partial(self.__subscription_callback, - attribute_name=subscription, - reader=modulo_readers.read_clproto_message, - user_callback=user_callback), - self._qos) + self._inputs[parsed_signal_name] = \ + self.create_subscription(message_type, topic_name, + partial(self.__subscription_callback, + attribute_name=subscription, + reader=modulo_readers.read_clproto_message, + user_callback=user_callback), + self._qos) else: raise TypeError("The provided message type is not supported to create a component input.") else: diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 1b40a7ff8..b90298815 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.14 + 2.0.15 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 1babaa25e..9ea651092 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.14 + 2.0.15 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From 965a960273cc52253d451b3a423d3d17863b2648 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 22 Sep 2022 08:00:15 +0200 Subject: [PATCH 19/20] Add option to remove signals (#139) * Explicitly reset ROS pointers in communication handlers * Remove signal method (C++) * Remove signal method (Python) * Update tests * Update CHANGELOG * Fix * Clear all signals only on shutdown * 2.0.15 -> 2.0.16 --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- .../modulo_component_interfaces/package.xml | 2 +- .../modulo_components/ComponentInterface.hpp | 61 +++++++++++++++++++ .../modulo_components/LifecycleComponent.hpp | 2 +- .../modulo_components/component_interface.py | 22 +++++++ source/modulo_components/package.xml | 2 +- .../src/LifecycleComponent.cpp | 8 +-- .../component_public_interfaces.hpp | 3 + .../test/cpp/test_component.cpp | 5 +- .../test/cpp/test_component_interface.cpp | 6 +- .../test/cpp/test_lifecycle_component.cpp | 5 +- .../test/python/test_component.py | 5 +- .../test/python/test_component_interface.py | 5 +- .../communication/PublisherHandler.hpp | 10 +++ .../communication/SubscriptionHandler.hpp | 10 +++ source/modulo_core/package.xml | 2 +- 18 files changed, 138 insertions(+), 15 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2d1001cb7..c8c627c0b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,7 @@ Release Versions: - Correctly list rclcpp_components as build dependency (#137) - Add option to declare signals (#136) - Correct default value for user callback on add_input (#138) +- Add option to remove signals (#139) ## 2.0.0 ### August 05, 2022 diff --git a/VERSION b/VERSION index b8061b50d..a14da2902 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.15 +2.0.16 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 6c39130ee..5e142a5a6 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.0.15 +PROJECT_NUMBER = 2.0.16 # 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/package.xml b/source/modulo_component_interfaces/package.xml index ae5795cc5..b95b29e20 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.15 + 2.0.16 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 0b48fe1a8..62304d496 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -241,6 +241,18 @@ class ComponentInterface : public NodeT { */ void declare_output(const std::string& signal_name, const std::string& default_topic = "", bool fixed_topic = false); + /** + * @brief Remove an input from the map of inputs. + * @param signal_name The name of the input + */ + void remove_input(const std::string& signal_name); + + /** + * @brief Remove an output from the map of outputs. + * @param signal_name The name of the output + */ + void remove_output(const std::string& signal_name); + /** * @brief Add and configure an input signal of the component. * @tparam DataT Type of the data pointer @@ -470,6 +482,19 @@ class ComponentInterface : public NodeT { */ void set_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate); + /** + * @brief Remove a signal from the map of inputs or outputs. + * @tparam T The type of the map entry (SubscriptionInterface or PublisherInterface) + * @param signal_name The name of the signal to remove + * @param signal_map The map of signals (either inputs or outputs) + * @param skip_check If true, skip the check if the signal exists in the map and return false otherwise + * @return True if the signal was removed, false if it didn't exist + */ + template + bool remove_signal( + const std::string& signal_name, std::map>& signal_map, bool skip_check = false + ); + /** * @brief Declare a signal to create the topic parameter without adding it to the map of signals. * @param signal_name The name of the signal @@ -807,6 +832,42 @@ inline void ComponentInterface::set_predicate( this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); } +template +template +inline bool ComponentInterface::remove_signal( + const std::string& signal_name, std::map>& signal_map, bool skip_check +) { + if (!skip_check && signal_map.find(signal_name) == signal_map.cend()) { + return false; + } else { + RCLCPP_DEBUG_STREAM(this->get_logger(), "Removing signal '" << signal_name << "'."); + signal_map.at(signal_name).reset(); + return signal_map.erase(signal_name); + } +} + +template +inline void ComponentInterface::remove_input(const std::string& signal_name) { + if (!this->template remove_signal(signal_name, this->inputs_)) { + auto parsed_signal_name = utilities::parse_topic_name(signal_name); + if(!this->template remove_signal(parsed_signal_name, this->inputs_)) { + RCLCPP_DEBUG_STREAM(this->get_logger(), + "Unknown input '" << signal_name << "' (parsed name was '" << parsed_signal_name << "')."); + } + } +} + +template +inline void ComponentInterface::remove_output(const std::string& signal_name) { + if (!this->template remove_signal(signal_name, this->outputs_)) { + auto parsed_signal_name = utilities::parse_topic_name(signal_name); + if(!this->template remove_signal(parsed_signal_name, this->outputs_)) { + RCLCPP_DEBUG_STREAM(this->get_logger(), + "Unknown output '" << signal_name << "' (parsed name was '" << parsed_signal_name << "')."); + } + } +} + template inline void ComponentInterface::declare_signal( const std::string& signal_name, const std::string& type, const std::string& default_topic, bool fixed_topic diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index c39a77a69..99b024218 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -258,7 +258,7 @@ class LifecycleComponent : public ComponentInterface::create_output; diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 85622b6bf..ef84b1ec1 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -330,6 +330,18 @@ def trigger(self, trigger_name: str): self._triggers[trigger_name] = True self._publish_predicate(trigger_name) + def remove_output(self, signal_name): + if signal_name not in self._outputs.keys(): + parsed_signal_name = parse_topic_name(signal_name) + if parsed_signal_name not in self._outputs.keys(): + self.get_logger().debug(f"Unknown output '{signal_name}' (parsed name was '{parsed_signal_name}').") + return + signal_name = parsed_signal_name + if "publisher" in self._outputs[signal_name].keys(): + self.destroy_publisher(self._outputs[signal_name]["publisher"]) + self._outputs.pop(signal_name) + self.get_logger().debug(f"Removing signal '{signal_name}'.") + def _create_output(self, signal_name: str, data: str, message_type: MsgT, clproto_message_type: clproto.MessageType, default_topic: str, fixed_topic: bool) -> str: """ @@ -365,6 +377,16 @@ def _create_output(self, signal_name: str, data: str, message_type: MsgT, clprot except Exception as e: raise AddSignalError(f"{e}") + def remove_input(self, signal_name: str): + if not self.destroy_subscription(self._inputs.pop(signal_name, None)): + parsed_signal_name = parse_topic_name(signal_name) + if not self.destroy_subscription(self._inputs.pop(parsed_signal_name, None)): + self.get_logger().debug(f"Unknown input '{signal_name}' (parsed name was '{parsed_signal_name}').") + return + self.get_logger().debug(f"Removing signal '{parsed_signal_name}'.") + return + self.get_logger().debug(f"Removing signal '{signal_name}'.") + def __subscription_callback(self, message: MsgT, attribute_name: str, reader: Callable, user_callback: Callable): """ Subscription callback for the ROS subscriptions. diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index b90298815..25594999a 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.15 + 2.0.16 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index f6d11237a..6df180e82 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -92,8 +92,7 @@ LifecycleComponent::on_cleanup(const rclcpp_lifecycle::State& previous_state) { } bool LifecycleComponent::handle_cleanup() { - bool result = this->on_cleanup_callback(); - return result && this->cleanup_signals(); + return this->on_cleanup_callback(); } bool LifecycleComponent::on_cleanup_callback() { @@ -190,7 +189,8 @@ LifecycleComponent::on_shutdown(const rclcpp_lifecycle::State& previous_state) { } bool LifecycleComponent::handle_shutdown() { - return this->on_shutdown_callback(); + bool result = this->on_shutdown_callback(); + return result && this->clear_signals(); } bool LifecycleComponent::on_shutdown_callback() { @@ -294,7 +294,7 @@ bool LifecycleComponent::configure_outputs() { return success; } -bool LifecycleComponent::cleanup_signals() { +bool LifecycleComponent::clear_signals() { RCLCPP_DEBUG(this->get_logger(), "Clearing all inputs and outputs."); this->inputs_.clear(); this->outputs_.clear(); diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 436804b34..82da44f5c 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -29,6 +29,7 @@ class ComponentInterfacePublicInterface : public ComponentInterface { using ComponentInterface::triggers_; using ComponentInterface::declare_input; using ComponentInterface::declare_output; + using ComponentInterface::remove_input; using ComponentInterface::add_input; using ComponentInterface::add_service; using ComponentInterface::inputs_; @@ -77,6 +78,7 @@ class ComponentPublicInterface : public Component { ) : Component(node_options, fallback_name) {} using Component::add_output; using Component::outputs_; + using Component::remove_output; }; class LifecycleComponentPublicInterface : public LifecycleComponent { @@ -87,5 +89,6 @@ class LifecycleComponentPublicInterface : public LifecycleComponent { using LifecycleComponent::configure_outputs; using LifecycleComponent::activate_outputs; using LifecycleComponent::outputs_; + using LifecycleComponent::remove_output; }; }// namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component.cpp b/source/modulo_components/test/cpp/test_component.cpp index a8a13576f..a194b589b 100644 --- a/source/modulo_components/test/cpp/test_component.cpp +++ b/source/modulo_components/test/cpp/test_component.cpp @@ -26,7 +26,7 @@ class ComponentTest : public ::testing::Test { std::shared_ptr component_; }; -TEST_F(ComponentTest, AddOutput) { +TEST_F(ComponentTest, AddRemoveOutput) { std::shared_ptr data = make_shared_state(CartesianState::Random("test")); component_->add_output("_tEsT_#1@3", data); EXPECT_TRUE(component_->outputs_.find("test_13") != component_->outputs_.end()); @@ -36,5 +36,8 @@ TEST_F(ComponentTest, AddOutput) { component_->add_output("test_13", new_data); EXPECT_EQ(component_->outputs_.at("test_13")->get_message_pair()->get_type(), modulo_core::communication::MessageType::ENCODED_STATE); + + component_->remove_output("test_13"); + EXPECT_TRUE(component_->outputs_.find("test_13") == component_->outputs_.end()); } } // namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 4cedb9d3e..c93785616 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -90,7 +90,7 @@ TYPED_TEST(ComponentInterfaceTest, DeclareSignal) { EXPECT_TRUE(this->component_->outputs_.find("output") == this->component_->outputs_.end()); } -TYPED_TEST(ComponentInterfaceTest, AddInput) { +TYPED_TEST(ComponentInterfaceTest, AddRemoveInput) { auto data = std::make_shared(true); EXPECT_NO_THROW(this->component_->add_input("_tEsT_#1@3", data)); EXPECT_FALSE(this->component_->inputs_.find("test_13") == this->component_->inputs_.end()); @@ -106,6 +106,10 @@ TYPED_TEST(ComponentInterfaceTest, AddInput) { "test_13", [](const std::shared_ptr) {}); EXPECT_EQ(this->component_->inputs_.at("test_13")->get_message_pair()->get_type(), modulo_core::communication::MessageType::BOOL); + + // remove input + this->component_->remove_input("test_13"); + EXPECT_TRUE(this->component_->inputs_.find("test_13") == this->component_->inputs_.end()); } TYPED_TEST(ComponentInterfaceTest, AddInputWithUserCallback) { diff --git a/source/modulo_components/test/cpp/test_lifecycle_component.cpp b/source/modulo_components/test/cpp/test_lifecycle_component.cpp index 02a4d9361..8d625172b 100644 --- a/source/modulo_components/test/cpp/test_lifecycle_component.cpp +++ b/source/modulo_components/test/cpp/test_lifecycle_component.cpp @@ -26,7 +26,7 @@ class LifecycleComponentTest : public ::testing::Test { std::shared_ptr component_; }; -TEST_F(LifecycleComponentTest, AddOutput) { +TEST_F(LifecycleComponentTest, AddRemoveOutput) { std::shared_ptr data = make_shared_state(CartesianState::Random("test")); component_->add_output("test", data); auto outputs_iterator = component_->outputs_.find("test"); @@ -39,5 +39,8 @@ TEST_F(LifecycleComponentTest, AddOutput) { component_->add_output("test", new_data); EXPECT_EQ(component_->outputs_.at("test")->get_message_pair()->get_type(), modulo_core::communication::MessageType::ENCODED_STATE); + + component_->remove_output("test_13"); + EXPECT_TRUE(component_->outputs_.find("test_13") == component_->outputs_.end()); } } // namespace modulo_components diff --git a/source/modulo_components/test/python/test_component.py b/source/modulo_components/test/python/test_component.py index 0b4aa9d3b..e9e0d9e49 100644 --- a/source/modulo_components/test/python/test_component.py +++ b/source/modulo_components/test/python/test_component.py @@ -8,7 +8,7 @@ def component(ros_context): yield Component('component') -def test_add_output(component): +def test_add_remove_output(component): component.add_output("_tEsT_#1@3", "test", Bool) assert "test_13" in component._outputs.keys() assert component.get_parameter_value("test_13_topic") == "~/test_13" @@ -19,3 +19,6 @@ def test_add_output(component): component.add_output("test_13", "test", String) assert component._outputs["test_13"]["message_type"] == Bool + + component.remove_output("test_13") + assert "test_13" not in component._inputs.keys() diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index cb25b2e6a..a298afe51 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -61,7 +61,7 @@ def test_declare_signal(component_interface): assert "test_again" not in component_interface._outputs.keys() -def test_add_input(component_interface): +def test_add_remove_input(component_interface): component_interface.add_input("_tEsT_#1@3", "test", Bool) assert "test_13" in component_interface._inputs.keys() assert component_interface.get_parameter_value("test_13_topic") == "~/test_13" @@ -73,6 +73,9 @@ def test_add_input(component_interface): component_interface.add_input("test_13", "test", String) assert component_interface._inputs["test_13"].msg_type == Bool + component_interface.remove_input("test_13") + assert "test_13" not in component_interface._inputs.keys() + def test_add_service(component_interface): def empty_callback(): diff --git a/source/modulo_core/include/modulo_core/communication/PublisherHandler.hpp b/source/modulo_core/include/modulo_core/communication/PublisherHandler.hpp index 60e6630e2..f1a34de1c 100644 --- a/source/modulo_core/include/modulo_core/communication/PublisherHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/PublisherHandler.hpp @@ -24,6 +24,11 @@ class PublisherHandler : public PublisherInterface { */ PublisherHandler(PublisherType type, std::shared_ptr publisher); + /** + * @brief Destructor to explicitly reset the publisher pointer. + */ + ~PublisherHandler() override; + /** * @brief Activate the ROS publisher if applicable. * @throws modulo_core::exceptions::NullPointerException if the publisher pointer is null @@ -58,6 +63,11 @@ template PublisherHandler::PublisherHandler(PublisherType type, std::shared_ptr publisher) : PublisherInterface(type), publisher_(std::move(publisher)) {} +template +PublisherHandler::~PublisherHandler() { + this->publisher_.reset(); +} + template void PublisherHandler::on_activate() { if (this->publisher_ == nullptr) { diff --git a/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp b/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp index 5edb270b2..f3377ba19 100644 --- a/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp @@ -19,6 +19,11 @@ class SubscriptionHandler : public SubscriptionInterface { */ explicit SubscriptionHandler(std::shared_ptr message_pair = nullptr); + /** + * @brief Destructor to explicitly reset the subscription pointer. + */ + ~SubscriptionHandler() override; + /** * @brief Getter of the ROS subscription. */ @@ -71,6 +76,11 @@ class SubscriptionHandler : public SubscriptionInterface { std::function user_callback_ = []{}; ///< User callback to be executed after the subscription callback }; +template +SubscriptionHandler::~SubscriptionHandler() { + this->subscription_.reset(); +} + template std::shared_ptr> SubscriptionHandler::get_subscription() const { return this->subscription_; diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 9ea651092..eb0ccb33f 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.15 + 2.0.16 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard From 86171a9402d915ee29d4572d1122334fa2f01ea5 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Fri, 21 Oct 2022 15:13:15 +0200 Subject: [PATCH 20/20] Release version 2.1.0 --- CHANGELOG.md | 12 +++++++++++- VERSION | 2 +- doxygen/doxygen.conf | 2 +- source/modulo_component_interfaces/package.xml | 2 +- source/modulo_components/package.xml | 2 +- source/modulo_core/package.xml | 2 +- 6 files changed, 16 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c8c627c0b..1ac428ed7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,12 +1,22 @@ # CHANGELOG Release Versions: +- [2.1.0](#210) - [2.0.0](#200) - [1.1.0](#110) - [1.0.0](#100) - [Pre-release versions](#pre-release-versions) -## Upcoming changes (in development) +## 2.1.0 +### October 21, 2022 + +Version 2.1.0 contains several smaller improvements to the Modulo project, such as more consistency with parameter +naming, declaring and removing of signals, and cleaner exception handling and logging. Another new feature is the +ability to provide a user-defined callback for input signals. + +There have also been some changes to support both galactic and humble releases of ROS. + +### Changes - Support multiple build test actions (#117) - Manually export modulo_core dependencies (#118) diff --git a/VERSION b/VERSION index a14da2902..7ec1d6db4 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.0.16 +2.1.0 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 5e142a5a6..6f6171936 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.0.16 +PROJECT_NUMBER = 2.1.0 # 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/package.xml b/source/modulo_component_interfaces/package.xml index b95b29e20..924c85f91 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.0.16 + 2.1.0 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard TODO: License declaration diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 25594999a..3ba33f124 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.0.16 + 2.1.0 TODO Baptiste Busch Enrico Eberhard diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index eb0ccb33f..9a4adb004 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.0.16 + 2.1.0 Modulo Core communication and translation utilities for interoperability with EPFL Control Libraries Baptiste Busch Enrico Eberhard