Fix Hardware spawner and add tests for it

controller_manager/CMakeLists.txt

@@ -193,6 +193,15 @@ if(BUILD_TESTING)
     ros2_control_test_assets::ros2_control_test_assets
   )
 
+  ament_add_gmock(test_hardware_spawner
+    test/test_hardware_spawner
+    TIMEOUT 120
+  )
+  target_link_libraries(test_hardware_spawner
+    controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
   install(FILES test/test_controller_spawner_with_fallback_controllers.yaml
     DESTINATION test)
 

controller_manager/controller_manager/hardware_spawner.py

@@ -57,7 +57,7 @@ def has_service_names(node, node_name, node_namespace, service_names):
 def is_hardware_component_loaded(
     node, controller_manager, hardware_component, service_timeout=0.0
 ):
-    components = list_hardware_components(node, hardware_component, service_timeout).component
+    components = list_hardware_components(node, controller_manager, service_timeout).component
     return any(c.name == hardware_component for c in components)
 
 
@@ -82,34 +82,33 @@ def handle_set_component_state_service_call(
         )
 
 
-def activate_components(node, controller_manager_name, components_to_activate):
+def activate_component(node, controller_manager_name, component_to_activate):
     active_state = State()
     active_state.id = State.PRIMARY_STATE_ACTIVE
     active_state.label = "active"
-    for component in components_to_activate:
-        handle_set_component_state_service_call(
-            node, controller_manager_name, component, active_state, "activated"
-        )
+    handle_set_component_state_service_call(
+        node, controller_manager_name, component_to_activate, active_state, "activated"
+    )
 
 
-def configure_components(node, controller_manager_name, components_to_configure):
+def configure_component(node, controller_manager_name, component_to_configure):
     inactive_state = State()
     inactive_state.id = State.PRIMARY_STATE_INACTIVE
     inactive_state.label = "inactive"
-    for component in components_to_configure:
-        handle_set_component_state_service_call(
-            node, controller_manager_name, component, inactive_state, "configured"
-        )
+    handle_set_component_state_service_call(
+        node, controller_manager_name, component_to_configure, inactive_state, "configured"
+    )
 
 
 def main(args=None):
     rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
     parser = argparse.ArgumentParser()
-    activate_or_confiigure_grp = parser.add_mutually_exclusive_group(required=True)
+    activate_or_configure_grp = parser.add_mutually_exclusive_group(required=True)
 
     parser.add_argument(
-        "hardware_component_name",
-        help="The name of the hardware component which should be activated.",
+        "hardware_component_names",
+        help="The name of the hardware components which should be activated.",
+        nargs="+",
     )
     parser.add_argument(
         "-c",
@@ -126,13 +125,13 @@ def main(args=None):
         type=float,
     )
     # add arguments which are mutually exclusive
-    activate_or_confiigure_grp.add_argument(
+    activate_or_configure_grp.add_argument(
         "--activate",
         help="Activates the given components. Note: Components are by default configured before activated. ",
         action="store_true",
         required=False,
     )
-    activate_or_confiigure_grp.add_argument(
+    activate_or_configure_grp.add_argument(
         "--configure",
         help="Configures the given components.",
         action="store_true",
@@ -141,9 +140,9 @@ def main(args=None):
 
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
+    hardware_components = args.hardware_component_names
     controller_manager_name = args.controller_manager
     controller_manager_timeout = args.controller_manager_timeout
-    hardware_component = [args.hardware_component_name]
     activate = args.activate
     configure = args.configure
 
@@ -156,24 +155,25 @@ def main(args=None):
             controller_manager_name = f"/{controller_manager_name}"
 
     try:
-        if not is_hardware_component_loaded(
-            node, controller_manager_name, hardware_component, controller_manager_timeout
-        ):
-            node.get_logger().warn(
-                bcolors.WARNING
-                + "Hardware Component is not loaded - state can not be changed."
-                + bcolors.ENDC
-            )
-        elif activate:
-            activate_components(node, controller_manager_name, hardware_component)
-        elif configure:
-            configure_components(node, controller_manager_name, hardware_component)
-        else:
-            node.get_logger().error(
-                'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag'
-            )
-            parser.print_help()
-            return 0
+        for hardware_component in hardware_components:
+            if not is_hardware_component_loaded(
+                node, controller_manager_name, hardware_component, controller_manager_timeout
+            ):
+                node.get_logger().warn(
+                    bcolors.WARNING
+                    + "Hardware Component is not loaded - state can not be changed."
+                    + bcolors.ENDC
+                )
+            elif activate:
+                activate_component(node, controller_manager_name, hardware_component)
+            elif configure:
+                configure_component(node, controller_manager_name, hardware_component)
+            else:
+                node.get_logger().error(
+                    'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag'
+                )
+                parser.print_help()
+                return 0
     except KeyboardInterrupt:
         pass
     except ServiceNotFoundError as err:

controller_manager/test/test_hardware_spawner.cpp

@@ -0,0 +1,227 @@
+// Copyright 2021 PAL Robotics S.L.
+//
+// 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 <gtest/gtest.h>
+
+#include <cstdlib>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_manager/controller_manager.hpp"
+#include "controller_manager_test_common.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "test_chainable_controller/test_chainable_controller.hpp"
+#include "test_controller/test_controller.hpp"
+
+using ::testing::_;
+using ::testing::Return;
+
+using namespace std::chrono_literals;
+class TestHardwareSpawner : public ControllerManagerFixture<controller_manager::ControllerManager>
+{
+public:
+  TestHardwareSpawner() : ControllerManagerFixture<controller_manager::ControllerManager>()
+  {
+    cm_->set_parameter(
+      rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware"));
+  }
+
+  void SetUp() override
+  {
+    ControllerManagerFixture::SetUp();
+
+    update_executor_ =
+      std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::ExecutorOptions(), 2);
+
+    update_executor_->add_node(cm_);
+    update_executor_spin_future_ =
+      std::async(std::launch::async, [this]() -> void { update_executor_->spin(); });
+    // This sleep is needed to prevent a too fast test from ending before the
+    // executor has began to spin, which causes it to hang
+    std::this_thread::sleep_for(50ms);
+  }
+
+  void TearDown() override { update_executor_->cancel(); }
+
+protected:
+  // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
+  std::shared_ptr<rclcpp::Executor> update_executor_;
+  std::future<void> update_executor_spin_future_;
+};
+
+int call_spawner(const std::string extra_args)
+{
+  std::string spawner_script =
+    "python3 -m coverage run --append --branch $(ros2 pkg prefix "
+    "controller_manager)/lib/controller_manager/hardware_spawner ";
+  return std::system((spawner_script + extra_args).c_str());
+}
+
+TEST_F(TestHardwareSpawner, spawner_with_no_arguments_errors)
+{
+  EXPECT_NE(call_spawner(""), 0) << "Missing mandatory arguments";
+}
+
+TEST_F(TestHardwareSpawner, spawner_without_manager_errors_with_given_timeout)
+{
+  EXPECT_NE(call_spawner("TestSystemHardware --controller-manager-timeout 1.0"), 0)
+    << "Wrong controller manager name";
+}
+
+TEST_F(TestHardwareSpawner, spawner_without_component_name_argument)
+{
+  EXPECT_NE(call_spawner("-c test_controller_manager"), 0)
+    << "Missing component name argument parameter";
+}
+
+TEST_F(TestHardwareSpawner, spawner_non_exising_hardware_component)
+{
+  EXPECT_NE(call_spawner("TestSystemHardware1 -c test_controller_manager"), 0)
+    << "Missing component name argument parameter";
+}
+
+TEST_F(TestHardwareSpawner, set_component_to_configured_state_and_back_to_activated)
+{
+  EXPECT_EQ(call_spawner("TestSystemHardware --configure -c test_controller_manager"), 0);
+
+  EXPECT_EQ(call_spawner("TestSystemHardware --activate -c test_controller_manager"), 0);
+}
+
+class TestHardwareSpawnerWithoutRobotDescription
+: public ControllerManagerFixture<controller_manager::ControllerManager>
+{
+public:
+  TestHardwareSpawnerWithoutRobotDescription()
+  : ControllerManagerFixture<controller_manager::ControllerManager>("")
+  {
+    cm_->set_parameter(rclcpp::Parameter(
+      "hardware_components_initial_state.unconfigured",
+      std::vector<std::string>{"TestSystemHardware"}));
+  }
+
+public:
+  void SetUp() override
+  {
+    ControllerManagerFixture::SetUp();
+
+    update_timer_ = cm_->create_wall_timer(
+      std::chrono::milliseconds(10),
+      [&]()
+      {
+        cm_->read(time_, PERIOD);
+        cm_->update(time_, PERIOD);
+        cm_->write(time_, PERIOD);
+      });
+
+    update_executor_ =
+      std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::ExecutorOptions(), 2);
+
+    update_executor_->add_node(cm_);
+    update_executor_spin_future_ =
+      std::async(std::launch::async, [this]() -> void { update_executor_->spin(); });
+    // This sleep is needed to prevent a too fast test from ending before the
+    // executor has began to spin, which causes it to hang
+    std::this_thread::sleep_for(50ms);
+  }
+
+  void TearDown() override { update_executor_->cancel(); }
+
+  rclcpp::TimerBase::SharedPtr robot_description_sending_timer_;
+
+protected:
+  rclcpp::TimerBase::SharedPtr update_timer_;
+
+  // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
+  std::shared_ptr<rclcpp::Executor> update_executor_;
+  std::future<void> update_executor_spin_future_;
+};
+
+TEST_F(TestHardwareSpawnerWithoutRobotDescription, when_no_robot_description_spawner_times_out)
+{
+  EXPECT_EQ(
+    call_spawner(
+      "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"),
+    256)
+    << "could not change hardware state because not robot description and not services for "
+       "controller "
+       "manager are active";
+}
+
+TEST_F(TestHardwareSpawnerWithoutRobotDescription, spawner_with_later_load_of_robot_description)
+{
+  // Delay sending robot description
+  robot_description_sending_timer_ = cm_->create_wall_timer(
+    std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); });
+
+  EXPECT_EQ(
+    call_spawner(
+      "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"),
+    256)
+    << "could not activate control because not robot description";
+  EXPECT_EQ(
+    call_spawner(
+      "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 2.5"),
+    0);
+}
+
+class TestHardwareSpawnerWithNamespacedCM
+: public ControllerManagerFixture<controller_manager::ControllerManager>
+{
+public:
+  TestHardwareSpawnerWithNamespacedCM()
+  : ControllerManagerFixture<controller_manager::ControllerManager>(
+      ros2_control_test_assets::minimal_robot_urdf, "foo_namespace")
+  {
+    cm_->set_parameter(
+      rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware"));
+  }
+
+public:
+  void SetUp() override
+  {
+    ControllerManagerFixture::SetUp();
+
+    update_executor_ =
+      std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::ExecutorOptions(), 2);
+
+    update_executor_->add_node(cm_);
+    update_executor_spin_future_ =
+      std::async(std::launch::async, [this]() -> void { update_executor_->spin(); });
+    // This sleep is needed to prevent a too fast test from ending before the
+    // executor has began to spin, which causes it to hang
+    std::this_thread::sleep_for(50ms);
+  }
+
+  void TearDown() override { update_executor_->cancel(); }
+
+protected:
+  // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
+  std::shared_ptr<rclcpp::Executor> update_executor_;
+  std::future<void> update_executor_spin_future_;
+};
+
+TEST_F(TestHardwareSpawnerWithNamespacedCM, set_component_to_configured_state_cm_namespace)
+{
+  ControllerManagerRunner cm_runner(this);
+  EXPECT_EQ(
+    call_spawner(
+      "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"),
+    256)
+    << "Should fail without defining the namespace";
+  EXPECT_EQ(
+    call_spawner("TestSystemHardware --configure -c test_controller_manager --ros-args -r "
+                 "__ns:=/foo_namespace"),
+    0);
+}