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));