Merge branch 'master' into feature/quality-level-rep-2004

Signed-off-by: Nordmann Arne (CR/ADT3) <arne.nordmann@de.bosch.com>

Author: norro

README.md

@@ -2,7 +2,6 @@
 
 [![License](https://img.shields.io/badge/License-Apache%202-blue.svg)](https://github.com/micro-ROS/system_modes/blob/master/LICENSE)
 [![Build status](https://build.ros2.org/job/Ddev__system_modes__ubuntu_bionic_amd64/badge/icon?subject=Build%20farm%3A%20Dashing)](https://build.ros2.org/job/Ddev__system_modes__ubuntu_bionic_amd64/)
-[![Build status](http://build.ros2.org/job/Edev__system_modes__ubuntu_bionic_amd64/badge/icon?subject=Build%20farm%3A%20Eloquent)](http://build.ros2.org/job/Edev__system_modes__ubuntu_bionic_amd64/)
 [![Build status](http://build.ros2.org/job/Fdev__system_modes__ubuntu_focal_amd64/badge/icon?subject=Build%20farm%3A%20Foxy)](http://build.ros2.org/job/Fdev__system_modes__ubuntu_focal_amd64/)
 [![Build status](http://build.ros2.org/job/Rdev__system_modes__ubuntu_focal_amd64/badge/icon?subject=Build%20farm%3A%20Rolling)](http://build.ros2.org/job/Rdev__system_modes__ubuntu_focal_amd64/)
 [![Build status](https://github.com/micro-ROS/system_modes/workflows/Build%20action%3A%20Foxy%20%2B%20Rolling/badge.svg)](https://github.com/micro-ROS/system_modes/actions)

system_modes/CMakeLists.txt

@@ -41,7 +41,8 @@ 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)
+  src/system_modes/mode_inference.cpp
+  src/system_modes/mode_observer.cpp)
 target_include_directories(mode PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
@@ -179,13 +180,32 @@ if(BUILD_TESTING)
     target_link_libraries(test_mode_monitor mode)
   endif()
 
+  ament_add_gtest(test_mode_observer test/test_mode_observer.cpp)
+  if(TARGET test_mode_observer)
+    target_include_directories(test_mode_observer PUBLIC
+      ${rclcpp_INCLUDE_DIRS}
+      ${CMAKE_CURRENT_BINARY_DIR}/system_modes/
+    )
+    target_link_libraries(test_mode_observer mode)
+  endif()
+
+  # mode observer test node
+  add_executable(modes_observer_test_node test/launchtest/modes_observer_test_node.cpp)
+  target_include_directories(modes_observer_test_node PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
+  target_link_libraries(modes_observer_test_node mode)
+  install(TARGETS modes_observer_test_node DESTINATION lib/${PROJECT_NAME})
+
   # Launch Tests
   find_package(launch_testing_ament_cmake REQUIRED)
   set(launch_tests
     "two_lifecycle_nodes"           # Mode Manager with Lifecycle Nodes
     "two_mixed_nodes"               # Mode Manager with Lifecycle and non-Lifecycle Nodes
     "two_independent_hierarchies"   # Mode Manager for two independent hierarchies of nodes
-    "manager_and_monitor")          # Mode Manager and Mode Monitor
+    "manager_and_monitor"           # Mode Manager and Mode Monitor
+    "redundant_mode_changes"        # Ignore redundant mode changes
+    "modes_observer")               # Mode Observer
 
   # Launch Test: Mode Manager with Lifecycle Nodes
   foreach(test_name ${launch_tests})

system_modes/README.md

@@ -89,6 +89,12 @@ Since the introduced (sub-)systems are not concrete software entities, their sta
   Before attempting a state or mode change for a system or node, the [mode manager](#mode_manager) publishes information about the request.
   The according topics might need to be *latched* in order to allow nodes to do the inference after joining a running system.
 
+#### Mode Observer
+
+Additionally, the library comprises a *Mode Observer* that serves as a local cache of states and modes of all observed system parts. The mode observer will try to obtain the current state and mode initially via sevrice calls (GetState, GetMode) and subsequently monitors according transitions events and mode events.
+
+The mode observer is supposed to be instantiated within a ROS 2 node to have states and modes available locally for fast access, see this exemplary [ModeObserverNode](https://github.com/micro-ROS/system_modes/blob/master/system_modes/test/launchtest/modes_observer_test_node.cpp) used for testing.
+
 ### Mode Manager
 
 The mode manager is a ROS node that accepts an SHM file (see [above](#system-modes)) as command line parameter. It parses the SHM file and creates the according services, publishers, and subscribers to manage the system and its modes.

system_modes/changelog.rst

@@ -2,6 +2,12 @@
 Changelog for package system_modes_examples
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.5.0 (2020-03-16)
+-----------
+* Atomic parameter setting https://github.com/micro-ROS/system_modes/issues/59
+* Bug fixing
+* More tests
+
 0.4.2 (2020-12-17)
 -----------
 * Error handling and rules feature no longer experimental

system_modes/include/system_modes/mode_impl.hpp

@@ -90,6 +90,11 @@ struct StateAndMode
     }
     return state_label_(state) + "." + mode;
   }
+
+  bool unknown()
+  {
+    return state == State::PRIMARY_STATE_UNKNOWN;
+  }
 };
 
 class ModeImpl

system_modes/include/system_modes/mode_observer.hpp

@@ -0,0 +1,98 @@
+// 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 <rclcpp/rclcpp.hpp>
+
+#include <lifecycle_msgs/msg/transition_event.hpp>
+#include <lifecycle_msgs/srv/get_state.hpp>
+
+#include <map>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <utility>
+
+#include "system_modes/mode.hpp"
+#include "system_modes/msg/mode_event.hpp"
+#include "system_modes/srv/get_mode.hpp"
+
+namespace system_modes
+{
+
+using std::map;
+using std::mutex;
+using std::string;
+
+using lifecycle_msgs::msg::TransitionEvent;
+using lifecycle_msgs::srv::GetState;
+using rclcpp::node_interfaces::NodeBaseInterface;
+using system_modes::msg::ModeEvent;
+using system_modes::srv::GetMode;
+
+/**
+ * Mode observer provides a local system modes cache.
+ *
+ * The mode observer serves as a local system modes cache, instantiated by a node. The mode
+ * observer will initially try to gain current states/modes from all observes entities and will
+ * then continuously monitor transitions to keep up to date.
+ */
+class ModeObserver
+{
+public:
+  explicit ModeObserver(std::weak_ptr<rclcpp::Node>);
+  virtual ~ModeObserver() = default;
+
+  /**
+   * Getter for observed state and mode.
+   *
+   * Returns cached observed state and mode for requested system entity (system or node).
+   */
+  virtual StateAndMode
+  get(const string & part_name);
+
+  /**
+   * Add part to list of observed parts.
+   */
+  virtual void
+  observe(const string & part_name);
+
+  /**
+   * Remove part from list of observed parts.
+   */
+  virtual void
+  stop_observing(const string & part_name);
+
+protected:
+  virtual void
+  transition_callback(
+    const TransitionEvent::SharedPtr msg,
+    const string & part_name);
+
+  virtual void
+  mode_event_callback(
+    const ModeEvent::SharedPtr msg,
+    const string & part_name);
+
+private:
+  std::weak_ptr<rclcpp::Node> node_handle_;
+  map<string, StateAndMode> cache_;
+  mutable std::shared_timed_mutex mutex_;
+
+  map<string, std::shared_ptr<rclcpp::Subscription<TransitionEvent>>> state_subs_;
+  map<string, std::shared_ptr<rclcpp::Subscription<ModeEvent>>> mode_subs_;
+};
+
+}  // namespace system_modes

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.2</version>
+  <version>0.5.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,
@@ -34,6 +34,7 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>launch_testing_ros</test_depend>
+  <test_depend>ros2run</test_depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
 

system_modes/src/system_modes/mode_handling.cpp

@@ -28,7 +28,6 @@
 #include <utility>
 #include <vector>
 
-using std::endl;
 using std::pair;
 using std::mutex;
 using std::string;

system_modes/src/system_modes/mode_inference.cpp

@@ -28,7 +28,6 @@
 #include <utility>
 #include <vector>
 
-using std::endl;
 using std::mutex;
 using std::string;
 using std::make_pair;
@@ -223,6 +222,7 @@ ModeInference::infer_system(const string & part)
       // error-processing?
       if (stateAndMode.state == State::TRANSITION_STATE_ERRORPROCESSING) {
         this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, "");
+
         return StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, "");
       }
 
@@ -236,6 +236,7 @@ ModeInference::infer_system(const string & part)
                 "', inference failed.");
       }
     }
+
     return StateAndMode(state, "");
   }
 
@@ -245,15 +246,16 @@ ModeInference::infer_system(const string & part)
   string targetMode = stateAndModeTarget->second.mode;
   if (targetState == State::PRIMARY_STATE_INACTIVE) {
     // target: inactive
-    for (auto part : default_mode->get_parts()) {
-      auto stateAndMode = this->get_or_infer(part);
+    for (auto partpart : default_mode->get_parts()) {
+      auto stateAndMode = this->get_or_infer(partpart);
 
       if (stateAndMode.state == State::TRANSITION_STATE_ERRORPROCESSING) {
         // Note: If current entity was in an active mode that allowed a part to
         // be in error-processing (by dont-care) and the current entity is requested
         // to switch to inactive, then the actual state of the current entity will
         // go to error-processing until the mentioned part recovers.
         this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, "");
+
         return StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, "");
       }
 
@@ -262,9 +264,11 @@ ModeInference::infer_system(const string & part)
         stateAndMode.state != State::PRIMARY_STATE_UNCONFIGURED)
       {
         this->systems_[part] = StateAndMode(State::TRANSITION_STATE_DEACTIVATING, "");
+
         return StateAndMode(State::TRANSITION_STATE_DEACTIVATING, "");
       }
     }
+
     return StateAndMode(State::PRIMARY_STATE_INACTIVE, "");
   } else if (targetState == State::PRIMARY_STATE_ACTIVE) {
     ModeConstPtr mode;
@@ -281,6 +285,7 @@ ModeInference::infer_system(const string & part)
         // TODO(anordman): consider DONT-CARE
         if (stateAndMode.state == State::TRANSITION_STATE_ERRORPROCESSING) {
           this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, "");
+
           return StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, "");
         }
 
@@ -302,6 +307,7 @@ ModeInference::infer_system(const string & part)
       if (inTargetMode) {
         // Target state and target mode reached, all good!
         this->systems_[part] = StateAndMode(State::PRIMARY_STATE_ACTIVE, targetMode);
+
         return StateAndMode(State::PRIMARY_STATE_ACTIVE, targetMode);
       }
     }
@@ -316,6 +322,7 @@ ModeInference::infer_system(const string & part)
         // TODO(anordman): consider DONT-CARE
         if (stateAndMode.state == State::TRANSITION_STATE_ERRORPROCESSING) {
           this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, "");
+
           return StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, "");
         }
 
@@ -331,11 +338,13 @@ ModeInference::infer_system(const string & part)
       if (foundMode) {
         // We are in a non-target mode, this means we are still activating
         this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ACTIVATING, mode.first);
+
         return StateAndMode(State::TRANSITION_STATE_ACTIVATING, mode.first);
       }
     }
 
     this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ACTIVATING, "");
+
     return StateAndMode(State::TRANSITION_STATE_ACTIVATING, "");
   }
 
@@ -616,8 +625,8 @@ const std::vector<string>
 ModeInference::get_nodes() const
 {
   std::vector<string> result;
-  for (auto node : this->nodes_) {
-    result.push_back(node.first);
+  for (auto part : this->nodes_) {
+    result.push_back(part.first);
   }
   return result;
 }
@@ -626,8 +635,8 @@ const std::vector<string>
 ModeInference::get_systems() const
 {
   std::vector<string> result;
-  for (auto node : this->systems_) {
-    result.push_back(node.first);
+  for (auto part : this->systems_) {
+    result.push_back(part.first);
   }
   return result;
 }

system_modes/src/system_modes/mode_manager.cpp

@@ -432,6 +432,21 @@ ModeManager::change_mode(
     return false;
   }
 
+  // Mode change redundant?
+  try {
+    auto current = mode_inference_->get_or_infer(node_name);
+    if (current.mode.compare(mode_name) == 0) {
+      RCLCPP_ERROR(
+        this->get_logger(),
+        "Redundant mode change of %s to %s - ignored.",
+        node_name.c_str(),
+        mode_name.c_str());
+      return true;
+    }
+  } catch (...) {
+    // Okay, if we don't know anything about this part, yet
+  }
+
   // Publish info about this request
   auto info = std::make_shared<ModeEvent>();
   info->goal_mode.label = mode_name;
@@ -633,7 +648,6 @@ ModeManager::publish_transitions()
       info->start_state.label = state_label_(from.state);
       info->goal_state.id = to.state;
       info->goal_state.label = state_label_(to.state);
-      this->transition_pub_[part]->publish(TransitionEvent());
       this->transition_pub_[part]->publish(*info);
     }
     if (from.mode.compare(to.mode) != 0) {