From d87ab20417dbf9400bcbf9f012fe4e4b479f8dff Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 5 Jun 2023 15:49:47 +0200 Subject: [PATCH 1/3] Bump uesteibar/reviewer-lottery from 2 to 3 (#1048) Bumps [uesteibar/reviewer-lottery](https://github.com/uesteibar/reviewer-lottery) from 2 to 3. - [Release notes](https://github.com/uesteibar/reviewer-lottery/releases) - [Commits](https://github.com/uesteibar/reviewer-lottery/compare/v2...v3) --- updated-dependencies: - dependency-name: uesteibar/reviewer-lottery dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/reviewer_lottery.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 94f3d9bde6..772809825f 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -8,6 +8,6 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: uesteibar/reviewer-lottery@v2 + - uses: uesteibar/reviewer-lottery@v3 with: repo-token: ${{ secrets.GITHUB_TOKEN }} From 50ad772ab2f57225a37ab2df3adf1baa384ca8a8 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 5 Jun 2023 15:50:10 +0200 Subject: [PATCH 2/3] Bump ros-tooling/action-ros-ci from 0.3.0 to 0.3.2 (#1047) Bumps [ros-tooling/action-ros-ci](https://github.com/ros-tooling/action-ros-ci) from 0.3.0 to 0.3.2. - [Release notes](https://github.com/ros-tooling/action-ros-ci/releases) - [Commits](https://github.com/ros-tooling/action-ros-ci/compare/0.3.0...0.3.2) --- updated-dependencies: - dependency-name: ros-tooling/action-ros-ci dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 1df5175f10..97f5b4373b 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -20,7 +20,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.3.0 + - uses: ros-tooling/action-ros-ci@0.3.2 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 46b998c326..faadc4e9d9 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,7 +32,7 @@ jobs: - uses: actions/checkout@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.0 + - uses: ros-tooling/action-ros-ci@0.3.2 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package From d299208e8e2c6b927adbf27391f68aced974892c Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 7 Jun 2023 23:12:26 +0200 Subject: [PATCH 3/3] =?UTF-8?q?[CM]=20Use=20`robot=5Fdescription`=20topic?= =?UTF-8?q?=20instead=20of=20parameter=20and=20don't=20crash=20on=20empty?= =?UTF-8?q?=20URDF=20=F0=9F=A6=BF=20(#940)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * on startup wait for robot description, however allow receiving later --------- Co-authored-by: Dr. Denis --- controller_manager/CMakeLists.txt | 11 +++ .../controller_manager/controller_manager.hpp | 8 ++ controller_manager/package.xml | 1 + controller_manager/src/controller_manager.cpp | 58 +++++++++++- .../test/controller_manager_test_common.hpp | 57 +++++++++--- .../test_controller_manager_urdf_passing.cpp | 88 +++++++++++++++++++ .../doc/mock_components_userdoc.rst | 2 +- .../hardware_interface/resource_manager.hpp | 12 +++ hardware_interface/src/resource_manager.cpp | 3 + .../test/test_resource_manager.cpp | 28 ++++++ 10 files changed, 253 insertions(+), 15 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 1d9b062efa..1118856e48 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp realtime_tools + std_msgs ) find_package(ament_cmake REQUIRED) @@ -143,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/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 582cbc5f24..631d5173aa 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -51,6 +51,8 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" namespace controller_manager { @@ -82,6 +84,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC virtual ~ControllerManager() = default; + CONTROLLER_MANAGER_PUBLIC + void robot_description_callback(const std_msgs::msg::String & msg); + CONTROLLER_MANAGER_PUBLIC void init_resource_manager(const std::string & robot_description); @@ -316,6 +321,7 @@ class ControllerManager : public rclcpp::Node std::vector get_controller_names(); std::pair split_command_interface( const std::string & command_interface); + void subscribe_to_robot_description_topic(); /** * Clear request lists used when switching controllers. The lists are shared between "callback" @@ -504,6 +510,8 @@ class ControllerManager : public rclcpp::Node std::vector activate_command_interface_request_, deactivate_command_interface_request_; + rclcpp::Subscription::SharedPtr robot_description_subscription_; + struct SwitchParams { bool do_switch = {false}; diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 246bf5f9d4..c9dd319577 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -26,6 +26,7 @@ ros2_control_test_assets ros2param ros2run + std_msgs ament_cmake_gmock ros2_control_test_assets diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6b6efb0a5c..69a3bbb5f3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -151,13 +151,20 @@ ControllerManager::ControllerManager( } std::string robot_description = ""; + // TODO(destogl): remove support at the end of 2023 get_parameter("robot_description", robot_description); if (robot_description.empty()) { - throw std::runtime_error("Unable to initialize resource manager, no robot description found."); + subscribe_to_robot_description_topic(); + } + else + { + RCLCPP_WARN( + get_logger(), + "[Deprecated] Passing the robot description parameter directly to the control_manager node " + "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead."); + init_resource_manager(robot_description); } - - init_resource_manager(robot_description); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( @@ -183,12 +190,55 @@ ControllerManager::ControllerManager( { RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } + + subscribe_to_robot_description_topic(); + diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); } +void ControllerManager::subscribe_to_robot_description_topic() +{ + // set QoS to transient local to get messages that have already been published + // (if robot state publisher starts before controller manager) + RCLCPP_INFO( + get_logger(), "Subscribing to '~/robot_description' topic for robot description file."); + robot_description_subscription_ = create_subscription( + "~/robot_description", rclcpp::QoS(1).transient_local(), + std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); +} + +void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) +{ + RCLCPP_INFO(get_logger(), "Received robot description file."); + RCLCPP_DEBUG( + get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); + // TODO(Manuel): errors should probably be caught since we don't want controller_manager node + // to die if a non valid urdf is passed. However, should maybe be fine tuned. + try + { + if (resource_manager_->is_urdf_already_loaded()) + { + RCLCPP_WARN( + get_logger(), + "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " + "description file."); + return; + } + init_resource_manager(robot_description.data.c_str()); + } + catch (std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + get_logger(), + "The published robot description file (urdf) seems not to be genuine. The following error " + "was caught:" + << e.what()); + } +} + void ControllerManager::init_resource_manager(const std::string & robot_description) { // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong... @@ -199,7 +249,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript std::vector configure_components_on_start = std::vector({}); if (get_parameter("configure_components_on_start", configure_components_on_start)) { - RCLCPP_WARN_STREAM( + RCLCPP_WARN( get_logger(), "[Deprecated]: Usage of parameter \"configure_components_on_start\" is deprecated. Use " "hardware_spawner instead."); diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index b6e6acac0e..78c3fcb06b 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -22,15 +22,20 @@ #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" #include "rclcpp/utilities.hpp" +#include "std_msgs/msg/string.hpp" + #include "ros2_control_test_assets/descriptions.hpp" #include "test_controller_failed_init/test_controller_failed_init.hpp" @@ -60,21 +65,51 @@ template class ControllerManagerFixture : public ::testing::Test { public: + explicit ControllerManagerFixture( + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, + const bool & pass_urdf_as_parameter = false) + : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) + { + executor_ = std::make_shared(); + // 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( + std::make_unique(), executor_, TEST_CM_NAME); + } + else + { + // can be removed later, needed if we want to have the deprecated way of passing the robot + // description file to the controller manager covered by tests + if (pass_urdf_as_parameter_) + { + cm_ = std::make_shared( + std::make_unique(robot_description_, true, true), + executor_, TEST_CM_NAME); + } + else + { + // TODO(Manuel) : passing via topic not working in test setup, tested cm does + // not receive msg. Have to check this... + + // this is just a workaround to skip passing + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + // mimic topic call + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); + } + } + } + static void SetUpTestCase() { rclcpp::init(0, nullptr); } static void TearDownTestCase() { rclcpp::shutdown(); } - void SetUp() - { - executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf, true, true), - executor_, TEST_CM_NAME); - run_updater_ = false; - } + void SetUp() override { run_updater_ = false; } - void TearDown() { stopCmUpdater(); } + void TearDown() override { stopCmUpdater(); } void startCmUpdater() { @@ -120,6 +155,8 @@ class ControllerManagerFixture : public ::testing::Test std::thread updater_; bool run_updater_; + const std::string robot_description_; + const bool pass_urdf_as_parameter_; }; 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..102cbdfbd4 --- /dev/null +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -0,0 +1,88 @@ +// 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" + +#include "ros2_control_test_assets/descriptions.hpp" + +class TestControllerManagerWithTestableCM; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerManagerWithTestableCM; + + FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + +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 +{ +public: + // create cm with no urdf + TestControllerManagerWithTestableCM() + : ControllerManagerFixture("", false) + { + } +}; + +TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) +{ + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); +} + +TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) +{ + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + // mimic callback + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_urdf; + cm_->robot_description_callback(msg); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); +} + +TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed) +{ + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + // mimic callback + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf; + cm_->robot_description_callback(msg); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); +} + +INSTANTIATE_TEST_SUITE_P( + test_best_effort, TestControllerManagerWithTestableCM, testing::Values(best_effort)); diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index f143ea816c..573037a58d 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -28,7 +28,7 @@ Parameters ,,,,,,,,,, disable_commands (optional; boolean; default: false) - Disables mirroring commands to states. + Disables mirroring commands to states. This option is helpful to simulate an erroneous connection to the hardware when nothing breaks, but suddenly there is no feedback from a hardware interface. Or it can help you to test your setup when the hardware is running without feedback, i.e., in open loop configuration. diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 4d2f995756..4ea3ae9a5f 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -90,6 +90,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void load_urdf(const std::string & urdf, bool validate_interfaces = true); + /** + * @brief if the resource manager load_urdf(...) function has been called this returns true. + * We want to permit to load the urdf later on but we currently don't want to permit multiple + * calls to load_urdf (reloading/loading different urdf). + * + * @return true if resource manager's load_urdf() has been already called. + * @return false if resource manager's load_urdf() has not been yet called. + */ + bool is_urdf_already_loaded() const; + /// Claim a state interface given its key. /** * The resource is claimed as long as being in scope. @@ -413,6 +423,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; + + bool is_urdf_loaded__ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2e23c34d94..f32d24f890 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -703,6 +703,7 @@ ResourceManager::ResourceManager( // CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { + is_urdf_loaded__ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; @@ -741,6 +742,8 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac resource_storage_->systems_.size()); } +bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } + // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) { diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index e1b9e240e7..aedf2b862d 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -212,6 +212,34 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) } } +TEST_F(ResourceManagerTest, no_load_urdf_function_called) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.is_urdf_already_loaded()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) +{ + TestableResourceManager rm; + EXPECT_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); + ASSERT_TRUE(rm.is_urdf_already_loaded()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid) +{ + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.is_urdf_already_loaded()); +} + +TEST_F(ResourceManagerTest, can_load_urdf_later) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.is_urdf_already_loaded()); + rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.is_urdf_already_loaded()); +} + TEST_F(ResourceManagerTest, resource_claiming) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf);