From c338a94ce06f90d66a627feee6f90aac70f0b3b9 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 14 Apr 2023 09:03:41 +0200 Subject: [PATCH] Not working! tried waiting for urdf to arrive before starting controllers 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 | 10 +++ .../test/controller_manager_test_common.hpp | 36 ++++++++--- .../test_controller_manager_urdf_passing.cpp | 63 +++++++++++++++++++ 3 files changed, 102 insertions(+), 7 deletions(-) create mode 100644 controller_manager/test/test_controller_manager_urdf_passing.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index ef2f2e3af9..3d38403332 100644 --- a/controller_manager/CMakeLists.txt +++ b/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 diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 6d1b72698f..7514f6db30 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -22,11 +22,13 @@ #include #include #include +#include #include #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 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(); - // 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( @@ -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("robot_description_publisher", ns_); description_pub_ = urdf_publisher_node_->create_publisher( "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( std::make_unique(), 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("service_caller_node", ns_); + executor_->add_node(service_caller_node_); + auto client = + service_caller_node_->create_client( + "get_hw_interfaces"); + auto request = + std::make_shared(); + 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 urdf_publisher_node_; rclcpp::Publisher::SharedPtr description_pub_; + std::shared_ptr service_caller_node_; }; class TestControllerManagerSrvs diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp new file mode 100644 index 0000000000..a4327e4c8a --- /dev/null +++ b/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 +#include +#include +#include +#include + +#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 resource_manager, + std::shared_ptr 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, + public testing::WithParamInterface +{ +}; + +// 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));