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

Author: norro

CPPLINT.cfg

@@ -1,2 +1,2 @@
 set noparent
-filter=-build/include_order,-build/header_guard,-runtime/string
+filter=-build/header_guard,-runtime/string

codecov.yml

@@ -0,0 +1,4 @@
+ignore:
+  - "system_modes_examples"
+  - "system_modes/test"
+  - "**/*_node.cpp"

system_modes/CMakeLists.txt

@@ -109,6 +109,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   set(MODE_FILE_CORRECT ${CMAKE_CURRENT_SOURCE_DIR}/test/test_modes.yaml)
+  set(MODE_FILE_RULES ${CMAKE_CURRENT_SOURCE_DIR}/test/test_modes_rules.yaml)
   set(MODE_FILE_WRONG ${CMAKE_CURRENT_SOURCE_DIR}/test/test_modes_wrong.yaml)
   configure_file(test/modefiles.h.in ${CMAKE_CURRENT_BINARY_DIR}/system_modes/modefiles.h)
   include_directories(BEFORE ${CMAKE_CURRENT_BINARY_DIR})
@@ -147,12 +148,44 @@ if(BUILD_TESTING)
     target_link_libraries(test_mode_inference mode)
   endif()
 
+  ament_add_gtest(test_mode_handling test/test_mode_handling.cpp)
+  if(TARGET test_mode_handling)
+    target_include_directories(test_mode_handling PUBLIC
+      ${rclcpp_INCLUDE_DIRS}
+      ${CMAKE_CURRENT_BINARY_DIR}/system_modes/
+    )
+    target_link_libraries(test_mode_handling mode)
+  endif()
+
+  ament_add_gtest(test_mode_manager
+    test/test_mode_manager.cpp
+    src/system_modes/mode_manager.cpp)
+  if(TARGET test_mode_manager)
+    target_include_directories(test_mode_manager PUBLIC
+      ${rclcpp_INCLUDE_DIRS}
+      ${CMAKE_CURRENT_BINARY_DIR}/system_modes/
+    )
+    target_link_libraries(test_mode_manager mode)
+  endif()
+
+  ament_add_gtest(test_mode_monitor
+    test/test_mode_monitor.cpp
+    src/system_modes/mode_monitor.cpp)
+  if(TARGET test_mode_monitor)
+    target_include_directories(test_mode_monitor PUBLIC
+      ${rclcpp_INCLUDE_DIRS}
+      ${CMAKE_CURRENT_BINARY_DIR}/system_modes/
+    )
+    target_link_libraries(test_mode_monitor mode)
+  endif()
+
   # 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
+    "two_independent_hierarchies"   # Mode Manager for two independent hierarchies of nodes
+    "manager_and_monitor")          # Mode Manager and Mode Monitor
 
   # Launch Test: Mode Manager with Lifecycle Nodes
   foreach(test_name ${launch_tests})

system_modes/src/mode_manager_node.cpp

@@ -47,7 +47,6 @@ using lifecycle_msgs::msg::TransitionEvent;
 using rcl_interfaces::msg::ParameterType;
 using rcl_interfaces::msg::ParameterEvent;
 
-string modelfile, loglevel;
 shared_ptr<ModeManager> manager;
 
 void transition_callback(

system_modes/src/mode_monitor_node.cpp

@@ -50,11 +50,6 @@ using lifecycle_msgs::msg::TransitionEvent;
 using rcl_interfaces::msg::ParameterType;
 using rcl_interfaces::msg::ParameterEvent;
 
-string modelfile, loglevel;
-bool debug = false;
-unsigned int rate = 1000;
-bool verbose = false;
-
 shared_ptr<system_modes::ModeMonitor> monitor;
 
 void transition_callback(

system_modes/src/system_modes/mode_handling.cpp

@@ -14,18 +14,19 @@
 // limitations under the License.
 #include "system_modes/mode_handling.hpp"
 
+#include <shared_mutex>
+
+#include <lifecycle_msgs/msg/state.hpp>
 #include <rclcpp/parameter.hpp>
 #include <rclcpp/parameter_map.hpp>
-#include <lifecycle_msgs/msg/state.hpp>
 #include <rcl_yaml_param_parser/parser.h>
 
 #include <map>
+#include <memory>
 #include <mutex>
 #include <string>
-#include <vector>
-#include <memory>
 #include <utility>
-#include <shared_mutex>
+#include <vector>
 
 using std::endl;
 using std::pair;
@@ -135,8 +136,7 @@ ModeHandling::get_rules_for(const std::string & system, const StateAndMode & tar
 {
   std::vector<ModeRule> rules;
   try {
-    auto rulesmap = this->rules_[system];
-    for (auto rule : rulesmap) {
+    for (auto rule : this->rules_[system]) {
       if (target == rule.second.system_target) {
         rules.push_back(rule.second);
       }

system_modes/src/system_modes/mode_impl.cpp

@@ -14,14 +14,14 @@
 // limitations under the License.
 #include "system_modes/mode_impl.hpp"
 
+#include <lifecycle_msgs/msg/state.hpp>
+#include <lifecycle_msgs/msg/transition.hpp>
+
+#include <exception>
 #include <map>
 #include <string>
-#include <vector>
 #include <utility>
-#include <exception>
-
-#include <lifecycle_msgs/msg/state.hpp>
-#include <lifecycle_msgs/msg/transition.hpp>
+#include <vector>
 
 using std::map;
 using std::pair;

system_modes/src/system_modes/mode_inference.cpp

@@ -14,18 +14,19 @@
 // limitations under the License.
 #include "system_modes/mode_inference.hpp"
 
+#include <shared_mutex>
+
+#include <lifecycle_msgs/msg/state.hpp>
 #include <rclcpp/parameter.hpp>
 #include <rclcpp/parameter_map.hpp>
-#include <lifecycle_msgs/msg/state.hpp>
 #include <rcl_yaml_param_parser/parser.h>
 
 #include <map>
+#include <memory>
 #include <mutex>
 #include <string>
-#include <vector>
-#include <memory>
 #include <utility>
-#include <shared_mutex>
+#include <vector>
 
 using std::endl;
 using std::mutex;

system_modes/src/system_modes/mode_monitor.cpp

@@ -20,9 +20,9 @@
 #include <lifecycle_msgs/srv/change_state.hpp>
 #include <lifecycle_msgs/srv/get_state.hpp>
 
-#include <rcl_yaml_param_parser/parser.h>
 #include <rclcpp/parameter_map.hpp>
 #include <rcl_interfaces/srv/list_parameters.hpp>
+#include <rcl_yaml_param_parser/parser.h>
 
 #include <string>
 #include <memory>

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

@@ -0,0 +1,80 @@
+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@'
+
+    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)
+
+    mode_monitor = ExecuteProcess(
+        cmd=[
+            "ros2",
+            "run",
+            "system_modes",
+            "mode_monitor",
+            "--ros-args",
+            "-p",
+            "modelfile:=" + modelfile,
+            "-p",
+            "debug:=True",
+            "-p",
+            "verbose:=True",
+            "-p",
+            "rate:=200"],
+        name='mode_monitor',
+        emulate_tty=True,
+        output='screen')
+
+    launch_description = LaunchDescription()
+    launch_description.add_action(mode_manager)
+    launch_description.add_action(mode_monitor)
+    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, mode_monitor):
+        """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=mode_monitor,
+            output_filter=output_filter,
+            timeout=15,
+            stream='stdout')
+
+        import time
+        time.sleep(1)