Merge latest dev into master (#49)

Mode manager is now publishing transitions that are inferred by the mode
inference, i.e. state and mode transitions of systems, mode transitions
of nodes.

* Stricter constness in mode inference
* ModeEvent now includes start mode (of transition)
* Adds inference of transitions to mode inference
* Tests for transition inference
* Mode manager publishs inferred transitions
* Version push to 0.4.0

#42

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

Author: norro

.gitignore

@@ -0,0 +1 @@
+.vscode

system_modes/changelog.rst

@@ -2,6 +2,12 @@
 Changelog for package system_modes
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+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.3.0 (2020-07-23)
 -----------
 * removed boost dependencies (was: program options)

system_modes/include/system_modes/mode_inference.hpp

@@ -53,27 +53,38 @@ class ModeInference
   virtual void update_param(const std::string &, rclcpp::Parameter &);
   virtual void update_target(const std::string &, StateAndMode);
 
-  virtual StateAndMode get(const std::string & part);
+  virtual StateAndMode get(const std::string & part) const;
   virtual StateAndMode get_or_infer(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 StateAndMode infer_system(const std::string & part);
+
+  /**
+   * Infers latest transitions of systems
+   *
+   * Returns map of last inferred transitions of systems into new states or
+   * new modes. State transitions of nodes don't have to be inferred, as
+   * nodes publish their state transitions. For nodes, we only need to infer
+   * mode transitions.
+   */
+  virtual Deviation infer_transitions();
 
-  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);
+  virtual StateAndMode get_target(const std::string & part) const;
+  virtual ModeConstPtr get_mode(const std::string & part, const std::string & mode) const;
+  virtual std::vector<std::string> get_available_modes(const std::string & part) const;
 
   virtual ~ModeInference() = default;
 
 protected:
-  virtual bool matching_parameters(const rclcpp::Parameter &, const rclcpp::Parameter &);
+  virtual bool matching_parameters(const rclcpp::Parameter &, const rclcpp::Parameter &) const;
   virtual void read_modes_from_model(const std::string & model_path);
   virtual void add_param_to_mode(ModeBasePtr, const rclcpp::Parameter &);
 
 private:
-  StatesMap nodes_, nodes_target_;
-  StatesMap systems_, systems_target_;
+  StatesMap nodes_, nodes_target_, nodes_cache_;
+  StatesMap systems_, systems_target_, systems_cache_;
+
   std::map<std::string, ModeMap> modes_;
   ParametersMap parameters_;
 
@@ -83,6 +94,8 @@ class ModeInference
     param_mutex_;
   mutable std::shared_timed_mutex
     nodes_target_mutex_, systems_target_mutex_;
+  mutable std::shared_timed_mutex
+    nodes_cache_mutex_, systems_cache_mutex_;
 };
 
 }  // namespace system_modes

system_modes/include/system_modes/mode_manager.hpp

@@ -90,6 +90,8 @@ class ModeManager : public rclcpp::Node
     const std::string &,
     const std::string &);
 
+  virtual void publish_transitions();
+
 private:
   std::shared_ptr<ModeInference> mode_inference_;
 
@@ -117,16 +119,23 @@ class ModeManager : public rclcpp::Node
   std::map<std::string, rclcpp::AsyncParametersClient::SharedPtr>
   param_change_clients_;
 
-  // Lifecycle transition request
+  // Lifecycle transition publisher
+  std::map<std::string, rclcpp::Publisher<lifecycle_msgs::msg::TransitionEvent>::SharedPtr>
+  transition_pub_;
   std::map<std::string, rclcpp::Publisher<lifecycle_msgs::msg::TransitionEvent>::SharedPtr>
   state_request_pub_;
 
-  // Mode transition request publisher
+  // Mode transition publisher
+  std::map<std::string, rclcpp::Publisher<system_modes::msg::ModeEvent>::SharedPtr>
+  mode_transition_pub_;
   std::map<std::string, rclcpp::Publisher<system_modes::msg::ModeEvent>::SharedPtr>
   mode_request_pub_;
 
   // Remember states and modes of the systems
   std::map<std::string, StateAndMode> current_modes_;
+
+  // Timer to check for and publish recent transitions
+  rclcpp::TimerBase::SharedPtr transition_timer_;
 };
 
 }  // namespace system_modes

system_modes/msg/ModeEvent.msg

@@ -1,2 +1,8 @@
+# The time point at which this event occurred.
 uint64 timestamp
+
+# The starting mode from which this event transitioned.
+Mode start_mode
+
+# The end mode of this transition event.
 Mode goal_mode

system_modes/package.xml

@@ -2,8 +2,14 @@
 <?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.3.0</version>
-  <description>Model-based distributed configuration handling.</description>
+  <version>0.4.0</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,
+    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.
+  </description>
   <maintainer email="arne.nordmann@bosch.com">Arne Nordmann</maintainer>
   <license>Apache License 2.0</license>