Modes observer (#68)

* Initial mode observer class
* Mutex modes cache for concurrent read/write
* Check for service availability- for the initial service calls, check for service availability first, timeout 500ms.
* Minimal unit tests and launch test for modes observer
* Documentation of mode observer

#66

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

Author: norro

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,14 +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
-    "redundant_mode_changes")       # Ignore redundant mode changes
+    "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/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/src/system_modes/mode_observer.cpp

@@ -0,0 +1,147 @@
+// 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.
+#include "system_modes/mode_observer.hpp"
+
+#include <lifecycle_msgs/msg/state.hpp>
+#include <lifecycle_msgs/msg/transition_event.hpp>
+
+#include <functional>
+#include <map>
+#include <string>
+#include <vector>
+#include <memory>
+
+#include "system_modes/msg/mode_event.hpp"
+
+using std::function;
+using std::map;
+using std::mutex;
+using std::placeholders::_1;
+using std::string;
+using std::vector;
+
+using lifecycle_msgs::msg::State;
+using lifecycle_msgs::msg::Transition;
+using lifecycle_msgs::msg::TransitionEvent;
+using lifecycle_msgs::srv::GetState;
+
+using system_modes::msg::ModeEvent;
+using system_modes::srv::GetMode;
+
+using namespace std::chrono_literals;
+using shared_mutex = std::shared_timed_mutex;
+
+namespace system_modes
+{
+
+ModeObserver::ModeObserver(std::weak_ptr<rclcpp::Node> handle)
+: node_handle_(handle)
+{
+}
+
+StateAndMode
+ModeObserver::get(const std::string & part_name)
+{
+  std::shared_lock<shared_mutex> lock(this->mutex_);
+  try {
+    return cache_.at(part_name);
+  } catch (const std::exception & e) {
+    std::cerr << e.what() << '\n';
+    return StateAndMode();
+  }
+}
+
+void
+ModeObserver::observe(const std::string & part_name)
+{
+  std::unique_lock<shared_mutex> lock(mutex_);
+
+  cache_[part_name] = StateAndMode();
+
+  // Initial try to get current state via service request
+  std::string topic = part_name + "/get_state";
+  auto gsrequest = std::make_shared<GetState::Request>();
+  auto stateclient = node_handle_.lock()->create_client<GetState>(topic);
+  if (stateclient->wait_for_service(std::chrono::microseconds(500))) {
+    auto state = stateclient->async_send_request(gsrequest);
+    if (rclcpp::spin_until_future_complete(node_handle_.lock(), state) ==
+      rclcpp::FutureReturnCode::SUCCESS)
+    {
+      auto result = state.get();
+      cache_[part_name].state = result->current_state.id;
+    }
+  }
+
+  // Initial try to get current mode via service request
+  topic = part_name + "/get_mode";
+  auto gmrequest = std::make_shared<GetMode::Request>();
+  auto modeclient = node_handle_.lock()->create_client<GetMode>(topic);
+  if (modeclient->wait_for_service(std::chrono::microseconds(500))) {
+    auto mode = modeclient->async_send_request(gmrequest);
+    if (rclcpp::spin_until_future_complete(node_handle_.lock(), mode) ==
+      rclcpp::FutureReturnCode::SUCCESS)
+    {
+      auto result = mode.get();
+      cache_[part_name].mode = result->current_mode;
+    }
+  }
+
+  // Set up transition subscriber and mode event subscriber for continuous observation
+  topic = part_name + "/transition_event";
+  function<void(TransitionEvent::SharedPtr)> transition_cb =
+    bind(&ModeObserver::transition_callback, this, _1, part_name);
+  auto state_sub = node_handle_.lock()->create_subscription<TransitionEvent>(
+    topic,
+    rclcpp::SystemDefaultsQoS(),
+    transition_cb);
+  state_subs_.emplace(part_name, state_sub);
+  topic = part_name + "/mode_event";
+  function<void(ModeEvent::SharedPtr)> mode_cb =
+    bind(&ModeObserver::mode_event_callback, this, _1, part_name);
+  auto mode_sub = node_handle_.lock()->create_subscription<ModeEvent>(
+    topic,
+    rclcpp::SystemDefaultsQoS(),
+    mode_cb);
+  mode_subs_.emplace(part_name, mode_sub);
+}
+
+void
+ModeObserver::stop_observing(const std::string & part_name)
+{
+  std::unique_lock<shared_mutex> lock(mutex_);
+  state_subs_.erase(part_name);
+  mode_subs_.erase(part_name);
+  cache_.erase(part_name);
+}
+
+void
+ModeObserver::transition_callback(
+  const TransitionEvent::SharedPtr msg,
+  const string & part_name)
+{
+  std::unique_lock<shared_mutex> lock(mutex_);
+  cache_[part_name].state = msg->goal_state.id;
+}
+
+void
+ModeObserver::mode_event_callback(
+  const ModeEvent::SharedPtr msg,
+  const string & part_name)
+{
+  std::unique_lock<shared_mutex> lock(mutex_);
+  cache_[part_name].mode = msg->goal_mode.label;
+}
+
+}  // namespace system_modes

system_modes/test/launchtest/modes_observer.launch.py.in

@@ -0,0 +1,72 @@
+import os
+
+import unittest
+
+import ament_index_python
+import launch
+import launch_ros
+import launch_testing.actions
+import launch_testing_ros
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+
+
+def generate_test_description():
+    os.environ['OSPL_VERBOSITY'] = '8'
+    os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}'
+
+    modelfile = '@MODELFILE@'
+
+    modes_observer = ExecuteProcess(
+        cmd=[
+            "ros2",
+            "run",
+            "system_modes",
+            "modes_observer_test_node"],
+        name='modes_observer_test_node',
+        emulate_tty=True,
+        output='screen')
+
+    mode_manager = launch.actions.IncludeLaunchDescription(
+        launch.launch_description_sources.PythonLaunchDescriptionSource(
+            ament_index_python.packages.get_package_share_directory(
+                'system_modes') + '/launch/mode_manager.launch.py'),
+        launch_arguments={'modelfile': modelfile}.items())
+
+    test_nodes = ExecuteProcess(
+        cmd=[
+            "@PYTHON_EXECUTABLE@",
+            "@TEST_NODES@"
+            ],
+        name='test_nodes',
+        emulate_tty=True,
+        output='screen')
+
+    launch_description = LaunchDescription()
+    launch_description.add_action(modes_observer)
+    launch_description.add_action(mode_manager)
+    launch_description.add_action(test_nodes)
+    launch_description.add_action(launch_testing.actions.ReadyToTest())
+
+    return launch_description, locals()
+
+class TestModeManagement(unittest.TestCase):
+
+    def test_processes_output(self, proc_output, modes_observer):
+        """Check manager and nodes logging output for expected strings."""
+
+        from launch_testing.tools.output import get_default_filtered_prefixes
+        output_filter = launch_testing_ros.tools.basic_output_filter(
+            filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'],
+            filtered_rmw_implementation='@RMW_IMPLEMENTATION@'
+        )
+        proc_output.assertWaitFor(
+            expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"),
+            process=modes_observer,
+            output_filter=output_filter,
+            timeout=15,
+            stream='stdout')
+
+        import time
+        time.sleep(1)