diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index c4f9c56800..c8a69a2d50 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* move critical variables to the private context (`#1623 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + 4.13.0 (2024-07-08) ------------------- * [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 17f39c8478..43fd269803 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -16,7 +16,6 @@ #define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_HPP_ #include -#include #include #include "controller_interface/controller_interface_base.hpp" diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 1b5fd2e059..9eaee9f15b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -25,7 +25,6 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp/version.h" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -258,12 +257,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy protected: std::vector command_interfaces_; std::vector state_interfaces_; - unsigned int update_rate_ = 0; - bool is_async_ = false; - std::string urdf_ = ""; private: std::shared_ptr node_; + unsigned int update_rate_ = 0; + bool is_async_ = false; + std::string urdf_ = ""; }; using ControllerInterfaceBaseSharedPtr = std::shared_ptr; diff --git a/controller_interface/include/semantic_components/range_sensor.hpp b/controller_interface/include/semantic_components/range_sensor.hpp index baa9022c75..e4af8b7f86 100644 --- a/controller_interface/include/semantic_components/range_sensor.hpp +++ b/controller_interface/include/semantic_components/range_sensor.hpp @@ -15,7 +15,6 @@ #ifndef SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ #define SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ -#include #include #include diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 1ee83bd019..30137a23e4 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.13.0 + 4.14.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 0f11bba71c..a039501aa1 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -14,14 +14,8 @@ #include "controller_interface/controller_interface.hpp" -#include -#include -#include #include -#include "hardware_interface/types/lifecycle_state_names.hpp" -#include "lifecycle_msgs/msg/state.hpp" - namespace controller_interface { ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 6b87ba5cda..db9fb99d81 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -19,7 +19,6 @@ #include #include -#include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" namespace controller_interface diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index f9684c27fd..a9a581d3bb 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -15,14 +15,13 @@ #ifndef TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#include + #include #include -#include "gmock/gmock.h" - #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" constexpr char TEST_CONTROLLER_NAME[] = "testable_chainable_controller"; constexpr double INTERFACE_VALUE = 1989.0; diff --git a/controller_interface/test/test_controller_interface.hpp b/controller_interface/test/test_controller_interface.hpp index 426890220a..b90be419c7 100644 --- a/controller_interface/test/test_controller_interface.hpp +++ b/controller_interface/test/test_controller_interface.hpp @@ -16,7 +16,6 @@ #define TEST_CONTROLLER_INTERFACE_HPP_ #include "controller_interface/controller_interface.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" constexpr char TEST_CONTROLLER_NAME[] = "testable_controller_interface"; diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index 1e22239215..5829bcbcdd 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -17,7 +17,6 @@ #include #include #include "ament_index_cpp/get_package_prefix.hpp" -#include "rclcpp/rclcpp.hpp" class FriendControllerWithOptions : public controller_with_options::ControllerWithOptions { diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index fc909df361..d16f0fe0d8 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -16,11 +16,9 @@ #define TEST_CONTROLLER_WITH_OPTIONS_HPP_ #include -#include #include #include "controller_interface/controller_interface.hpp" -#include "hardware_interface/types/lifecycle_state_names.hpp" namespace controller_with_options { diff --git a/controller_interface/test/test_force_torque_sensor.hpp b/controller_interface/test/test_force_torque_sensor.hpp index b26517a90c..148d0fbabd 100644 --- a/controller_interface/test/test_force_torque_sensor.hpp +++ b/controller_interface/test/test_force_torque_sensor.hpp @@ -19,12 +19,12 @@ #ifndef TEST_FORCE_TORQUE_SENSOR_HPP_ #define TEST_FORCE_TORQUE_SENSOR_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "semantic_components/force_torque_sensor.hpp" // implementing and friending so we can access member variables diff --git a/controller_interface/test/test_imu_sensor.hpp b/controller_interface/test/test_imu_sensor.hpp index 801a425546..4b73ed7821 100644 --- a/controller_interface/test/test_imu_sensor.hpp +++ b/controller_interface/test/test_imu_sensor.hpp @@ -19,12 +19,12 @@ #ifndef TEST_IMU_SENSOR_HPP_ #define TEST_IMU_SENSOR_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "semantic_components/imu_sensor.hpp" // implementing and friending so we can access member variables diff --git a/controller_interface/test/test_semantic_component_interface.hpp b/controller_interface/test/test_semantic_component_interface.hpp index 1996ddd193..bd00493915 100644 --- a/controller_interface/test/test_semantic_component_interface.hpp +++ b/controller_interface/test/test_semantic_component_interface.hpp @@ -19,11 +19,12 @@ #ifndef TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_ #define TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_ +#include + #include #include #include "geometry_msgs/msg/wrench.hpp" -#include "gmock/gmock.h" #include "semantic_components/semantic_component_interface.hpp" // implementing and friending so we can access member variables diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 1ef5eef03b..397ed8b5ca 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Remove noqa (`#1626 `_) +* Fix controller starting with later load of robot description test (`#1624 `_) +* [CM] Remove support for the description parameter and use only `robot_description` topic (`#1358 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Henry Moore, Sai Kishor Kothakota + 4.13.0 (2024-07-08) ------------------- * Change the spamming checking interface ERROR to DEBUG (`#1605 `_) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index c3c0de7739..7c437d35e0 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -152,6 +152,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_controller_manager_urdf_passing controller_manager + test_controller ros2_control_test_assets::ros2_control_test_assets ) ament_target_dependencies(test_controller_manager_urdf_passing diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 01528552d0..14d0ad68d1 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -30,7 +30,7 @@ def generate_load_controller_launch_description( 'unload_on_kill' LaunchArguments and a Node action that runs the controller_manager spawner node to load and activate a controller - Examples # noqa: D416 + Examples -------- # Assuming the controller type and controller parameters are known to the controller_manager generate_load_controller_launch_description('joint_state_broadcaster') diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 5053eed3e4..02e532866d 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -48,7 +48,7 @@ jitter, using a lowlatency kernel can improve things a lot with being really eas Subscribers ----------- -~/robot_description [std_msgs::msg::String] +robot_description [std_msgs::msg::String] String with the URDF xml, e.g., from ``robot_state_publisher``. Reloading of the URDF is not supported yet. All joints defined in the ````-tag have to be present in the URDF. diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index f83339edc7..2f326ff302 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -42,17 +41,12 @@ #include "controller_manager_msgs/srv/unload_controller.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" -#include "hardware_interface/handle.hpp" #include "hardware_interface/resource_manager.hpp" #include "pluginlib/class_loader.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/node.hpp" -#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 @@ -356,7 +350,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(); + void init_controller_manager(); /** * Clear request lists used when switching controllers. The lists are shared between "callback" @@ -585,6 +579,7 @@ class ControllerManager : public rclcpp::Node std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; + rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; struct SwitchParams { diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index d13e9c56bd..9ce1aab8b6 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -19,11 +19,10 @@ #ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ -#include #include #include #include -#include "controller_interface/controller_interface.hpp" +#include "controller_interface/controller_interface_base.hpp" #include "hardware_interface/controller_info.hpp" namespace controller_manager diff --git a/controller_manager/package.xml b/controller_manager/package.xml index c8da38364a..84a676a483 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.13.0 + 4.14.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fb9bce879a..48dbbc720a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -14,7 +14,6 @@ #include "controller_manager/controller_manager.hpp" -#include #include #include #include @@ -24,7 +23,6 @@ #include "controller_manager_msgs/msg/hardware_component_state.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp/version.h" #include "rclcpp_lifecycle/state.hpp" @@ -200,38 +198,7 @@ ControllerManager::ControllerManager( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { - if (!get_parameter("update_rate", update_rate_)) - { - RCLCPP_WARN( - get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); - } - - robot_description_ = ""; - // TODO(destogl): remove support at the end of 2023 - get_parameter("robot_description", robot_description_); - if (robot_description_.empty()) - { - 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 the 'robot_description' topic from 'robot_state_publisher' instead."); - init_resource_manager(robot_description_); - if (is_resource_manager_initialized()) - { - RCLCPP_WARN( - get_logger(), - "You have to restart the framework when using robot description from parameter!"); - init_services(); - } - } - - diagnostics_updater_.setHardwareID("ros2_control"); - diagnostics_updater_.add( - "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + init_controller_manager(); } ControllerManager::ControllerManager( @@ -251,21 +218,7 @@ ControllerManager::ControllerManager( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { - if (!get_parameter("update_rate", update_rate_)) - { - RCLCPP_WARN( - get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); - } - - if (is_resource_manager_initialized()) - { - init_services(); - } - subscribe_to_robot_description_topic(); - - diagnostics_updater_.setHardwareID("ros2_control"); - diagnostics_updater_.add( - "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + init_controller_manager(); } ControllerManager::ControllerManager( @@ -282,6 +235,12 @@ ControllerManager::ControllerManager( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { + init_controller_manager(); +} + +void ControllerManager::init_controller_manager() +{ + // Get parameters needed for RT "update" loop to work if (!get_parameter("update_rate", update_rate_)) { RCLCPP_WARN( @@ -292,15 +251,17 @@ ControllerManager::ControllerManager( { init_services(); } - subscribe_to_robot_description_topic(); - - diagnostics_updater_.setHardwareID("ros2_control"); - diagnostics_updater_.add( - "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); -} + else + { + robot_description_notification_timer_ = create_wall_timer( + std::chrono::seconds(1), + [&]() + { + RCLCPP_WARN( + get_logger(), "Waiting for data on 'robot_description' topic to finish initialization"); + }); + } -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) robot_description_subscription_ = create_subscription( @@ -309,6 +270,11 @@ void ControllerManager::subscribe_to_robot_description_topic() RCLCPP_INFO( get_logger(), "Subscribing to '%s' topic for robot description.", robot_description_subscription_->get_topic_name()); + + // Setup diagnostics + diagnostics_updater_.setHardwareID("ros2_control"); + diagnostics_updater_.add( + "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); } void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) @@ -321,13 +287,16 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & { RCLCPP_WARN( get_logger(), - "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " - "description file."); + "ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description."); return; } init_resource_manager(robot_description_); if (is_resource_manager_initialized()) { + RCLCPP_INFO( + get_logger(), + "Resource Manager has been successfully initialized. Starting Controller Manager " + "services..."); init_services(); } } @@ -397,6 +366,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); resource_manager_->set_component_state(component, active_state); } + robot_description_notification_timer_->cancel(); } void ControllerManager::init_services() @@ -898,6 +868,15 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) { + if (!is_resource_manager_initialized()) + { + RCLCPP_ERROR( + get_logger(), + "Resource Manager is not initialized yet! Please provide robot description on " + "'robot_description' topic before trying to switch controllers."); + return controller_interface::return_type::ERROR; + } + switch_params_ = SwitchParams(); if (!deactivate_request_.empty() || !activate_request_.empty()) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 6dd7d72fb2..7addaf6cee 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -12,14 +12,12 @@ // 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 "rclcpp/rclcpp.hpp" #include "realtime_tools/thread_priority.hpp" using namespace std::chrono_literals; diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index f8cf9a7c11..72326e8c6c 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -22,22 +22,18 @@ #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" namespace { @@ -65,46 +61,18 @@ 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) + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf) + : robot_description_(robot_description) { 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( + rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()), + executor_, TEST_CM_NAME); + // We want to be able to not pass robot description immediately + if (!robot_description_.empty()) { - cm_ = std::make_shared( - std::make_unique( - rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()), - 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_, rm_node_->get_node_clock_interface(), - rm_node_->get_node_logging_interface(), true, 100), - executor_, TEST_CM_NAME); - } - else - { - // TODO(mamueluth) : 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( - rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()), - executor_, TEST_CM_NAME); - // mimic topic call - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; - cm_->robot_description_callback(msg); - } + pass_robot_description_to_cm_and_rm(robot_description_); } time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); } @@ -140,6 +108,17 @@ class ControllerManagerFixture : public ::testing::Test } } + void pass_robot_description_to_cm_and_rm( + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf) + { + // 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 - mimic topic call + auto msg = std_msgs::msg::String(); + msg.data = robot_description; + cm_->robot_description_callback(msg); + } + void switch_test_controllers( const std::vector & start_controllers, const std::vector & stop_controllers, const int strictness, @@ -162,7 +141,6 @@ class ControllerManagerFixture : public ::testing::Test std::thread updater_; bool run_updater_; const std::string robot_description_; - const bool pass_urdf_as_parameter_; rclcpp::Time time_; protected: diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 625d7ed90f..7e3419fbc9 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -15,7 +15,6 @@ #include "test_controller.hpp" #include -#include #include #include "lifecycle_msgs/msg/state.hpp" diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index bf183c7bad..4e5f827cd0 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -15,7 +15,6 @@ #ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_ #define TEST_CONTROLLER__TEST_CONTROLLER_HPP_ -#include #include #include diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp index 1b2276bf3c..673b9bd8bd 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp @@ -14,11 +14,6 @@ #include "test_controller_failed_init.hpp" -#include -#include - -#include "lifecycle_msgs/msg/transition.hpp" - namespace test_controller_failed_init { TestControllerFailedInit::TestControllerFailedInit() : controller_interface::ControllerInterface() diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp index 3f24df4e29..bd17e0ead4 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp @@ -15,10 +15,7 @@ #ifndef TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_ #define TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_ -#include -#include - -#include "controller_manager/controller_manager.hpp" +#include "controller_interface/controller_interface.hpp" #include "controller_manager/visibility_control.h" namespace test_controller_failed_init diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 3e2f91e91a..5093250dfe 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -15,11 +15,9 @@ #include #include #include -#include #include #include "controller_manager/controller_manager.hpp" -#include "controller_manager_msgs/srv/list_controllers.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "test_controller/test_controller.hpp" diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 4fdfe3fb9f..af04cc5406 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -22,7 +22,6 @@ #include "controller_manager_test_common.hpp" #include "controller_interface/controller_interface.hpp" -#include "controller_manager/controller_manager.hpp" #include "controller_manager_msgs/srv/list_controller_types.hpp" #include "controller_manager_msgs/srv/list_controllers.hpp" #include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 5db4cfc683..1556211a7f 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -20,8 +20,12 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" - #include "ros2_control_test_assets/descriptions.hpp" +#include "test_controller/test_controller.hpp" + +const auto CONTROLLER_NAME = "test_controller"; +using test_controller::TEST_CONTROLLER_CLASS_NAME; +using strvec = std::vector; class TestControllerManagerWithTestableCM; @@ -55,10 +59,46 @@ class TestControllerManagerWithTestableCM { public: // create cm with no urdf - TestControllerManagerWithTestableCM() - : ControllerManagerFixture("", false) + TestControllerManagerWithTestableCM() : ControllerManagerFixture("") {} + + void prepare_controller() { + ASSERT_FALSE(cm_->is_resource_manager_initialized()); + test_controller_ = std::make_shared(); + cm_->add_controller(test_controller_, CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); + ASSERT_NE(test_controller_, nullptr); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller_->get_state().id()); } + + void configure_and_try_switch_that_returns_error() + { + // configure controller + cm_->configure_controller(CONTROLLER_NAME); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::ready, + controller_interface::return_type::ERROR); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + } + + void try_successful_switch() + { + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout, + controller_interface::return_type::OK); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_->get_state().id()); + } + + std::shared_ptr test_controller_; }; TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called) @@ -68,7 +108,9 @@ TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_compo TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback) { - ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); + ASSERT_FALSE(cm_->is_resource_manager_initialized()); + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->is_resource_manager_initialized()); // mimic callback auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_robot_urdf; @@ -80,7 +122,10 @@ TEST_P( TestControllerManagerWithTestableCM, expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description) { - ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); + ASSERT_FALSE(cm_->is_resource_manager_initialized()); + pass_robot_description_to_cm_and_rm( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf); + ASSERT_FALSE(cm_->is_resource_manager_initialized()); // wrong urdf auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_uninitializable_robot_urdf; @@ -93,5 +138,93 @@ TEST_P( ASSERT_TRUE(cm_->resource_manager_->are_components_initialized()); } +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description) +{ + prepare_controller(); + + // setup state interfaces to claim from controllers + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller_->set_state_interface_configuration(state_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->is_resource_manager_initialized()); + + try_successful_switch(); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description) +{ + prepare_controller(); + + // setup command interfaces to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller_->set_command_interface_configuration(cmd_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->is_resource_manager_initialized()); + + try_successful_switch(); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_after_some_time) +{ + prepare_controller(); + + // setup state interfaces to claim from controllers + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller_->set_state_interface_configuration(state_itfs_cfg); + + // setup command interfaces to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller_->set_command_interface_configuration(cmd_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->is_resource_manager_initialized()); + + try_successful_switch(); +} + INSTANTIATE_TEST_SUITE_P( test_best_effort, TestControllerManagerWithTestableCM, testing::Values(best_effort)); diff --git a/controller_manager/test/test_controller_manager_with_namespace.cpp b/controller_manager/test/test_controller_manager_with_namespace.cpp index b83eca0c55..58d22f4eed 100644 --- a/controller_manager/test/test_controller_manager_with_namespace.cpp +++ b/controller_manager/test/test_controller_manager_with_namespace.cpp @@ -15,13 +15,10 @@ #include #include #include -#include #include #include "controller_manager/controller_manager.hpp" -#include "controller_manager_msgs/srv/list_controllers.hpp" #include "controller_manager_test_common.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "test_controller/test_controller.hpp" using ::testing::_; diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp index 86f3c8b76c..d5695e41e5 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp @@ -14,11 +14,6 @@ #include "test_controller_with_interfaces.hpp" -#include -#include - -#include "lifecycle_msgs/msg/transition.hpp" - namespace test_controller_with_interfaces { TestControllerWithInterfaces::TestControllerWithInterfaces() diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp index 96aaed11ce..ed083c17cb 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp @@ -15,11 +15,8 @@ #ifndef TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ #define TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ -#include -#include - -#include "controller_interface/visibility_control.h" -#include "controller_manager/controller_manager.hpp" +#include "controller_interface/controller_interface.hpp" +#include "controller_manager/visibility_control.h" namespace test_controller_with_interfaces { diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 946ffbb4dc..4a81f3bf12 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -19,12 +19,9 @@ #include #include -#include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_manager.hpp" -#include "controller_manager_msgs/srv/list_controllers.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/parameter.hpp" #include "test_chainable_controller/test_chainable_controller.hpp" #include "test_controller/test_controller.hpp" diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index c0c9bbd9a4..c7b67e0cfe 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -22,7 +22,6 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_msgs/msg/hardware_component_state.hpp" -#include "controller_manager_msgs/srv/list_controllers.hpp" #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -66,8 +65,6 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs cm_ = std::make_shared(executor_, TEST_CM_NAME); run_updater_ = false; - cm_->set_parameter( - rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); cm_->set_parameter(rclcpp::Parameter( "hardware_components_initial_state.unconfigured", std::vector({TEST_SYSTEM_HARDWARE_NAME}))); @@ -75,16 +72,8 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs "hardware_components_initial_state.inactive", std::vector({TEST_SENSOR_HARDWARE_NAME}))); - std::string robot_description = ""; - cm_->get_parameter("robot_description", robot_description); - if (robot_description.empty()) - { - throw std::runtime_error( - "Unable to initialize resource manager, no robot description found."); - } - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; + msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); @@ -371,21 +360,8 @@ class TestControllerManagerHWManagementSrvsWithoutParams cm_ = std::make_shared(executor_, TEST_CM_NAME); run_updater_ = false; - // TODO(destogl): separate this to init_tests method where parameter can be set for each test - // separately - cm_->set_parameter( - rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); - - std::string robot_description = ""; - cm_->get_parameter("robot_description", robot_description); - if (robot_description.empty()) - { - throw std::runtime_error( - "Unable to initialize resource manager, no robot description found."); - } - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; + msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index ed443ea3d4..19d34c0c11 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -19,11 +19,11 @@ #include #include -#include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "test_controller/test_controller.hpp" +#include "test_controller_failed_init/test_controller_failed_init.hpp" using test_controller::TEST_CONTROLLER_CLASS_NAME; using ::testing::_; diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 3ee141edaa..c0305324b6 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -14,11 +14,9 @@ #include #include -#include #include #include -#include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index a5dd8fcda3..7680d8270c 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -19,7 +19,6 @@ #include #include -#include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -32,6 +31,7 @@ using ::testing::Return; using namespace std::chrono_literals; class TestLoadController : public ControllerManagerFixture { +public: void SetUp() override { ControllerManagerFixture::SetUp(); @@ -380,3 +380,84 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } } + +class TestLoadControllerWithoutRobotDescription +: public ControllerManagerFixture +{ +public: + TestLoadControllerWithoutRobotDescription() + : ControllerManagerFixture("") + { + } + + 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::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 update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spawner_times_out) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 256) + << "could not spawn controller because not robot description and not services for controller " + "manager are active"; +} + +TEST_F( + TestLoadControllerWithoutRobotDescription, + controller_starting_with_later_load_of_robot_description) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + // Delay sending robot description + robot_description_sending_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); }); + + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0) + << "could not activate control because not robot description"; + } + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } +} diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index f6d6fad69e..9e210bc4e7 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-07-23) +------------------- + 4.13.0 (2024-07-08) ------------------- * [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index b0bd54a0a7..143117f512 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.13.0 + 4.14.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/doc/debugging.rst b/doc/debugging.rst new file mode 100644 index 0000000000..5349c98a52 --- /dev/null +++ b/doc/debugging.rst @@ -0,0 +1,80 @@ +Debugging +^^^^^^^^^ + +All controllers and hardware components are plugins loaded into the ``controller_manager``. Therefore, the debugger must be attached to the ``controller_manager``. If multiple ``controller_manager`` instances are running on your robot or machine, you need to attach the debugger to the ``controller_manager`` associated with the hardware component or controller you want to debug. + +How-To +****************** + +* Install ``xterm``, ``gdb`` and ``gdbserver`` on your system + + .. code-block:: bash + + sudo apt install xterm gdb gdbserver + +* Make sure you run a "debug" or "release with debug information" build: + This is done by passing ``--cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo`` to ``colcon build``. + Remember that in release builds some breakpoints might not behave as you expect as the the corresponding line might have been optimized by the compiler. For such cases, a full Debug build (``--cmake-args -DCMAKE_BUILD_TYPE=Debug``) is recommended. + +* Adapt the launch file to run the controller manager with the debugger attached: + + * Version A: Run it directly with the gdb CLI: + + Add ``prefix=['xterm -e gdb -ex run --args']`` to the ``controller_manager`` node entry in your launch file. + Due to how ``ros2launch`` works we need to run the specific node in a separate terminal instance. + + * Version B: Run it with gdbserver: + + Add ``prefix=['gdbserver localhost:3000']`` to the ``controller_manager`` node entry in your launch file. + Afterwards, you can either attach a gdb CLI instance or any IDE of your choice to that ``gdbserver`` instance. + Ensure you start your debugger from a terminal where you have sourced your workspace to properly resolve all paths. + + Example launch file entry: + + .. code-block:: python + + # Obtain the controller config file for the ros2 control node + controller_config_file = get_package_file("", "config/controllers.yaml") + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[controller_config_file], + output="both", + emulate_tty=True, + remappings=[ + ("~/robot_description", "/robot_description") + ], + prefix=['xterm -e gdb -ex run --args'] # or prefix=['gdbserver localhost:3000'] + ) + + ld.add_action(controller_manager) + + +Additional notes +***************** + +* Debugging plugins + + You can only set breakpoints in plugins after the plugin has been loaded. In the ros2_control context this means after the controller / hardware component has been loaded: + +* Debug builds + + It's often practical to include debug information only for the specific package you want to debug. + ``colcon build --packages-select [package_name] --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo`` or ``colcon build --packages-select [package_name] --cmake-args -DCMAKE_BUILD_TYPE=Debug`` + +* Realtime + +.. warning:: + The ``update/on_activate/on_deactivate`` method of a controller and the ``read/write/on_activate/perform_command_mode_switch`` methods of a hardware component all run in the context of the realtime update loop. Setting breakpoints there can and will cause issues that might even break your hardware in the worst case. + +From experience, it might be better to use meaningful logs for the real-time context (with caution) or to add additional debug state interfaces (or publishers in the case of a controller). + +However, running the controller_manager and your plugin with gdb can still be very useful for debugging errors such as segfaults, as you can gather a full backtrace. + +References +*********** + +* `ROS 2 and GDB `_ +* `Using GDB to debug a plugin `_ +* `GDB CLI Tutorial `_ diff --git a/doc/index.rst b/doc/index.rst index b1d8e21c7d..09a2ddf745 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -28,3 +28,12 @@ Concepts Joint Kinematics <../hardware_interface/doc/joints_userdoc.rst> Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst> Mock Components <../hardware_interface/doc/mock_components_userdoc.rst> + +===================================== +Guidelines and Best Practices +===================================== + +.. toctree:: + :titlesonly: + + Debugging the Controller Manager and Plugins diff --git a/doc/migration/Jazzy.rst b/doc/migration.rst similarity index 87% rename from doc/migration/Jazzy.rst rename to doc/migration.rst index 1880f5d380..68359f072f 100644 --- a/doc/migration/Jazzy.rst +++ b/doc/migration.rst @@ -64,6 +64,7 @@ controller_manager +* The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). Use ``robot_description`` topic instead, e.g., you can use the `robot_state_publisher `_. For an example, see `this PR `_ where the change was applied to the demo repository. hardware_interface ****************** diff --git a/doc/migration/Galactic.rst b/doc/migration/Galactic.rst deleted file mode 100644 index 4c6e958951..0000000000 --- a/doc/migration/Galactic.rst +++ /dev/null @@ -1,53 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Galactic.rst - -Foxy to Galactic -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -hardware_interface -************************************** -Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle. -The following list shows a set of quick changes to port existing hardware components to Galactic: - -1. Rename ``configure`` to ``on_init`` and change return type to ``CallbackReturn`` - -2. If using BaseInterface as base class then you should remove it. Specifically, change: - - .. code-block:: c++ - - hardware_interface::BaseInterface - - to - - .. code-block:: c++ - - hardware_interface::[Actuator|Sensor|System]Interface - -3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp`` - -4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary - -5. replace first three lines in ``on_init`` to - - .. code-block:: c++ - - if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) - { - return CallbackReturn::ERROR; - } - - -6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;`` - -7. Remove all lines with ``status_ = ...`` or ``status::...`` - -8. Rename ``start()`` to ``on_activate(const rclcpp_lifecycle::State & previous_state)`` and - ``stop()`` to ``on_deactivate(const rclcpp_lifecycle::State & previous_state)`` - -9. Change return type of ``on_activate`` and ``on_deactivate`` to ``CallbackReturn`` - -10. Change last return of ``on_activate`` and ``on_deactivate`` to ``return CallbackReturn::SUCCESS;`` - -11. If you have any ``return_type::ERROR`` in ``on_init``, ``on_activate``, or ``in_deactivate`` change to ``CallbackReturn::ERROR`` - -12. If you get link errors with undefined refernences to symbols in ``rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface``, then add - ``rclcpp_lifecyle`` package dependency to ``CMakeLists.txt`` and ``package.xml`` diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes.rst similarity index 97% rename from doc/release_notes/Jazzy.rst rename to doc/release_notes.rst index 5d1773afe8..a4a6f31734 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes.rst @@ -68,6 +68,7 @@ controller_manager The parameters within the ``ros2_control`` tag are not supported any more. +* The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). hardware_interface ****************** diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 76ddab43cc..37f8b10795 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* [ResourceManager] Make destructor virtual for use in derived classes (`#1607 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + 4.13.0 (2024-07-08) ------------------- * [ResourceManager] Propagate access to logger and clock interfaces to HardwareComponent (`#1585 `_) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 3a1d7a5974..3534d9f587 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -19,12 +19,13 @@ #include #include +#include "hardware_interface/actuator_interface.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" #include "rclcpp/duration.hpp" -#include "rclcpp/logging.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 88b36d460b..b637068fe3 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -15,7 +15,6 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ -#include #include #include @@ -25,7 +24,7 @@ #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" -#include "rclcpp/logging.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/hardware_interface/include/hardware_interface/async_components.hpp b/hardware_interface/include/hardware_interface/async_components.hpp index 73495c92f8..0504419b38 100644 --- a/hardware_interface/include/hardware_interface/async_components.hpp +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -17,15 +17,12 @@ #include #include -#include #include #include "hardware_interface/actuator.hpp" #include "hardware_interface/sensor.hpp" #include "hardware_interface/system.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/node.hpp" #include "rclcpp/time.hpp" namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index d5d999cca8..38ca0cf89d 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -16,7 +16,6 @@ #define HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ #include -#include #include #include "hardware_interface/hardware_info.hpp" diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1aea017754..dc536e51be 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -16,10 +16,8 @@ #define HARDWARE_INTERFACE__HANDLE_HPP_ #include -#include #include "hardware_interface/macros.hpp" -#include "hardware_interface/visibility_control.h" namespace hardware_interface { diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index e7d47bcaa4..092ed21000 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -19,7 +19,6 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_ -#include #include #include diff --git a/hardware_interface/include/hardware_interface/lexical_casts.hpp b/hardware_interface/include/hardware_interface/lexical_casts.hpp index 042361e392..dd69dad7a6 100644 --- a/hardware_interface/include/hardware_interface/lexical_casts.hpp +++ b/hardware_interface/include/hardware_interface/lexical_casts.hpp @@ -15,10 +15,6 @@ #ifndef HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ #define HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ -#include -#include -#include -#include #include #include "hardware_interface/visibility_control.h" diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 5313162c29..065ecabed9 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -27,11 +27,10 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/sensor.hpp" #include "hardware_interface/system.hpp" +#include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/lifecycle_state_names.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" -#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" namespace hardware_interface @@ -75,7 +74,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager ResourceManager(const ResourceManager &) = delete; - ~ResourceManager(); + virtual ~ResourceManager(); /// Load resources from on a given URDF. /** diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index d8e55aa4ad..4126910d44 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -17,15 +17,15 @@ #include #include -#include #include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" #include "rclcpp/duration.hpp" -#include "rclcpp/logging.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 79f689d0c6..2cff8b2a3a 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -15,7 +15,6 @@ #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ -#include #include #include @@ -25,7 +24,7 @@ #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" -#include "rclcpp/logging.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 1ca4260750..9308872ef8 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -17,15 +17,15 @@ #include #include -#include #include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" #include "rclcpp/duration.hpp" -#include "rclcpp/logging.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 8e41438ba5..f9173fb754 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,7 +15,6 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ -#include #include #include @@ -25,7 +24,7 @@ #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" -#include "rclcpp/logging.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp b/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp index f51f64f836..fcc21031d6 100644 --- a/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp +++ b/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp @@ -17,8 +17,6 @@ #ifndef HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_ #define HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_ -#include - namespace hardware_interface { namespace lifecycle_state_names diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index fbb8547ab1..7987780ba0 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -25,6 +25,7 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/visibility_control.h" using hardware_interface::return_type; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index a3a1d6c77a..c5f09e74bd 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.13.0 + 4.14.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 7e50c07eb0..66419402f8 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -14,7 +14,6 @@ #include "hardware_interface/actuator.hpp" -#include #include #include #include diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 938deeb0fb..42432dda8d 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include -#include #include #include #include diff --git a/hardware_interface/src/lexical_casts.cpp b/hardware_interface/src/lexical_casts.cpp index 940beb6d0f..f9bdbedb57 100644 --- a/hardware_interface/src/lexical_casts.cpp +++ b/hardware_interface/src/lexical_casts.cpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include + #include "hardware_interface/lexical_casts.hpp" namespace hardware_interface diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 67f9ed56da..abe36ba262 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -20,14 +20,12 @@ #include #include #include -#include #include #include -#include "hardware_interface/component_parser.hpp" #include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rcutils/logging_macros.h" +#include "rclcpp/logging.hpp" namespace mock_components { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index a6eadd49fe..5b13e5d331 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -33,9 +33,9 @@ #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" +#include "rclcpp/logging.hpp" #include "rcutils/logging_macros.h" namespace hardware_interface diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index c90bc85055..15d24fc7e7 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -28,7 +28,6 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/node.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" namespace diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 42ccdae8fa..986c32d37b 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include "hardware_interface/actuator.hpp" diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index 47b19f9769..ff57d6a63d 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -14,7 +14,6 @@ #include #include -#include #include #include "hardware_interface/sensor_interface.hpp" diff --git a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp index 9997466106..bb41f16228 100644 --- a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp @@ -18,7 +18,6 @@ #include #include -#include #include #include diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index 390056d990..dacbf308fa 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include "hardware_interface/actuator_interface.hpp" diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index fe06a64223..5942f14d42 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include -#include #include #include "hardware_interface/system_interface.hpp" diff --git a/hardware_interface/test/test_inst_hardwares.cpp b/hardware_interface/test/test_inst_hardwares.cpp index ddd3aea0ad..62b80dae61 100644 --- a/hardware_interface/test/test_inst_hardwares.cpp +++ b/hardware_interface/test/test_inst_hardwares.cpp @@ -15,11 +15,8 @@ #include #include "hardware_interface/actuator.hpp" -#include "hardware_interface/actuator_interface.hpp" #include "hardware_interface/sensor.hpp" -#include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/system.hpp" -#include "hardware_interface/system_interface.hpp" class TestInstantiationHardwares : public ::testing::Test { diff --git a/hardware_interface/test/test_macros.cpp b/hardware_interface/test/test_macros.cpp index a7c56849d1..18e9e292c0 100644 --- a/hardware_interface/test/test_macros.cpp +++ b/hardware_interface/test/test_macros.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include #include "hardware_interface/macros.hpp" -#include "gmock/gmock.h" - class TestMacros : public ::testing::Test { protected: diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 520f28540d..b0a25e3835 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Fix typos in test_resource_manager.cpp (`#1609 `_) +* Contributors: Henry Moore, Parker Drouillard + 4.13.0 (2024-07-08) ------------------- * [ResourceManager] Propagate access to logger and clock interfaces to HardwareComponent (`#1585 `_) diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index f65de8598d..7594d8e32c 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.13.0 + 4.14.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 1277fecfb6..b7149369ed 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include "hardware_interface/actuator_interface.hpp" diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 3fc2ef2445..7f5a476c14 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include "hardware_interface/sensor_interface.hpp" diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 502f5b4c21..6724a1d019 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include -#include #include #include "hardware_interface/system_interface.hpp" diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 5fb155fa3a..51d81a90ab 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -137,9 +137,9 @@ void test_load_and_initialized_components_failure(const std::string & urdf) EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration")); } -TEST_F(ResourceManagerTest, test_unitilizable_hardware) +TEST_F(ResourceManagerTest, test_uninitializable_hardware) { - SCOPED_TRACE("test_unitilizable_hardware_no_validation"); + SCOPED_TRACE("test_uninitializable_hardware_no_validation"); // If the the hardware can not be initialized and load_and_initialize_components didn't try to // validate the interfaces, the interface should not show up test_load_and_initialized_components_failure( diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index 18e51342f6..dcb3026e19 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -19,10 +19,11 @@ #include -#include #include #include +#include + #include "hardware_interface/resource_manager.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 5fea915b6b..da9bd28fed 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + 4.13.0 (2024-07-08) ------------------- * [JointLimits] Add Saturation Joint Limiter that uses clamping method (`#971 `_) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index ed2f269812..42403d56ae 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -20,7 +20,6 @@ #include #include -#include "joint_limits/joint_limits.hpp" #include "joint_limits/joint_limits_rosparam.hpp" #include "joint_limits/visibility_control.h" #include "rclcpp/node.hpp" diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index b23607f53d..379cee0357 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -22,7 +22,7 @@ #include #include "joint_limits/joint_limits.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/node.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace // utilities diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 3641fc5c75..a69cc2791c 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -18,12 +18,9 @@ #define JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ #include -#include -#include #include #include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 6e8f35255d..9609522838 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.13.0 + 4.14.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index 2f8ac9636f..f2a3adae1e 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -19,7 +19,6 @@ #include #include "rclcpp/duration.hpp" -#include "rcutils/logging_macros.h" constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops constexpr double VALUE_CONSIDERED_ZERO = 1e-10; diff --git a/joint_limits/test/joint_limits_rosparam_test.cpp b/joint_limits/test/joint_limits_rosparam_test.cpp index 5f376562a6..7d03eb20ca 100644 --- a/joint_limits/test/joint_limits_rosparam_test.cpp +++ b/joint_limits/test/joint_limits_rosparam_test.cpp @@ -14,12 +14,9 @@ /// \author Adolfo Rodriguez Tsouroukdissian -#include - -#include "gtest/gtest.h" +#include #include "joint_limits/joint_limits_rosparam.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" class JointLimitsRosParamTest : public ::testing::Test diff --git a/joint_limits/test/joint_limits_urdf_test.cpp b/joint_limits/test/joint_limits_urdf_test.cpp index 562293d475..82c7e13613 100644 --- a/joint_limits/test/joint_limits_urdf_test.cpp +++ b/joint_limits/test/joint_limits_urdf_test.cpp @@ -13,8 +13,9 @@ // limitations under the License. /// \author Adolfo Rodriguez Tsouroukdissian +#include + #include "joint_limits/joint_limits_urdf.hpp" -#include "gtest/gtest.h" using std::string; diff --git a/joint_limits/test/test_joint_saturation_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp index 3da0706177..20097ae591 100644 --- a/joint_limits/test/test_joint_saturation_limiter.hpp +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -23,7 +23,6 @@ #include "joint_limits/joint_limiter_interface.hpp" #include "joint_limits/joint_limits.hpp" #include "pluginlib/class_loader.hpp" -#include "rclcpp/duration.hpp" #include "rclcpp/node.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 0861761188..3c38360f50 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-07-23) +------------------- + 4.13.0 (2024-07-08) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index c7687e3cd6..501c543ccb 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.13.0 + 4.14.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 7dac855794..a61a3273f7 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + 4.13.0 (2024-07-08) ------------------- * [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index eba16c1e71..b0076b859b 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -15,8 +15,6 @@ #ifndef ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_ #define ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_ -#include - namespace ros2_control_test_assets { // 1. Industrial Robots with only one interface diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 1912391b5a..b6192e8c45 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.13.0 + 4.14.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 7ed24372b0..b1d9dcd002 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-07-23) +------------------- + 4.13.0 (2024-07-08) ------------------- * Remove ament linters (`#1601 `_) diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index d1693cdf2a..9adae5d2b5 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.13.0 + 4.14.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 5205c0f024..0a19536fe5 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.13.0", + version="4.14.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 065861ca07..b5f9960575 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-07-23) +------------------- + 4.13.0 (2024-07-08) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index e4356b90fe..e678b1f37a 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.13.0 + 4.14.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 69d38c7273..bbd9e75dcc 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.13.0", + version="4.14.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 63bd6aeb89..694f4c72cf 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + 4.13.0 (2024-07-08) ------------------- diff --git a/transmission_interface/include/transmission_interface/accessor.hpp b/transmission_interface/include/transmission_interface/accessor.hpp index dd58e55744..a72b0f39b0 100644 --- a/transmission_interface/include/transmission_interface/accessor.hpp +++ b/transmission_interface/include/transmission_interface/accessor.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 85da01116d..b6b4353dd8 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -16,7 +16,6 @@ #define TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_ #include -#include #include #include diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 39f5df6f61..a8757e9c02 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -21,6 +21,7 @@ #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "transmission_interface/accessor.hpp" #include "transmission_interface/exception.hpp" #include "transmission_interface/transmission.hpp" diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index bc1c0a78d8..024a019bfd 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -15,8 +15,6 @@ #ifndef TRANSMISSION_INTERFACE__HANDLE_HPP_ #define TRANSMISSION_INTERFACE__HANDLE_HPP_ -#include - #include "hardware_interface/handle.hpp" namespace transmission_interface diff --git a/transmission_interface/include/transmission_interface/transmission.hpp b/transmission_interface/include/transmission_interface/transmission.hpp index 697e1a4eb7..6275f37711 100644 --- a/transmission_interface/include/transmission_interface/transmission.hpp +++ b/transmission_interface/include/transmission_interface/transmission.hpp @@ -16,8 +16,6 @@ #include #include -#include -#include #include #include "transmission_interface/handle.hpp" diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 726ddc70cc..5740c2c68f 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.13.0 + 4.14.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl diff --git a/transmission_interface/src/differential_transmission_loader.cpp b/transmission_interface/src/differential_transmission_loader.cpp index 37f5b708c7..87ee55fa25 100644 --- a/transmission_interface/src/differential_transmission_loader.cpp +++ b/transmission_interface/src/differential_transmission_loader.cpp @@ -16,12 +16,9 @@ #include -#include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_list_macros.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/rclcpp.hpp" #include "transmission_interface/differential_transmission.hpp" namespace transmission_interface diff --git a/transmission_interface/src/four_bar_linkage_transmission_loader.cpp b/transmission_interface/src/four_bar_linkage_transmission_loader.cpp index 93b42e33bf..e39ad96cbb 100644 --- a/transmission_interface/src/four_bar_linkage_transmission_loader.cpp +++ b/transmission_interface/src/four_bar_linkage_transmission_loader.cpp @@ -16,12 +16,9 @@ #include -#include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_list_macros.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/rclcpp.hpp" #include "transmission_interface/four_bar_linkage_transmission.hpp" namespace transmission_interface diff --git a/transmission_interface/src/simple_transmission_loader.cpp b/transmission_interface/src/simple_transmission_loader.cpp index aaa86bff22..7dd410f534 100644 --- a/transmission_interface/src/simple_transmission_loader.cpp +++ b/transmission_interface/src/simple_transmission_loader.cpp @@ -16,12 +16,9 @@ #include -#include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_list_macros.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/rclcpp.hpp" #include "transmission_interface/simple_transmission.hpp" namespace transmission_interface diff --git a/transmission_interface/test/differential_transmission_loader_test.cpp b/transmission_interface/test/differential_transmission_loader_test.cpp index 946c4703b5..77a7ebed78 100644 --- a/transmission_interface/test/differential_transmission_loader_test.cpp +++ b/transmission_interface/test/differential_transmission_loader_test.cpp @@ -20,11 +20,9 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" #include "ros2_control_test_assets/descriptions.hpp" #include "transmission_interface/differential_transmission.hpp" -#include "transmission_interface/differential_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" using testing::DoubleNear; diff --git a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp index 720cad68b4..8d3e1b2010 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp @@ -20,11 +20,9 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" #include "ros2_control_test_assets/descriptions.hpp" #include "transmission_interface/four_bar_linkage_transmission.hpp" -#include "transmission_interface/four_bar_linkage_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" using testing::DoubleNear; diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index f74e4def6a..08d98ec734 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_test.cpp @@ -26,7 +26,6 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using testing::DoubleNear; -using testing::Not; using transmission_interface::ActuatorHandle; using transmission_interface::Exception; using transmission_interface::FourBarLinkageTransmission; diff --git a/transmission_interface/test/simple_transmission_loader_test.cpp b/transmission_interface/test/simple_transmission_loader_test.cpp index 968f64c6e8..7433c1bed3 100644 --- a/transmission_interface/test/simple_transmission_loader_test.cpp +++ b/transmission_interface/test/simple_transmission_loader_test.cpp @@ -20,11 +20,9 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" #include "ros2_control_test_assets/descriptions.hpp" #include "transmission_interface/simple_transmission.hpp" -#include "transmission_interface/simple_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" using testing::DoubleNear; diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index 33a14f92d1..d687fbfd21 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -21,7 +21,6 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; -using std::vector; using transmission_interface::ActuatorHandle; using transmission_interface::Exception; using transmission_interface::JointHandle; diff --git a/transmission_interface/test/test_transmission_parser.cpp b/transmission_interface/test/test_transmission_parser.cpp deleted file mode 100644 index 3a127e4caa..0000000000 --- a/transmission_interface/test/test_transmission_parser.cpp +++ /dev/null @@ -1,441 +0,0 @@ -// 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 "transmission_interface/transmission_parser.hpp" - -using namespace ::testing; // NOLINT - -class TestTransmissionParser : public Test -{ -public: - void SetUp() override - { - valid_urdf_xml_ = - R"( - - - - - - - - - - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - PositionJointInterface - - - 1 - PositionJointInterface - - - - transmission_interface/SimpleTransmission - - VelocityJointInterface - - - 60 - VelocityJointInterface - - - - Gazebo/Orange - - - 0.2 - 0.2 - Gazebo/Black - - - 0.2 - 0.2 - Gazebo/Orange - - - - - - - )"; - - wrong_urdf_xml_ = - R"( - - - - - - - - - - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - 1 - - - - Gazebo/Orange - - - 0.2 - 0.2 - Gazebo/Black - - - 0.2 - 0.2 - Gazebo/Orange - - - - - - - )"; - } - - std::string valid_urdf_xml_; - std::string wrong_urdf_xml_; -}; - -using transmission_interface::parse_transmissions_from_urdf; -using transmission_interface::TransmissionInfo; - -TEST_F(TestTransmissionParser, successfully_parse_valid_urdf) -{ - const auto transmissions = parse_transmissions_from_urdf(valid_urdf_xml_); - - ASSERT_THAT(transmissions, SizeIs(2)); - - // first transmission - EXPECT_EQ("rrbot_tran1", transmissions[0].name); - EXPECT_EQ("transmission_interface/SimpleTransmission", transmissions[0].type); - - ASSERT_THAT(transmissions[0].joints, SizeIs(1)); - ASSERT_THAT(transmissions[0].joints[0].interfaces, SizeIs(1)); - EXPECT_EQ("rrbot_joint1", transmissions[0].joints[0].name); - EXPECT_EQ("PositionJointInterface", transmissions[0].joints[0].interfaces[0]); - - ASSERT_THAT(transmissions[0].actuators, SizeIs(1)); - ASSERT_THAT(transmissions[0].actuators[0].interfaces, SizeIs(1)); - EXPECT_EQ("rrbot_motor1", transmissions[0].actuators[0].name); - EXPECT_EQ("PositionJointInterface", transmissions[0].actuators[0].interfaces[0]); - EXPECT_EQ(1, transmissions[0].actuators[0].mechanical_reduction); - - // second transmission - EXPECT_EQ("rrbot_tran2", transmissions[1].name); - EXPECT_EQ("transmission_interface/SimpleTransmission", transmissions[1].type); - - ASSERT_THAT(transmissions[1].joints, SizeIs(1)); - ASSERT_THAT(transmissions[1].joints[0].interfaces, SizeIs(1)); - EXPECT_EQ("rrbot_joint2", transmissions[1].joints[0].name); - EXPECT_EQ("VelocityJointInterface", transmissions[1].joints[0].interfaces[0]); - - ASSERT_THAT(transmissions[1].actuators, SizeIs(1)); - ASSERT_THAT(transmissions[1].actuators[0].interfaces, SizeIs(1)); - EXPECT_EQ("rrbot_motor2", transmissions[1].actuators[0].name); - EXPECT_EQ("VelocityJointInterface", transmissions[1].actuators[0].interfaces[0]); - EXPECT_EQ(60, transmissions[1].actuators[0].mechanical_reduction); -} - -TEST_F(TestTransmissionParser, empty_string_throws_error) -{ - ASSERT_THROW(transmission_interface::parse_transmissions_from_urdf(""), std::runtime_error); -} - -TEST_F(TestTransmissionParser, empty_urdf_returns_empty) -{ - const auto transmissions = transmission_interface::parse_transmissions_from_urdf( - ""); - ASSERT_THAT(transmissions, IsEmpty()); -} - -TEST_F(TestTransmissionParser, wrong_urdf_throws_error) -{ - EXPECT_THROW( - transmission_interface::parse_transmissions_from_urdf(wrong_urdf_xml_), std::runtime_error); -}