Merge branch 'dev' into feature/rules

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

.github/workflows/lint.yaml

@@ -0,0 +1,45 @@
+name: Lint system modes
+on:
+  pull_request:
+
+jobs:
+  ament_xmllint:
+    name: ament_xmllint
+    runs-on: ubuntu-18.04
+    steps:
+    - uses: actions/checkout@v2
+    - uses: ros-tooling/setup-ros@0.0.25
+    - uses: ros-tooling/action-ros2-lint@0.0.6
+      with:
+        linter: xmllint
+        package-name: system_modes system_modes_examples
+
+  ament_lint_cpp:
+    name: ament_${{ matrix.linter }}
+    runs-on: ubuntu-18.04
+    strategy:
+      fail-fast: false
+      matrix:
+          linter: [cppcheck, cpplint, uncrustify]
+    steps:
+    - uses: actions/checkout@v2
+    - uses: ros-tooling/setup-ros@0.0.25
+    - uses: ros-tooling/action-ros2-lint@0.0.6
+      with:
+        linter: ${{ matrix.linter }}
+        package-name: system_modes system_modes_examples
+
+  ament_lint_py:
+    name: ament_${{ matrix.linter }}
+    runs-on: ubuntu-18.04
+    strategy:
+      fail-fast: false
+      matrix:
+        linter: [flake8, pep257]
+    steps:
+    - uses: actions/checkout@v2
+    - uses: ros-tooling/setup-ros@0.0.25
+    - uses: ros-tooling/action-ros2-lint@0.0.6
+      with:
+        linter: ${{ matrix.linter }}
+        package-name: system_modes system_modes_examples

.github/workflows/test.yaml

@@ -0,0 +1,22 @@
+name: Test system modes
+on:
+  pull_request:
+  push:
+    branches:
+      - master
+
+jobs:
+  build_and_test:
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      matrix:
+        os: [ubuntu-18.04]
+    steps:
+      - uses: ros-tooling/setup-ros@0.0.25
+      - uses: ros-tooling/action-ros-ci@0.0.15
+        with:
+          package-name: system_modes system_modes_examples
+          target-ros2-distro: foxy
+          colcon-mixin-name: coverage-gcc
+          colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml

.gitignore

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

system_modes/CMakeLists.txt

@@ -25,7 +25,6 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
-find_package(Boost 1.58 COMPONENTS program_options REQUIRED)
 
 # generate service
 rosidl_generate_interfaces(${PROJECT_NAME}
@@ -53,8 +52,6 @@ ament_target_dependencies(mode
   "lifecycle_msgs"
   "rosidl_typesupport_cpp"
   "std_msgs"
-  "Boost"
-  "Boost_PROGRAM_OPTIONS"
   "builtin_interfaces"
 )
 #rosidl_target_interfaces(mode

system_modes/changelog.rst

@@ -2,6 +2,27 @@
 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)
+* 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
+
+0.2.2 (2020-07-13)
+-----------
+* introduced StateAndMode struct to bundle lifecycle state and system mode
+
 0.2.0 (2020-02-13)
 -----------
 * integration with ROS 2 launch

system_modes/include/system_modes/mode_impl.hpp

@@ -61,17 +61,18 @@ struct StateAndMode
       return true;
     }
 
-    return (cmp.mode.compare(mode) == 0 ||                           // same mode
+    return 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
+           (mode.compare(DEFAULT_MODE) == 0 && cmp.mode.empty());    // DEFAULT_MODE the same
   }
 
   bool operator!=(const StateAndMode & cmp) const
   {
     return !(*this == cmp);
   }
 
-  void from_string(const std::string & sam) {
+  void from_string(const std::string & sam)
+  {
     auto dot = sam.find(".");
     if (dot != std::string::npos) {
       state = state_id_(sam.substr(0, dot));
@@ -82,7 +83,8 @@ struct StateAndMode
     }
   }
 
-  std::string as_string() const {
+  std::string as_string() const
+  {
     if (state != State::PRIMARY_STATE_ACTIVE || mode.empty()) {
       return state_label_(state);
     }

system_modes/include/system_modes/mode_inference.hpp

@@ -54,31 +54,40 @@ 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);
 
-  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);
-
+  /**
+   * 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 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;
+  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:
   ModeHandling * mode_handling_;
 
-  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_;
 
@@ -88,6 +97,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

@@ -41,11 +41,11 @@ namespace system_modes
 class ModeManager : public rclcpp::Node
 {
 public:
-  explicit ModeManager(const std::string & model_path);
+  ModeManager();
   ModeManager(const ModeManager &) = delete;
 
   std::shared_ptr<ModeInference> inference();
-  virtual void handle_system_deviation(const std::string& reason);
+  virtual void handle_system_deviation(const std::string & reason);
 
   virtual ~ModeManager() = default;
 
@@ -67,26 +67,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_;
@@ -118,16 +124,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/include/system_modes/mode_monitor.hpp

@@ -37,7 +37,7 @@ namespace system_modes
 class ModeMonitor : public rclcpp::Node
 {
 public:
-  ModeMonitor(const std::string & model_path, unsigned int rate, bool verbose, bool clear);
+  ModeMonitor();
   ModeMonitor(const ModeMonitor &) = delete;
 
   std::shared_ptr<ModeInference> inference();

system_modes/launch/mode_manager.launch.py

@@ -21,15 +21,12 @@
 
 
 def generate_launch_description():
-    launch.actions.DeclareLaunchArgument('modelfile', description='Path to modelfile')
-
-    node = launch_ros.actions.Node(
-        package='system_modes',
-        executable='mode_manager',
-        parameters=[{'modelfile': launch.substitutions.LaunchConfiguration('modelfile')}],
-        output='screen')
-
-    description = launch.LaunchDescription()
-    description.add_action(node)
-
-    return description
+    return launch.LaunchDescription([
+        launch.actions.DeclareLaunchArgument(
+            'modelfile',
+            description='Path to modelfile'),
+        launch_ros.actions.Node(
+            package='system_modes',
+            executable='mode_manager',
+            parameters=[{'modelfile': launch.substitutions.LaunchConfiguration('modelfile')}],
+            output='screen')])

system_modes/launch/mode_monitor.launch.py

@@ -15,21 +15,34 @@
 
 import launch
 import launch.actions
-import launch.substitutions
 
 import launch_ros.actions
 
 
 def generate_launch_description():
-    launch.actions.DeclareLaunchArgument('modelfile', description='Path to modelfile')
-
-    node = launch_ros.actions.Node(
-        package='system_modes',
-        executable='mode_monitor',
-        parameters=[{'modelfile': launch.substitutions.LaunchConfiguration('modelfile')}],
-        output='screen')
-
-    description = launch.LaunchDescription()
-    description.add_action(node)
-
-    return description
+    return launch.LaunchDescription([
+        launch.actions.DeclareLaunchArgument(
+            'modelfile',
+            description='Path to modelfile'),
+        launch.actions.DeclareLaunchArgument(
+            'debug',
+            default_value='false',
+            description='Debug'),
+        launch.actions.DeclareLaunchArgument(
+            'verbose',
+            default_value='false',
+            description='Print mode parametrization'),
+        launch.actions.DeclareLaunchArgument(
+            'rate',
+            default_value='1000',
+            description='Monitor refresh rate in ms'),
+        launch_ros.actions.Node(
+            package='system_modes',
+            executable='mode_monitor',
+            parameters=[{
+                'modelfile': launch.substitutions.LaunchConfiguration('modelfile'),
+                'debug': launch.substitutions.LaunchConfiguration('debug'),
+                'verbose': launch.substitutions.LaunchConfiguration('verbose'),
+                'rate': launch.substitutions.LaunchConfiguration('rate')
+                }],
+            output='screen')])

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