Merge branch 'dev' into feature/win-and-macos-build

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

Author: norro

.gitignore

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

system_modes/changelog.rst

@@ -2,7 +2,19 @@
 Changelog for package system_modes
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-0.2.3 (2020-02-13)
+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)
+* changed mode service specifications (less redundancy)
+* https://github.com/micro-ROS/system_modes/issues/24
+
+0.2.3 (2020-07-23)
 -----------
 * improved StateAndMode struct
 * testing

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

@@ -65,26 +65,32 @@ class ModeManager : public rclcpp::Node
 
   // Mode service callbacks
   virtual void on_change_mode(
-    const std::shared_ptr<rmw_request_id_t>,
+    const std::string &,
     const std::shared_ptr<system_modes::srv::ChangeMode::Request>,
     std::shared_ptr<system_modes::srv::ChangeMode::Response>);
   virtual void on_get_mode(
-    const std::shared_ptr<rmw_request_id_t>,
-    const std::shared_ptr<system_modes::srv::GetMode::Request>,
+    const std::string &,
     std::shared_ptr<system_modes::srv::GetMode::Response>);
   virtual void on_get_available_modes(
-    const std::shared_ptr<rmw_request_id_t>,
-    const std::shared_ptr<system_modes::srv::GetAvailableModes::Request>,
+    const std::string &,
     std::shared_ptr<system_modes::srv::GetAvailableModes::Response>);
 
   virtual bool change_state(
-    const std::string & node,
+    const std::string &,
     unsigned int,
     bool transitive = true);
-  virtual bool change_mode(const std::string & node, const std::string & mode);
+  virtual bool change_mode(
+    const std::string &,
+    const std::string &);
 
-  virtual void change_part_state(const std::string & node, unsigned int);
-  virtual void change_part_mode(const std::string & node, const std::string & mode);
+  virtual void change_part_state(
+    const std::string &,
+    unsigned int);
+  virtual void change_part_mode(
+    const std::string &,
+    const std::string &);
+
+  virtual void publish_transitions();
 
 private:
   std::shared_ptr<ModeInference> mode_inference_;
@@ -113,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.2.3</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>
 
@@ -15,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>

system_modes/src/mode_monitor_node.cpp

@@ -69,7 +69,7 @@ void mode_change_callback(
   const string & node_name)
 {
   monitor->inference()->update_state(node_name, State::PRIMARY_STATE_ACTIVE);
-  monitor->inference()->update_mode(node_name, msg->goal_mode.label.c_str());
+  monitor->inference()->update_mode(node_name, msg->goal_mode.label);
 }
 
 void transition_request_callback(
@@ -93,7 +93,7 @@ void mode_request_callback(
 {
   monitor->inference()->update_target(
     node_name,
-    StateAndMode(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label.c_str()));
+    StateAndMode(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label));
 }
 
 void