Dev

README.md

@@ -22,6 +22,8 @@ $ `colcon build --packages-select-regex system_modes`
 
 Have a look at the [system_modes_examples](./system_modes_examples/) documentation to try your installation.
 
+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.
+
 ## License
 
 ROS 2 System Modes are open-sourced under the Apache-2.0 license. See the

system_modes/CMakeLists.txt

@@ -16,7 +16,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 # find dependencies
-find_package(ament_cmake_ros REQUIRED)
+find_package(ament_cmake REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -38,7 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   DEPENDENCIES builtin_interfaces
 )
 
-add_library(mode
+add_library(mode SHARED
   src/system_modes/mode.cpp
   src/system_modes/mode_impl.cpp
   src/system_modes/mode_inference.cpp)
@@ -53,14 +53,15 @@ ament_target_dependencies(mode
   "rosidl_typesupport_cpp"
   "std_msgs"
   "Boost"
+  "Boost_PROGRAM_OPTIONS"
   "builtin_interfaces"
 )
 #rosidl_target_interfaces(mode
 # ${PROJECT_NAME} "rosidl_typesupport_cpp")
 # TODO Should work with the two lines above, but doesn't
 include_directories(../../build/system_modes/rosidl_generator_cpp/)
 link_directories(../../build/system_modes/)
-target_link_libraries(mode system_modes__rosidl_typesupport_cpp boost_program_options)
+target_link_libraries(mode system_modes__rosidl_typesupport_cpp)
 
   # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
@@ -132,6 +133,16 @@ if(BUILD_TESTING)
     )
     target_link_libraries(test_mode mode)
   endif()
+
+  message(STATUS "CMAKE_CURRENT_BINARY_DIR: ${CMAKE_CURRENT_BINARY_DIR}/system_modes/")
+  ament_add_gtest(test_mode_inference test/test_mode_inference.cpp)
+  if(TARGET test_mode_inference)
+    target_include_directories(test_mode_inference PUBLIC
+      ${rclcpp_INCLUDE_DIRS}
+      ${CMAKE_CURRENT_BINARY_DIR}/system_modes/
+    )
+    target_link_libraries(test_mode_inference mode)
+  endif()
 endif()
 
 ament_export_include_directories(include)

system_modes/README.md

@@ -120,3 +120,11 @@ The mode monitor is a ROS node that accepts an SHM file (see [above](#system-mod
 
 Running the monitor:  
 $ `ros2 launch system_modes mode_monitor.launch.py modelfile:=[path/to/modelfile.yaml]`
+
+## 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.
+
+When designing system modes for your system, try to focus on platform-specific aspects (so those that are generally present) rather than aspects specific to a certain application. Good examples are *degraded* and *performance* modes of the platform, bad examples are system modes to encode the current number of (grasping) re-tries.
+
+Also, do not model any "read-only" system modes, e.g., modes that discretize/encode a read-only internal state. An example for such a mode specification to avoid is *low energy* and *full*, discretizing the charging level of a battery component. The System Modes concept assumes that the activatability of a system mode of a given node or subsystem should depend only on the states and modes of the other nodes and subsystems (and on the higher-level task executed by some deliberation layer). Note that the same applies to the ROS 2 node lifecycle states (*Unconfigured*, *Inactive*, etc.). The only exception is the *ErrorProcessing* state, which can be entered autonomously by the node itself. Within the mode inference, if a node performs a transition to *ErrorProcessing*, this is automatically propagated upwards as inferred state along the hierarchy. It is up to the deliberation layer to handle the failure of this node or subsystem.

system_modes/include/system_modes/mode.hpp

@@ -41,8 +41,6 @@ using DefaultModePtr = std::shared_ptr<DefaultMode>;
 
 using ModeMap = std::map<std::string, ModeConstPtr>;
 
-static const char DEFAULT_MODE[] = "__DEFAULT__";
-
 class ModeBase
 {
 public:
@@ -56,14 +54,14 @@ class ModeBase
   virtual void set_parameters(const std::vector<rclcpp::Parameter> & parameters) = 0;
   virtual void set_part_mode(
     const std::string & part,
-    const std::pair<unsigned int, std::string> stateAndMode) = 0;
+    const StateAndMode stateAndMode) = 0;
 
   virtual rclcpp::Parameter get_parameter(const std::string & param_name) const;
   virtual std::vector<std::string> get_parameter_names() const;
   virtual const std::vector<rclcpp::Parameter> get_parameters() const;
 
   virtual const std::vector<std::string> get_parts() const;
-  virtual const std::pair<unsigned int, std::string> get_part_mode(const std::string & part) const;
+  virtual const StateAndMode get_part_mode(const std::string & part) const;
 
   virtual std::string print() const;
 
@@ -83,7 +81,7 @@ class DefaultMode : public ModeBase
 
   virtual void set_part_mode(
     const std::string & part,
-    const std::pair<unsigned int, std::string> stateAndMode);
+    const StateAndMode stateAndMode);
 };
 
 class Mode : public ModeBase
@@ -100,7 +98,7 @@ class Mode : public ModeBase
 
   virtual void set_part_mode(
     const std::string & part,
-    const std::pair<unsigned int, std::string> stateAndMode);
+    const StateAndMode stateAndMode);
 };
 
 inline std::ostream & operator<<(std::ostream & os, const Mode & mode)

system_modes/include/system_modes/mode_impl.hpp

@@ -27,6 +27,26 @@
 namespace system_modes
 {
 
+static const char DEFAULT_MODE[] = "__DEFAULT__";
+
+struct StateAndMode {
+  unsigned int state;
+  std::string mode;
+
+  StateAndMode(unsigned int newstate = 0, const std::string& newmode = "")
+  {
+    state = newstate;
+    mode = newmode;
+  }
+  bool operator!=(const StateAndMode& cmp) const
+  {
+    return (cmp.state == state                                       // same state
+      && (cmp.mode.compare(mode) == 0                                // same mode
+        || (cmp.mode.compare(DEFAULT_MODE) == 0 && mode.empty())     // we consider empty and
+        || (mode.compare(DEFAULT_MODE) == 0 && cmp.mode.empty())));  // DEFAULT_MODE the same
+  }
+};
+
 class ModeImpl
 {
 public:
@@ -40,26 +60,26 @@ class ModeImpl
   virtual void add_parameters(const std::vector<rclcpp::Parameter> & parameters);
   virtual void add_part_mode(
     const std::string & part,
-    const std::pair<unsigned int, std::string> stateAndMode);
+    const StateAndMode stateAndMode);
 
   virtual void set_parameter(const rclcpp::Parameter & parameter);
   virtual void set_parameters(const std::vector<rclcpp::Parameter> & parameters);
   virtual void set_part_mode(
     const std::string & part,
-    const std::pair<unsigned int, std::string> stateAndMode);
+    const StateAndMode stateAndMode);
 
   virtual std::vector<std::string> get_parameter_names() const;
   virtual rclcpp::Parameter get_parameter(const std::string & param_name) const;
   virtual bool get_parameter(const std::string & param_name, rclcpp::Parameter & parameter) const;
   virtual const std::vector<rclcpp::Parameter> get_parameters() const;
 
   virtual const std::vector<std::string> get_parts() const;
-  virtual const std::pair<unsigned int, std::string> get_part_mode(const std::string & part) const;
+  virtual const StateAndMode get_part_mode(const std::string & part) const;
 
 protected:
   std::string name_;
   std::map<std::string, rclcpp::Parameter> param_;
-  std::map<std::string, std::pair<unsigned int, std::string>> part_modes_;
+  std::map<std::string, StateAndMode> part_modes_;
   mutable std::mutex mutex_;
 };
 

system_modes/include/system_modes/mode_inference.hpp

@@ -31,6 +31,10 @@
 namespace system_modes
 {
 
+typedef std::map<std::string, StateAndMode > StatesMap;
+typedef std::map<std::string, std::map<std::string, rclcpp::Parameter> > ParametersMap;
+typedef std::map<std::string, std::pair<StateAndMode, StateAndMode > > Deviation;
+
 class ModeInference
 {
 public:
@@ -43,19 +47,20 @@ class ModeInference
   virtual const std::vector<std::string> get_all_parts_of(
     const std::string & system) const;
 
+  virtual void update(const std::string &, const StateAndMode &);
   virtual void update_state(const std::string &, unsigned int);
   virtual void update_mode(const std::string &, const std::string &);
   virtual void update_param(const std::string &, rclcpp::Parameter &);
-  virtual void update_target(const std::string &, std::pair<unsigned int, std::string>);
+  virtual void update_target(const std::string &, StateAndMode);
 
-  virtual std::pair<unsigned int, std::string> get(const std::string & part);
-  virtual std::pair<unsigned int, std::string> get_or_infer(const std::string & part);
+  virtual StateAndMode get(const std::string & part);
+  virtual StateAndMode get_or_infer(const std::string & part);
 
-  virtual std::pair<unsigned int, std::string> infer(const std::string & part);
-  virtual std::pair<unsigned int, std::string> infer_system(const std::string & part);
-  virtual std::pair<unsigned int, std::string> infer_node(const std::string & part);
+  virtual StateAndMode infer(const std::string & part);
+  virtual StateAndMode infer_system(const std::string & part);
+  virtual StateAndMode infer_node(const std::string & part);
 
-  virtual std::pair<unsigned int, std::string> get_target(const std::string & part);
+  virtual StateAndMode get_target(const std::string & part);
   virtual ModeConstPtr get_mode(const std::string & part, const std::string & mode);
   virtual std::vector<std::string> get_available_modes(const std::string & part);
 
@@ -67,10 +72,10 @@ class ModeInference
   virtual void add_param_to_mode(ModeBasePtr, const rclcpp::Parameter &);
 
 private:
-  std::map<std::string, std::pair<unsigned int, std::string>> nodes_, nodes_target_;
-  std::map<std::string, std::pair<unsigned int, std::string>> systems_, systems_target_;
+  StatesMap nodes_, nodes_target_;
+  StatesMap systems_, systems_target_;
   std::map<std::string, ModeMap> modes_;
-  std::map<std::string, std::map<std::string, rclcpp::Parameter>> parameters_;
+  ParametersMap parameters_;
 
   mutable std::shared_timed_mutex
     nodes_mutex_, systems_mutex_,

system_modes/package.xml

@@ -7,16 +7,15 @@
   <maintainer email="arne.nordmann@bosch.com">Arne Nordmann</maintainer>
   <license>Apache License 2.0</license>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>std_msgs</depend>
   <depend>builtin_interfaces</depend>
   <depend>rosidl_default_generators</depend>
   <depend>libboost-program-options-dev</depend>
 
-  <buildtool_depend>ament_cmake_ros</buildtool_depend>
-
-  <test_depend>ament_cmake</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_pep257</test_depend>

system_modes/src/mode_manager_node.cpp

@@ -40,6 +40,7 @@ using std::shared_ptr;
 
 using system_modes::ModeManager;
 using system_modes::DEFAULT_MODE;
+using system_modes::StateAndMode;
 using system_modes::msg::ModeEvent;
 
 using lifecycle_msgs::msg::State;
@@ -108,11 +109,11 @@ void transition_request_callback(
   if (msg->goal_state.id != State::PRIMARY_STATE_ACTIVE) {
     manager->inference()->update_target(
       node_name,
-      make_pair(msg->goal_state.id, ""));
+      StateAndMode(msg->goal_state.id, ""));
   } else {
     manager->inference()->update_target(
       node_name,
-      make_pair(msg->goal_state.id, DEFAULT_MODE));
+      StateAndMode(msg->goal_state.id, DEFAULT_MODE));
   }
 }
 
@@ -122,7 +123,7 @@ void mode_request_callback(
 {
   manager->inference()->update_target(
     node_name,
-    make_pair(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label.c_str()));
+    StateAndMode(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label.c_str()));
 }
 
 void