├── .github └── workflows │ └── ci.yaml ├── .gitignore ├── 3rd-party-licenses.txt ├── CONTRIBUTING.md ├── CPPLINT.cfg ├── LICENSE ├── NOTICE ├── README.md ├── codecov.yml ├── launch_system_modes ├── CHANGELOG.rst ├── README.md ├── launch_system_modes │ ├── __init__.py │ ├── actions │ │ ├── __init__.py │ │ ├── node.py │ │ ├── system.py │ │ └── system_part.py │ ├── event_handlers │ │ ├── __init__.py │ │ ├── on_mode_changed.py │ │ └── on_state_transition.py │ └── events │ │ ├── __init__.py │ │ ├── change_mode.py │ │ ├── change_state.py │ │ ├── mode_changed.py │ │ └── state_transition.py ├── package.xml ├── resource │ └── launch_system_modes ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ ├── test_launch_system_modes │ ├── test_change_mode.py │ ├── test_change_state.py │ ├── test_mode_changed.py │ ├── test_node.py │ ├── test_state_transition.py │ └── test_system.py │ └── test_pep257.py ├── system_modes ├── CHANGELOG.rst ├── CMakeLists.txt ├── QUALITY_DECLARATION.md ├── README.md ├── doc │ ├── lifecycle-extended.png │ └── requirements.md ├── include │ └── system_modes │ │ ├── mode.hpp │ │ ├── mode_handling.hpp │ │ ├── mode_impl.hpp │ │ ├── mode_inference.hpp │ │ ├── mode_manager.hpp │ │ ├── mode_monitor.hpp │ │ └── mode_observer.hpp ├── launch │ ├── mode_manager.launch.py │ └── mode_monitor.launch.py ├── package.xml ├── src │ ├── mode_manager_node.cpp │ ├── mode_monitor_node.cpp │ └── system_modes │ │ ├── mode.cpp │ │ ├── mode_handling.cpp │ │ ├── mode_impl.cpp │ │ ├── mode_inference.cpp │ │ ├── mode_manager.cpp │ │ ├── mode_monitor.cpp │ │ └── mode_observer.cpp └── test │ ├── launchtest │ ├── manager_and_monitor.launch.py.in │ ├── manager_and_monitor.py │ ├── manager_and_monitor_expected_output.regex │ ├── manager_and_monitor_modes.yaml │ ├── modes_observer.launch.py.in │ ├── modes_observer.py │ ├── modes_observer_expected_output.regex │ ├── modes_observer_modes.yaml │ ├── modes_observer_test_node.cpp │ ├── redundant_mode_changes.launch.py.in │ ├── redundant_mode_changes.py │ ├── redundant_mode_changes_expected_output.regex │ ├── redundant_mode_changes_modes.yaml │ ├── two_independent_hierarchies.launch.py.in │ ├── two_independent_hierarchies.py │ ├── two_independent_hierarchies_expected_output.regex │ ├── two_independent_hierarchies_modes.yaml │ ├── two_lifecycle_nodes.launch.py.in │ ├── two_lifecycle_nodes.py │ ├── two_lifecycle_nodes_expected_output.regex │ ├── two_lifecycle_nodes_modes.yaml │ ├── two_mixed_nodes.launch.py.in │ ├── two_mixed_nodes.py │ ├── two_mixed_nodes_expected_output.regex │ └── two_mixed_nodes_modes.yaml │ ├── modefiles.h.in │ ├── test_default_mode.cpp │ ├── test_mode.cpp │ ├── test_mode_handling.cpp │ ├── test_mode_inference.cpp │ ├── test_mode_manager.cpp │ ├── test_mode_monitor.cpp │ ├── test_mode_observer.cpp │ ├── test_modes.yaml │ ├── test_modes_rules.yaml │ ├── test_modes_wrong.yaml │ └── test_state_and_mode_struct.cpp ├── system_modes_examples ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── doc │ ├── screenshot-manager-deviation.png │ ├── screenshot-manager.png │ ├── screenshot-monitor-active.png │ ├── screenshot-monitor-inactive.png │ ├── screenshot-monitor-moderate.png │ ├── screenshot-monitor-performance.png │ └── screenshot-monitor.png ├── example_modes.yaml ├── example_modes_with_namespace.yaml ├── launch │ ├── drive_base.launch.py │ ├── example_system.launch.py │ ├── example_system_start_drive_base.launch.py │ ├── example_system_started.launch.py │ └── manipulator.launch.py ├── package.xml └── src │ ├── drive_base.cpp │ └── manipulator.cpp ├── system_modes_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── msg │ ├── Mode.msg │ └── ModeEvent.msg ├── package.xml └── srv │ ├── ChangeMode.srv │ ├── GetAvailableModes.srv │ └── GetMode.srv └── test_launch_system_modes ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── package.xml └── test ├── expected_output.regex ├── node_test.launch.py.in ├── system_test.launch.py.in └── test_modes.yaml /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | name: "CI: humble, jazzy, rolling" 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | schedule: 9 | # Run once per day to detect broken dependencies. 10 | - cron: '17 6 * * *' 11 | 12 | jobs: 13 | build: 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | fail-fast: false 17 | matrix: 18 | include: 19 | - ros_distribution: "humble" 20 | os: "ubuntu-22.04" 21 | - ros_distribution: "jazzy" 22 | os: "ubuntu-24.04" 23 | - ros_distribution: "rolling" 24 | os: "ubuntu-24.04" 25 | steps: 26 | - uses: actions/checkout@v3 27 | - uses: ros-tooling/setup-ros@v0.7 28 | with: 29 | required-ros-distributions: ${{ matrix.ros_distribution }} 30 | - uses : ros-tooling/action-ros-ci@v0.3 31 | with: 32 | package-name: "launch_system_modes system_modes system_modes_examples system_modes_msgs test_launch_system_modes " 33 | target-ros2-distro: ${{ matrix.ros_distribution }} 34 | colcon-defaults: | 35 | { 36 | "build": { 37 | "mixin": ["coverage-gcc", "coverage-pytest"] 38 | }, 39 | "test": { 40 | "mixin": ["coverage-pytest"] 41 | } 42 | } 43 | colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml 44 | - uses: codecov/codecov-action@v3 45 | with: 46 | file: ros_ws/lcov/total_coverage.info 47 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | __pycache__/ 3 | -------------------------------------------------------------------------------- /3rd-party-licenses.txt: -------------------------------------------------------------------------------- 1 | Third Party Licenses 2 | ==================== 3 | 4 | This repository does not include any third-party components. 5 | -------------------------------------------------------------------------------- /CPPLINT.cfg: -------------------------------------------------------------------------------- 1 | set noparent 2 | filter=-build/header_guard,-runtime/string 3 | -------------------------------------------------------------------------------- /NOTICE: -------------------------------------------------------------------------------- 1 | # This is the official list of system_modes copyright holders and authors. 2 | # 3 | # Often employers or academic institutions have ownership over code that is 4 | # written in certain circumstances, so please do due diligence to ensure that 5 | # you have the right to submit the code. 6 | # 7 | # When adding J Random Contributor's name to this file, either J's name on its 8 | # own or J's name associated with J's organization's name should be added, 9 | # depending on whether J's employer (or academic institution) has ownership 10 | # over code that is written for this project. 11 | # 12 | # How to add names to this file: 13 | # Individual's name . 14 | # 15 | # If Individual's organization is copyright holder of her contributions add the 16 | # organization's name, optionally also the contributor's name: 17 | # 18 | # Organization's name 19 | # Individual's name 20 | # 21 | # Please keep the list sorted. 22 | 23 | Open Robotics 24 | Jacob Perron 25 | Scott Logan 26 | 27 | Robert Bosch GmbH 28 | Arne Nordmann 29 | Ralph Lange 30 | 31 | BSH Hausgeräte GmbH 32 | Thilak Raj 33 | 34 | Arne Nordmann 35 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS 2 System Modes 2 | 3 | [![License](https://img.shields.io/badge/License-Apache%202-blue.svg)](https://github.com/micro-ROS/system_modes/blob/master/LICENSE) 4 | [![Build status](http://build.ros2.org/job/Hdev__system_modes__ubuntu_jammy_amd64/badge/icon?subject=Build%20farm%3A%20Humble)](http://build.ros2.org/job/Hdev__system_modes__ubuntu_jammy_amd64/) 5 | [![Build status](http://build.ros2.org/job/Jdev__system_modes__ubuntu_noble_amd64/badge/icon?subject=Build%20farm%3A%20Jazzy)](http://build.ros2.org/job/Jdev__system_modes__ubuntu_noble_amd64/) 6 | [![Build status](http://build.ros2.org/job/Rdev__system_modes__ubuntu_noble_amd64/badge/icon?subject=Build%20farm%3A%20Rolling)](http://build.ros2.org/job/Rdev__system_modes__ubuntu_noble_amd64/) 7 | [![Build status](https://github.com/micro-ROS/system_modes/workflows/CI%3A%20humble%2C%20jazzy%2C%20rolling/badge.svg)](https://github.com/micro-ROS/system_modes/actions) 8 | [![Code coverage](https://codecov.io/gh/micro-ROS/system_modes/branch/master/graph/badge.svg)](https://codecov.io/gh/micro-ROS/system_modes) 9 | 10 | This repository explores a system modes concept that is implemented for ROS 2 in these packages: 11 | * [system_modes_msgs](./system_modes_msgs/) provides the message types and services for system modes 12 | * [system_modes](./system_modes/) provides a library for system mode inference, a mode manager, and a mode monitor 13 | * [system_modes_examples](./system_modes_examples/) implements a simple example 14 | * [launch_system_modes](./launch_system_modes/) launch actions, events, and event handlers for system modes 15 | * [test_launch_system_modes](./test_launch_system_modes/) launch test for the launch_system_modes` package 16 | 17 | For further information, please contact [Arne Nordmann](https://github.com/norro) or [Ralph Lange](https://github.com/ralph-lange). 18 | 19 | ## Purpose of the Project 20 | 21 | This software is not ready for production use. It has neither been developed nor 22 | tested for a specific use case. However, the license conditions of the 23 | applicable Open Source licenses allow you to adapt the software to your needs. 24 | Before using it in a safety relevant setting, make sure that the software 25 | fulfills your requirements and adjust it according to any applicable safety 26 | standards, e.g., ISO 26262. 27 | 28 | ## How to Build, Test, Install, and Use 29 | 30 | After you cloned this repository into your ROS 2 workspace folder, you may build and install the [system_modes](./system_modes/) package and the [system_modes_examples](./system_modes_examples/) package using colcon: 31 | $ `colcon build --packages-select-regex system_modes` 32 | 33 | Have a look at the [system_modes_examples](./system_modes_examples/) documentation to try your installation. 34 | 35 | For using this package and designing system modes for your system, please refer to the [How to Apply](./system_modes/README.md#how-to-apply) section. 36 | 37 | ## License 38 | 39 | ROS 2 System Modes are open-sourced under the Apache-2.0 license. See the 40 | [LICENSE](LICENSE) file for details. 41 | 42 | For a list of other open-source components included in ROS 2 system_modes, 43 | see the file [3rd-party-licenses.txt](3rd-party-licenses.txt). 44 | 45 | ## Quality assurance 46 | 47 | The colcon_test tool is used for quality assurances, which includes cpplint, uncrustify, flake8, xmllint and various other tools. 48 | 49 | Unit tests based on [gtest](https://github.com/google/googletest) are located in the [./system_modes/test](system_modes/test) folder. 50 | 51 | ## Known Issues/Limitations 52 | 53 | Please notice the following issues/limitations: 54 | 55 | * Currently, (sub-)systems managed by the mode manager are not recognized by the `ros2 lifecycle` tool (*"Node not found"*). So to trigger lifecycle transitions in (sub-)systems, you have to go with the `ros2 service call` tool. Check the [system_modes_examples](./system_modes_examples/) documentation for example calls. 56 | * The [Error Handling and Rules](./system_modes/README.md#error-handling-and-rules-experimental) feature is still experimental and might be subject to major changes. However, if no rules are specified in the model file, this feature is not used. 57 | * The mode inference and the error handling and rules feature do not work as intended if some of the involved nodes are non-lifecycle nodes. 58 | 59 | ## Acknowledgments 60 | 61 | This activity has received funding from the European Research Council (ERC) under the European Union's Horizon 2020 research and innovation programme (grant agreement n° 780785). 62 | -------------------------------------------------------------------------------- /codecov.yml: -------------------------------------------------------------------------------- 1 | ignore: 2 | - "system_modes_examples" 3 | - "system_modes/test" 4 | - "**/*_node.cpp" 5 | -------------------------------------------------------------------------------- /launch_system_modes/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package system_modes_examples 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.0 (2020-07-21) 6 | ------------------ 7 | 8 | * More flexibility in specifying the default mode, any mode can be now default mode 9 | https://github.com/micro-ROS/system_modes/issues/69 10 | 11 | 0.8.0 (2020-04-22) 12 | ------------------ 13 | 14 | * Launch integration, i.e. launch actions, events, and event handlers for system modes 15 | -------------------------------------------------------------------------------- /launch_system_modes/README.md: -------------------------------------------------------------------------------- 1 | General information about this repository, including legal information, build instructions and known issues/limitations, can be found in the [README](../README.md) of the repository root. 2 | 3 | # The launch_system_modes package 4 | 5 | This [ROS 2](https://index.ros.org/doc/ros2/) package provides a launch actions, events, and event handlers for the use of the [system_modes](../system_modes/) package. 6 | 7 | General information about this repository, including legal information, project context, build instructions and known issues/limitations, are given in [README.md](../README.md) in the repository root. 8 | 9 | ## Launch System Modes Package 10 | 11 | The actions, events, and event handlers implemented for system modes are: 12 | 13 | ### Actions 14 | 15 | Two launch actions are implemented for system modes: 16 | 17 | * `System`: Declares a *system*, consisting of further system parts, allowing system mode specific launch events and event handlers. 18 | * `Node`: Declares a *node*, i.e. a lifecycle node with system modes. It inherits from the [launch_ros/lifecycle_node](https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/actions/lifecycle_node.py) action and allows further system mode specific events and event handlers. 19 | 20 | ### Events 21 | 22 | * `ChangeMode`: Trigger a mode change in a `System` or `Node` 23 | * `ChangeState`: Trigger a state transition in a `System`, since [launch_ros/ChangeState](https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/events/lifecycle/change_state.py) only works for lifecycle nodes, not systems. 24 | * `ModeChanged`: Emitted when a `System` or `Node` changed its mode. 25 | * `StateTransition`: Emitted when a `System` changed its state, since [launch_ros/StateTransition](https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/events/lifecycle/state_transition.py) only works for lifecycle nodes, not systems. 26 | 27 | ### Event Handlers 28 | 29 | * `OnModeChanged`: Event handler for mode changes of a `System` or `Node` 30 | * `OnStateTransition`: Event handler for state transitions of a `System` 31 | 32 | ## Examples 33 | 34 | Two examples show the use of launch_system_modes: 35 | 36 | 1. [system_modes_examples/launch/example_system_start_drive_base.launch.py](../system_modes_examples/launch/example_system_start_drive_base.launch.py) starts an *actuation* system with two system parts, the nodes *drive_base* and *manipulator*. It will then: 37 | 1. trigger a *configure* transition for the *drive_base* system part ([lines 62 - 65](../system_modes_examples/launch/example_system_start_drive_base.launch.py#L62-L65)) 38 | 1. a state change handler ([lines 86 - 90](../system_modes_examples/launch/example_system_start_drive_base.launch.py#L86-L90)) notices the successful transition and triggers an *activate* transition for the *drive_base* system part ([lines 67 - 71](../system_modes_examples/launch/example_system_start_drive_base.launch.py#L67-L71)) 39 | 1. another state change handler ([lines 92 - 96](../system_modes_examples/launch/example_system_start_drive_base.launch.py#L92-L96)) notices the successful transition to *active* and triggers a mode change of the *drive_base* system part to its default mode ([lines 73 - 77](../system_modes_examples/launch/example_system_start_drive_base.launch.py#L73-L77)) 40 | 1. a mode change handler ([lines 98 - 102](../system_modes_examples/launch/example_system_start_drive_base.launch.py#L98-L102)) notices the successful transition to the default mode and triggers a mode change of the *drive_base* system part to its *FAST* mode ([lines 79 - 83](../system_modes_examples/launch/example_system_start_drive_base.launch.py#L79-L83)) 41 | 1. [system_modes_examples/launch/example_system_started.launch.py](../system_modes_examples/launch/example_system_started.launch.py) starts the same system, but uses according events and event handlers for the *system* instead. It will: 42 | 1. trigger a *configure* transition for the *actuation* system ([lines 60 - 63](../system_modes_examples/launch/example_system_started.launch.py#L62-L65)) 43 | 1. a state change handler ([lines 78 - 82](../system_modes_examples/launch/example_system_started.launch.py#L78-L82)) notices the successful transition and triggers an *activate* transition for the *actuation* system ([lines 65 - 69](../system_modes_examples/launch/example_system_started.launch.py#L65-L69)) 44 | 1. a mode change handler ([lines 84 - 88](../system_modes_examples/launch/example_system_started.launch.py#L84-L88)) notices the successful transition to the default mode and triggers a mode change of the *actuation* system to its *PERMORMANCE* mode ([lines 71 - 75](../system_modes_examples/launch/example_system_started.launch.py#L71-L75)) 45 | -------------------------------------------------------------------------------- /launch_system_modes/launch_system_modes/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Main entry point for the `launch_system_modes` package.""" 16 | 17 | from . import actions 18 | from . import event_handlers 19 | from . import events 20 | 21 | __all__ = [ 22 | 'actions', 23 | 'event_handlers', 24 | 'events', 25 | ] 26 | -------------------------------------------------------------------------------- /launch_system_modes/launch_system_modes/actions/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Package for launch_system_modes.actions.""" 16 | 17 | from .node import Node 18 | from .system import System 19 | from .system_part import SystemPart 20 | 21 | __all__ = [ 22 | 'Node', 23 | 'System', 24 | 'SystemPart', 25 | ] 26 | -------------------------------------------------------------------------------- /launch_system_modes/launch_system_modes/actions/system_part.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | 16 | class SystemPart: 17 | 18 | def __init__(self, name, namespace): 19 | """ 20 | Construct a SystemPart action. 21 | 22 | This is a marker class to be able to match systems and lifecycle nodes within the event 23 | handlers. 24 | 25 | :param name: The name of the lifecycle node or system. 26 | :param namespace: The namespace of the lifecycle node or system. 27 | """ 28 | self.__part_name = name 29 | self.__namespace = namespace 30 | 31 | def get_name(self): 32 | return self.__part_name 33 | -------------------------------------------------------------------------------- /launch_system_modes/launch_system_modes/event_handlers/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Package for launch_system_modes.event_handlers.""" 16 | 17 | from .on_mode_changed import OnModeChanged 18 | from .on_state_transition import OnStateTransition 19 | 20 | __all__ = [ 21 | 'OnModeChanged', 22 | 'OnStateTransition', 23 | ] 24 | -------------------------------------------------------------------------------- /launch_system_modes/launch_system_modes/event_handlers/on_mode_changed.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Module for OnModeChanged class.""" 16 | 17 | from typing import Callable 18 | from typing import Optional 19 | from typing import Text 20 | 21 | from launch.event import Event 22 | from launch.event_handler import EventHandler 23 | from launch.some_actions_type import SomeActionsType 24 | from launch.some_substitutions_type import SomeSubstitutionsType 25 | 26 | from ..actions import SystemPart 27 | from ..events import ModeChanged 28 | 29 | 30 | class OnModeChanged(EventHandler): 31 | """Convenience class for handling a mode change of a system part.""" 32 | 33 | def __init__( 34 | self, 35 | *, 36 | entities: SomeActionsType, 37 | target_system_part: SystemPart = None, 38 | goal_mode: Optional[SomeSubstitutionsType] = None, 39 | matcher: Optional[Callable[[Event], bool]] = None, 40 | **kwargs 41 | ) -> None: 42 | """ 43 | Create an OnModeChanged event handler. 44 | 45 | There are several matching options, each of which is compared with the 46 | event and must match it to have the handler handle the event. 47 | Passing None for any of them will prevent that matching option from 48 | being considered (and therefore not required) when matching the event. 49 | If matcher is given, the other conditions are not considered. 50 | """ 51 | if not isinstance(target_system_part, (SystemPart, type(None))): 52 | raise RuntimeError("OnModeChanged requires a 'SystemPart' action as the target") 53 | # Handle optional matcher argument. 54 | self.__custom_matcher = matcher 55 | if self.__custom_matcher is None: 56 | self.__custom_matcher = ( 57 | lambda event: ( 58 | isinstance(event, ModeChanged) and ( 59 | target_system_part is None or 60 | event.action == target_system_part 61 | ) and (event.goal_mode == goal_mode) 62 | ) 63 | ) 64 | # Call parent init. 65 | super().__init__( 66 | matcher=self.__custom_matcher, 67 | entities=entities, 68 | **kwargs 69 | ) 70 | self.__goal_mode = goal_mode 71 | self.__target_system_part = target_system_part 72 | print('OnModeChanged event handler initialized for part "' 73 | + self.__target_system_part.get_name() 74 | + '" and mode: ' + self.__goal_mode) 75 | 76 | @property 77 | def handler_description(self) -> Text: 78 | """Return the string description of the handler.""" 79 | return '' 80 | 81 | @property 82 | def matcher_description(self): 83 | """Return the string description of the matcher.""" 84 | if self.__target_system_part is None: 85 | return 'event == ModeChanged' 86 | return 'event == ModeChanged and event.action == LifecycleNode({})'.format( 87 | hex(id(self.__target_system_part)) 88 | ) 89 | -------------------------------------------------------------------------------- /launch_system_modes/launch_system_modes/event_handlers/on_state_transition.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Module for OnStateTransition class.""" 16 | 17 | from typing import Callable 18 | from typing import Optional 19 | from typing import Text 20 | 21 | from launch.event import Event 22 | from launch.event_handler import EventHandler 23 | from launch.some_actions_type import SomeActionsType 24 | from launch.some_substitutions_type import SomeSubstitutionsType 25 | 26 | from ..actions import System 27 | from ..events import StateTransition 28 | 29 | 30 | class OnStateTransition(EventHandler): 31 | """Convenience class for handling a mode change of a system part.""" 32 | 33 | def __init__( 34 | self, 35 | *, 36 | entities: SomeActionsType, 37 | target_system_part: System = None, 38 | transition: Optional[SomeSubstitutionsType] = None, 39 | start_state: Optional[SomeSubstitutionsType] = None, 40 | goal_state: Optional[SomeSubstitutionsType] = None, 41 | matcher: Optional[Callable[[Event], bool]] = None, 42 | **kwargs 43 | ) -> None: 44 | """ 45 | Create an OnStateTransition event handler. 46 | 47 | There are several matching options, each of which is compared with the 48 | event and must match it to have the handler handle the event. 49 | Passing None for any of them will prevent that matching option from 50 | being considered (and therefore not required) when matching the event. 51 | If matcher is given, the other conditions are not considered. 52 | """ 53 | if not isinstance(target_system_part, (System, type(None))): 54 | raise RuntimeError("OnStateTransition requires a 'System' action as the target") 55 | 56 | self.__target_system_part = target_system_part 57 | # Handle optional matcher argument. 58 | self.__custom_matcher = matcher 59 | if self.__custom_matcher is None: 60 | self.__custom_matcher = ( 61 | lambda event: ( 62 | isinstance(event, StateTransition) and ( 63 | target_system_part is None or 64 | event.action == target_system_part 65 | ) and ( 66 | transition is None or 67 | event.transition == transition 68 | ) and ( 69 | start_state is None or 70 | event.start_state == start_state 71 | ) and ( 72 | goal_state is None or 73 | event.goal_state == goal_state 74 | ) 75 | ) 76 | ) 77 | # Call parent init. 78 | super().__init__( 79 | matcher=self.__custom_matcher, 80 | entities=entities, 81 | **kwargs 82 | ) 83 | print('OnStateTransition event handler initialized for part "' 84 | + target_system_part.get_name() 85 | + '" and target state: ' + goal_state) 86 | 87 | @property 88 | def handler_description(self) -> Text: 89 | """Return the string description of the handler.""" 90 | return '' 91 | 92 | @property 93 | def matcher_description(self): 94 | """Return the string description of the matcher.""" 95 | if self.__target_system_part is None: 96 | return 'event == StateTransition' 97 | return 'event == StateTransition and event.action == System({})'.format( 98 | hex(id(self.__target_system_part)) 99 | ) 100 | -------------------------------------------------------------------------------- /launch_system_modes/launch_system_modes/events/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Package for launch_system_modes.events.""" 16 | 17 | from .change_mode import ChangeMode 18 | from .change_state import ChangeState 19 | from .mode_changed import ModeChanged 20 | from .state_transition import StateTransition 21 | 22 | __all__ = [ 23 | 'ChangeMode', 24 | 'ChangeState', 25 | 'ModeChanged', 26 | 'StateTransition', 27 | ] 28 | -------------------------------------------------------------------------------- /launch_system_modes/launch_system_modes/events/change_mode.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Module for ChangeMode event.""" 16 | 17 | from typing import Callable 18 | 19 | from launch.event import Event 20 | 21 | if False: 22 | from launch_system_modes.actions import SystemPart # noqa: F401 23 | 24 | 25 | class ChangeMode(Event): 26 | """Event emitted when a system mode change is requested for a system or node.""" 27 | 28 | name = 'launch_system_modes.events.ChangeMode' 29 | 30 | def __init__( 31 | self, 32 | *, 33 | system_part_matcher: Callable[['SystemPart'], bool], 34 | mode_name: str 35 | ) -> None: 36 | """ 37 | Create a ChangeMode event. 38 | 39 | :param: system_part_matcher is a callable which returns True if the 40 | given system part should be affected by this event. 41 | :param: mode_name is the name of the requested mode 42 | """ 43 | super().__init__() 44 | self.__system_part_matcher = system_part_matcher 45 | self.__mode_name = mode_name 46 | print('ChangeMode event initialized for mode: '+mode_name) 47 | 48 | @property 49 | def system_part_matcher(self) -> Callable[['SystemPart'], bool]: 50 | """Getter for system_part_matcher.""" 51 | return self.__system_part_matcher 52 | 53 | @property 54 | def mode_name(self) -> int: 55 | """Getter for mode_name.""" 56 | return self.__mode_name 57 | -------------------------------------------------------------------------------- /launch_system_modes/launch_system_modes/events/change_state.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Module for ChangeState event.""" 16 | 17 | from typing import Callable 18 | 19 | from launch.event import Event 20 | 21 | if False: 22 | from ..actions import System # noqa: F401 23 | 24 | 25 | class ChangeState(Event): 26 | """Event emitted when a state change is requested for a *system*.""" 27 | 28 | name = 'launch_system_modes.events.ChangeState' 29 | 30 | def __init__( 31 | self, 32 | *, 33 | system_part_matcher: Callable[['System'], bool], 34 | transition_id: int 35 | ) -> None: 36 | """ 37 | Create a ChangeState event. 38 | 39 | :param: system_part_matcher is a callable which returns True if the 40 | given lifecycle node should be affected by this event. 41 | :param: transition_id is the name of the requested mode 42 | """ 43 | super().__init__() 44 | self.__system_part_matcher = system_part_matcher 45 | self.__transition_id = transition_id 46 | if transition_id == 1: 47 | print('ChangeState event initialized for transition: configure') 48 | elif transition_id == 3: 49 | print('ChangeState event initialized for transition: activate') 50 | else: 51 | print('ChangeState event initialized for transition: ' + str(transition_id)) 52 | 53 | @property 54 | def system_part_matcher(self) -> Callable[['System'], bool]: 55 | """Getter for system_part_matcher.""" 56 | return self.__system_part_matcher 57 | 58 | @property 59 | def transition_id(self) -> int: 60 | """Getter for transition_id.""" 61 | return self.__transition_id 62 | -------------------------------------------------------------------------------- /launch_system_modes/launch_system_modes/events/mode_changed.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Module for ModeChanged event.""" 16 | 17 | from typing import Text 18 | 19 | from launch.event import Event 20 | 21 | import system_modes_msgs.msg 22 | 23 | if False: 24 | from ..actions import SystemPart # noqa: F401 25 | 26 | 27 | class ModeChanged(Event): 28 | """Event emitted when a system mode changed.""" 29 | 30 | name = 'launch_ros.events.ModeChanged' 31 | 32 | def __init__( 33 | self, 34 | *, 35 | action: 'SystemPart', 36 | msg: system_modes_msgs.msg.ModeEvent 37 | ) -> None: 38 | """ 39 | Create a ModeChanged event. 40 | 41 | :param: action the instance of class::`SystemPart` that generated this event 42 | :param: msg the instance of the ROS message ModeEvent that generated this event 43 | """ 44 | super().__init__() 45 | self.__action = action 46 | self.__msg = msg 47 | self.__timestamp = msg.timestamp 48 | self.__start_mode = msg.start_mode.label 49 | self.__goal_mode = msg.goal_mode.label 50 | print('ModeChanged event created: ' + self.__action.get_name() + ' changed to ' 51 | + self.__goal_mode) 52 | 53 | @property 54 | def action(self) -> 'SystemPart': 55 | """Getter for action.""" 56 | return self.__action 57 | 58 | @property 59 | def msg(self) -> system_modes_msgs.msg.ModeEvent: 60 | """Getter for msg.""" 61 | return self.__msg 62 | 63 | @property 64 | def timestamp(self) -> int: 65 | """Getter for timestamp.""" 66 | return self.__timestamp 67 | 68 | @property 69 | def start_mode(self) -> Text: 70 | """Getter for start_mode.""" 71 | return self.__start_mode 72 | 73 | @property 74 | def goal_mode(self) -> Text: 75 | """Getter for goal_mode.""" 76 | return self.__goal_mode 77 | -------------------------------------------------------------------------------- /launch_system_modes/launch_system_modes/events/state_transition.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Module for StateTransition event.""" 16 | 17 | from typing import Text 18 | 19 | from launch.event import Event 20 | 21 | import lifecycle_msgs.msg 22 | 23 | if False: 24 | from ..actions import System # noqa: F401 25 | 26 | 27 | class StateTransition(Event): 28 | """Event emitted when a state changed.""" 29 | 30 | name = 'launch_system_modes.events.StateTransition' 31 | 32 | def __init__( 33 | self, 34 | *, 35 | action: 'System', 36 | msg: lifecycle_msgs.msg.TransitionEvent 37 | ) -> None: 38 | """ 39 | Create a StateTransition event. 40 | 41 | :param: action the instance of class::`System` that generated this event 42 | :param: msg the instance of the ROS message TransitionEvent that generated this event 43 | """ 44 | super().__init__() 45 | self.__action = action 46 | self.__msg = msg 47 | self.__transition = msg.transition.label 48 | self.__start_state = msg.start_state.label 49 | self.__goal_state = msg.goal_state.label 50 | print('StateTransition event initialized for: ' + self.__transition) 51 | 52 | @property 53 | def action(self) -> 'System': 54 | """Getter for action.""" 55 | return self.__action 56 | 57 | @property 58 | def msg(self) -> lifecycle_msgs.msg.TransitionEvent: 59 | """Getter for msg.""" 60 | return self.__msg 61 | 62 | @property 63 | def transition(self) -> Text: 64 | """Getter for the transition.""" 65 | return self.__transition 66 | 67 | @property 68 | def start_state(self) -> Text: 69 | """Getter for the start state.""" 70 | return self.__start_state 71 | 72 | @property 73 | def goal_state(self) -> Text: 74 | """Getter for the goal state.""" 75 | return self.__goal_state 76 | -------------------------------------------------------------------------------- /launch_system_modes/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | launch_system_modes 7 | 0.9.0 8 | 9 | System modes specific extensions to the launch tool, i.e. launch actions, events, and event 10 | handlers for system modes. 11 | 12 | Arne Nordmann 13 | Ralph Lange 14 | Apache License 2.0 15 | 16 | ament_index_python 17 | launch 18 | system_modes_msgs 19 | osrf_pycommon 20 | rclpy 21 | python3-importlib-metadata 22 | python3-yaml 23 | 24 | ament_copyright 25 | ament_flake8 26 | ament_pep257 27 | python3-pytest 28 | 29 | 30 | ament_python 31 | 32 | 33 | -------------------------------------------------------------------------------- /launch_system_modes/resource/launch_system_modes: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/micro-ROS/system_modes/6153cb8263c0dcf5f0f7015b795418e374f4695a/launch_system_modes/resource/launch_system_modes -------------------------------------------------------------------------------- /launch_system_modes/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/launch_system_modes 3 | [install] 4 | install-scripts=$base/lib/launch_system_modes 5 | -------------------------------------------------------------------------------- /launch_system_modes/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages 2 | from setuptools import setup 3 | 4 | package_name = 'launch_system_modes' 5 | 6 | setup( 7 | name=package_name, 8 | version='0.7.1', 9 | packages=find_packages(exclude=['test']), 10 | data_files=[ 11 | ('share/ament_index/resource_index/packages', 12 | ['resource/' + package_name]), 13 | ('share/' + package_name, ['package.xml']), 14 | ], 15 | install_requires=[ 16 | 'launch_ros', 17 | 'lifecycle_msgs', 18 | 'setuptools', 19 | 'system_modes_msgs', 20 | ], 21 | zip_safe=True, 22 | maintainer='Arne Nordmann', 23 | maintainer_email='github@norro.de', 24 | keywords=['ROS', 'launch', 'system modes'], 25 | description='System modes specific extensions to launch_ros.', 26 | license='Apache License 2.0', 27 | tests_require=['pytest'], 28 | entry_points={ 29 | 'console_scripts': [ 30 | ], 31 | }, 32 | ) 33 | -------------------------------------------------------------------------------- /launch_system_modes/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /launch_system_modes/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /launch_system_modes/test/test_launch_system_modes/test_change_mode.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Tests for the Mode Change event.""" 16 | 17 | import launch 18 | 19 | from launch_system_modes.actions import Node, System 20 | from launch_system_modes.events import ChangeMode 21 | 22 | import pytest 23 | 24 | 25 | def test_system_part_construction(): 26 | drive_base = Node( 27 | package='system_modes_examples', 28 | executable='drive_base', 29 | name='drive_base', 30 | namespace='', 31 | output='screen') 32 | actuation = System( 33 | name='actuation', 34 | namespace='') 35 | 36 | # create change mode 37 | ChangeMode( 38 | system_part_matcher=launch.events.matchers.matches_action(drive_base), 39 | mode_name='__DEFAULT__', 40 | ) 41 | ChangeMode( 42 | system_part_matcher=launch.events.matchers.matches_action(actuation), 43 | mode_name='__DEFAULT__', 44 | ) 45 | 46 | # Construction with missing mandatory parameters 47 | with pytest.raises(TypeError): 48 | ChangeMode( 49 | system_part_matcher=launch.events.matchers.matches_action(drive_base) 50 | ) 51 | with pytest.raises(TypeError): 52 | ChangeMode( 53 | mode_name='__DEFAULT__' 54 | ) 55 | 56 | # try emitting / integration with launch actions 57 | launch.actions.EmitEvent( 58 | event=ChangeMode( 59 | system_part_matcher=launch.events.matchers.matches_action(actuation), 60 | mode_name='PERFORMANCE')) 61 | -------------------------------------------------------------------------------- /launch_system_modes/test/test_launch_system_modes/test_change_state.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Tests for the Mode State event.""" 16 | 17 | import launch 18 | 19 | from launch_system_modes.actions import System 20 | from launch_system_modes.events import ChangeState 21 | 22 | import pytest 23 | 24 | 25 | def test_system_part_construction(): 26 | actuation = System( 27 | name='actuation', 28 | namespace='') 29 | 30 | # create change mode 31 | ChangeState( 32 | system_part_matcher=launch.events.matchers.matches_action(actuation), 33 | transition_id=0, 34 | ) 35 | 36 | # Construction with missing mandatory parameters 37 | with pytest.raises(TypeError): 38 | ChangeState( 39 | system_part_matcher=launch.events.matchers.matches_action(actuation) 40 | ) 41 | with pytest.raises(TypeError): 42 | ChangeState( 43 | transition_id=0, 44 | ) 45 | 46 | # try emitting / integration with launch actions 47 | launch.actions.EmitEvent( 48 | event=ChangeState( 49 | system_part_matcher=launch.events.matchers.matches_action(actuation), 50 | transition_id=0)) 51 | -------------------------------------------------------------------------------- /launch_system_modes/test/test_launch_system_modes/test_mode_changed.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Tests for the Mode Changed event.""" 16 | 17 | import launch 18 | 19 | from launch_system_modes.actions import Node, System 20 | from launch_system_modes.events import ModeChanged 21 | 22 | import pytest 23 | import system_modes_msgs 24 | 25 | 26 | def test_system_part_construction(): 27 | drive_base = Node( 28 | package='system_modes_examples', 29 | executable='drive_base', 30 | name='drive_base', 31 | namespace='', 32 | output='screen') 33 | actuation = System( 34 | name='actuation', 35 | namespace='') 36 | 37 | mode_event = system_modes_msgs.msg.ModeEvent() 38 | mode_event.start_mode.label = '__DEFAULT__' 39 | mode_event.goal_mode.label = 'FAST' 40 | 41 | # create mode changed event 42 | ModeChanged( 43 | action=drive_base, 44 | msg=mode_event, 45 | ) 46 | mc = ModeChanged( 47 | action=actuation, 48 | msg=mode_event, 49 | ) 50 | assert mc.start_mode == '__DEFAULT__' 51 | assert mc.goal_mode == 'FAST' 52 | 53 | # Construction with missing mandatory parameters 54 | with pytest.raises(TypeError): 55 | ModeChanged( 56 | action=drive_base, 57 | ) 58 | with pytest.raises(TypeError): 59 | ModeChanged( 60 | msg=mode_event, 61 | ) 62 | 63 | # try emitting / integration with launch actions 64 | launch.actions.EmitEvent( 65 | event=ModeChanged( 66 | action=actuation, 67 | msg=mode_event)) 68 | -------------------------------------------------------------------------------- /launch_system_modes/test/test_launch_system_modes/test_node.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Tests for the Node Action.""" 16 | 17 | from launch import LaunchContext 18 | from launch_system_modes.actions import Node 19 | 20 | import pytest 21 | 22 | 23 | def test_system_part_construction(): 24 | Node( 25 | package='system_modes_examples', 26 | executable='drive_base', 27 | name='drive_base', 28 | namespace='', 29 | output='screen') 30 | 31 | # Construction with missing mandatory parameters 32 | with pytest.raises(TypeError): 33 | Node( 34 | package='system_modes_examples', 35 | name='drive_base', 36 | namespace='', 37 | output='screen') 38 | with pytest.raises(TypeError): 39 | Node( 40 | package='system_modes_examples', 41 | executable='drive_base', 42 | namespace='', 43 | output='screen') 44 | with pytest.raises(TypeError): 45 | Node( 46 | package='system_modes_examples', 47 | executable='drive_base', 48 | name='drive_base', 49 | output='screen') 50 | 51 | 52 | def test_node_name(): 53 | part = Node( 54 | package='system_modes_examples', 55 | executable='drive_base', 56 | name='drive_base', 57 | namespace='testtest', 58 | ) 59 | lc = LaunchContext() 60 | part._perform_substitutions(lc) 61 | assert part.is_node_name_fully_specified() is True 62 | -------------------------------------------------------------------------------- /launch_system_modes/test/test_launch_system_modes/test_state_transition.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Tests for the State Transition event.""" 16 | 17 | import launch 18 | 19 | from launch_system_modes.actions import System 20 | from launch_system_modes.events import StateTransition 21 | 22 | import lifecycle_msgs 23 | import pytest 24 | 25 | 26 | def test_system_part_construction(): 27 | actuation = System( 28 | name='actuation', 29 | namespace='') 30 | 31 | transition_msg = lifecycle_msgs.msg.TransitionEvent() 32 | transition_msg.start_state.label = 'inactive' 33 | transition_msg.goal_state.label = 'active' 34 | 35 | # create state trasition event 36 | st = StateTransition( 37 | action=actuation, 38 | msg=transition_msg 39 | ) 40 | assert st.start_state == 'inactive' 41 | assert st.goal_state == 'active' 42 | 43 | # Construction with missing mandatory parameters 44 | with pytest.raises(TypeError): 45 | StateTransition( 46 | action=actuation, 47 | ) 48 | with pytest.raises(TypeError): 49 | StateTransition( 50 | msg=transition_msg 51 | ) 52 | 53 | # try emitting / integration with launch actions 54 | launch.actions.EmitEvent( 55 | event=StateTransition( 56 | action=actuation, 57 | msg=transition_msg)) 58 | -------------------------------------------------------------------------------- /launch_system_modes/test/test_launch_system_modes/test_system.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Robert Bosch GmbH 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Tests for the SystemPart Action.""" 16 | 17 | from launch_system_modes.actions import System 18 | 19 | import pytest 20 | 21 | 22 | def test_system_part_construction(): 23 | System( 24 | name='actuation', 25 | namespace='') 26 | 27 | # Construction without namespace 28 | with pytest.raises(TypeError): 29 | System( 30 | namespace='') 31 | with pytest.raises(TypeError): 32 | System( 33 | name='actuation') 34 | -------------------------------------------------------------------------------- /launch_system_modes/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /system_modes/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package system_modes_examples 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.0 (2020-07-21) 6 | ------------------ 7 | 8 | * More flexibility in specifying the default mode, any mode can be now default mode 9 | https://github.com/micro-ROS/system_modes/issues/69 10 | 11 | 0.8.0 (2020-04-22) 12 | ------------------ 13 | 14 | * Launch integration, i.e. launch actions, events, and event handlers for system modes 15 | 16 | 0.7.1 (2020-04-22) 17 | ------------------ 18 | 19 | * Improved metadata for ROS 2 package releases 20 | 21 | 0.7.0 (2020-04-22) 22 | ------------------ 23 | 24 | * Launch tests now using launch_ros node action https://github.com/micro-ROS/system_modes/pull/72 25 | * Introduced separate interface package, system_modes_msgs https://github.com/micro-ROS/system_modes/pull/74 26 | 27 | 0.6.0 (2020-03-16) 28 | ------------------ 29 | 30 | * Introduced mode observer https://github.com/micro-ROS/system_modes/issues/59 31 | * Mode manager prevents redundant mode changes https://github.com/micro-ROS/system_modes/pull/67 32 | * Minor bugfix in inference 33 | 34 | 0.5.0 (2020-03-16) 35 | ------------------ 36 | * Atomic parameter setting https://github.com/micro-ROS/system_modes/issues/59 37 | * Bug fixing 38 | * More tests 39 | 40 | 0.4.2 (2020-12-17) 41 | ------------------ 42 | * Error handling and rules feature no longer experimental 43 | * Fixed bugs in monitor and tests 44 | 45 | 0.4.1 (2020-10-29) 46 | ------------------ 47 | * Include experimental error handling and rules feature 48 | * https://github.com/micro-ROS/system_modes/issues/13 49 | * CI for ubuntu 20.04 ROS 2 rolling 50 | 51 | 0.4.0 (2020-09-30) 52 | ------------------ 53 | * publish inferred state and mode transitions 54 | * https://github.com/micro-ROS/system_modes/issues/42 55 | 56 | 0.3.0 (2020-07-23) 57 | ------------------ 58 | * removed boost dependencies (was: program options) 59 | * changed mode service specifications (less redundancy) 60 | * https://github.com/micro-ROS/system_modes/issues/24 61 | 62 | 0.2.3 (2020-07-23) 63 | ------------------ 64 | * improved StateAndMode struct 65 | * testing 66 | 67 | 0.2.2 (2020-07-13) 68 | ------------------ 69 | * introduced StateAndMode struct to bundle lifecycle state and system mode 70 | 71 | 0.2.0 (2020-02-13) 72 | ------------------ 73 | * integration with ROS 2 launch 74 | * updated docs 75 | 76 | 0.1.6 (2019-10-31) 77 | ------------------ 78 | * fixed QoS configuration for parameter event subscribers 79 | 80 | 0.1.5 (2019-10-21 81 | ------------------- 82 | * migration to ROS 2 eloquent elusor 83 | 84 | 0.1.2 (2019-03-18) 85 | ------------------ 86 | * fixed dependencies in package.xml 87 | 88 | 0.1.1 (2019-03-08) 89 | ------------------ 90 | * first public release for ROS 2 system modes 91 | -------------------------------------------------------------------------------- /system_modes/doc/lifecycle-extended.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/micro-ROS/system_modes/6153cb8263c0dcf5f0f7015b795418e374f4695a/system_modes/doc/lifecycle-extended.png -------------------------------------------------------------------------------- /system_modes/doc/requirements.md: -------------------------------------------------------------------------------- 1 | # Requirements 2 | 3 | We discussed our first analysis results with the architect and developers of the above use-case as well as experts for robotics systems and software engineering and derived the following general requirements for the intended concepts and abstractions for (1.) system runtime configuration and (2.) system error and contingency diagnosis. 4 | 5 | ## System Runtime Configuration 6 | 7 | The envisioned approach shall provide concepts for modeling of the system hierarchy with regard to system configuration / modes as well as corresponding framework functionalities. 8 | 9 | 1. Model 10 | * **Hierarchical modeling of subsystems** 11 | * The envisioned approach shall allow to define subsystems consisting of multiple components (ROS nodes, micro-ROS nodes) in a hierarchical or at least two-staged (component - sub-system - system) manner. 12 | * TODO: Are such hierarchies also relevant for diagnosis/monitoring? If yes, explore whether the same hierarchy should be used. 13 | * Some robotic frameworks feature meta-components or hierarchical components. We prefer a more flexible approach allowing to define subsystems for each aspect (system modes, scheduling, distribution, ...) individually. This approach gives more flexibility - e.g., to treat all device drivers in a uniform manner with regard to scheduling but put the camera driver and the feature and obstacle recognition into one sub-system with regard to system configuration. Also, it allows the application developer to only use mechanisms and framework features that are actually relevant for his system - e.g., distribution might be irrelevant for small robotic systems. 14 | * **Modeling of system, sub-system and component modes** 15 | * Based on the definition the system, sub-systems and components, the envisioned system runtime configuration concept shall provide an approach for defining individual modes of each of these elements and specify the interconnection between them. 16 | * **Standard but extensible component runtime lifecycle** 17 | * The component runtime lifecycle (component mode/state) model shall be based on the OMG standard used in [ROS 2](http://design.ros2.org/articles/node_lifecycle.html) and which also resembles the component states used in [Smartsoft](http://servicerobotik-ulm.de/drupal/?q=node/46). 18 | * **Modeling of error propagation and severity within subsystems** 19 | * Describe causal dependencies between components of a (sub)system with regard to errors. (Sources of inspiration are Fault-Tree Analysis and various academic works.) 20 | * **Integrate component modes/states with component-specific set of parameters into one model** 21 | * Use of parameter model in style of dynamic reconfigure of ROS1. 22 | * However, this would introduce a dependency between component lifecycle and component parameters? 23 | * **Timing/causality-aware switching between modes** 24 | * Two simple patterns in first step: (1.) Reconfigure components of a (sub)system sequentially according to a given list or (2.) reconfigure them simultaneously. 25 | 26 | 1. Implementation 27 | * **Modeling of system, sub-system and component modes** 28 | * Strive for lightweight approach by a simple configuration file or even use of C++ API. 29 | * Should be well manageable (diffable, mergeable) with Git. 30 | * **API primitives for defining preconditions and invariants with regard to system configuration** 31 | 32 | 1. Runtime/Execution 33 | * **Distributed system state across multiple computing devices (uCs, uPs)** 34 | * The envisioned approach shall support the communication of the modes/states of subsystems or components located on different computing devices to always obtain a consistent system view. 35 | * It shall also consider that a computing device may reconfigure its sub-systems or components (in a limited scope) independent of the sub-systems and components other computing devices. 36 | 37 | ## System Error and Contingency Diagnosis 38 | 39 | The envisioned approach shall also provide API primitives for detection of typical system errors and contingencies. 40 | 41 | 1. **Monitors for communication layers** 42 | * Check for receive rates, latencies, ... 43 | 44 | 1. **Operating system monitors** 45 | 46 | 1. **Templates and APIs for hardware monitors** 47 | * Basic blocks for GPIOs (voltage, current) 48 | * Basic blocks for connected HW modules (via SPI, I²C) 49 | 50 | 1. **Templates and APIs for monitoring of functional properties** 51 | * No need of status messages here, but use of function-oriented topics 52 | * Building blocks/patterns for analysis of time-series (with a sliding window) for bounds, for delays, ... 53 | -------------------------------------------------------------------------------- /system_modes/include/system_modes/mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #pragma once 16 | 17 | #include "system_modes/mode_impl.hpp" 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace system_modes 32 | { 33 | 34 | class ModeBase; 35 | class DefaultMode; 36 | class Mode; 37 | 38 | using ModeBasePtr = std::shared_ptr; 39 | using ModeConstPtr = std::shared_ptr; 40 | using ModePtr = std::shared_ptr; 41 | using DefaultModePtr = std::shared_ptr; 42 | 43 | using ModeMap = std::map; 44 | 45 | class ModeBase 46 | { 47 | public: 48 | explicit ModeBase(const std::string & mode_name); 49 | virtual ~ModeBase() = default; 50 | // cppcheck-suppress unknownMacro 51 | RCLCPP_DISABLE_COPY(ModeBase) 52 | 53 | std::string get_name() const; 54 | 55 | virtual void set_parameter(const rclcpp::Parameter & parameter) = 0; 56 | virtual void set_parameters(const std::vector & parameters) = 0; 57 | virtual void set_part_mode( 58 | const std::string & part, 59 | const StateAndMode stateAndMode) = 0; 60 | 61 | virtual rclcpp::Parameter get_parameter(const std::string & param_name) const; 62 | virtual std::vector get_parameter_names() const; 63 | virtual const std::vector get_parameters() const; 64 | 65 | virtual const std::vector get_parts() const; 66 | virtual const StateAndMode get_part_mode(const std::string & part) const; 67 | 68 | virtual std::string print() const; 69 | 70 | protected: 71 | ModeImpl mode_impl_; 72 | }; 73 | 74 | /** 75 | * Represents a default system mode. 76 | * 77 | * For default modes of nodes it holds the according parametrization, for 78 | * default modes of systems, it holds the according states and modes of its 79 | * system parts. 80 | */ 81 | class DefaultMode : public ModeBase 82 | { 83 | public: 84 | DefaultMode(); 85 | explicit DefaultMode(const std::string & mode_name) = delete; 86 | // cppcheck-suppress unknownMacro 87 | RCLCPP_DISABLE_COPY(DefaultMode) 88 | 89 | virtual void set_parameter(const rclcpp::Parameter & parameter); 90 | virtual void set_parameters(const std::vector & parameters); 91 | 92 | virtual void set_part_mode( 93 | const std::string & part, 94 | const StateAndMode stateAndMode); 95 | }; 96 | 97 | /** 98 | * Represents a (non-default) system mode. 99 | * 100 | * For default modes of nodes it holds the according parametrization, for 101 | * default modes of systems, it holds the according states and modes of its 102 | * system parts. 103 | */ 104 | class Mode : public ModeBase 105 | { 106 | public: 107 | explicit Mode(const std::string & mode_name) = delete; 108 | Mode(const std::string & mode_name, const DefaultModePtr default_mode); 109 | // cppcheck-suppress unknownMacro 110 | RCLCPP_DISABLE_COPY(Mode) 111 | 112 | virtual ~Mode() = default; 113 | 114 | virtual void set_parameter(const rclcpp::Parameter & parameter); 115 | virtual void set_parameters(const std::vector & parameters); 116 | 117 | virtual void set_part_mode( 118 | const std::string & part, 119 | const StateAndMode stateAndMode); 120 | }; 121 | 122 | inline std::ostream & operator<<(std::ostream & os, const Mode & mode) 123 | { 124 | os.precision(3); 125 | os << mode.print(); 126 | return os; 127 | } 128 | 129 | } // namespace system_modes 130 | -------------------------------------------------------------------------------- /system_modes/include/system_modes/mode_handling.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "system_modes/mode.hpp" 30 | #include "system_modes/mode_impl.hpp" 31 | 32 | namespace system_modes 33 | { 34 | 35 | struct ModeRule 36 | { 37 | std::string name; 38 | 39 | std::string system; 40 | StateAndMode system_target; 41 | 42 | std::string part; 43 | StateAndMode part_actual; 44 | 45 | StateAndMode new_system_target; 46 | }; 47 | 48 | using RulesMap = std::map; 49 | 50 | /** 51 | * Reads and parses the error handling rules from the sytem modes and hierarchy file (SMH file). 52 | * Provides getters to receive all rules that are applicable for a certain system state, usually 53 | * a system deviation. 54 | */ 55 | class ModeHandling 56 | { 57 | public: 58 | explicit ModeHandling(const std::string & model_path); 59 | // cppcheck-suppress unknownMacro 60 | RCLCPP_DISABLE_COPY(ModeHandling) 61 | 62 | virtual ~ModeHandling() = default; 63 | virtual const std::vector get_rules_for( 64 | const std::string & system, 65 | const StateAndMode & target); 66 | 67 | protected: 68 | mutable std::shared_timed_mutex rules_mutex_; 69 | 70 | private: 71 | std::map rules_; 72 | 73 | virtual void read_rules_from_model(const std::string & model_path); 74 | virtual void add_rule( 75 | const std::string & part, 76 | const std::string & rule_name, 77 | const rclcpp::Parameter & rule_param); 78 | }; 79 | 80 | } // namespace system_modes 81 | -------------------------------------------------------------------------------- /system_modes/include/system_modes/mode_impl.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | using lifecycle_msgs::msg::State; 30 | 31 | namespace system_modes 32 | { 33 | 34 | static const char DEFAULT_MODE[] = "__DEFAULT__"; 35 | 36 | 37 | unsigned int state_id_(const std::string & state_label); 38 | const std::string state_label_(unsigned int state_id); 39 | 40 | unsigned int transition_id_(const std::string & transition_label); 41 | const std::string transition_label_(unsigned int transition_id); 42 | 43 | unsigned int goal_state_(unsigned int transition_id); 44 | 45 | struct StateAndMode 46 | { 47 | unsigned int state; 48 | std::string mode; 49 | 50 | explicit StateAndMode(unsigned int newstate = 0, const std::string & newmode = "") 51 | { 52 | state = newstate; 53 | mode = newmode; 54 | } 55 | 56 | bool operator==(const StateAndMode & cmp) const 57 | { 58 | if (cmp.state != state) { 59 | return false; 60 | } else if (cmp.state != State::PRIMARY_STATE_ACTIVE) { 61 | return true; 62 | } 63 | 64 | return cmp.mode.compare(mode) == 0 || // same mode 65 | (cmp.mode.compare(DEFAULT_MODE) == 0 && mode.empty()) || // we consider empty and 66 | (mode.compare(DEFAULT_MODE) == 0 && cmp.mode.empty()); // DEFAULT_MODE the same 67 | } 68 | 69 | bool operator!=(const StateAndMode & cmp) const 70 | { 71 | return !(*this == cmp); 72 | } 73 | 74 | void from_string(const std::string & sam) 75 | { 76 | auto dot = sam.find("."); 77 | if (dot != std::string::npos) { 78 | state = state_id_(sam.substr(0, dot)); 79 | mode = sam.substr(dot + 1); 80 | } else { 81 | state = state_id_(sam); 82 | mode = ""; 83 | } 84 | } 85 | 86 | std::string as_string() const 87 | { 88 | if (state != State::PRIMARY_STATE_ACTIVE || mode.empty()) { 89 | return state_label_(state); 90 | } 91 | return state_label_(state) + "." + mode; 92 | } 93 | 94 | bool unknown() 95 | { 96 | return state == State::PRIMARY_STATE_UNKNOWN; 97 | } 98 | }; 99 | 100 | class ModeImpl 101 | { 102 | public: 103 | explicit ModeImpl(const std::string & mode_name); 104 | virtual ~ModeImpl() = default; 105 | ModeImpl(const ModeImpl & copy) = delete; 106 | 107 | virtual std::string get_name() const; 108 | 109 | virtual void add_parameter(const rclcpp::Parameter & parameter); 110 | virtual void add_parameters(const std::vector & parameters); 111 | virtual void add_part_mode( 112 | const std::string & part, 113 | const StateAndMode stateAndMode); 114 | 115 | virtual void set_parameter(const rclcpp::Parameter & parameter); 116 | virtual void set_parameters(const std::vector & parameters); 117 | virtual void set_part_mode( 118 | const std::string & part, 119 | const StateAndMode stateAndMode); 120 | 121 | virtual std::vector get_parameter_names() const; 122 | virtual rclcpp::Parameter get_parameter(const std::string & param_name) const; 123 | virtual bool get_parameter(const std::string & param_name, rclcpp::Parameter & parameter) const; 124 | virtual const std::vector get_parameters() const; 125 | 126 | virtual const std::vector get_parts() const; 127 | virtual const StateAndMode get_part_mode(const std::string & part) const; 128 | 129 | protected: 130 | std::string name_; 131 | std::map param_; 132 | std::map part_modes_; 133 | mutable std::mutex mutex_; 134 | }; 135 | 136 | } // namespace system_modes 137 | -------------------------------------------------------------------------------- /system_modes/include/system_modes/mode_inference.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "system_modes/mode.hpp" 30 | #include "system_modes/mode_handling.hpp" 31 | 32 | namespace system_modes 33 | { 34 | 35 | typedef std::map StatesMap; 36 | typedef std::map> ParametersMap; 37 | typedef std::map> Deviation; 38 | 39 | /** 40 | * Reads and parses the sytem modes and hierarchy file (SMH file) and provides 41 | * getters to access the resulting model. Provides setters to inform inference about 42 | * the live system state (update current state and mode targets as well as current 43 | * parametization). 44 | */ 45 | class ModeInference 46 | { 47 | public: 48 | explicit ModeInference(const std::string & model_path); 49 | // cppcheck-suppress unknownMacro 50 | RCLCPP_DISABLE_COPY(ModeInference) 51 | 52 | virtual const std::vector get_all_parts() const; 53 | virtual const std::vector get_nodes() const; 54 | virtual const std::vector get_systems() const; 55 | virtual const std::vector get_all_parts_of( 56 | const std::string & system) const; 57 | 58 | virtual void update(const std::string &, const StateAndMode &); 59 | virtual void update_state(const std::string &, unsigned int); 60 | virtual void update_mode(const std::string &, const std::string &); 61 | virtual void update_param(const std::string &, rclcpp::Parameter &); 62 | virtual void update_target(const std::string &, StateAndMode); 63 | 64 | virtual StateAndMode get(const std::string & part) const; 65 | virtual StateAndMode get_or_infer(const std::string & part); 66 | 67 | virtual StateAndMode infer(const std::string & part); 68 | virtual StateAndMode infer_node(const std::string & part); 69 | virtual StateAndMode infer_system(const std::string & part); 70 | 71 | /** 72 | * Infers latest transitions of systems 73 | * 74 | * Returns map of last inferred transitions of systems into new states or 75 | * new modes. State transitions of nodes don't have to be inferred, as 76 | * nodes publish their state transitions. For nodes, we only need to infer 77 | * mode transitions. 78 | */ 79 | virtual Deviation infer_transitions(); 80 | 81 | /** 82 | * Infers deviations of the _target_ system state (state and mode of systems and nodes) 83 | * and their respective _actual_ state. 84 | */ 85 | virtual Deviation get_deviation(); 86 | 87 | virtual StateAndMode get_target(const std::string & part) const; 88 | virtual ModeConstPtr get_mode(const std::string & part, const std::string & mode) const; 89 | virtual std::vector get_available_modes(const std::string & part) const; 90 | 91 | virtual ~ModeInference() = default; 92 | 93 | protected: 94 | virtual bool matching_parameters(const rclcpp::Parameter &, const rclcpp::Parameter &) const; 95 | virtual void read_modes_from_model(const std::string & model_path); 96 | virtual void add_param_to_mode(ModeBasePtr, const rclcpp::Parameter &); 97 | 98 | private: 99 | ModeHandling * mode_handling_; 100 | 101 | StatesMap nodes_, nodes_target_, nodes_cache_; 102 | StatesMap systems_, systems_target_, systems_cache_; 103 | std::map modes_; 104 | ParametersMap parameters_; 105 | 106 | mutable std::shared_timed_mutex 107 | nodes_mutex_, systems_mutex_, 108 | modes_mutex_, parts_mutex_, 109 | param_mutex_; 110 | mutable std::shared_timed_mutex 111 | nodes_target_mutex_, systems_target_mutex_; 112 | mutable std::shared_timed_mutex 113 | nodes_cache_mutex_, systems_cache_mutex_; 114 | }; 115 | 116 | } // namespace system_modes 117 | -------------------------------------------------------------------------------- /system_modes/include/system_modes/mode_monitor.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "system_modes/mode.hpp" 27 | #include "system_modes/mode_inference.hpp" 28 | 29 | #include "system_modes_msgs/srv/change_mode.hpp" 30 | #include "system_modes_msgs/srv/get_mode.hpp" 31 | #include "system_modes_msgs/srv/get_available_modes.hpp" 32 | #include "system_modes_msgs/msg/mode_event.hpp" 33 | 34 | namespace system_modes 35 | { 36 | 37 | /** 38 | * Based on mode inference and live updates from the system, this class monitors the live 39 | * system state and renders a simple command-line view of the known resp. inferred state 40 | * of all systems and system parts within the SMH file. 41 | */ 42 | class ModeMonitor : public rclcpp::Node 43 | { 44 | public: 45 | ModeMonitor(); 46 | ModeMonitor(const ModeMonitor &) = delete; 47 | 48 | std::shared_ptr inference(); 49 | 50 | virtual ~ModeMonitor() = default; 51 | 52 | protected: 53 | virtual void refresh() const; 54 | virtual std::string header() const; 55 | virtual std::string format_line( 56 | const std::string &, 57 | unsigned int, unsigned int, unsigned int, 58 | const std::string &, const std::string &) const; 59 | 60 | inline const std::string MONITOR_HLINE(const std::string & label) const; 61 | 62 | std::shared_ptr mode_inference_; 63 | std::string model_path_; 64 | 65 | private: 66 | rclcpp::TimerBase::SharedPtr timer_; 67 | 68 | unsigned int rate_; 69 | bool clear_screen_, verbose_; 70 | }; 71 | 72 | } // namespace system_modes 73 | -------------------------------------------------------------------------------- /system_modes/include/system_modes/mode_observer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include "system_modes/mode.hpp" 29 | #include "system_modes_msgs/msg/mode_event.hpp" 30 | #include "system_modes_msgs/srv/get_mode.hpp" 31 | 32 | namespace system_modes 33 | { 34 | 35 | using std::map; 36 | using std::mutex; 37 | using std::string; 38 | 39 | using lifecycle_msgs::msg::TransitionEvent; 40 | using lifecycle_msgs::srv::GetState; 41 | using rclcpp::node_interfaces::NodeBaseInterface; 42 | using system_modes_msgs::msg::ModeEvent; 43 | using system_modes_msgs::srv::GetMode; 44 | 45 | /** 46 | * Mode observer provides a local system modes cache. 47 | * 48 | * The mode observer serves as a local system modes cache, instantiated by a node. The mode 49 | * observer will initially try to gain current states/modes from all observes entities and will 50 | * then continuously monitor transitions to keep up to date. 51 | */ 52 | class ModeObserver 53 | { 54 | public: 55 | explicit ModeObserver(std::weak_ptr); 56 | virtual ~ModeObserver() = default; 57 | 58 | /** 59 | * Getter for observed state and mode. 60 | * 61 | * Returns cached observed state and mode for requested system entity (system or node). 62 | */ 63 | virtual StateAndMode 64 | get(const string & part_name); 65 | 66 | /** 67 | * Add part to list of observed parts. 68 | */ 69 | virtual void 70 | observe(const string & part_name); 71 | 72 | /** 73 | * Remove part from list of observed parts. 74 | */ 75 | virtual void 76 | stop_observing(const string & part_name); 77 | 78 | protected: 79 | virtual void 80 | transition_callback( 81 | const TransitionEvent::SharedPtr msg, 82 | const string & part_name); 83 | 84 | virtual void 85 | mode_event_callback( 86 | const ModeEvent::SharedPtr msg, 87 | const string & part_name); 88 | 89 | private: 90 | std::weak_ptr node_handle_; 91 | map cache_; 92 | mutable std::shared_timed_mutex mutex_; 93 | 94 | map>> state_subs_; 95 | map>> mode_subs_; 96 | }; 97 | 98 | } // namespace system_modes 99 | -------------------------------------------------------------------------------- /system_modes/launch/mode_manager.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 - for information on the respective copyright owner 2 | # see the NOTICE file and/or the repository https://github.com/micro-ROS/system_modes. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import launch 17 | import launch.actions 18 | import launch.substitutions 19 | 20 | import launch_ros.actions 21 | 22 | 23 | def generate_launch_description(): 24 | return launch.LaunchDescription([ 25 | launch.actions.DeclareLaunchArgument( 26 | 'modelfile', 27 | description='Path to modelfile'), 28 | launch_ros.actions.Node( 29 | package='system_modes', 30 | executable='mode_manager', 31 | parameters=[{'modelfile': launch.substitutions.LaunchConfiguration('modelfile')}], 32 | output='screen')]) 33 | -------------------------------------------------------------------------------- /system_modes/launch/mode_monitor.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 - for information on the respective copyright owner 2 | # see the NOTICE file and/or the repository https://github.com/micro-ROS/system_modes. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import launch 17 | import launch.actions 18 | 19 | import launch_ros.actions 20 | 21 | 22 | def generate_launch_description(): 23 | return launch.LaunchDescription([ 24 | launch.actions.DeclareLaunchArgument( 25 | 'modelfile', 26 | description='Path to modelfile'), 27 | launch.actions.DeclareLaunchArgument( 28 | 'debug', 29 | default_value='false', 30 | description='Debug'), 31 | launch.actions.DeclareLaunchArgument( 32 | 'verbose', 33 | default_value='false', 34 | description='Print mode parametrization'), 35 | launch.actions.DeclareLaunchArgument( 36 | 'rate', 37 | default_value='1000', 38 | description='Monitor refresh rate in ms'), 39 | launch_ros.actions.Node( 40 | package='system_modes', 41 | executable='mode_monitor', 42 | parameters=[{ 43 | 'modelfile': launch.substitutions.LaunchConfiguration('modelfile'), 44 | 'debug': launch.substitutions.LaunchConfiguration('debug'), 45 | 'verbose': launch.substitutions.LaunchConfiguration('verbose'), 46 | 'rate': launch.substitutions.LaunchConfiguration('rate') 47 | }], 48 | output='screen')]) 49 | -------------------------------------------------------------------------------- /system_modes/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | system_modes 5 | 0.9.0 6 | 7 | The system modes concept assumes that a robotics system is built 8 | from components with a lifecycle. It adds a notion of (sub-)systems, 9 | hiararchically grouping these nodes, as well as a notion of modes 10 | that determine the configuration of these nodes and (sub-)systems in 11 | terms of their parameter values. 12 | 13 | Arne Nordmann 14 | Ralph Lange 15 | Apache License 2.0 16 | 17 | https://micro.ros.org/docs/concepts/client_library/lifecycle_and_system_modes/ 18 | https://github.com/micro-ROS/system_modes 19 | https://github.com/micro-ROS/system_modes/issues 20 | 21 | ament_cmake 22 | 23 | builtin_interfaces 24 | rclcpp 25 | rclcpp_lifecycle 26 | system_modes_msgs 27 | 28 | launch_ros 29 | 30 | ament_cmake_gtest 31 | ament_cmake_gmock 32 | ament_cmake_pep257 33 | ament_cmake_flake8 34 | ament_cmake_cpplint 35 | ament_cmake_cppcheck 36 | ament_cmake_uncrustify 37 | ament_index_python 38 | ament_lint_auto 39 | launch_testing_ament_cmake 40 | launch_testing_ros 41 | ros2run 42 | 43 | 44 | ament_cmake 45 | 46 | 47 | -------------------------------------------------------------------------------- /system_modes/src/system_modes/mode.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #include "system_modes/mode.hpp" 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | using std::string; 26 | using std::vector; 27 | using std::out_of_range; 28 | using std::runtime_error; 29 | using rclcpp::Parameter; 30 | using lifecycle_msgs::msg::State; 31 | using lifecycle_msgs::msg::Transition; 32 | 33 | namespace system_modes 34 | { 35 | 36 | ModeBase::ModeBase(const string & mode_name) 37 | : mode_impl_(mode_name) 38 | { 39 | } 40 | 41 | string 42 | ModeBase::get_name() const 43 | { 44 | return this->mode_impl_.get_name(); 45 | } 46 | 47 | Parameter 48 | ModeBase::get_parameter(const string & param_name) const 49 | { 50 | return this->mode_impl_.get_parameter(param_name); 51 | } 52 | 53 | vector 54 | ModeBase::get_parameter_names() const 55 | { 56 | return this->mode_impl_.get_parameter_names(); 57 | } 58 | 59 | const vector 60 | ModeBase::get_parameters() const 61 | { 62 | return this->mode_impl_.get_parameters(); 63 | } 64 | 65 | const vector 66 | ModeBase::get_parts() const 67 | { 68 | return this->mode_impl_.get_parts(); 69 | } 70 | 71 | const StateAndMode 72 | ModeBase::get_part_mode(const string & part) const 73 | { 74 | return this->mode_impl_.get_part_mode(part); 75 | } 76 | 77 | string 78 | ModeBase::print() const 79 | { 80 | std::ostringstream os(std::ostringstream::out); 81 | os.precision(2); 82 | os << this->get_name() << "<"; 83 | 84 | bool first = true; 85 | for (auto p : this->get_parameter_names()) { 86 | if (!first) { 87 | os << ", "; 88 | } else { 89 | first = false; 90 | } 91 | os << p << ":" << this->get_parameter(p).value_to_string(); 92 | } 93 | first = true; 94 | for (auto p : this->get_parts()) { 95 | if (!first) { 96 | os << ", "; 97 | } else { 98 | first = false; 99 | } 100 | auto stateAndMode = this->get_part_mode(p); 101 | os << p << ":" << stateAndMode.state << stateAndMode.mode; 102 | } 103 | 104 | os << ">"; 105 | return os.str(); 106 | } 107 | 108 | DefaultMode::DefaultMode() 109 | : ModeBase(DEFAULT_MODE) 110 | { 111 | } 112 | 113 | void 114 | DefaultMode::set_parameter(const Parameter & parameter) 115 | { 116 | this->mode_impl_.add_parameter(parameter); 117 | } 118 | 119 | void 120 | DefaultMode::set_parameters(const vector & parameters) 121 | { 122 | this->mode_impl_.add_parameters(parameters); 123 | } 124 | 125 | void 126 | DefaultMode::set_part_mode( 127 | const std::string & part, 128 | const StateAndMode stateAndMode) 129 | { 130 | if (stateAndMode.mode.empty()) { 131 | this->mode_impl_.add_part_mode(part, StateAndMode(stateAndMode.state, DEFAULT_MODE)); 132 | } else { 133 | this->mode_impl_.add_part_mode(part, stateAndMode); 134 | } 135 | } 136 | 137 | Mode::Mode(const string & mode_name, const DefaultModePtr default_mode) 138 | : ModeBase(mode_name) 139 | { 140 | if (!default_mode) { 141 | throw std::runtime_error("Default mode must not be empty."); 142 | } 143 | 144 | auto default_params = default_mode->get_parameter_names(); 145 | for (auto p : default_params) { 146 | this->mode_impl_.add_parameter(default_mode->get_parameter(p)); 147 | } 148 | 149 | auto default_parts = default_mode->get_parts(); 150 | for (auto p : default_parts) { 151 | this->mode_impl_.add_part_mode(p, default_mode->get_part_mode(p)); 152 | } 153 | } 154 | 155 | void 156 | Mode::set_parameter(const Parameter & parameter) 157 | { 158 | this->mode_impl_.set_parameter(parameter); 159 | } 160 | 161 | void 162 | Mode::set_parameters(const vector & parameters) 163 | { 164 | this->mode_impl_.set_parameters(parameters); 165 | } 166 | 167 | void 168 | Mode::set_part_mode( 169 | const std::string & part, 170 | const StateAndMode stateAndMode) 171 | { 172 | if (stateAndMode.mode.empty()) { 173 | this->mode_impl_.add_part_mode(part, StateAndMode(stateAndMode.state, DEFAULT_MODE)); 174 | } else { 175 | this->mode_impl_.add_part_mode(part, stateAndMode); 176 | } 177 | } 178 | 179 | } // namespace system_modes 180 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/manager_and_monitor.launch.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import unittest 4 | 5 | import ament_index_python 6 | import launch 7 | import launch_ros 8 | import launch_testing.actions 9 | import launch_testing_ros 10 | 11 | from launch import LaunchDescription 12 | from launch.actions import ExecuteProcess 13 | 14 | 15 | def generate_test_description(): 16 | os.environ['OSPL_VERBOSITY'] = '8' 17 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' 18 | 19 | modelfile = '@MODELFILE@' 20 | 21 | mode_manager = launch_ros.actions.Node( 22 | package='system_modes', 23 | executable='mode_manager', 24 | emulate_tty=True, 25 | output='screen', 26 | parameters=[ 27 | {"modelfile": modelfile} 28 | ]) 29 | 30 | test_nodes = ExecuteProcess( 31 | cmd=[ 32 | "@PYTHON_EXECUTABLE@", 33 | "@TEST_NODES@" 34 | ], 35 | name='test_nodes', 36 | emulate_tty=True) 37 | 38 | mode_monitor = launch_ros.actions.Node( 39 | package='system_modes', 40 | executable='mode_monitor', 41 | emulate_tty=True, 42 | output='screen', 43 | parameters=[ 44 | {"modelfile": modelfile}, 45 | {"debug": True}, 46 | {"verbose": True}, 47 | {"rate": 300}, 48 | ]) 49 | 50 | launch_description = LaunchDescription() 51 | launch_description.add_action(mode_manager) 52 | launch_description.add_action(mode_monitor) 53 | launch_description.add_action(test_nodes) 54 | launch_description.add_action(launch_testing.actions.ReadyToTest()) 55 | 56 | return launch_description, locals() 57 | 58 | class TestModeManagement(unittest.TestCase): 59 | 60 | def test_processes_output(self, proc_output, mode_monitor): 61 | """Check manager and nodes logging output for expected strings.""" 62 | 63 | from launch_testing.tools.output import get_default_filtered_prefixes 64 | output_filter = launch_testing_ros.tools.basic_output_filter( 65 | filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], 66 | filtered_rmw_implementation='@RMW_IMPLEMENTATION@' 67 | ) 68 | proc_output.assertWaitFor( 69 | expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), 70 | process=mode_monitor, 71 | output_filter=output_filter, 72 | timeout=15, 73 | stream='stdout') 74 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/manager_and_monitor.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | 3 | from lifecycle_msgs.srv import ChangeState 4 | from rcl_interfaces.msg import SetParametersResult 5 | 6 | import rclpy 7 | from rclpy.executors import SingleThreadedExecutor 8 | from rclpy.node import Node 9 | from rclpy.parameter import Parameter 10 | 11 | from system_modes_msgs.srv import ChangeMode 12 | 13 | 14 | class FakeLifecycleNode(Node): 15 | 16 | def __init__(self, name): 17 | super().__init__(name) 18 | 19 | self.declare_parameter('foo', 0.0) 20 | self.declare_parameter('bar', 'ZERO') 21 | self.add_on_set_parameters_callback(self.parameter_callback) 22 | 23 | # State change service 24 | self.srv = self.create_service( 25 | ChangeState, 26 | self.get_name() + '/change_state', 27 | self.change_state_callback) 28 | 29 | def parameter_callback(self, params): 30 | for p in params: 31 | if p.name == 'bar' and p.type_ == Parameter.Type.STRING: 32 | self.get_logger().info('Parameter %s:%s:%s' % (self.get_name(), p.name, p.value)) 33 | if p.name == 'foo' and p.type_ == Parameter.Type.DOUBLE: 34 | self.get_logger().info('Parameter %s:%s:%s' % (self.get_name(), p.name, p.value)) 35 | return SetParametersResult(successful=True) 36 | 37 | def change_state_callback(self, request, response): 38 | response.success = True 39 | self.get_logger().info('Transition %s:%s' % (self.get_name(), request.transition.label)) 40 | 41 | return response 42 | 43 | 44 | class LifecycleClient(Node): 45 | 46 | def __init__(self): 47 | super().__init__('system_modes_test_client') 48 | 49 | self.clis = self.create_client(ChangeState, '/sys/change_state') 50 | while not self.clis.wait_for_service(timeout_sec=1.0): 51 | self.get_logger().info('service not available, waiting again...') 52 | self.reqs = ChangeState.Request() 53 | 54 | self.clim = self.create_client(ChangeMode, '/sys/change_mode') 55 | while not self.clim.wait_for_service(timeout_sec=1.0): 56 | self.get_logger().info('service not available, waiting again...') 57 | self.reqm = ChangeMode.Request() 58 | 59 | def configure_system(self): 60 | self.reqs.transition.id = 1 61 | self.reqs.transition.label = 'configure' 62 | self.future = self.clis.call_async(self.reqs) 63 | 64 | def activate_system(self): 65 | self.reqs.transition.id = 3 66 | self.reqs.transition.label = 'activate' 67 | self.future = self.clis.call_async(self.reqs) 68 | 69 | def change_mode(self, mode): 70 | self.reqm.mode_name = mode 71 | self.future = self.clim.call_async(self.reqm) 72 | 73 | 74 | def main(args=None): 75 | rclpy.init(args=args) 76 | try: 77 | executor = SingleThreadedExecutor() 78 | node_a = FakeLifecycleNode('A') 79 | node_b = FakeLifecycleNode('B') 80 | 81 | executor.add_node(node_a) 82 | executor.add_node(node_b) 83 | 84 | lc = LifecycleClient() 85 | 86 | try: 87 | lc.configure_system() 88 | executor.spin_once(timeout_sec=1) 89 | executor.spin_once(timeout_sec=1) 90 | sleep(2) # give the system some time to converge 91 | 92 | lc.activate_system() 93 | executor.spin_once(timeout_sec=1) 94 | executor.spin_once(timeout_sec=1) 95 | executor.spin_once(timeout_sec=1) 96 | sleep(2) # give the system some time to converge 97 | 98 | lc.change_mode('CC') 99 | executor.spin() 100 | finally: 101 | executor.shutdown() 102 | node_a.destroy_node() 103 | node_b.destroy_node() 104 | finally: 105 | rclpy.shutdown() 106 | return 0 107 | 108 | 109 | if __name__ == '__main__': 110 | main() 111 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/manager_and_monitor_expected_output.regex: -------------------------------------------------------------------------------- 1 | System.Modes.Monitor 2 | active 3 | CC\(\*\) 4 | BB\(\*\).*BB 5 | FF\(\*\).* -------------------------------------------------------------------------------- /system_modes/test/launchtest/manager_and_monitor_modes.yaml: -------------------------------------------------------------------------------- 1 | # system modes example 2 | --- 3 | 4 | sys: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | A 9 | B 10 | modes: 11 | __DEFAULT__: 12 | A: inactive 13 | B: active 14 | DD: 15 | A: active.AA 16 | B: active.EE 17 | CC: 18 | A: active.BB 19 | B: active.FF 20 | 21 | A: 22 | ros__parameters: 23 | type: node 24 | modes: 25 | __DEFAULT__: 26 | ros__parameters: 27 | foo: 0.1 28 | AA: 29 | ros__parameters: 30 | foo: 0.1 31 | BB: 32 | ros__parameters: 33 | foo: 0.2 34 | 35 | B: 36 | ros__parameters: 37 | type: node 38 | modes: 39 | __DEFAULT__: 40 | ros__parameters: 41 | foo: 0.1 42 | bar: ONE 43 | EE: 44 | ros__parameters: 45 | foo: 0.2 46 | bar: TWO 47 | FF: 48 | ros__parameters: 49 | foo: 0.9 50 | bar: THREE 51 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/modes_observer.launch.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import unittest 4 | 5 | import ament_index_python 6 | import launch 7 | import launch_ros 8 | import launch_testing.actions 9 | import launch_testing_ros 10 | 11 | from launch import LaunchDescription 12 | from launch.actions import ExecuteProcess 13 | 14 | 15 | def generate_test_description(): 16 | os.environ['OSPL_VERBOSITY'] = '8' 17 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' 18 | 19 | modelfile = '@MODELFILE@' 20 | 21 | modes_observer = launch_ros.actions.Node( 22 | package='system_modes', 23 | executable='modes_observer_test_node', 24 | emulate_tty=True, 25 | output='screen') 26 | 27 | mode_manager = launch_ros.actions.Node( 28 | package='system_modes', 29 | executable='mode_manager', 30 | emulate_tty=True, 31 | output='screen', 32 | parameters=[ 33 | {"modelfile": modelfile} 34 | ]) 35 | 36 | test_nodes = ExecuteProcess( 37 | cmd=[ 38 | "@PYTHON_EXECUTABLE@", 39 | "@TEST_NODES@" 40 | ], 41 | name='test_nodes',) 42 | 43 | launch_description = LaunchDescription() 44 | launch_description.add_action(modes_observer) 45 | launch_description.add_action(mode_manager) 46 | launch_description.add_action(test_nodes) 47 | launch_description.add_action(launch_testing.actions.ReadyToTest()) 48 | 49 | return launch_description, locals() 50 | 51 | class TestModeManagement(unittest.TestCase): 52 | 53 | def test_processes_output(self, proc_output, modes_observer): 54 | """Check manager and nodes logging output for expected strings.""" 55 | 56 | from launch_testing.tools.output import get_default_filtered_prefixes 57 | output_filter = launch_testing_ros.tools.basic_output_filter( 58 | filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], 59 | filtered_rmw_implementation='@RMW_IMPLEMENTATION@' 60 | ) 61 | proc_output.assertWaitFor( 62 | expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), 63 | process=modes_observer, 64 | output_filter=output_filter, 65 | timeout=15, 66 | stream='stdout') 67 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/modes_observer.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | 3 | from lifecycle_msgs.msg import TransitionEvent 4 | from lifecycle_msgs.srv import ChangeState 5 | import rclpy 6 | from rclpy.executors import SingleThreadedExecutor 7 | from rclpy.node import Node 8 | from rclpy.parameter import Parameter 9 | 10 | from system_modes_msgs.msg import ModeEvent 11 | from system_modes_msgs.srv import ChangeMode 12 | 13 | 14 | class FakeLifecycleNode(Node): 15 | 16 | def __init__(self, name): 17 | super().__init__(name) 18 | 19 | self.declare_parameter('foo', 0.0) 20 | self.declare_parameter('bar', 'ZERO') 21 | 22 | self.pubs = self.create_publisher( 23 | TransitionEvent, self.get_name() + '/transition_event', 24 | 10) 25 | self.pubm = self.create_publisher(ModeEvent, self.get_name() + '/mode_event', 10) 26 | 27 | # State change service 28 | self.srvs = self.create_service( 29 | ChangeState, 30 | self.get_name() + '/change_state', 31 | self.change_state_callback) 32 | self.add_on_set_parameters_callback(self.parameter_callback) 33 | 34 | def change_state_callback(self, request, response): 35 | response.success = True 36 | 37 | msg = TransitionEvent() 38 | if request.transition.id == 1 or request.transition.id == 4: 39 | print('node ', self.get_name(), ' pretending to go to "inactive"') 40 | msg.transition = request.transition 41 | msg.goal_state.id = 2 42 | msg.goal_state.label = 'inactive' 43 | elif request.transition.id == 3: 44 | print('node ', self.get_name(), ' pretending to go to "active"') 45 | msg.transition = request.transition 46 | msg.goal_state.id = 3 47 | msg.goal_state.label = 'active' 48 | self.pubs.publish(msg) 49 | 50 | return response 51 | 52 | def parameter_callback(self, params): 53 | for p in params: 54 | if p.name == 'foo' and p.type_ == Parameter.Type.DOUBLE: 55 | msg = ModeEvent() 56 | if p.value == 0.2: 57 | msg.goal_mode.label = 'EE' 58 | elif p.value == 0.9: 59 | msg.goal_mode.label = 'FF' 60 | else: 61 | # default mode 62 | msg.goal_mode.label = '__DEFAULT__' 63 | self.pubm.publish(msg) 64 | self.successful = True 65 | return self 66 | 67 | 68 | class LifecycleClient(Node): 69 | 70 | ready = False 71 | 72 | def __init__(self): 73 | super().__init__('system_modes_test_client') 74 | 75 | self.clis = self.create_client(ChangeState, '/sys/change_state') 76 | while not self.clis.wait_for_service(timeout_sec=1.0): 77 | self.get_logger().info('service not available, waiting again...') 78 | self.reqs = ChangeState.Request() 79 | 80 | self.clim = self.create_client(ChangeMode, '/sys/change_mode') 81 | while not self.clim.wait_for_service(timeout_sec=1.0): 82 | self.get_logger().info('service not available, waiting again...') 83 | self.reqm = ChangeMode.Request() 84 | 85 | self.ready = True 86 | 87 | def configure_system(self): 88 | while not self.ready: 89 | sleep(.2) 90 | self.reqs.transition.id = 1 91 | self.reqs.transition.label = 'configure' 92 | self.future = self.clis.call_async(self.reqs) 93 | 94 | def activate_system(self): 95 | self.reqs.transition.id = 3 96 | self.reqs.transition.label = 'activate' 97 | self.future = self.clis.call_async(self.reqs) 98 | 99 | def change_mode(self, mode): 100 | self.reqm.mode_name = mode 101 | self.future = self.clim.call_async(self.reqm) 102 | 103 | 104 | def main(args=None): 105 | rclpy.init(args=args) 106 | try: 107 | executor = SingleThreadedExecutor() 108 | node_a = FakeLifecycleNode('A') 109 | node_b = FakeLifecycleNode('B') 110 | 111 | executor.add_node(node_a) 112 | executor.add_node(node_b) 113 | 114 | lc = LifecycleClient() 115 | 116 | try: 117 | lc.configure_system() 118 | executor.spin_once(timeout_sec=1) 119 | executor.spin_once(timeout_sec=1) 120 | sleep(2) # give the system some time to converge 121 | 122 | lc.activate_system() 123 | executor.spin_once(timeout_sec=1) 124 | executor.spin_once(timeout_sec=1) 125 | executor.spin_once(timeout_sec=1) 126 | sleep(2) # give the system some time to converge 127 | 128 | lc.change_mode('CC') 129 | executor.spin() 130 | finally: 131 | executor.shutdown() 132 | node_a.destroy_node() 133 | node_b.destroy_node() 134 | finally: 135 | rclpy.shutdown() 136 | return 0 137 | 138 | 139 | if __name__ == '__main__': 140 | main() 141 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/modes_observer_expected_output.regex: -------------------------------------------------------------------------------- 1 | cached:.*:inactive 2 | cached:sys:active.__DEFAULT__ 3 | cached:A:inactive 4 | cached:B:active.__DEFAULT__ 5 | cached:sys:active.CC 6 | cached:A:active.BB 7 | cached:B:active.FF 8 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/modes_observer_modes.yaml: -------------------------------------------------------------------------------- 1 | # system modes example 2 | --- 3 | 4 | sys: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | A 9 | B 10 | modes: 11 | __DEFAULT__: 12 | A: inactive 13 | B: active 14 | DD: 15 | A: active.AA 16 | B: active.EE 17 | CC: 18 | A: active.BB 19 | B: active.FF 20 | 21 | A: 22 | ros__parameters: 23 | type: node 24 | modes: 25 | __DEFAULT__: 26 | ros__parameters: 27 | foo: 0.1 28 | AA: 29 | ros__parameters: 30 | foo: 0.1 31 | BB: 32 | ros__parameters: 33 | foo: 0.2 34 | 35 | B: 36 | ros__parameters: 37 | type: node 38 | modes: 39 | __DEFAULT__: 40 | ros__parameters: 41 | foo: 0.1 42 | bar: ONE 43 | EE: 44 | ros__parameters: 45 | foo: 0.2 46 | bar: TWO 47 | FF: 48 | ros__parameters: 49 | foo: 0.9 50 | bar: THREE 51 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/modes_observer_test_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "system_modes/mode_observer.hpp" 31 | 32 | using std::cerr; 33 | using std::cout; 34 | using std::string; 35 | using std::vector; 36 | using std::function; 37 | using std::make_pair; 38 | using std::shared_ptr; 39 | 40 | using system_modes::ModeObserver; 41 | using system_modes::DEFAULT_MODE; 42 | using system_modes::StateAndMode; 43 | using system_modes_msgs::msg::ModeEvent; 44 | 45 | using lifecycle_msgs::msg::State; 46 | using lifecycle_msgs::msg::TransitionEvent; 47 | 48 | using rcl_interfaces::msg::ParameterType; 49 | using rcl_interfaces::msg::ParameterEvent; 50 | 51 | using namespace std::chrono_literals; 52 | 53 | class ModeObserverNode : public rclcpp::Node 54 | { 55 | public: 56 | ModeObserverNode() 57 | : Node("testobservernode") 58 | { 59 | } 60 | void start_observing() 61 | { 62 | observer = std::make_shared(shared_from_this()); 63 | observer->observe("sys"); 64 | observer->observe("A"); 65 | observer->observe("B"); 66 | 67 | periodic_timer = this->create_wall_timer( 68 | 500ms, 69 | [this]() { 70 | std::cout << "cached:sys:" << observer->get("sys").as_string() << std::endl; 71 | std::cout << "cached:A:" << observer->get("A").as_string() << std::endl; 72 | std::cout << "cached:B:" << observer->get("B").as_string() << std::endl; 73 | }); 74 | 75 | auto sm = observer->get("foo"); 76 | assert(sm.state == 0); 77 | assert(sm.mode.compare("") == 0); 78 | } 79 | 80 | private: 81 | std::shared_ptr observer; 82 | rclcpp::TimerBase::SharedPtr periodic_timer; 83 | }; 84 | 85 | int main(int argc, char * argv[]) 86 | { 87 | using namespace std::placeholders; 88 | 89 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 90 | rclcpp::init(argc, argv); 91 | 92 | auto observer = std::make_shared(); 93 | observer->start_observing(); 94 | 95 | rclcpp::executors::SingleThreadedExecutor exe; 96 | exe.add_node(observer); 97 | exe.spin(); 98 | 99 | rclcpp::shutdown(); 100 | 101 | return 0; 102 | } 103 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/redundant_mode_changes.launch.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import unittest 4 | 5 | import ament_index_python 6 | import launch 7 | import launch_ros 8 | import launch_testing 9 | import launch_testing.actions 10 | import launch_testing.asserts 11 | import launch_testing.util 12 | import launch_testing_ros 13 | 14 | from launch import LaunchDescription 15 | from launch.actions import ExecuteProcess 16 | from launch.events import matches_action 17 | from launch.events.process import ShutdownProcess 18 | 19 | import lifecycle_msgs.msg 20 | 21 | 22 | def generate_test_description(): 23 | os.environ['OSPL_VERBOSITY'] = '8' 24 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' 25 | 26 | modelfile = '@MODELFILE@' 27 | 28 | mode_manager = launch_ros.actions.Node( 29 | package='system_modes', 30 | executable='mode_manager', 31 | emulate_tty=True, 32 | output='screen', 33 | parameters=[ 34 | {"modelfile": modelfile} 35 | ]) 36 | 37 | test_nodes = ExecuteProcess( 38 | cmd=[ 39 | "@PYTHON_EXECUTABLE@", 40 | "@TEST_NODES@" 41 | ], 42 | name='test_two_lifecycle_nodes', 43 | emulate_tty=True, 44 | output='screen') 45 | 46 | launch_description = LaunchDescription() 47 | launch_description.add_action(mode_manager) 48 | launch_description.add_action(test_nodes) 49 | launch_description.add_action(launch_testing.util.KeepAliveProc()) 50 | launch_description.add_action(launch_testing.actions.ReadyToTest()) 51 | 52 | return launch_description, locals() 53 | 54 | class TestModeManagement(unittest.TestCase): 55 | 56 | def test_processes_output(self, proc_output, test_nodes): 57 | """Check manager and nodes logging output for expected strings.""" 58 | 59 | from launch_testing.tools.output import get_default_filtered_prefixes 60 | output_filter = launch_testing_ros.tools.basic_output_filter( 61 | filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], 62 | filtered_rmw_implementation='@rmw_implementation@' 63 | ) 64 | proc_output.assertWaitFor( 65 | expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), 66 | process=test_nodes, 67 | output_filter=output_filter, 68 | timeout=15 69 | ) 70 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/redundant_mode_changes_expected_output.regex: -------------------------------------------------------------------------------- 1 | Transition [AB] 2 | Parameter callback #0 A:foo:0.2 3 | Parameter callback #1 A:foo:0.3 4 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/redundant_mode_changes_modes.yaml: -------------------------------------------------------------------------------- 1 | # system modes example 2 | --- 3 | 4 | sys: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | A 9 | B 10 | modes: 11 | __DEFAULT__: 12 | A: inactive 13 | B: active 14 | DD: 15 | A: active.AA 16 | B: active.EE 17 | CC: 18 | A: active.BB 19 | B: active.FF 20 | 21 | A: 22 | ros__parameters: 23 | type: node 24 | modes: 25 | __DEFAULT__: 26 | ros__parameters: 27 | foo: 0.1 28 | AA: 29 | ros__parameters: 30 | foo: 0.2 31 | BB: 32 | ros__parameters: 33 | foo: 0.3 34 | 35 | B: 36 | ros__parameters: 37 | type: node 38 | modes: 39 | __DEFAULT__: 40 | ros__parameters: 41 | foo: 0.1 42 | bar: ONE 43 | EE: 44 | ros__parameters: 45 | foo: 0.2 46 | bar: TWO 47 | FF: 48 | ros__parameters: 49 | foo: 0.9 50 | bar: THREE 51 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_independent_hierarchies.launch.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import unittest 4 | 5 | import ament_index_python 6 | import launch 7 | import launch_ros 8 | import launch_testing 9 | import launch_testing.actions 10 | import launch_testing.asserts 11 | import launch_testing.util 12 | import launch_testing_ros 13 | 14 | from launch import LaunchDescription 15 | from launch.actions import ExecuteProcess 16 | from launch.events import matches_action 17 | from launch.events.process import ShutdownProcess 18 | 19 | import lifecycle_msgs.msg 20 | 21 | 22 | def generate_test_description(): 23 | os.environ['OSPL_VERBOSITY'] = '8' 24 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' 25 | 26 | modelfile = '@MODELFILE@' 27 | 28 | mode_manager = launch_ros.actions.Node( 29 | package='system_modes', 30 | executable='mode_manager', 31 | emulate_tty=True, 32 | output='screen', 33 | parameters=[ 34 | {"modelfile": modelfile} 35 | ]) 36 | 37 | test_nodes = ExecuteProcess( 38 | cmd=[ 39 | "@PYTHON_EXECUTABLE@", 40 | "@TEST_NODES@" 41 | ], 42 | name='test_four_lifecycle_nodes', 43 | emulate_tty=True, 44 | output='screen') 45 | 46 | launch_description = LaunchDescription() 47 | launch_description.add_action(mode_manager) 48 | launch_description.add_action(test_nodes) 49 | launch_description.add_action(launch_testing.util.KeepAliveProc()) 50 | launch_description.add_action(launch_testing.actions.ReadyToTest()) 51 | 52 | return launch_description, locals() 53 | 54 | class TestModeManagement(unittest.TestCase): 55 | 56 | def test_processes_output(self, proc_output, test_nodes): 57 | """Check manager and nodes logging output for expected strings.""" 58 | 59 | from launch_testing.tools.output import get_default_filtered_prefixes 60 | output_filter = launch_testing_ros.tools.basic_output_filter( 61 | filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], 62 | filtered_rmw_implementation='@rmw_implementation@' 63 | ) 64 | proc_output.assertWaitFor( 65 | expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), 66 | process=test_nodes, 67 | output_filter=output_filter, 68 | timeout=15 69 | ) 70 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_independent_hierarchies.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | 3 | from lifecycle_msgs.srv import ChangeState 4 | from rcl_interfaces.msg import SetParametersResult 5 | 6 | import rclpy 7 | from rclpy.executors import SingleThreadedExecutor 8 | from rclpy.node import Node 9 | from rclpy.parameter import Parameter 10 | 11 | from system_modes_msgs.srv import ChangeMode 12 | 13 | 14 | class FakeLifecycleNode(Node): 15 | 16 | def __init__(self, name): 17 | super().__init__(name) 18 | 19 | self.declare_parameter('foo', 0.0) 20 | self.declare_parameter('bar', 'ZERO') 21 | self.add_on_set_parameters_callback(self.parameter_callback) 22 | 23 | # State change service 24 | self.srv = self.create_service( 25 | ChangeState, 26 | self.get_name() + '/change_state', 27 | self.change_state_callback) 28 | 29 | def parameter_callback(self, params): 30 | for p in params: 31 | if p.name == 'bar' and p.type_ == Parameter.Type.STRING: 32 | self.get_logger().info('Parameter %s:%s:%s' % (self.get_name(), p.name, p.value)) 33 | if p.name == 'foo' and p.type_ == Parameter.Type.DOUBLE: 34 | self.get_logger().info('Parameter %s:%s:%s' % (self.get_name(), p.name, p.value)) 35 | return SetParametersResult(successful=True) 36 | 37 | def change_state_callback(self, request, response): 38 | response.success = True 39 | self.get_logger().info('Transition %s:%s' % (self.get_name(), request.transition.label)) 40 | 41 | return response 42 | 43 | 44 | class LifecycleClient(Node): 45 | 46 | def __init__(self, system_name): 47 | super().__init__('system_modes_test_client') 48 | 49 | self.clis = self.create_client(ChangeState, system_name + '/change_state') 50 | while not self.clis.wait_for_service(timeout_sec=1.0): 51 | self.get_logger().info('service not available, waiting again...') 52 | self.reqs = ChangeState.Request() 53 | 54 | self.clim = self.create_client(ChangeMode, system_name + '/change_mode') 55 | while not self.clim.wait_for_service(timeout_sec=1.0): 56 | self.get_logger().info('service not available, waiting again...') 57 | self.reqm = ChangeMode.Request() 58 | 59 | def configure_system(self): 60 | self.reqs.transition.id = 1 61 | self.reqs.transition.label = 'configure' 62 | self.future = self.clis.call_async(self.reqs) 63 | 64 | def activate_system(self): 65 | self.reqs.transition.id = 3 66 | self.reqs.transition.label = 'activate' 67 | self.future = self.clis.call_async(self.reqs) 68 | 69 | def change_mode(self, mode): 70 | self.reqm.mode_name = mode 71 | self.future = self.clim.call_async(self.reqm) 72 | 73 | 74 | def main(args=None): 75 | rclpy.init(args=args) 76 | try: 77 | executor = SingleThreadedExecutor() 78 | node_a = FakeLifecycleNode('A') 79 | node_b = FakeLifecycleNode('B') 80 | node_c = FakeLifecycleNode('C') 81 | node_d = FakeLifecycleNode('D') 82 | 83 | executor.add_node(node_a) 84 | executor.add_node(node_b) 85 | executor.add_node(node_c) 86 | executor.add_node(node_d) 87 | 88 | lc = LifecycleClient('sys') 89 | lc2 = LifecycleClient('sys2') 90 | 91 | try: 92 | lc.configure_system() 93 | executor.spin_once(timeout_sec=1) 94 | executor.spin_once(timeout_sec=1) 95 | lc2.configure_system() 96 | executor.spin_once(timeout_sec=1) 97 | executor.spin_once(timeout_sec=1) 98 | sleep(2) # give the system some time to converge 99 | 100 | lc.activate_system() 101 | executor.spin_once(timeout_sec=1) 102 | executor.spin_once(timeout_sec=1) 103 | executor.spin_once(timeout_sec=1) 104 | lc2.activate_system() 105 | executor.spin_once(timeout_sec=1) 106 | executor.spin_once(timeout_sec=1) 107 | executor.spin_once(timeout_sec=1) 108 | sleep(2) # give the system some time to converge 109 | 110 | lc.change_mode('CC') 111 | executor.spin_once(timeout_sec=1) 112 | executor.spin_once(timeout_sec=1) 113 | lc2.change_mode('DD') 114 | executor.spin() 115 | finally: 116 | executor.shutdown() 117 | node_a.destroy_node() 118 | node_b.destroy_node() 119 | node_c.destroy_node() 120 | node_d.destroy_node() 121 | finally: 122 | rclpy.shutdown() 123 | return 0 124 | 125 | 126 | if __name__ == '__main__': 127 | main() 128 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_independent_hierarchies_expected_output.regex: -------------------------------------------------------------------------------- 1 | Transition [AB]:configure 2 | Parameter D:bar:ONE 3 | Parameter D:bar:TWO 4 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_independent_hierarchies_modes.yaml: -------------------------------------------------------------------------------- 1 | # system modes example 2 | --- 3 | 4 | sys: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | A 9 | B 10 | modes: 11 | __DEFAULT__: 12 | A: inactive 13 | B: active 14 | DD: 15 | A: active.AA 16 | B: active.EE 17 | CC: 18 | A: active.BB 19 | B: active.FF 20 | 21 | A: 22 | ros__parameters: 23 | type: node 24 | modes: 25 | __DEFAULT__: 26 | ros__parameters: 27 | foo: 0.1 28 | AA: 29 | ros__parameters: 30 | foo: 0.1 31 | BB: 32 | ros__parameters: 33 | foo: 0.2 34 | 35 | B: 36 | ros__parameters: 37 | type: node 38 | modes: 39 | __DEFAULT__: 40 | ros__parameters: 41 | foo: 0.1 42 | bar: ONE 43 | EE: 44 | ros__parameters: 45 | foo: 0.2 46 | bar: TWO 47 | FF: 48 | ros__parameters: 49 | foo: 0.9 50 | bar: THREE 51 | 52 | sys2: 53 | ros__parameters: 54 | type: system 55 | parts: 56 | C 57 | D 58 | modes: 59 | __DEFAULT__: 60 | C: inactive 61 | D: active 62 | DD: 63 | C: active.AA 64 | D: active.EE 65 | CC: 66 | C: active.BB 67 | D: active.FF 68 | 69 | C: 70 | ros__parameters: 71 | type: node 72 | modes: 73 | __DEFAULT__: 74 | ros__parameters: 75 | foo: 0.1 76 | AA: 77 | ros__parameters: 78 | foo: 0.1 79 | BB: 80 | ros__parameters: 81 | foo: 0.2 82 | 83 | D: 84 | ros__parameters: 85 | type: node 86 | modes: 87 | __DEFAULT__: 88 | ros__parameters: 89 | foo: 0.1 90 | bar: ONE 91 | EE: 92 | ros__parameters: 93 | foo: 0.2 94 | bar: TWO 95 | FF: 96 | ros__parameters: 97 | foo: 0.9 98 | bar: THREE 99 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_lifecycle_nodes.launch.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import unittest 4 | 5 | import ament_index_python 6 | import launch 7 | import launch_ros 8 | import launch_testing 9 | import launch_testing.actions 10 | import launch_testing.asserts 11 | import launch_testing.util 12 | import launch_testing_ros 13 | 14 | from launch import LaunchDescription 15 | from launch.actions import ExecuteProcess 16 | from launch.events import matches_action 17 | from launch.events.process import ShutdownProcess 18 | 19 | import lifecycle_msgs.msg 20 | 21 | 22 | def generate_test_description(): 23 | os.environ['OSPL_VERBOSITY'] = '8' 24 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' 25 | 26 | modelfile = '@MODELFILE@' 27 | 28 | mode_manager = launch_ros.actions.Node( 29 | package='system_modes', 30 | executable='mode_manager', 31 | emulate_tty=True, 32 | output='screen', 33 | parameters=[ 34 | {"modelfile": modelfile} 35 | ]) 36 | 37 | test_nodes = ExecuteProcess( 38 | cmd=[ 39 | "@PYTHON_EXECUTABLE@", 40 | "@TEST_NODES@" 41 | ], 42 | name='test_two_lifecycle_nodes', 43 | emulate_tty=True, 44 | output='screen') 45 | 46 | launch_description = LaunchDescription() 47 | launch_description.add_action(mode_manager) 48 | launch_description.add_action(test_nodes) 49 | launch_description.add_action(launch_testing.util.KeepAliveProc()) 50 | launch_description.add_action(launch_testing.actions.ReadyToTest()) 51 | 52 | return launch_description, locals() 53 | 54 | class TestModeManagement(unittest.TestCase): 55 | 56 | def test_processes_output(self, proc_output, test_nodes): 57 | """Check manager and nodes logging output for expected strings.""" 58 | 59 | from launch_testing.tools.output import get_default_filtered_prefixes 60 | output_filter = launch_testing_ros.tools.basic_output_filter( 61 | filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], 62 | filtered_rmw_implementation='@rmw_implementation@' 63 | ) 64 | proc_output.assertWaitFor( 65 | expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), 66 | process=test_nodes, 67 | output_filter=output_filter, 68 | timeout=15 69 | ) 70 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_lifecycle_nodes.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | 3 | from lifecycle_msgs.srv import ChangeState 4 | from rcl_interfaces.msg import SetParametersResult 5 | 6 | import rclpy 7 | from rclpy.executors import SingleThreadedExecutor 8 | from rclpy.node import Node 9 | from rclpy.parameter import Parameter 10 | 11 | from system_modes_msgs.srv import ChangeMode 12 | 13 | 14 | class FakeLifecycleNode(Node): 15 | 16 | def __init__(self, name, ns=''): 17 | super().__init__(name, namespace=ns) 18 | 19 | self.declare_parameter('foo', 0.0) 20 | self.declare_parameter('bar', 'ZERO') 21 | self.add_on_set_parameters_callback(self.parameter_callback) 22 | 23 | # State change service 24 | self.srv = self.create_service( 25 | ChangeState, 26 | self.get_name() + '/change_state', 27 | self.change_state_callback) 28 | 29 | def parameter_callback(self, params): 30 | for p in params: 31 | if p.name == 'bar' and p.type_ == Parameter.Type.STRING: 32 | self.get_logger().info('Parameter %s/%s:%s:%s' % ( 33 | ('' if self.get_namespace() == '/' else self.get_namespace()), 34 | self.get_name(), 35 | p.name, 36 | p.value)) 37 | if p.name == 'foo' and p.type_ == Parameter.Type.DOUBLE: 38 | self.get_logger().info('Parameter %s/%s:%s:%s' % ( 39 | ('' if self.get_namespace() == '/' else self.get_namespace()), 40 | self.get_name(), 41 | p.name, 42 | p.value)) 43 | return SetParametersResult(successful=True) 44 | 45 | def change_state_callback(self, request, response): 46 | response.success = True 47 | self.get_logger().info('Transition %s/%s:%s' % ( 48 | ('' if self.get_namespace() == '/' else self.get_namespace()), 49 | self.get_name(), 50 | request.transition.label)) 51 | 52 | return response 53 | 54 | 55 | class LifecycleClient(Node): 56 | 57 | def __init__(self): 58 | super().__init__('system_modes_test_client') 59 | 60 | self.clis = self.create_client(ChangeState, '/sys/change_state') 61 | while not self.clis.wait_for_service(timeout_sec=1.0): 62 | self.get_logger().info('service not available, waiting again...') 63 | self.reqs = ChangeState.Request() 64 | 65 | self.clim = self.create_client(ChangeMode, '/sys/change_mode') 66 | while not self.clim.wait_for_service(timeout_sec=1.0): 67 | self.get_logger().info('service not available, waiting again...') 68 | self.reqm = ChangeMode.Request() 69 | 70 | def configure_system(self): 71 | self.reqs.transition.id = 1 72 | self.reqs.transition.label = 'configure' 73 | self.future = self.clis.call_async(self.reqs) 74 | 75 | def activate_system(self): 76 | self.reqs.transition.id = 3 77 | self.reqs.transition.label = 'activate' 78 | self.future = self.clis.call_async(self.reqs) 79 | 80 | def change_mode(self, mode): 81 | self.reqm.mode_name = mode 82 | self.future = self.clim.call_async(self.reqm) 83 | 84 | 85 | def main(args=None): 86 | rclpy.init(args=args) 87 | try: 88 | executor = SingleThreadedExecutor() 89 | node_a = FakeLifecycleNode('A') 90 | node_b = FakeLifecycleNode('B', ns='foo') 91 | 92 | executor.add_node(node_a) 93 | executor.add_node(node_b) 94 | 95 | lc = LifecycleClient() 96 | 97 | try: 98 | lc.configure_system() 99 | executor.spin_once(timeout_sec=1) 100 | executor.spin_once(timeout_sec=1) 101 | sleep(2) # give the system some time to converge 102 | 103 | lc.activate_system() 104 | executor.spin_once(timeout_sec=1) 105 | executor.spin_once(timeout_sec=1) 106 | executor.spin_once(timeout_sec=1) 107 | sleep(2) # give the system some time to converge 108 | 109 | lc.change_mode('CC') 110 | executor.spin() 111 | finally: 112 | executor.shutdown() 113 | node_a.destroy_node() 114 | node_b.destroy_node() 115 | finally: 116 | rclpy.shutdown() 117 | return 0 118 | 119 | 120 | if __name__ == '__main__': 121 | main() 122 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_lifecycle_nodes_expected_output.regex: -------------------------------------------------------------------------------- 1 | Transition \/(A|foo\/B):configure 2 | Parameter \/foo\/B:bar:ONE 3 | Parameter \/A:foo:0\.2 4 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_lifecycle_nodes_modes.yaml: -------------------------------------------------------------------------------- 1 | # system modes example 2 | --- 3 | 4 | sys: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | A 9 | foo/B 10 | modes: 11 | __DEFAULT__: 12 | A: inactive 13 | foo/B: active 14 | DD: 15 | A: active.AA 16 | foo/B: active.EE 17 | CC: 18 | A: active.BB 19 | foo/B: active.FF 20 | 21 | A: 22 | ros__parameters: 23 | type: node 24 | modes: 25 | __DEFAULT__: 26 | ros__parameters: 27 | foo: 0.1 28 | AA: 29 | ros__parameters: 30 | foo: 0.1 31 | BB: 32 | ros__parameters: 33 | foo: 0.2 34 | 35 | foo/B: 36 | ros__parameters: 37 | type: node 38 | modes: 39 | __DEFAULT__: 40 | ros__parameters: 41 | foo: 0.1 42 | bar: ONE 43 | EE: 44 | ros__parameters: 45 | foo: 0.2 46 | bar: TWO 47 | FF: 48 | ros__parameters: 49 | foo: 0.9 50 | bar: THREE 51 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_mixed_nodes.launch.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import unittest 4 | 5 | import ament_index_python 6 | import launch 7 | import launch_ros 8 | import launch_testing 9 | import launch_testing.actions 10 | import launch_testing.asserts 11 | import launch_testing.util 12 | import launch_testing_ros 13 | 14 | from launch import LaunchDescription 15 | from launch.actions import ExecuteProcess 16 | from launch.events import matches_action 17 | from launch.events.process import ShutdownProcess 18 | 19 | import lifecycle_msgs.msg 20 | 21 | 22 | def generate_test_description(): 23 | os.environ['OSPL_VERBOSITY'] = '8' 24 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' 25 | 26 | modelfile = '@MODELFILE@' 27 | 28 | mode_manager = launch_ros.actions.Node( 29 | package='system_modes', 30 | executable='mode_manager', 31 | emulate_tty=True, 32 | output='screen', 33 | parameters=[ 34 | {"modelfile": modelfile} 35 | ]) 36 | 37 | test_nodes = ExecuteProcess( 38 | cmd=[ 39 | "@PYTHON_EXECUTABLE@", 40 | "@TEST_NODES@" 41 | ], 42 | name='test_two_mixed_nodes', 43 | emulate_tty=True, 44 | output='screen') 45 | 46 | launch_description = LaunchDescription() 47 | launch_description.add_action(mode_manager) 48 | launch_description.add_action(test_nodes) 49 | launch_description.add_action(launch_testing.util.KeepAliveProc()) 50 | launch_description.add_action(launch_testing.actions.ReadyToTest()) 51 | 52 | return launch_description, locals() 53 | 54 | class TestModeManagement(unittest.TestCase): 55 | 56 | def test_processes_output(self, proc_output, test_nodes): 57 | """Check manager and nodes logging output for expected strings.""" 58 | 59 | from launch_testing.tools.output import get_default_filtered_prefixes 60 | output_filter = launch_testing_ros.tools.basic_output_filter( 61 | filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], 62 | filtered_rmw_implementation='@rmw_implementation@' 63 | ) 64 | proc_output.assertWaitFor( 65 | expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), 66 | process=test_nodes, 67 | output_filter=output_filter, 68 | timeout=15 69 | ) 70 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_mixed_nodes.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | 3 | from lifecycle_msgs.srv import ChangeState 4 | from rcl_interfaces.msg import SetParametersResult 5 | 6 | import rclpy 7 | from rclpy.executors import SingleThreadedExecutor 8 | from rclpy.node import Node 9 | from rclpy.parameter import Parameter 10 | 11 | from system_modes_msgs.srv import ChangeMode 12 | 13 | 14 | class FakeLifecycleNode(Node): 15 | 16 | def __init__(self, name): 17 | super().__init__(name) 18 | 19 | self.declare_parameter('foo', 0.0) 20 | self.declare_parameter('bar', 'ZERO') 21 | self.add_on_set_parameters_callback(self.parameter_callback) 22 | 23 | # State change service 24 | self.srv = self.create_service( 25 | ChangeState, 26 | self.get_name() + '/change_state', 27 | self.change_state_callback) 28 | 29 | def parameter_callback(self, params): 30 | for p in params: 31 | if p.name == 'bar' and p.type_ == Parameter.Type.STRING: 32 | self.get_logger().info('Parameter %s:%s:%s' % (self.get_name(), p.name, p.value)) 33 | if p.name == 'foo' and p.type_ == Parameter.Type.DOUBLE: 34 | self.get_logger().info('Parameter %s:%s:%s' % (self.get_name(), p.name, p.value)) 35 | return SetParametersResult(successful=True) 36 | 37 | def change_state_callback(self, request, response): 38 | response.success = True 39 | self.get_logger().info('Transition %s:%s' % (self.get_name(), request.transition.label)) 40 | 41 | return response 42 | 43 | 44 | class NormalNode(Node): 45 | 46 | def __init__(self, name): 47 | super().__init__(name) 48 | 49 | self.declare_parameter('foo', 0.0) 50 | self.declare_parameter('bar', 'ZERO') 51 | self.add_on_set_parameters_callback(self.parameter_callback) 52 | 53 | def parameter_callback(self, params): 54 | for p in params: 55 | if p.name == 'bar' and p.type_ == Parameter.Type.STRING: 56 | self.get_logger().info('Parameter %s:%s:%s' % (self.get_name(), p.name, p.value)) 57 | if p.name == 'foo' and p.type_ == Parameter.Type.DOUBLE: 58 | self.get_logger().info('Parameter %s:%s:%f' % (self.get_name(), p.name, p.value)) 59 | return SetParametersResult(successful=True) 60 | 61 | 62 | class LifecycleClient(Node): 63 | 64 | def __init__(self): 65 | super().__init__('system_modes_test_client') 66 | 67 | self.clis = self.create_client(ChangeState, '/sys/change_state') 68 | while not self.clis.wait_for_service(timeout_sec=1.0): 69 | self.get_logger().info('service not available, waiting again...') 70 | self.reqs = ChangeState.Request() 71 | 72 | self.clim = self.create_client(ChangeMode, '/sys/change_mode') 73 | while not self.clim.wait_for_service(timeout_sec=1.0): 74 | self.get_logger().info('service not available, waiting again...') 75 | self.reqm = ChangeMode.Request() 76 | 77 | def configure_system(self): 78 | self.reqs.transition.id = 1 79 | self.reqs.transition.label = 'configure' 80 | self.future = self.clis.call_async(self.reqs) 81 | 82 | def activate_system(self): 83 | self.reqs.transition.id = 3 84 | self.reqs.transition.label = 'activate' 85 | self.future = self.clis.call_async(self.reqs) 86 | 87 | def change_mode(self, mode): 88 | self.reqm.mode_name = mode 89 | self.future = self.clim.call_async(self.reqm) 90 | 91 | 92 | def main(args=None): 93 | rclpy.init(args=args) 94 | try: 95 | executor = SingleThreadedExecutor() 96 | node_a = FakeLifecycleNode('A') 97 | node_b = NormalNode('B') 98 | 99 | executor.add_node(node_a) 100 | executor.add_node(node_b) 101 | 102 | lc = LifecycleClient() 103 | 104 | try: 105 | lc.configure_system() 106 | executor.spin_once(timeout_sec=1) 107 | executor.spin_once(timeout_sec=1) 108 | sleep(2) # give the system some time to converge 109 | 110 | lc.activate_system() 111 | executor.spin_once(timeout_sec=1) 112 | executor.spin_once(timeout_sec=1) 113 | executor.spin_once(timeout_sec=1) 114 | sleep(2) # give the system some time to converge 115 | 116 | lc.change_mode('CC') 117 | executor.spin() 118 | finally: 119 | executor.shutdown() 120 | node_a.destroy_node() 121 | node_b.destroy_node() 122 | finally: 123 | rclpy.shutdown() 124 | return 0 125 | 126 | 127 | if __name__ == '__main__': 128 | main() 129 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_mixed_nodes_expected_output.regex: -------------------------------------------------------------------------------- 1 | Transition [AB]:configure 2 | Parameter B:bar:ONE 3 | Parameter B:foo:0\.1 4 | Parameter A:foo:0\.2 5 | -------------------------------------------------------------------------------- /system_modes/test/launchtest/two_mixed_nodes_modes.yaml: -------------------------------------------------------------------------------- 1 | # system modes example 2 | --- 3 | 4 | sys: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | A 9 | B 10 | modes: 11 | __DEFAULT__: 12 | A: inactive 13 | B: active 14 | DD: 15 | A: active.AA 16 | B: active.EE 17 | CC: 18 | A: active.BB 19 | B: active.FF 20 | 21 | A: 22 | ros__parameters: 23 | type: node 24 | modes: 25 | __DEFAULT__: 26 | ros__parameters: 27 | foo: 0.1 28 | AA: 29 | ros__parameters: 30 | foo: 0.1 31 | BB: 32 | ros__parameters: 33 | foo: 0.2 34 | 35 | B: 36 | ros__parameters: 37 | type: node 38 | modes: 39 | __DEFAULT__: 40 | ros__parameters: 41 | foo: 0.1 42 | bar: ONE 43 | EE: 44 | ros__parameters: 45 | foo: 0.2 46 | bar: TWO 47 | FF: 48 | ros__parameters: 49 | foo: 0.9 50 | bar: THREE 51 | -------------------------------------------------------------------------------- /system_modes/test/modefiles.h.in: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | const static std::string MODE_FILE_CORRECT = "@MODE_FILE_CORRECT@"; 6 | const static std::string MODE_FILE_RULES = "@MODE_FILE_RULES@"; 7 | const static std::string MODE_FILE_WRONG = "@MODE_FILE_WRONG@"; 8 | -------------------------------------------------------------------------------- /system_modes/test/test_default_mode.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "system_modes/mode.hpp" 25 | 26 | using std::string; 27 | using std::vector; 28 | using rclcpp::Parameter; 29 | 30 | using system_modes::DefaultMode; 31 | using system_modes::DefaultModePtr; 32 | 33 | class TestDefaultMode : public ::testing::Test 34 | { 35 | protected: 36 | static void SetUpTestCase() 37 | { 38 | rclcpp::init(0, nullptr); 39 | } 40 | 41 | void SetUp() 42 | { 43 | default_mode = DefaultModePtr(new DefaultMode()); 44 | } 45 | 46 | void TearDown() 47 | { 48 | } 49 | 50 | DefaultModePtr default_mode; 51 | }; 52 | 53 | /* 54 | Testing client construction and destruction. 55 | */ 56 | TEST_F(TestDefaultMode, construction_and_destruction) { 57 | { 58 | DefaultMode mode; 59 | EXPECT_EQ("__DEFAULT__", mode.get_name()); 60 | } 61 | } 62 | 63 | /* 64 | Testing add parameter 65 | */ 66 | TEST_F(TestDefaultMode, set_parameter) { 67 | { 68 | vector parameter_names({"foo"}); 69 | 70 | Parameter param1("foo", "bar"); 71 | vector parameters; 72 | parameters.push_back(param1); 73 | 74 | default_mode->set_parameter(param1); 75 | 76 | EXPECT_EQ("bar", default_mode->get_parameter("foo").as_string()); 77 | 78 | EXPECT_EQ(parameters.size(), default_mode->get_parameters().size()); 79 | EXPECT_EQ(parameter_names, default_mode->get_parameter_names()); 80 | } 81 | } 82 | 83 | /* 84 | Testing add parameters 85 | */ 86 | TEST_F(TestDefaultMode, set_parameters) { 87 | { 88 | vector parameter_names({"foo", "fubar"}); 89 | 90 | Parameter param1("foo", "bar"); 91 | Parameter param2("fubar", 0.123); 92 | 93 | vector parameters; 94 | parameters.push_back(param1); 95 | parameters.push_back(param2); 96 | 97 | default_mode->set_parameters(parameters); 98 | 99 | EXPECT_EQ("bar", default_mode->get_parameter("foo").as_string()); 100 | EXPECT_EQ(0.123, default_mode->get_parameter("fubar").as_double()); 101 | 102 | EXPECT_EQ(parameters.size(), default_mode->get_parameters().size()); 103 | EXPECT_EQ(parameter_names, default_mode->get_parameter_names()); 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /system_modes/test/test_mode.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "system_modes/mode.hpp" 25 | 26 | using std::string; 27 | using std::vector; 28 | using rclcpp::Parameter; 29 | 30 | using system_modes::Mode; 31 | using system_modes::DefaultMode; 32 | using system_modes::DefaultModePtr; 33 | 34 | class TestMode : public ::testing::Test 35 | { 36 | protected: 37 | static void SetUpTestCase() 38 | { 39 | rclcpp::init(0, nullptr); 40 | } 41 | 42 | void SetUp() 43 | { 44 | default_mode = DefaultModePtr(new DefaultMode()); 45 | 46 | Parameter param1("foo", "bar"); 47 | Parameter param2("fubar", 0.123); 48 | 49 | vector parameters; 50 | parameters.push_back(param1); 51 | parameters.push_back(param2); 52 | 53 | default_mode->set_parameters(parameters); 54 | } 55 | 56 | void TearDown() 57 | { 58 | } 59 | 60 | DefaultModePtr default_mode; 61 | }; 62 | 63 | /* 64 | Testing client construction and destruction. 65 | */ 66 | TEST_F(TestMode, construction_and_destruction) { 67 | { 68 | vector parameter_names({"foo", "fubar"}); 69 | DefaultModePtr default_mode_null; 70 | 71 | Mode * mode; 72 | 73 | EXPECT_THROW( 74 | mode = new Mode("MODE", default_mode_null), 75 | std::runtime_error); 76 | EXPECT_NO_THROW(mode = new Mode("MODE", default_mode)); 77 | 78 | EXPECT_EQ(parameter_names, mode->get_parameter_names()); 79 | EXPECT_EQ("bar", mode->get_parameter("foo").as_string()); 80 | EXPECT_EQ(0.123, mode->get_parameter("fubar").as_double()); 81 | } 82 | } 83 | 84 | /* 85 | Testing set parameter 86 | */ 87 | TEST_F(TestMode, set_parameter) { 88 | { 89 | vector parameter_names({"foo", "fubar"}); 90 | 91 | Parameter param1("foo", "baz"); 92 | Parameter param2("fubar", 0.234); 93 | Parameter param3("fuuubar", true); 94 | 95 | Mode mode("MODE", default_mode); 96 | 97 | mode.set_parameter(param1); 98 | mode.set_parameter(param2); 99 | EXPECT_EQ("baz", mode.get_parameter("foo").as_string()); 100 | EXPECT_EQ(0.234, mode.get_parameter("fubar").as_double()); 101 | 102 | EXPECT_THROW(mode.set_parameter(param3), std::out_of_range); 103 | 104 | EXPECT_EQ(parameter_names, mode.get_parameter_names()); 105 | } 106 | } 107 | 108 | /* 109 | Testing set parameters 110 | */ 111 | TEST_F(TestMode, set_parameters) { 112 | { 113 | vector parameter_names({"foo", "fubar"}); 114 | 115 | Parameter param1("foo", "baz"); 116 | Parameter param2("fubar", 0.234); 117 | 118 | vector parameters; 119 | parameters.push_back(param1); 120 | parameters.push_back(param2); 121 | 122 | Mode mode("MODE", default_mode); 123 | EXPECT_NO_THROW(mode.set_parameters(parameters)); 124 | 125 | EXPECT_EQ("baz", mode.get_parameter("foo").as_string()); 126 | EXPECT_EQ(0.234, mode.get_parameter("fubar").as_double()); 127 | 128 | EXPECT_EQ(parameters.size(), mode.get_parameters().size()); 129 | EXPECT_EQ(parameter_names, mode.get_parameter_names()); 130 | 131 | Parameter param3("fuuubar", true); 132 | parameters.push_back(param3); 133 | EXPECT_THROW(mode.set_parameters(parameters), std::out_of_range); 134 | 135 | EXPECT_EQ((unsigned int) 2, mode.get_parameters().size()); 136 | EXPECT_EQ(parameter_names, mode.get_parameter_names()); 137 | } 138 | } 139 | -------------------------------------------------------------------------------- /system_modes/test/test_mode_handling.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "system_modes/modefiles.h" 26 | #include "system_modes/mode_handling.hpp" 27 | 28 | using std::string; 29 | using std::vector; 30 | using rclcpp::Parameter; 31 | 32 | using system_modes::ModeHandling; 33 | using system_modes::StateAndMode; 34 | 35 | using lifecycle_msgs::msg::State; 36 | 37 | /* 38 | Testing parsing of mode files 39 | */ 40 | TEST(TestModeFilesParse, constructor) { 41 | ModeHandling * handling; 42 | 43 | EXPECT_NO_THROW(handling = new ModeHandling(MODE_FILE_CORRECT)); 44 | EXPECT_NO_THROW(handling = new ModeHandling(MODE_FILE_RULES)); 45 | 46 | EXPECT_THROW( 47 | handling = new ModeHandling("incorrect path"), 48 | std::runtime_error); 49 | 50 | (void) handling; 51 | } 52 | 53 | /* 54 | Testing parsing of mode files 55 | */ 56 | TEST(TestModeFilesParse, parse_rules) { 57 | ModeHandling * handling = new ModeHandling(MODE_FILE_RULES); 58 | 59 | auto target = StateAndMode(State::PRIMARY_STATE_ACTIVE, ""); 60 | EXPECT_EQ(0u, handling->get_rules_for("system", target).size()); 61 | 62 | target = StateAndMode(State::PRIMARY_STATE_ACTIVE, "AA"); 63 | EXPECT_EQ(2u, handling->get_rules_for("system", target).size()); 64 | 65 | target = StateAndMode(State::PRIMARY_STATE_ACTIVE, "BB"); 66 | EXPECT_EQ(2u, handling->get_rules_for("system", target).size()); 67 | } 68 | 69 | /* 70 | Testing parsing of mode files 71 | */ 72 | TEST(TestModeFilesParse, test_rules) { 73 | ModeHandling * handling = new ModeHandling(MODE_FILE_RULES); 74 | 75 | auto rules = handling->get_rules_for("system", StateAndMode(State::PRIMARY_STATE_ACTIVE, "AA")); 76 | 77 | EXPECT_EQ("degrade_from_AA", rules[0].name); 78 | EXPECT_EQ("system", rules[0].system); 79 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_ACTIVE, "AA"), rules[0].system_target); 80 | EXPECT_EQ("part0", rules[0].part); 81 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_INACTIVE), rules[0].part_actual); 82 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_ACTIVE), rules[0].new_system_target); 83 | 84 | EXPECT_EQ("inactive_from_AA", rules[1].name); 85 | EXPECT_EQ("system", rules[1].system); 86 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_ACTIVE, "AA"), rules[1].system_target); 87 | EXPECT_EQ("part1", rules[1].part); 88 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_INACTIVE), rules[1].part_actual); 89 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_INACTIVE), rules[1].new_system_target); 90 | 91 | rules = handling->get_rules_for("system", StateAndMode(State::PRIMARY_STATE_ACTIVE, "BB")); 92 | 93 | EXPECT_EQ("degrade_from_BB", rules[0].name); 94 | EXPECT_EQ("system", rules[0].system); 95 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_ACTIVE, "BB"), rules[0].system_target); 96 | EXPECT_EQ("part0", rules[0].part); 97 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_INACTIVE), rules[0].part_actual); 98 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_ACTIVE), rules[0].new_system_target); 99 | 100 | EXPECT_EQ("inactive_from_BB", rules[1].name); 101 | EXPECT_EQ("system", rules[1].system); 102 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_ACTIVE, "BB"), rules[1].system_target); 103 | EXPECT_EQ("part1", rules[1].part); 104 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_INACTIVE), rules[1].part_actual); 105 | EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_INACTIVE), rules[1].new_system_target); 106 | } 107 | -------------------------------------------------------------------------------- /system_modes/test/test_mode_manager.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "system_modes/modefiles.h" 22 | #include "system_modes/mode_manager.hpp" 23 | 24 | using system_modes::ModeManager; 25 | 26 | TEST(TestModeManager, initialization) { 27 | ModeManager * manager; 28 | 29 | EXPECT_THROW(manager = new ModeManager(), std::invalid_argument); 30 | 31 | (void) manager; 32 | } 33 | -------------------------------------------------------------------------------- /system_modes/test/test_mode_monitor.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "system_modes/modefiles.h" 22 | #include "system_modes/mode_monitor.hpp" 23 | 24 | using system_modes::ModeMonitor; 25 | 26 | TEST(TestModeMonitor, initialization) { 27 | ModeMonitor * monitor; 28 | 29 | EXPECT_THROW(monitor = new ModeMonitor(), std::invalid_argument); 30 | 31 | (void) monitor; 32 | } 33 | -------------------------------------------------------------------------------- /system_modes/test/test_mode_observer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include "system_modes/modefiles.h" 23 | #include "system_modes/mode_observer.hpp" 24 | 25 | using system_modes::ModeObserver; 26 | using system_modes::StateAndMode; 27 | 28 | class TestModeObserver : public ::testing::Test 29 | { 30 | protected: 31 | static void SetUpTestCase() 32 | { 33 | rclcpp::init(0, nullptr); 34 | } 35 | 36 | void SetUp() 37 | { 38 | rclcpp::executors::SingleThreadedExecutor exe; 39 | my_node = std::make_shared("testobserver"); 40 | observer = new ModeObserver(my_node); 41 | } 42 | 43 | void TearDown() 44 | { 45 | delete observer; 46 | } 47 | 48 | ModeObserver * observer; 49 | std::shared_ptr my_node; 50 | }; 51 | 52 | TEST_F(TestModeObserver, initialization) { 53 | auto my_node = std::make_shared("testobserver"); 54 | 55 | ModeObserver * my_observer; 56 | EXPECT_NO_THROW(my_observer = new ModeObserver(my_node)); 57 | 58 | (void) my_observer; 59 | } 60 | 61 | TEST_F(TestModeObserver, start_stop_observing) { 62 | EXPECT_NO_THROW(observer->observe("foo")); 63 | 64 | EXPECT_NO_THROW(observer->stop_observing("foo")); 65 | EXPECT_NO_THROW(observer->stop_observing("bar")); 66 | } 67 | 68 | TEST_F(TestModeObserver, observing) { 69 | EXPECT_NO_THROW(observer->observe("foo")); 70 | 71 | StateAndMode sm; 72 | EXPECT_NO_THROW(sm = observer->get("foo")); 73 | EXPECT_EQ(0u, sm.state); 74 | EXPECT_STREQ("", sm.mode.c_str()); 75 | } 76 | -------------------------------------------------------------------------------- /system_modes/test/test_modes.yaml: -------------------------------------------------------------------------------- 1 | # config/test 2 | --- 3 | 4 | system: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | part0 9 | part1 10 | modes: 11 | __DEFAULT__: 12 | part0: inactive 13 | part1: active 14 | AA: 15 | part0: active.FOO 16 | part1: active.AAA 17 | BB: 18 | part0: active.BAR 19 | part1: active.BBB 20 | 21 | part0: 22 | ros__parameters: 23 | type: node 24 | modes: 25 | __DEFAULT__: 26 | ros__parameters: 27 | foo: 0.1 28 | bar: WARN 29 | FOO: 30 | ros__parameters: 31 | foo: 0.1 32 | bar: DBG 33 | BAR: 34 | ros__parameters: 35 | foo: 0.2 36 | bar: ERR 37 | 38 | part1: 39 | ros__parameters: 40 | type: node 41 | modes: 42 | __DEFAULT__: 43 | ros__parameters: 44 | foo: 0.1 45 | bar: WARN 46 | AAA: 47 | ros__parameters: 48 | foo: 0.2 49 | bar: DBG 50 | BBB: 51 | ros__parameters: 52 | foo: 0.9 53 | bar: ERR 54 | 55 | -------------------------------------------------------------------------------- /system_modes/test/test_modes_rules.yaml: -------------------------------------------------------------------------------- 1 | # config/test 2 | --- 3 | 4 | system: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | part0 9 | part1 10 | modes: 11 | __DEFAULT__: 12 | part0: inactive 13 | part1: active 14 | AA: 15 | part0: active.FOO 16 | part1: active.AAA 17 | BB: 18 | part0: active.BAR 19 | part1: active.BBB 20 | rules: 21 | degrade_from_AA: 22 | if_target: active.AA 23 | if_part: [part0, inactive] 24 | new_target: active.__DEFAULT__ 25 | degrade_from_BB: 26 | if_target: active.BB 27 | if_part: [part0, inactive] 28 | new_target: active.__DEFAULT__ 29 | inactive_from_AA: 30 | if_target: active.AA 31 | if_part: [part1, inactive] 32 | new_target: inactive 33 | inactive_from_BB: 34 | if_target: active.BB 35 | if_part: [part1, inactive] 36 | new_target: inactive 37 | 38 | part0: 39 | ros__parameters: 40 | type: node 41 | modes: 42 | __DEFAULT__: 43 | ros__parameters: 44 | foo: 0.1 45 | bar: WARN 46 | FOO: 47 | ros__parameters: 48 | foo: 0.1 49 | bar: DBG 50 | BAR: 51 | ros__parameters: 52 | foo: 0.2 53 | bar: ERR 54 | 55 | part1: 56 | ros__parameters: 57 | type: node 58 | modes: 59 | __DEFAULT__: 60 | ros__parameters: 61 | foo: 0.1 62 | bar: WARN 63 | AAA: 64 | ros__parameters: 65 | foo: 0.2 66 | bar: DBG 67 | BBB: 68 | ros__parameters: 69 | foo: 0.9 70 | bar: ERR 71 | 72 | -------------------------------------------------------------------------------- /system_modes/test/test_modes_wrong.yaml: -------------------------------------------------------------------------------- 1 | # config/test 2 | --- 3 | 4 | system: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | testPart 9 | testPart2 10 | modes: 11 | __DEFAULT__: 12 | testPart: INACTIVE 13 | testPart2: ACTIVE 14 | AA: 15 | testPart: ACTIVE.WEAK 16 | testPart2: ACTIVE.SLOW 17 | BB: 18 | testPart: ACTIVE.STRONG 19 | testPart2: ACTIVE.FAST 20 | 21 | testPart: 22 | ros__parameters: 23 | type: node 24 | modes: 25 | __DEFAULT__: 26 | ros__parameters: 27 | foo: 0.1 28 | bar: WARN 29 | FOO: 30 | ros__parameters: 31 | wrong: 0.1 32 | bar: DBG 33 | BAR: 34 | ros__parameters: 35 | foo: 0.2 36 | wrong: ERR 37 | 38 | testPart2: 39 | ros__parameters: 40 | type: node 41 | modes: 42 | __DEFAULT__: 43 | ros__parameters: 44 | foo: 0.1 45 | bar: WARN 46 | AAA: 47 | ros__parameters: 48 | foo: 0.2 49 | bar: DBG 50 | BBB: 51 | ros__parameters: 52 | foo: 0.9 53 | bar: ERR 54 | 55 | -------------------------------------------------------------------------------- /system_modes/test/test_state_and_mode_struct.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include "system_modes/mode.hpp" 23 | 24 | using std::string; 25 | using namespace std::string_literals; 26 | 27 | using system_modes::StateAndMode; 28 | using lifecycle_msgs::msg::State; 29 | 30 | class TestStateAndMode : public ::testing::Test 31 | { 32 | protected: 33 | void SetUp() 34 | { 35 | unknown.state = State::PRIMARY_STATE_UNKNOWN; 36 | 37 | inactive.state = State::PRIMARY_STATE_INACTIVE; 38 | 39 | active.state = State::PRIMARY_STATE_ACTIVE; 40 | 41 | active_default.state = State::PRIMARY_STATE_ACTIVE; 42 | active_default.mode = "__DEFAULT__"; 43 | 44 | active_foo.state = State::PRIMARY_STATE_ACTIVE; 45 | active_foo.mode = "FOO"; 46 | } 47 | 48 | void TearDown() 49 | { 50 | } 51 | 52 | StateAndMode unknown, inactive, active, active_default, active_foo; 53 | }; 54 | 55 | TEST_F(TestStateAndMode, initialization_as_unknown) { 56 | { 57 | StateAndMode initial; 58 | EXPECT_EQ(unknown, initial); 59 | } 60 | } 61 | 62 | TEST_F(TestStateAndMode, comparison) { 63 | { 64 | EXPECT_EQ(inactive, inactive); 65 | EXPECT_EQ(active, active); 66 | EXPECT_EQ(active, active_default); 67 | 68 | EXPECT_NE(active, inactive); 69 | EXPECT_NE(active, active_foo); 70 | EXPECT_NE(active_default, active_foo); 71 | EXPECT_NE(active_default, inactive); 72 | 73 | active_foo.state = State::PRIMARY_STATE_INACTIVE; 74 | EXPECT_EQ(inactive, active_foo); 75 | } 76 | } 77 | 78 | TEST_F(TestStateAndMode, string_getter) { 79 | { 80 | EXPECT_EQ("inactive"s, inactive.as_string()); 81 | EXPECT_EQ("active"s, active.as_string()); 82 | EXPECT_EQ("active.__DEFAULT__"s, active_default.as_string()); 83 | EXPECT_EQ("active.FOO"s, active_foo.as_string()); 84 | active_foo.state = State::PRIMARY_STATE_INACTIVE; 85 | EXPECT_EQ("inactive"s, active_foo.as_string()); 86 | } 87 | } 88 | 89 | TEST_F(TestStateAndMode, string_setter) { 90 | { 91 | StateAndMode copy; 92 | 93 | copy.from_string("active"); 94 | EXPECT_EQ(active, copy); 95 | 96 | copy.from_string("active.__DEFAULT__"); 97 | EXPECT_EQ(active_default, copy); 98 | 99 | copy.from_string("active.FOO"); 100 | EXPECT_EQ(active_foo, copy); 101 | 102 | copy.from_string("inactive"); 103 | EXPECT_EQ(inactive, copy); 104 | 105 | copy.from_string(active.as_string()); 106 | EXPECT_EQ(active, copy); 107 | 108 | copy.from_string(active_foo.as_string()); 109 | EXPECT_EQ(active_foo, copy); 110 | 111 | copy.from_string(inactive.as_string()); 112 | EXPECT_EQ(inactive, copy); 113 | } 114 | } 115 | 116 | TEST_F(TestStateAndMode, string_unknown) { 117 | { 118 | EXPECT_FALSE(inactive.unknown()); 119 | EXPECT_FALSE(active.unknown()); 120 | EXPECT_FALSE(active_default.unknown()); 121 | EXPECT_FALSE(active_foo.unknown()); 122 | EXPECT_TRUE(unknown.unknown()); 123 | } 124 | } 125 | -------------------------------------------------------------------------------- /system_modes_examples/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package system_modes_examples 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.0 (2020-07-21) 6 | ------------------ 7 | 8 | * More flexibility in specifying the default mode, any mode can be now default mode 9 | https://github.com/micro-ROS/system_modes/issues/69 10 | 11 | 0.8.0 (2020-04-22) 12 | ------------------ 13 | 14 | * Launch integration, i.e. launch actions, events, and event handlers for system modes 15 | 16 | 0.7.1 (2020-04-22) 17 | ------------------ 18 | 19 | * Improved metadata for ROS 2 package releases 20 | 21 | 0.7.0 (2020-04-22) 22 | ------------------ 23 | 24 | * Launch tests now using launch_ros node action https://github.com/micro-ROS/system_modes/pull/72 25 | * Introduced separate interface package, system_modes_msgs https://github.com/micro-ROS/system_modes/pull/74 26 | 27 | 0.6.0 (2020-03-16) 28 | ------------------ 29 | 30 | * Introduced mode observer https://github.com/micro-ROS/system_modes/issues/59 31 | * Mode manager prevents redundant mode changes https://github.com/micro-ROS/system_modes/pull/67 32 | * Minor bugfix in inference 33 | 34 | 0.5.0 (2020-03-16) 35 | ------------------ 36 | * Atomic parameter setting https://github.com/micro-ROS/system_modes/issues/59 37 | * Bug fixing 38 | * More tests 39 | 40 | 0.4.2 (2020-12-17) 41 | ------------------ 42 | * Error handling and rules feature no longer experimental 43 | * Fixed bugs in monitor and tests 44 | 45 | 0.4.1 (2020-10-29) 46 | ------------------ 47 | * Include experimental error handling and rules feature 48 | * https://github.com/micro-ROS/system_modes/issues/13 49 | * CI for ubuntu 20.04 ROS 2 rolling 50 | 51 | 0.4.0 (2020-09-30) 52 | ------------------ 53 | * publish inferred state and mode transitions 54 | * https://github.com/micro-ROS/system_modes/issues/42 55 | 56 | 0.3.0 (2020-07-23) 57 | ------------------ 58 | * removed boost dependencies (was: program options) 59 | * changed mode service specifications (less redundancy) 60 | * https://github.com/micro-ROS/system_modes/issues/24 61 | 62 | 0.2.3 (2020-07-23) 63 | ------------------ 64 | * improved StateAndMode struct 65 | * testing 66 | 67 | 0.2.2 (2020-07-13) 68 | ------------------ 69 | * introduced StateAndMode struct to bundle lifecycle state and system mode 70 | 71 | 0.2.0 (2020-02-13) 72 | ------------------ 73 | * integration with ROS 2 launch 74 | * updated docs 75 | 76 | 0.1.6 (2019-10-31) 77 | ------------------ 78 | * fixed QoS configuration for parameter event subscribers 79 | 80 | 0.1.5 (2019-10-21 81 | ------------------- 82 | * migration to ROS 2 eloquent elusor 83 | 84 | 0.1.2 (2019-03-18) 85 | ------------------ 86 | * fixed dependencies in package.xml 87 | 88 | 0.1.1 (2019-03-08) 89 | ------------------ 90 | * first public release for ROS 2 system modes 91 | -------------------------------------------------------------------------------- /system_modes_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(system_modes_examples) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(system_modes REQUIRED) 21 | 22 | # drive_base 23 | add_executable(drive_base src/drive_base.cpp) 24 | target_include_directories(drive_base PUBLIC 25 | $ 26 | $) 27 | ament_target_dependencies(drive_base 28 | "system_modes" 29 | ) 30 | install(TARGETS drive_base 31 | EXPORT export_${PROJECT_NAME} 32 | DESTINATION lib/${PROJECT_NAME}) 33 | 34 | # manipulator 35 | add_executable(manipulator src/manipulator.cpp) 36 | target_include_directories(manipulator PUBLIC 37 | $ 38 | $) 39 | ament_target_dependencies(manipulator 40 | "system_modes" 41 | ) 42 | install(TARGETS manipulator 43 | EXPORT export_${PROJECT_NAME} 44 | DESTINATION lib/${PROJECT_NAME}) 45 | 46 | # launch 47 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) 48 | 49 | # SMH file 50 | install(FILES example_modes.yaml 51 | DESTINATION share/${PROJECT_NAME}/ 52 | ) 53 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) 54 | 55 | if(BUILD_TESTING) 56 | find_package(ament_lint_auto REQUIRED) 57 | find_package(ament_cmake_gtest REQUIRED) 58 | find_package(ament_cmake_gmock REQUIRED) 59 | # the following line skips the linter which checks for copyrights 60 | # remove the line when a copyright and license is present in all source files 61 | set(ament_cmake_copyright_FOUND TRUE) 62 | # the following line skips cpplint (only works in a git repo) 63 | # remove the line when this package is a git repo 64 | set(ament_cmake_cpplint_FOUND TRUE) 65 | ament_lint_auto_find_test_dependencies() 66 | endif() 67 | 68 | ament_package() 69 | -------------------------------------------------------------------------------- /system_modes_examples/doc/screenshot-manager-deviation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/micro-ROS/system_modes/6153cb8263c0dcf5f0f7015b795418e374f4695a/system_modes_examples/doc/screenshot-manager-deviation.png -------------------------------------------------------------------------------- /system_modes_examples/doc/screenshot-manager.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/micro-ROS/system_modes/6153cb8263c0dcf5f0f7015b795418e374f4695a/system_modes_examples/doc/screenshot-manager.png -------------------------------------------------------------------------------- /system_modes_examples/doc/screenshot-monitor-active.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/micro-ROS/system_modes/6153cb8263c0dcf5f0f7015b795418e374f4695a/system_modes_examples/doc/screenshot-monitor-active.png -------------------------------------------------------------------------------- /system_modes_examples/doc/screenshot-monitor-inactive.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/micro-ROS/system_modes/6153cb8263c0dcf5f0f7015b795418e374f4695a/system_modes_examples/doc/screenshot-monitor-inactive.png -------------------------------------------------------------------------------- /system_modes_examples/doc/screenshot-monitor-moderate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/micro-ROS/system_modes/6153cb8263c0dcf5f0f7015b795418e374f4695a/system_modes_examples/doc/screenshot-monitor-moderate.png -------------------------------------------------------------------------------- /system_modes_examples/doc/screenshot-monitor-performance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/micro-ROS/system_modes/6153cb8263c0dcf5f0f7015b795418e374f4695a/system_modes_examples/doc/screenshot-monitor-performance.png -------------------------------------------------------------------------------- /system_modes_examples/doc/screenshot-monitor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/micro-ROS/system_modes/6153cb8263c0dcf5f0f7015b795418e374f4695a/system_modes_examples/doc/screenshot-monitor.png -------------------------------------------------------------------------------- /system_modes_examples/example_modes.yaml: -------------------------------------------------------------------------------- 1 | # system modes example 2 | --- 3 | 4 | actuation: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | manipulator 9 | drive_base 10 | modes: 11 | __DEFAULT__: 12 | manipulator: inactive 13 | drive_base: active 14 | MODERATE: 15 | manipulator: active.WEAK 16 | drive_base: active.SLOW 17 | PERFORMANCE: 18 | manipulator: active.STRONG 19 | drive_base: active.FAST 20 | rules: 21 | degrade_from_MODERATE: 22 | if_target: active.MODERATE 23 | if_part: [manipulator, inactive] 24 | new_target: active.__DEFAULT__ 25 | degrade_from_PERFORMANCE: 26 | if_target: active.PERFORMANCE 27 | if_part: [manipulator, inactive] 28 | new_target: active.__DEFAULT__ 29 | inactive_from_MODERATE: 30 | if_target: active.MODERATE 31 | if_part: [drive_base, inactive] 32 | new_target: inactive 33 | inactive_from_PERFORMANCE: 34 | if_target: active.PERFORMANCE 35 | if_part: [drive_base, inactive] 36 | new_target: inactive 37 | # if system in __DEFAULT__, don't degrade, don't deactivate, but recover 38 | 39 | manipulator: 40 | ros__parameters: 41 | type: node 42 | modes: 43 | __DEFAULT__: 44 | ros__parameters: 45 | max_torque: 0.1 46 | WEAK: 47 | ros__parameters: 48 | max_torque: 0.1 49 | STRONG: 50 | ros__parameters: 51 | max_torque: 0.2 52 | 53 | drive_base: 54 | ros__parameters: 55 | type: node 56 | modes: 57 | __DEFAULT__: 58 | ros__parameters: 59 | max_speed: 0.1 60 | controller: PID 61 | SLOW: 62 | ros__parameters: 63 | max_speed: 0.2 64 | controller: PID 65 | FAST: 66 | ros__parameters: 67 | max_speed: 0.9 68 | controller: MPC 69 | -------------------------------------------------------------------------------- /system_modes_examples/example_modes_with_namespace.yaml: -------------------------------------------------------------------------------- 1 | # system modes example 2 | --- 3 | 4 | actuation: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | drive_base 9 | left/manipulator 10 | right/manipulator 11 | modes: 12 | __DEFAULT__: 13 | drive_base: active 14 | left/manipulator: inactive 15 | right/manipulator: active.WEAK 16 | MODERATE: 17 | drive_base: active.SLOW 18 | left/manipulator: active.WEAK 19 | right/manipulator: active.WEAK 20 | PERFORMANCE: 21 | drive_base: active.FAST 22 | left/manipulator: active.STRONG 23 | right/manipulsator: active.STRONG 24 | 25 | left/manipulator: 26 | ros__parameters: 27 | type: node 28 | modes: 29 | __DEFAULT__: 30 | ros__parameters: 31 | max_torque: 0.1 32 | WEAK: 33 | ros__parameters: 34 | max_torque: 0.1 35 | STRONG: 36 | ros__parameters: 37 | max_torque: 0.2 38 | 39 | drive_base: 40 | ros__parameters: 41 | type: node 42 | modes: 43 | __DEFAULT__: 44 | ros__parameters: 45 | max_speed: 0.1 46 | controller: PID 47 | SLOW: 48 | ros__parameters: 49 | max_speed: 0.2 50 | controller: PID 51 | FAST: 52 | ros__parameters: 53 | max_speed: 0.9 54 | controller: MPC 55 | 56 | right/manipulator: 57 | ros__parameters: 58 | type: node 59 | modes: 60 | __DEFAULT__: 61 | ros__parameters: 62 | max_torque: 0.11 63 | WEAK: 64 | ros__parameters: 65 | max_torque: 0.05 66 | STRONG: 67 | ros__parameters: 68 | max_torque: 0.3 69 | -------------------------------------------------------------------------------- /system_modes_examples/launch/drive_base.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 - for information on the respective copyright owner 2 | # see the NOTICE file and/or the repository https://github.com/micro-ROS/system_modes. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import ament_index_python.packages 17 | 18 | import launch 19 | import launch.actions 20 | import launch_ros 21 | 22 | 23 | def generate_launch_description(): 24 | default_modelfile = ( 25 | ament_index_python.packages.get_package_share_directory('system_modes_examples') + 26 | '/example_modes.yaml') 27 | return launch.LaunchDescription([ 28 | launch_ros.actions.LifecycleNode( 29 | package='system_modes_examples', 30 | executable='drive_base', 31 | name='drive_base', 32 | namespace='', 33 | parameters=[{'modelfile': default_modelfile}], 34 | output='screen')]) 35 | -------------------------------------------------------------------------------- /system_modes_examples/launch/example_system.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 - for information on the respective copyright owner 2 | # see the NOTICE file and/or the repository https://github.com/micro-ROS/system_modes. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import ament_index_python.packages 17 | 18 | import launch 19 | import launch.actions 20 | import launch.launch_description_sources 21 | import launch.substitutions 22 | 23 | 24 | def generate_launch_description(): 25 | modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') + 26 | '/example_modes.yaml') 27 | 28 | mode_manager = launch.actions.IncludeLaunchDescription( 29 | launch.launch_description_sources.PythonLaunchDescriptionSource( 30 | ament_index_python.packages.get_package_share_directory( 31 | 'system_modes') + '/launch/mode_manager.launch.py'), 32 | launch_arguments={'modelfile': modelfile}.items()) 33 | 34 | drive_base = launch.actions.IncludeLaunchDescription( 35 | launch.launch_description_sources.PythonLaunchDescriptionSource( 36 | ament_index_python.packages.get_package_share_directory( 37 | 'system_modes_examples') + '/launch/drive_base.launch.py'), 38 | launch_arguments={'modelfile': modelfile}.items()) 39 | 40 | manipulator = launch.actions.IncludeLaunchDescription( 41 | launch.launch_description_sources.PythonLaunchDescriptionSource( 42 | ament_index_python.packages.get_package_share_directory( 43 | 'system_modes_examples') + '/launch/manipulator.launch.py'), 44 | launch_arguments={'modelfile': modelfile}.items()) 45 | 46 | description = launch.LaunchDescription() 47 | description.add_action(mode_manager) 48 | description.add_action(drive_base) 49 | description.add_action(manipulator) 50 | 51 | return description 52 | -------------------------------------------------------------------------------- /system_modes_examples/launch/example_system_start_drive_base.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 - for information on the respective copyright owner 2 | # see the NOTICE file and/or the repository https://github.com/micro-ROS/system_modes. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import ament_index_python.packages 17 | 18 | import launch 19 | import launch.actions 20 | import launch.events 21 | 22 | import launch.launch_description_sources 23 | import launch.substitutions 24 | 25 | import launch_ros.actions 26 | import launch_ros.events 27 | import launch_ros.events.lifecycle 28 | 29 | import launch_system_modes.actions 30 | import launch_system_modes.event_handlers 31 | import launch_system_modes.events 32 | 33 | import lifecycle_msgs 34 | 35 | 36 | def generate_launch_description(): 37 | modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') + 38 | '/example_modes.yaml') 39 | 40 | # Setup 41 | mode_manager = launch.actions.IncludeLaunchDescription( 42 | launch.launch_description_sources.PythonLaunchDescriptionSource( 43 | ament_index_python.packages.get_package_share_directory( 44 | 'system_modes') + '/launch/mode_manager.launch.py'), 45 | launch_arguments={'modelfile': modelfile}.items()) 46 | 47 | drive_base = launch_system_modes.actions.Node( 48 | package='system_modes_examples', 49 | executable='drive_base', 50 | name='drive_base', 51 | namespace='', 52 | output='screen') 53 | 54 | manipulator = launch_system_modes.actions.Node( 55 | package='system_modes_examples', 56 | executable='manipulator', 57 | name='manipulator', 58 | namespace='', 59 | output='screen') 60 | 61 | # Startup 62 | drive_base_configure = launch.actions.EmitEvent( 63 | event=launch_ros.events.lifecycle.ChangeState( 64 | lifecycle_node_matcher=launch.events.matchers.matches_action(drive_base), 65 | transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE)) 66 | 67 | drive_base_activate = launch.actions.EmitEvent( 68 | event=launch_ros.events.lifecycle.ChangeState( 69 | lifecycle_node_matcher=launch.events.matchers.matches_action(drive_base), 70 | transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, 71 | )) 72 | 73 | drive_base_change_mode_to_DEFAULT = launch.actions.EmitEvent( 74 | event=launch_system_modes.events.ChangeMode( 75 | system_part_matcher=launch.events.matchers.matches_action(drive_base), 76 | mode_name='__DEFAULT__', 77 | )) 78 | 79 | drive_base_change_mode_to_FAST = launch.actions.EmitEvent( 80 | event=launch_system_modes.events.ChangeMode( 81 | system_part_matcher=launch.events.matchers.matches_action(drive_base), 82 | mode_name='FAST', 83 | )) 84 | 85 | # Handlers 86 | on_inactive_handler = launch.actions.RegisterEventHandler( 87 | launch_ros.event_handlers.OnStateTransition( 88 | target_lifecycle_node=drive_base, 89 | goal_state='inactive', 90 | entities=[drive_base_activate])) 91 | 92 | on_active_handler = launch.actions.RegisterEventHandler( 93 | launch_ros.event_handlers.OnStateTransition( 94 | target_lifecycle_node=drive_base, 95 | goal_state='active', 96 | entities=[drive_base_change_mode_to_DEFAULT])) 97 | 98 | on_DEFAULT_mode = launch.actions.RegisterEventHandler( 99 | launch_system_modes.event_handlers.OnModeChanged( 100 | target_system_part=drive_base, 101 | goal_mode='__DEFAULT__', 102 | entities=[drive_base_change_mode_to_FAST])) 103 | 104 | description = launch.LaunchDescription() 105 | description.add_action(on_inactive_handler) 106 | description.add_action(on_active_handler) 107 | description.add_action(on_DEFAULT_mode) 108 | description.add_action(mode_manager) 109 | description.add_action(drive_base) 110 | description.add_action(manipulator) 111 | description.add_action(drive_base_configure) 112 | 113 | return description 114 | -------------------------------------------------------------------------------- /system_modes_examples/launch/example_system_started.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 - for information on the respective copyright owner 2 | # see the NOTICE file and/or the repository https://github.com/micro-ROS/system_modes. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | import ament_index_python.packages 16 | 17 | import launch 18 | import launch.actions 19 | import launch.events 20 | import launch.launch_description_sources 21 | import launch.substitutions 22 | 23 | import launch_system_modes.actions 24 | import launch_system_modes.event_handlers 25 | import launch_system_modes.events 26 | 27 | import lifecycle_msgs 28 | 29 | 30 | def generate_launch_description(): 31 | modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') + 32 | '/example_modes.yaml') 33 | 34 | # Setup 35 | mode_manager = launch.actions.IncludeLaunchDescription( 36 | launch.launch_description_sources.PythonLaunchDescriptionSource( 37 | ament_index_python.packages.get_package_share_directory( 38 | 'system_modes') + '/launch/mode_manager.launch.py'), 39 | launch_arguments={'modelfile': modelfile}.items()) 40 | 41 | actuation = launch_system_modes.actions.System( 42 | name='actuation', 43 | namespace='') 44 | 45 | drive_base = launch_system_modes.actions.Node( 46 | package='system_modes_examples', 47 | executable='drive_base', 48 | name='drive_base', 49 | namespace='', 50 | output='screen') 51 | 52 | manipulator = launch_system_modes.actions.Node( 53 | package='system_modes_examples', 54 | executable='manipulator', 55 | name='manipulator', 56 | namespace='', 57 | output='screen') 58 | 59 | # Startup 60 | actuation_configure = launch.actions.EmitEvent( 61 | event=launch_system_modes.events.ChangeState( 62 | system_part_matcher=launch.events.matchers.matches_action(actuation), 63 | transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE)) 64 | 65 | actuation_activate = launch.actions.EmitEvent( 66 | event=launch_system_modes.events.ChangeState( 67 | system_part_matcher=launch.events.matchers.matches_action(actuation), 68 | transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, 69 | )) 70 | 71 | actuation_change_mode_to_PERFORMANCE = launch.actions.EmitEvent( 72 | event=launch_system_modes.events.ChangeMode( 73 | system_part_matcher=launch.events.matchers.matches_action(actuation), 74 | mode_name='PERFORMANCE', 75 | )) 76 | 77 | # Handlers 78 | on_inactive_handler = launch.actions.RegisterEventHandler( 79 | launch_system_modes.event_handlers.OnStateTransition( 80 | target_system_part=actuation, 81 | goal_state='inactive', 82 | entities=[actuation_activate])) 83 | 84 | on_DEFAULT_mode = launch.actions.RegisterEventHandler( 85 | launch_system_modes.event_handlers.OnModeChanged( 86 | target_system_part=actuation, 87 | goal_mode='__DEFAULT__', 88 | entities=[actuation_change_mode_to_PERFORMANCE])) 89 | 90 | description = launch.LaunchDescription() 91 | description.add_action(on_inactive_handler) 92 | description.add_action(on_DEFAULT_mode) 93 | description.add_action(mode_manager) 94 | description.add_action(actuation) 95 | description.add_action(drive_base) 96 | description.add_action(manipulator) 97 | description.add_action(actuation_configure) 98 | return description 99 | -------------------------------------------------------------------------------- /system_modes_examples/launch/manipulator.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 - for information on the respective copyright owner 2 | # see the NOTICE file and/or the repository https://github.com/micro-ROS/system_modes. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import ament_index_python.packages 17 | 18 | import launch 19 | import launch.actions 20 | import launch_ros 21 | 22 | 23 | def generate_launch_description(): 24 | default_modelfile = ( 25 | ament_index_python.packages.get_package_share_directory('system_modes_examples') + 26 | '/example_modes.yaml') 27 | return launch.LaunchDescription([ 28 | launch_ros.actions.LifecycleNode( 29 | package='system_modes_examples', 30 | executable='manipulator', 31 | name='manipulator', 32 | namespace='', 33 | parameters=[{'modelfile': default_modelfile}], 34 | output='screen')]) 35 | -------------------------------------------------------------------------------- /system_modes_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | system_modes_examples 5 | 0.9.0 6 | 7 | Example systems and according launch files for the system_modes 8 | package. 9 | 10 | Arne Nordmann 11 | Ralph Lange 12 | Apache License 2.0 13 | 14 | https://micro.ros.org/docs/concepts/client_library/lifecycle_and_system_modes/ 15 | https://github.com/micro-ROS/system_modes 16 | https://github.com/micro-ROS/system_modes/issues 17 | 18 | ament_cmake 19 | 20 | rclcpp 21 | rclcpp_lifecycle 22 | system_modes_msgs 23 | system_modes 24 | 25 | launch 26 | ros2launch 27 | launch_system_modes 28 | 29 | ament_cmake_gtest 30 | ament_cmake_gmock 31 | ament_cmake_pep257 32 | ament_cmake_flake8 33 | ament_cmake_cpplint 34 | ament_cmake_cppcheck 35 | ament_cmake_uncrustify 36 | ament_lint_auto 37 | 38 | 39 | ament_cmake 40 | 41 | 42 | -------------------------------------------------------------------------------- /system_modes_examples/src/drive_base.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | using lifecycle_msgs::msg::State; 25 | using lifecycle_msgs::msg::Transition; 26 | using rclcpp_lifecycle::LifecycleNode; 27 | 28 | using namespace std::chrono_literals; 29 | 30 | namespace system_modes 31 | { 32 | namespace examples 33 | { 34 | 35 | // DriveBase node with two non-default modes: weak and strong 36 | class DriveBase : public LifecycleNode 37 | { 38 | public: 39 | DriveBase() 40 | : LifecycleNode("drive_base") 41 | { 42 | RCLCPP_INFO(get_logger(), "Constructed lifecycle node '%s'", this->get_name()); 43 | 44 | // Parameter declaration 45 | this->declare_parameter("max_speed", 0.0); 46 | this->declare_parameter("controller", "none"); 47 | 48 | auto param_change_callback = 49 | [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult 50 | { 51 | auto result = rcl_interfaces::msg::SetParametersResult(); 52 | result.successful = true; 53 | for (auto parameter : parameters) { 54 | RCLCPP_INFO( 55 | this->get_logger(), 56 | "parameter '%s' is now: %s", 57 | parameter.get_name().c_str(), 58 | parameter.value_to_string().c_str()); 59 | } 60 | return result; 61 | }; 62 | 63 | param_change_callback_handle_ = this->add_on_set_parameters_callback(param_change_callback); 64 | } 65 | 66 | DriveBase(const DriveBase &) = delete; 67 | ~DriveBase() = default; 68 | 69 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 70 | on_configure(const rclcpp_lifecycle::State &) 71 | { 72 | RCLCPP_INFO(get_logger(), "%s on_configure()", this->get_name()); 73 | 74 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 75 | } 76 | 77 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 78 | on_activate(const rclcpp_lifecycle::State &) 79 | { 80 | RCLCPP_INFO(get_logger(), "%s on_activate()", this->get_name()); 81 | 82 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 83 | } 84 | 85 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 86 | on_deactivate(const rclcpp_lifecycle::State &) 87 | { 88 | RCLCPP_INFO(get_logger(), "%s on_deactivate()", this->get_name()); 89 | 90 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 91 | } 92 | 93 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 94 | on_cleanup(const rclcpp_lifecycle::State &) 95 | { 96 | RCLCPP_INFO(get_logger(), "%s on_cleanup()", this->get_name()); 97 | 98 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 99 | } 100 | 101 | private: 102 | rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_change_callback_handle_; 103 | }; 104 | 105 | } // namespace examples 106 | } // namespace system_modes 107 | 108 | int main(int argc, char * argv[]) 109 | { 110 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 111 | 112 | rclcpp::init(argc, argv); 113 | rclcpp::executors::SingleThreadedExecutor exe; 114 | 115 | auto driver = std::make_shared(); 116 | exe.add_node(driver->get_node_base_interface()); 117 | exe.spin(); 118 | 119 | rclcpp::shutdown(); 120 | 121 | return 0; 122 | } 123 | -------------------------------------------------------------------------------- /system_modes_examples/src/manipulator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 - for information on the respective copyright owner 2 | // see the NOTICE file and/or the repository https://github.com/microros/system_modes 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | using lifecycle_msgs::msg::State; 25 | using lifecycle_msgs::msg::Transition; 26 | using rclcpp_lifecycle::LifecycleNode; 27 | 28 | using namespace std::chrono_literals; 29 | 30 | namespace system_modes 31 | { 32 | namespace examples 33 | { 34 | 35 | // Manipulator node with two non-default modes: weak and strong 36 | class Manipulator : public LifecycleNode 37 | { 38 | public: 39 | Manipulator() 40 | : LifecycleNode("manipulator") 41 | { 42 | RCLCPP_INFO(get_logger(), "Constructed lifecycle node '%s'", this->get_name()); 43 | 44 | // Parameter declaration 45 | this->declare_parameter("max_torque", 0.0); 46 | 47 | auto param_change_callback = 48 | [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult 49 | { 50 | auto result = rcl_interfaces::msg::SetParametersResult(); 51 | result.successful = true; 52 | for (auto parameter : parameters) { 53 | RCLCPP_INFO( 54 | this->get_logger(), 55 | "parameter '%s' is now: %s", 56 | parameter.get_name().c_str(), 57 | parameter.value_to_string().c_str()); 58 | } 59 | return result; 60 | }; 61 | 62 | param_change_callback_handle_ = this->add_on_set_parameters_callback(param_change_callback); 63 | } 64 | 65 | Manipulator(const Manipulator &) = delete; 66 | ~Manipulator() = default; 67 | 68 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 69 | on_configure(const rclcpp_lifecycle::State &) 70 | { 71 | RCLCPP_INFO(get_logger(), "%s on_configure()", this->get_name()); 72 | 73 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 74 | } 75 | 76 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 77 | on_activate(const rclcpp_lifecycle::State &) 78 | { 79 | RCLCPP_INFO(get_logger(), "%s on_activate()", this->get_name()); 80 | 81 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 82 | } 83 | 84 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 85 | on_deactivate(const rclcpp_lifecycle::State &) 86 | { 87 | RCLCPP_INFO(get_logger(), "%s on_deactivate()", this->get_name()); 88 | 89 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 90 | } 91 | 92 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 93 | on_cleanup(const rclcpp_lifecycle::State &) 94 | { 95 | RCLCPP_INFO(get_logger(), "%s on_cleanup()", this->get_name()); 96 | 97 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 98 | } 99 | 100 | private: 101 | rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_change_callback_handle_; 102 | }; 103 | 104 | } // namespace examples 105 | } // namespace system_modes 106 | 107 | int main(int argc, char * argv[]) 108 | { 109 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 110 | 111 | rclcpp::init(argc, argv); 112 | rclcpp::executors::SingleThreadedExecutor exe; 113 | 114 | auto manipulator = std::make_shared(); 115 | exe.add_node(manipulator->get_node_base_interface()); 116 | exe.spin(); 117 | 118 | rclcpp::shutdown(); 119 | 120 | return 0; 121 | } 122 | -------------------------------------------------------------------------------- /system_modes_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package system_modes_examples 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.0 (2020-07-21) 6 | ------------------ 7 | 8 | * More flexibility in specifying the default mode, any mode can be now default mode 9 | https://github.com/micro-ROS/system_modes/issues/69 10 | 11 | 0.8.0 (2020-04-22) 12 | ------------------ 13 | 14 | * Launch integration, i.e. launch actions, events, and event handlers for system modes 15 | 16 | 0.7.1 (2020-04-22) 17 | ------------------ 18 | 19 | * Improved metadata for ROS 2 package releases 20 | 21 | 0.7.0 (2020-04-22) 22 | ------------------ 23 | 24 | * Launch tests now using launch_ros node action https://github.com/micro-ROS/system_modes/pull/72 25 | * Introduced separate interface package, system_modes_msgs https://github.com/micro-ROS/system_modes/pull/74 26 | -------------------------------------------------------------------------------- /system_modes_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(system_modes_msgs) 4 | 5 | # Default to C++14 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 14) 8 | endif() 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rosidl_default_generators REQUIRED) 15 | 16 | # generate service 17 | rosidl_generate_interfaces(${PROJECT_NAME} 18 | "msg/Mode.msg" 19 | "msg/ModeEvent.msg" 20 | "srv/ChangeMode.srv" 21 | "srv/GetMode.srv" 22 | "srv/GetAvailableModes.srv" 23 | ADD_LINTER_TESTS 24 | ) 25 | 26 | if(BUILD_TESTING) 27 | find_package(ament_lint_auto REQUIRED) 28 | ament_lint_auto_find_test_dependencies() 29 | endif() 30 | 31 | ament_export_dependencies(rosidl_default_runtime) 32 | 33 | ament_package() 34 | -------------------------------------------------------------------------------- /system_modes_msgs/README.md: -------------------------------------------------------------------------------- 1 | # ROS 2 System Modes Messages 2 | 3 | This package provides the message types and services for the System Modes Library. 4 | 5 | ## Message Types 6 | 7 | * [msg/Mode.msg](./msg/Mode.msg) - Mode definition, along the lines of lifecycle_msgs [State](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/msg/State.msg), identifying a mode by a string. 8 | * [msg/ModeEvent.msg](./msg/Mode.msg) - Notifies about the transition of a system mode, along the lines of lifecycle_msgs [TransitionEvent](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/msg/TransitionEvent.msg). 9 | 10 | ## Services 11 | 12 | 1. [srv/GetMode.srv](./srv/GetMode.srv) - Requests the current mode of a (sub-)system or node, along the lines of lifecycle_msgs [GetState](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/srv/GetState.srv) 13 | 2. [srv/GetAvailableModes.srv](./srv/GetAvailableModes.srv) - Requests all available modes of a (sub-)system or node, along the lines of lifecycle_msgs [GetAvailableStates](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/srv/GetAvailableStates.srv) 14 | 3. [srv/ChangeMode.srv](./srv/ChangeMode.srv) - Requests a change to a certain mode, along the lines of lifecycle_msgs [ChangeState](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/srv/ChangeState.srv) 15 | -------------------------------------------------------------------------------- /system_modes_msgs/msg/Mode.msg: -------------------------------------------------------------------------------- 1 | string label -------------------------------------------------------------------------------- /system_modes_msgs/msg/ModeEvent.msg: -------------------------------------------------------------------------------- 1 | # The time point at which this event occurred. 2 | uint64 timestamp 3 | 4 | # The starting mode from which this event transitioned. 5 | Mode start_mode 6 | 7 | # The end mode of this transition event. 8 | Mode goal_mode 9 | -------------------------------------------------------------------------------- /system_modes_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | system_modes_msgs 5 | 0.9.0 6 | 7 | Interface package, containing message definitions and service definitions 8 | for the system modes package. 9 | 10 | Arne Nordmann 11 | Ralph Lange 12 | Apache License 2.0 13 | 14 | https://micro.ros.org/docs/concepts/client_library/lifecycle_and_system_modes/ 15 | https://github.com/micro-ROS/system_modes 16 | https://github.com/micro-ROS/system_modes/issues 17 | 18 | ament_cmake 19 | rosidl_default_generators 20 | 21 | rosidl_default_runtime 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | ament_cmake_cppcheck 26 | ament_cmake_cpplint 27 | 28 | rosidl_interface_packages 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /system_modes_msgs/srv/ChangeMode.srv: -------------------------------------------------------------------------------- 1 | string mode_name 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /system_modes_msgs/srv/GetAvailableModes.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string[] available_modes 3 | -------------------------------------------------------------------------------- /system_modes_msgs/srv/GetMode.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string current_mode 3 | -------------------------------------------------------------------------------- /test_launch_system_modes/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package system_modes_examples 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.0 (2020-07-21) 6 | ------------------ 7 | 8 | * More flexibility in specifying the default mode, any mode can be now default mode 9 | https://github.com/micro-ROS/system_modes/issues/69 10 | 11 | 0.8.0 (2020-04-22) 12 | ------------------ 13 | 14 | * Launch integration, i.e. launch actions, events, and event handlers for system modes 15 | -------------------------------------------------------------------------------- /test_launch_system_modes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(test_launch_system_modes) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | 7 | if(BUILD_TESTING) 8 | find_package(ament_lint_auto REQUIRED) 9 | # the following line skips the linter which checks for copyrights 10 | # remove the line when a copyright and license is present in all source files 11 | set(ament_cmake_copyright_FOUND TRUE) 12 | ament_lint_auto_find_test_dependencies() 13 | 14 | find_package(launch_testing_ament_cmake REQUIRED) 15 | file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/test_modes.yaml" MODELFILE) 16 | file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/expected_output" EXPECTED_OUTPUT) 17 | 18 | # Test launch actions, events, and event handlers with a node 19 | configure_file( 20 | "test/node_test.launch.py.in" 21 | "test/node_test.launch.py" 22 | @ONLY 23 | ) 24 | add_launch_test( 25 | "${CMAKE_CURRENT_BINARY_DIR}/test/node_test.launch.py" 26 | TARGET "node_test" 27 | TIMEOUT 30 28 | ENV) 29 | 30 | # Test launch actions, events, and event handlers with a system 31 | configure_file( 32 | "test/system_test.launch.py.in" 33 | "test/system_test.launch.py" 34 | @ONLY 35 | ) 36 | add_launch_test( 37 | "${CMAKE_CURRENT_BINARY_DIR}/test/system_test.launch.py" 38 | TARGET "system_test" 39 | TIMEOUT 30 40 | ENV) 41 | endif() 42 | 43 | ament_export_include_directories(include) 44 | ament_export_libraries(mode) 45 | ament_export_dependencies(rclcpp) 46 | ament_export_dependencies(rclcpp_lifecycle) 47 | ament_export_dependencies(lifecycle_msgs) 48 | ament_export_dependencies(system_modes_msgs) 49 | ament_package() 50 | -------------------------------------------------------------------------------- /test_launch_system_modes/README.md: -------------------------------------------------------------------------------- 1 | # ROS 2 System Modes 2 | 3 | The system modes concept assumes that a robotics system is built from components with a lifecycle. It adds a notion of (sub-)systems, hiararchically grouping these nodes, as well as a notion of *modes* that determine the configuration of these nodes and (sub-)systems in terms of their parameter values. 4 | 5 | General information about this repository, including legal information, project context, build instructions and known issues/limitations, are given in [README.md](../README.md) in the repository root. 6 | 7 | ## Test Launch System Modes Package 8 | 9 | The test_launch_system_modes package comprises launch tests for the launch_system_modes project, 10 | i.e. launch actions, events, and event handlers for system modes. 11 | -------------------------------------------------------------------------------- /test_launch_system_modes/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | test_launch_system_modes 5 | 0.9.0 6 | 7 | Launch tests for the launch_system_modes package, i.e. launch actions, events, and event 8 | handlers for system modes. 9 | 10 | Arne Nordmann 11 | Ralph Lange 12 | Apache License 2.0 13 | 14 | https://micro.ros.org/docs/concepts/client_library/lifecycle_and_system_modes/ 15 | https://github.com/micro-ROS/system_modes 16 | https://github.com/micro-ROS/system_modes/issues 17 | 18 | ament_cmake 19 | 20 | builtin_interfaces 21 | launch_system_modes 22 | lifecycle_msgs 23 | rclcpp 24 | rclcpp_lifecycle 25 | system_modes 26 | system_modes_msgs 27 | system_modes_examples 28 | 29 | ament_cmake_pep257 30 | ament_cmake_flake8 31 | ament_index_python 32 | ament_lint_auto 33 | launch_testing_ament_cmake 34 | launch_testing_ros 35 | 36 | 37 | ament_cmake 38 | 39 | 40 | -------------------------------------------------------------------------------- /test_launch_system_modes/test/expected_output.regex: -------------------------------------------------------------------------------- 1 | drive_base.*on_configure 2 | parameter.*controller.*now.*PID 3 | parameter.*controller.*now.*MPC 4 | -------------------------------------------------------------------------------- /test_launch_system_modes/test/node_test.launch.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import unittest 4 | 5 | import ament_index_python 6 | import launch 7 | import launch_ros 8 | import launch_testing 9 | import launch_testing.actions 10 | import launch_testing.asserts 11 | import launch_testing.util 12 | import launch_testing_ros 13 | 14 | from launch import LaunchDescription 15 | from launch.actions import ExecuteProcess 16 | from launch.events import matches_action 17 | from launch.events.process import ShutdownProcess 18 | 19 | import launch_system_modes 20 | 21 | import lifecycle_msgs.msg 22 | 23 | 24 | def generate_test_description(): 25 | os.environ['OSPL_VERBOSITY'] = '8' 26 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' 27 | 28 | modelfile = '@MODELFILE@' 29 | 30 | # Setup 31 | mode_manager = launch_ros.actions.Node( 32 | package='system_modes', 33 | executable='mode_manager', 34 | emulate_tty=True, 35 | output='screen', 36 | parameters=[ 37 | {"modelfile": modelfile} 38 | ]) 39 | 40 | drive_base = launch_system_modes.actions.Node( 41 | package='system_modes_examples', 42 | executable='drive_base', 43 | name='drive_base', 44 | namespace='', 45 | output='screen') 46 | 47 | # Startup 48 | drive_base_configure = launch.actions.TimerAction( 49 | period=2., 50 | actions=[ 51 | launch.actions.EmitEvent( 52 | event=launch_ros.events.lifecycle.ChangeState( 53 | lifecycle_node_matcher=launch.events.matchers.matches_action(drive_base), 54 | transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE)) 55 | ] 56 | ) 57 | 58 | drive_base_activate = launch.actions.EmitEvent( 59 | event=launch_ros.events.lifecycle.ChangeState( 60 | lifecycle_node_matcher=launch.events.matchers.matches_action(drive_base), 61 | transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, 62 | )) 63 | 64 | drive_base_change_mode_to_DEFAULT = launch.actions.EmitEvent( 65 | event=launch_system_modes.events.ChangeMode( 66 | system_part_matcher=launch.events.matchers.matches_action(drive_base), 67 | mode_name='__DEFAULT__', 68 | )) 69 | 70 | drive_base_change_mode_to_FAST = launch.actions.EmitEvent( 71 | event=launch_system_modes.events.ChangeMode( 72 | system_part_matcher=launch.events.matchers.matches_action(drive_base), 73 | mode_name='FAST', 74 | )) 75 | 76 | # Handlers 77 | on_inactive_handler = launch.actions.RegisterEventHandler( 78 | launch_ros.event_handlers.OnStateTransition( 79 | target_lifecycle_node=drive_base, 80 | goal_state='inactive', 81 | entities=[drive_base_activate])) 82 | 83 | on_active_handler = launch.actions.RegisterEventHandler( 84 | launch_ros.event_handlers.OnStateTransition( 85 | target_lifecycle_node=drive_base, 86 | goal_state='active', 87 | entities=[drive_base_change_mode_to_DEFAULT])) 88 | 89 | on_DEFAULT_mode = launch.actions.RegisterEventHandler( 90 | launch_system_modes.event_handlers.OnModeChanged( 91 | target_system_part=drive_base, 92 | goal_mode='__DEFAULT__', 93 | entities=[drive_base_change_mode_to_FAST])) 94 | 95 | launch_description = LaunchDescription() 96 | launch_description.add_action(mode_manager) 97 | launch_description.add_action(drive_base) 98 | launch_description.add_action(on_inactive_handler) 99 | launch_description.add_action(on_active_handler) 100 | launch_description.add_action(on_DEFAULT_mode) 101 | launch_description.add_action(launch_testing.util.KeepAliveProc()) 102 | launch_description.add_action(launch_testing.actions.ReadyToTest()) 103 | launch_description.add_action(drive_base_configure) 104 | 105 | return launch_description, locals() 106 | 107 | class TestModeManagement(unittest.TestCase): 108 | 109 | def test_processes_output(self, proc_output, drive_base): 110 | """Check manager and nodes logging output for expected strings.""" 111 | 112 | from launch_testing.tools.output import get_default_filtered_prefixes 113 | output_filter = launch_testing_ros.tools.basic_output_filter( 114 | filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], 115 | filtered_rmw_implementation='@rmw_implementation@' 116 | ) 117 | proc_output.assertWaitFor( 118 | expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), 119 | process=drive_base, 120 | output_filter=output_filter, 121 | timeout=15 122 | ) 123 | 124 | @launch_testing.post_shutdown_test() 125 | class TestModeManagementShutdown(unittest.TestCase): 126 | 127 | def test_last_process_exit_code(self, proc_info, drive_base): 128 | launch_testing.asserts.assertExitCodes(proc_info, process=drive_base) 129 | -------------------------------------------------------------------------------- /test_launch_system_modes/test/test_modes.yaml: -------------------------------------------------------------------------------- 1 | # system modes example 2 | --- 3 | 4 | actuation: 5 | ros__parameters: 6 | type: system 7 | parts: 8 | manipulator 9 | drive_base 10 | modes: 11 | __DEFAULT__: 12 | manipulator: inactive 13 | drive_base: active 14 | MODERATE: 15 | manipulator: active.WEAK 16 | drive_base: active.SLOW 17 | PERFORMANCE: 18 | manipulator: active.STRONG 19 | drive_base: active.FAST 20 | 21 | manipulator: 22 | ros__parameters: 23 | type: node 24 | modes: 25 | __DEFAULT__: 26 | ros__parameters: 27 | max_torque: 0.1 28 | WEAK: 29 | ros__parameters: 30 | max_torque: 0.1 31 | STRONG: 32 | ros__parameters: 33 | max_torque: 0.2 34 | 35 | drive_base: 36 | ros__parameters: 37 | type: node 38 | modes: 39 | __DEFAULT__: 40 | ros__parameters: 41 | max_speed: 0.1 42 | controller: PID 43 | SLOW: 44 | ros__parameters: 45 | max_speed: 0.2 46 | controller: PID 47 | FAST: 48 | ros__parameters: 49 | max_speed: 0.9 50 | controller: MPC 51 | --------------------------------------------------------------------------------