dev -> master (#52)

* Introduce state/mode deviation
  * Mode inference now capable of detecting deviation between intended and targeted system state/mode (needs minor fixes)
  * Mode manager checking for system deviation after each change (transition, mode change, parameter change)
* Comparison operator for StateAndMode
* Prototypical error handling and rules
  * Prototypical rules in example modes file
  * Adds ModeHandling classfor rules parsing
  * In case of deviations, mode manager finds the correct rules
  * First exemplary set of mode rules working
* Documentation of rules and the rules example
* Version bump to 0.4.1

#13

Signed-off-by: Arne Nordmann <arne.nordmann@de.bosch.com>

Author: norro

.github/workflows/lint.yaml

@@ -8,7 +8,7 @@ jobs:
     runs-on: ubuntu-18.04
     steps:
     - uses: actions/checkout@v2
-    - uses: ros-tooling/setup-ros@0.0.25
+    - uses: ros-tooling/setup-ros@0.0.26
     - uses: ros-tooling/action-ros2-lint@0.0.6
       with:
         linter: xmllint
@@ -23,7 +23,7 @@ jobs:
           linter: [cppcheck, cpplint, uncrustify]
     steps:
     - uses: actions/checkout@v2
-    - uses: ros-tooling/setup-ros@0.0.25
+    - uses: ros-tooling/setup-ros@0.0.26
     - uses: ros-tooling/action-ros2-lint@0.0.6
       with:
         linter: ${{ matrix.linter }}
@@ -38,7 +38,7 @@ jobs:
         linter: [flake8, pep257]
     steps:
     - uses: actions/checkout@v2
-    - uses: ros-tooling/setup-ros@0.0.25
+    - uses: ros-tooling/setup-ros@0.0.26
     - uses: ros-tooling/action-ros2-lint@0.0.6
       with:
         linter: ${{ matrix.linter }}

.github/workflows/test.yaml

@@ -13,8 +13,8 @@ jobs:
       matrix:
         os: [ubuntu-18.04]
     steps:
-      - uses: ros-tooling/setup-ros@0.0.25
-      - uses: ros-tooling/action-ros-ci@0.0.15
+      - uses: ros-tooling/setup-ros@0.0.26
+      - uses: ros-tooling/action-ros-ci@0.0.19
         with:
           package-name: system_modes system_modes_examples
           target-ros2-distro: foxy

README.md

@@ -17,7 +17,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.
@@ -37,6 +37,7 @@ see the file [3rd-party-licenses.txt](3rd-party-licenses.txt).
 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
 

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>

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,25 @@ 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 (Experimental)
+
+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.
+
+*Note:* This 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.
+
 ## 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.

system_modes/changelog.rst

@@ -1,6 +1,17 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package system_modes
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package system_modes_examples
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+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)
 -----------

system_modes/include/system_modes/mode_handling.hpp

@@ -0,0 +1,74 @@
+// 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);
+  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

system_modes/include/system_modes/mode_inference.hpp

@@ -27,6 +27,7 @@
 #include <utility>
 
 #include "system_modes/mode.hpp"
+#include "system_modes/mode_handling.hpp"
 
 namespace system_modes
 {
@@ -69,6 +70,7 @@ class ModeInference
    * mode transitions.
    */
   virtual Deviation infer_transitions();
+  virtual Deviation get_deviation();
 
   virtual StateAndMode get_target(const std::string & part) const;
   virtual ModeConstPtr get_mode(const std::string & part, const std::string & mode) const;
@@ -82,9 +84,10 @@ class ModeInference
   virtual void add_param_to_mode(ModeBasePtr, const rclcpp::Parameter &);
 
 private:
+  ModeHandling * mode_handling_;
+
   StatesMap nodes_, nodes_target_, nodes_cache_;
   StatesMap systems_, systems_target_, systems_cache_;
-
   std::map<std::string, ModeMap> modes_;
   ParametersMap parameters_;
 

system_modes/include/system_modes/mode_manager.hpp

@@ -28,6 +28,7 @@
 #include <memory>
 #include <utility>
 
+#include "system_modes/mode_handling.hpp"
 #include "system_modes/mode_inference.hpp"
 #include "system_modes/srv/change_mode.hpp"
 #include "system_modes/srv/get_mode.hpp"
@@ -44,6 +45,7 @@ class ModeManager : public rclcpp::Node
   ModeManager(const ModeManager &) = delete;
 
   std::shared_ptr<ModeInference> inference();
+  virtual void handle_system_deviation(const std::string & reason);
 
   virtual ~ModeManager() = default;
 
@@ -94,6 +96,9 @@ class ModeManager : public rclcpp::Node
 
 private:
   std::shared_ptr<ModeInference> mode_inference_;
+  std::shared_ptr<ModeHandling> mode_handling_;
+  rclcpp::TimerBase::SharedPtr periodic_timer_;
+  std::string model_path_;
 
   // Lifecycle change services
   std::map<std::string, rclcpp::Service<lifecycle_msgs::srv::ChangeState>::SharedPtr>

system_modes/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>system_modes</name>
-  <version>0.4.0</version>
+  <version>0.4.1</version>
   <description>
     The system modes concept assumes that a robotics system is built
     from components with a lifecycle. It adds a notion of (sub-)systems,
@@ -21,6 +21,8 @@
   <depend>builtin_interfaces</depend>
   <depend>rosidl_default_generators</depend>
 
+  <exec_depend>launch_ros</exec_depend>
+
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_pep257</test_depend>