Not working! tried waiting for urdf to arrive before starting control…

…lers in test

In tests we have to be sure, that our urdf arrived before continuing
with the actuell tests otherwise they will naturally fail...

controller_manager/CMakeLists.txt

@@ -144,6 +144,16 @@ if(BUILD_TESTING)
   ament_target_dependencies(test_controller_manager_srvs
     controller_manager_msgs
   )
+  ament_add_gmock(test_controller_manager_urdf_passing
+  test/test_controller_manager_urdf_passing.cpp
+  )
+  target_link_libraries(test_controller_manager_urdf_passing
+    controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+  ament_target_dependencies(test_controller_manager_urdf_passing
+    controller_manager_msgs
+  )
 
   add_library(test_controller_with_interfaces SHARED
     test/test_controller_with_interfaces/test_controller_with_interfaces.cpp

controller_manager/test/controller_manager_test_common.hpp

@@ -22,11 +22,13 @@
 #include <memory>
 #include <string>
 #include <thread>
+#include <utility>
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
 
 #include "controller_manager/controller_manager.hpp"
+#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
 #include "controller_manager_msgs/srv/switch_controller.hpp"
 
 #include "rclcpp/rclcpp.hpp"
@@ -63,14 +65,14 @@ template <typename CtrlMgr>
 class ControllerManagerFixture : public ::testing::Test
 {
 public:
-  //TODO parameter hinzufügen, welche hw erwartet wird
+  // TODO(Manuel): Maybe add parameter of which hardware is to be expected
   explicit ControllerManagerFixture(
     const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf,
     const std::string ns = "/", const bool & pass_urdf_as_parameter = false)
   : robot_description_(robot_description), ns_(ns), pass_urdf_as_parameter_(pass_urdf_as_parameter)
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    // We want ot be able to create a ResourceManager where no urdf file has been passed to
+    // We want to be able to create a ResourceManager where no urdf file has been passed to
     if (robot_description_.empty())
     {
       cm_ = std::make_shared<CtrlMgr>(
@@ -88,18 +90,37 @@ class ControllerManagerFixture : public ::testing::Test
       }
       else
       {
+        // First wie create a node and a publisher for publishing the passed robot description file
         urdf_publisher_node_ = std::make_shared<rclcpp::Node>("robot_description_publisher", ns_);
         description_pub_ = urdf_publisher_node_->create_publisher<std_msgs::msg::String>(
           "robot_description", rclcpp::QoS(1).transient_local());
         executor_->add_node(urdf_publisher_node_);
         publish_robot_description_file(robot_description_);
-
+        // Then we create controller manager which subscribes to topic and receive
+        // published robot description file. Publishing is transient_local so starting cm
+        // later should not pose problem and is closer to real world applications
         cm_ = std::make_shared<CtrlMgr>(
           std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
-
-        //TODO warten bis hw da ist und initialisiert
-        // von urdf wissen wir was da sein soll
-        // list hw
+        executor_->add_node(cm_);
+        // Now we have to wait for cm to process callback and initialize everything.
+        // We have to wait here, otherwise controllers can not be initialized since
+        // no hardware has been received.
+        service_caller_node_ = std::make_shared<rclcpp::Node>("service_caller_node", ns_);
+        executor_->add_node(service_caller_node_);
+        auto client =
+          service_caller_node_->create_client<controller_manager_msgs::srv::ListHardwareInterfaces>(
+            "get_hw_interfaces");
+        auto request =
+          std::make_shared<controller_manager_msgs::srv::ListHardwareInterfaces::Request>();
+        EXPECT_TRUE(client->wait_for_service(std::chrono::milliseconds(500)));
+        auto future = client->async_send_request(request);
+        EXPECT_EQ(
+          executor_->spin_until_future_complete(future, std::chrono::milliseconds(1000)),
+          rclcpp::FutureReturnCode::SUCCESS);
+        auto res = future.get();
+        auto command_interfaces = res->command_interfaces;
+        auto state_interfaces = res->state_interfaces;
+        // check for command-/stateinterfaces but spin_until_future_complete times out...
       }
     }
   }
@@ -168,6 +189,7 @@ class ControllerManagerFixture : public ::testing::Test
   const bool pass_urdf_as_parameter_;
   std::shared_ptr<rclcpp::Node> urdf_publisher_node_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr description_pub_;
+  std::shared_ptr<rclcpp::Node> service_caller_node_;
 };
 
 class TestControllerManagerSrvs

controller_manager/test/test_controller_manager_urdf_passing.cpp

@@ -0,0 +1,63 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// 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 <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "controller_manager/controller_manager.hpp"
+#include "controller_manager_test_common.hpp"
+
+class TestControllerManagerWithTestableCM;
+
+class TestableControllerManager : public controller_manager::ControllerManager
+{
+  friend TestControllerManagerWithTestableCM;
+
+  FRIEND_TEST(TestControllerManagerWithTestableCM, callback_gets_passed);
+  FRIEND_TEST(TestControllerManagerWithTestableCM, initial_failing);
+
+public:
+  TestableControllerManager(
+    std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
+    std::shared_ptr<rclcpp::Executor> executor,
+    const std::string & manager_node_name = "controller_manager",
+    const std::string & namespace_ = "")
+  : controller_manager::ControllerManager(
+      std::move(resource_manager), executor, manager_node_name, namespace_)
+  {
+  }
+};
+
+class TestControllerManagerWithTestableCM
+: public ControllerManagerFixture<TestableControllerManager>,
+  public testing::WithParamInterface<Strictness>
+{
+};
+
+// only exemplary to test if working not a useful test yet
+TEST_P(TestControllerManagerWithTestableCM, callback_gets_passed)
+{
+  ASSERT_FALSE(cm_->resource_manager_->load_urdf_called());
+}
+
+TEST_P(TestControllerManagerWithTestableCM, initial_failing)
+{
+  ASSERT_TRUE(cm_->resource_manager_->load_urdf_called());
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  test_best_effort, TestControllerManagerWithTestableCM, testing::Values(best_effort));