Tests for hardware spawner.

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

@@ -41,35 +41,10 @@ class bcolors:
     UNDERLINE = "\033[4m"
 
 
-def first_match(iterable, predicate):
-    return next((n for n in iterable if predicate(n)), None)
-
-
-def combine_name_and_namespace(name_and_namespace):
-    node_name, namespace = name_and_namespace
-    return namespace + ("" if namespace.endswith("/") else "/") + node_name
-
-
-def find_node_and_namespace(node, full_node_name):
-    node_names_and_namespaces = node.get_node_names_and_namespaces()
-    return first_match(
-        node_names_and_namespaces,
-        lambda n: combine_name_and_namespace(n) == full_node_name,
-    )
-
-
-def has_service_names(node, node_name, node_namespace, service_names):
-    client_names_and_types = node.get_service_names_and_types_by_node(node_name, node_namespace)
-    if not client_names_and_types:
-        return False
-    client_names, _ = zip(*client_names_and_types)
-    return all(service in client_names for service in 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)
 
 
@@ -117,11 +92,12 @@ def configure_components(node, controller_manager_name, components_to_configure)
 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.",
+        nargs="+"
     )
     parser.add_argument(
         "-c",
@@ -138,13 +114,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",
@@ -153,12 +129,15 @@ def main(args=None):
 
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
+    hardware_component = args.hardware_component_name
     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
 
+    print(f"CMD Arguments: {command_line_args}")
+    print(f"Arguments: {args}")
+
     node = Node("hardware_spawner")
     if not controller_manager_name.startswith("/"):
         spawner_namespace = node.get_namespace()

controller_manager/test/test_hardware_spawner.cpp

@@ -0,0 +1,210 @@
+// 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 = "ros2 run 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", "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"), 1)
+    << "could not activate control because not robot description";
+}
+
+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);
+}