micro-ROS · norro · Jul 24, 2020 · Aug 3, 2020 · Aug 3, 2020 · Aug 3, 2020
diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
@@ -0,0 +1,33 @@
+name: "Build action: Foxy + Rolling"
+
+on:
+  push:
+    branches: [ master, dev ]
+  pull_request:
+    branches: [ master, dev ]
+  schedule:
+    # Run once per day to detect broken dependencies.
+    - cron: '17 6 * * *'
+
+jobs:
+  build:
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      matrix:
+        os: [ ubuntu-20.04, macos-latest, windows-latest ]
+        ros_distribution: [ foxy, rolling ]
+    steps:
+    - uses: actions/checkout@v2
+    - uses: ros-tooling/setup-ros@0.1.2
+      with:
+        required-ros-distributions: ${{ matrix.ros_distribution }}
+    - uses : ros-tooling/action-ros-ci@0.1.1
+      with:
+        package-name: "system_modes system_modes_examples"
+        target-ros2-distro: ${{ matrix.ros_distribution }}
+        colcon-mixin-name: coverage-gcc
+        colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
+    - uses: codecov/codecov-action@v1.2.1
+      with:
+        file: ros_ws/lcov/total_coverage.info
diff --git a/.github/workflows/lint.yaml b/.github/workflows/lint.yaml
diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml
diff --git a/NOTICE b/NOTICE
@@ -20,5 +20,13 @@
 #
 # Please keep the list sorted.
 
+Open Robotics
+    Jacob Perron <jacob@openrobotics.org>
+    Scott Logan <logans@cottsay.net>
+
 Robert Bosch GmbH
     Arne Nordmann <arne.nordmann@de.bosch.com>
+    Ralph Lange <ralph.lange@de.bosch.com>
+
+BSH Hausgeräte GmbH
+    Thilak Raj <chikmagalore.thilak@bshg.com>
diff --git a/README.md b/README.md
@@ -1,5 +1,13 @@
 # ROS 2 System Modes
 
+[![License](https://img.shields.io/badge/License-Apache%202-blue.svg)](https://github.com/micro-ROS/system_modes/blob/master/LICENSE)
+[![Build status](https://build.ros2.org/job/Ddev__system_modes__ubuntu_bionic_amd64/badge/icon?subject=Build%20farm%3A%20Dashing)](https://build.ros2.org/job/Ddev__system_modes__ubuntu_bionic_amd64/)
+[![Build status](http://build.ros2.org/job/Edev__system_modes__ubuntu_bionic_amd64/badge/icon?subject=Build%20farm%3A%20Eloquent)](http://build.ros2.org/job/Edev__system_modes__ubuntu_bionic_amd64/)
+[![Build status](http://build.ros2.org/job/Fdev__system_modes__ubuntu_focal_amd64/badge/icon?subject=Build%20farm%3A%20Foxy)](http://build.ros2.org/job/Fdev__system_modes__ubuntu_focal_amd64/)
+[![Build status](http://build.ros2.org/job/Rdev__system_modes__ubuntu_focal_amd64/badge/icon?subject=Build%20farm%3A%20Rolling)](http://build.ros2.org/job/Rdev__system_modes__ubuntu_focal_amd64/)
+[![Build status](https://github.com/micro-ROS/system_modes/workflows/Build%20action%3A%20Foxy%20%2B%20Rolling/badge.svg)](https://github.com/micro-ROS/system_modes/actions)
+[![Code coverage](https://codecov.io/gh/micro-ROS/system_modes/branch/master/graph/badge.svg)](https://codecov.io/gh/micro-ROS/system_modes)
+
 This repository explores a system modes concept that is implemented for ROS 2 in two packages:
 * [system_modes](./system_modes/) provides a library for system mode inference, a mode manager, and a mode monitor
 * [system_modes_examples](./system_modes_examples/) implements a simple example
@@ -17,7 +25,7 @@ standards, e.g., ISO 26262.
 
 ## How to Build, Test, Install, and Use
 
-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:  
+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:
 $ `colcon build --packages-select-regex system_modes`
 
 Have a look at the [system_modes_examples](./system_modes_examples/) documentation to try your installation.
@@ -32,11 +40,18 @@ ROS 2 System Modes are open-sourced under the Apache-2.0 license. See the
 For a list of other open-source components included in ROS 2 system_modes,
 see the file [3rd-party-licenses.txt](3rd-party-licenses.txt).
 
+## Quality assurance
+
+The colcon_test tool is used for quality assurances, which includes cpplint, uncrustify, flake8, xmllint and various other tools.
+
+Unit tests based on [gtest](https://github.com/google/googletest) are located in the [./system_modes/test](system_modes/test) folder.
+
 ## Known Issues/Limitations
 
 Please notice the following issues/limitations:
 
 * 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.
+* 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.
 
 ## Acknowledgments
 

diff --git a/system_modes/CMakeLists.txt b/system_modes/CMakeLists.txt
@@ -40,6 +40,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
 add_library(mode SHARED
   src/system_modes/mode.cpp
   src/system_modes/mode_impl.cpp
+  src/system_modes/mode_handling.cpp
   src/system_modes/mode_inference.cpp)
 target_include_directories(mode PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -60,7 +61,7 @@ include_directories(../../build/system_modes/rosidl_generator_cpp/)
 link_directories(../../build/system_modes/)
 target_link_libraries(mode system_modes__rosidl_typesupport_cpp)
 
-  # Causes the visibility macros to use dllexport rather than dllimport,
+# Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(mode PRIVATE "SYSTEMMODES_BUILDING_LIBRARY")
 
@@ -105,9 +106,6 @@ if(BUILD_TESTING)
   # the following line skips the linter which checks for copyrights
   # remove the line when a copyright and license is present in all source files
   set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # remove the line when this package is a git repo
-  set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 
   set(MODE_FILE_CORRECT ${CMAKE_CURRENT_SOURCE_DIR}/test/test_modes.yaml)

diff --git a/system_modes/README.md b/system_modes/README.md
@@ -1,11 +1,11 @@
-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.
-
 # ROS 2 System Modes
 
 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.
 
 A list of (current and future) requirements for system modes can be found here: [requirements](./doc/requirements.md#system-runtime-configuration).
 
+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.
+
 ## System Modes Package
 
 The system modes concept is implemented as a package for ROS 2. This package provides a library for system mode inference, a mode manager, and a mode monitor.
@@ -105,7 +105,7 @@ The mode manager is a ROS node that accepts an SHM file (see [above](#system-mod
   * `/{system_or_node}/transition_request_info` - lifecycle_msgs/TransitionEvent
   * `/{system_or_node}/mode_request_info` - [system_modes/ModeEvent](./msg/ModeEvent.msg)
 
-Running the manager:  
+Running the manager:
 $ `ros2 launch system_modes mode_manager.launch.py modelfile:=[path/to/modelfile.yaml]`
 
 ### Mode Monitor
@@ -118,9 +118,23 @@ The mode monitor is a ROS node that accepts an SHM file (see [above](#system-mod
 
 ![mode_monitor](../system_modes_examples/doc/screenshot-monitor.png "Screenshot of the mode monitor from system_modes_examples")
 
-Running the monitor:  
+Running the monitor:
 $ `ros2 launch system_modes mode_monitor.launch.py modelfile:=[path/to/modelfile.yaml]`
 
+### Error Handling and Rules
+
+If the _actual_ state/mode of the system or any of its parts diverges from the _target_ state/mode, we define rules that try to bring the system back to a valid _target_ state/mode, e.g., a degraded mode. Rules work in a bottom-up manner, i.e. starting from correcting nodes before sub-systems before systems. Rules are basically defined in the following way:
+
+```pseudo
+if:
+ system.target == {target state/mode} && system.actual != {target state/mode} && part.actual == {specific state/mode}
+then:
+ system.target := {specific state/mode}
+```
+
+if _actual_ state/mode and _target_ state/mode diverge, but there is no rule for this exact situation, the bottom-up rules will just try to return the system/part to its _target_ state/mode.
+*Potentiall dangereous, to be discussed:* what's happening, if the system is already on its way. E.g., a system or part was just commanded to transition to _ACTIVE.foo_, but is currently _activating_ (so doing everything right). In this case we have to avoid that the bottom-up rules will trigger.
+
 ## How to Apply
 
 When designing the hierarchy of your system, try to group parts semantically, e.g., everything that belongs to *perception* or *navigation*. You want to group those parts of a system that are often jointly managed (initialized, shutdown, configured). Hierarchies don't necessarily need to be designed in one big tree, but can form several parallel trees.

diff --git a/system_modes/changelog.rst b/system_modes/changelog.rst
@@ -1,6 +1,28 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package system_modes
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package system_modes_examples
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.4.2 (2020-12-17)
+-----------
+* Error handling and rules feature no longer experimental
+* Fixed bugs in monitor and tests
+
+0.4.1 (2020-10-29)
+-----------
+* Include experimental error handling and rules feature
+* https://github.com/micro-ROS/system_modes/issues/13
+* CI for ubuntu 20.04 ROS 2 rolling
+
+0.4.0 (2020-09-30)
+-----------
+* publish inferred state and mode transitions
+* https://github.com/micro-ROS/system_modes/issues/42
+
+0.4.0 (2020-09-30)
+-----------
+* mode event now including start and goal mode
+* publish inferred state and mode transitions
+* https://github.com/micro-ROS/system_modes/issues/42
 
 0.4.0 (2020-09-30)
 -----------

diff --git a/system_modes/include/system_modes/mode.hpp b/system_modes/include/system_modes/mode.hpp
@@ -16,6 +16,7 @@
 
 #include "system_modes/mode_impl.hpp"
 
+#include <rclcpp/macros.hpp>
 #include <rclcpp/node_interfaces/node_parameters.hpp>
 #include <rclcpp/parameter_map.hpp>
 
@@ -46,6 +47,7 @@ class ModeBase
 public:
   explicit ModeBase(const std::string & mode_name);
   virtual ~ModeBase() = default;
+  // cppcheck-suppress unknownMacro
   RCLCPP_DISABLE_COPY(ModeBase)
 
   std::string get_name() const;
@@ -74,6 +76,7 @@ class DefaultMode : public ModeBase
 public:
   DefaultMode();
   explicit DefaultMode(const std::string & mode_name) = delete;
+  // cppcheck-suppress unknownMacro
   RCLCPP_DISABLE_COPY(DefaultMode)
 
   virtual void set_parameter(const rclcpp::Parameter & parameter);
@@ -89,6 +92,7 @@ class Mode : public ModeBase
 public:
   explicit Mode(const std::string & mode_name) = delete;
   Mode(const std::string & mode_name, const DefaultModePtr default_mode);
+  // cppcheck-suppress unknownMacro
   RCLCPP_DISABLE_COPY(Mode)
 
   virtual ~Mode() = default;

diff --git a/system_modes/include/system_modes/mode_handling.hpp b/system_modes/include/system_modes/mode_handling.hpp
@@ -0,0 +1,75 @@
+// Copyright (c) 2018 - for information on the respective copyright owner
+// see the NOTICE file and/or the repository https://github.com/microros/system_modes
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#pragma once
+
+#include <stdint.h>
+#include <shared_mutex>
+#include <rclcpp/parameter.hpp>
+#include <rclcpp/parameter_map.hpp>
+
+#include <map>
+#include <mutex>
+#include <string>
+#include <memory>
+#include <vector>
+#include <utility>
+
+#include "system_modes/mode.hpp"
+#include "system_modes/mode_impl.hpp"
+
+namespace system_modes
+{
+
+struct ModeRule
+{
+  std::string name;
+
+  std::string system;
+  StateAndMode system_target;
+
+  std::string part;
+  StateAndMode part_actual;
+
+  StateAndMode new_system_target;
+};
+
+using RulesMap = std::map<std::string, ModeRule>;
+
+class ModeHandling
+{
+public:
+  explicit ModeHandling(const std::string & model_path);
+  // cppcheck-suppress unknownMacro
+  RCLCPP_DISABLE_COPY(ModeHandling)
+
+  virtual ~ModeHandling() = default;
+  virtual const std::vector<ModeRule> get_rules_for(
+    const std::string & system,
+    const StateAndMode & target);
+
+protected:
+  mutable std::shared_timed_mutex rules_mutex_;
+
+private:
+  std::map<std::string, RulesMap> rules_;
+
+  virtual void read_rules_from_model(const std::string & model_path);
+  virtual void add_rule(
+    const std::string & part,
+    const std::string & rule_name,
+    const rclcpp::Parameter & rule_param);
+};
+
+}  // namespace system_modes