diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index da87fd6358..55a24efb5c 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -41,7 +41,6 @@ jobs: controller_manager controller_manager_msgs hardware_interface - joint_limits_interface ros2_control test_robot_hardware transmission_interface diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 36b6e45ce0..7af04cff12 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -12,7 +12,14 @@ jobs: - uses: ros-tooling/action-ros-lint@0.0.6 with: linter: copyright - package-name: controller_interface controller_manager controller_manager_msgs hardware_interface joint_limits_interface test_robot_hardware transmission_interface + package-name: + controller_interface + controller_manager + controller_manager_msgs + hardware_interface + ros2_control + test_robot_hardware + transmission_interface ament_xmllint: name: ament_xmllint @@ -23,7 +30,14 @@ jobs: - uses: ros-tooling/action-ros-lint@0.0.6 with: linter: xmllint - package-name: controller_interface controller_manager controller_manager_msgs hardware_interface joint_limits_interface test_robot_hardware transmission_interface + package-name: + controller_interface + controller_manager + controller_manager_msgs + hardware_interface + ros2_control + test_robot_hardware + transmission_interface ament_lint_cpp: name: ament_${{ matrix.linter }} @@ -38,4 +52,11 @@ jobs: - uses: ros-tooling/action-ros-lint@0.0.6 with: linter: ${{ matrix.linter }} - package-name: controller_interface controller_manager controller_manager_msgs hardware_interface joint_limits_interface test_robot_hardware transmission_interface + package-name: + controller_interface + controller_manager + controller_manager_msgs + hardware_interface + ros2_control + test_robot_hardware + transmission_interface diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index b7e02a5a72..f5e3131200 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -17,10 +17,12 @@ #include #include +#include #include "controller_interface/visibility_control.h" -#include "hardware_interface/robot_hardware.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -34,6 +36,26 @@ enum class return_type : std::uint8_t ERROR = 1, }; +/// Indicating which interfaces are to be claimed. +/** + * One might either claim all available command/state interfaces, + * specifying a set of individual interfaces, + * or none at all. + */ +enum class interface_configuration_type : std::uint8_t +{ + ALL = 0, + INDIVIDUAL = 1, + NONE = 2, +}; + +/// Configuring what command/state interfaces to claim. +struct InterfaceConfiguration +{ + interface_configuration_type type; + std::vector names = {}; +}; + class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: @@ -44,12 +66,26 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN virtual ~ControllerInterface() = default; + CONTROLLER_INTERFACE_PUBLIC + virtual + InterfaceConfiguration command_interface_configuration() const = 0; + + CONTROLLER_INTERFACE_PUBLIC + virtual + InterfaceConfiguration state_interface_configuration() const = 0; + + CONTROLLER_INTERFACE_PUBLIC + void assign_interfaces( + std::vector && command_interfaces, + std::vector && state_interfaces); + + CONTROLLER_INTERFACE_PUBLIC + void release_interfaces(); + CONTROLLER_INTERFACE_PUBLIC virtual return_type - init( - std::weak_ptr robot_hardware, - const std::string & controller_name); + init(const std::string & controller_name); CONTROLLER_INTERFACE_PUBLIC virtual @@ -61,7 +97,8 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN get_lifecycle_node(); protected: - std::weak_ptr robot_hardware_; + std::vector command_interfaces_; + std::vector state_interfaces_; std::shared_ptr lifecycle_node_; }; diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 7fff487c7c..56ac4c178b 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -16,17 +16,19 @@ #include #include +#include +#include namespace controller_interface { return_type -ControllerInterface::init( - std::weak_ptr robot_hardware, - const std::string & controller_name) +ControllerInterface::init(const std::string & controller_name) { - robot_hardware_ = robot_hardware; - lifecycle_node_ = std::make_shared(controller_name); + lifecycle_node_ = std::make_shared( + controller_name, + rclcpp::NodeOptions().allow_undeclared_parameters(true). + automatically_declare_parameters_from_overrides(true)); lifecycle_node_->register_on_configure( std::bind(&ControllerInterface::on_configure, this, std::placeholders::_1)); @@ -49,6 +51,20 @@ ControllerInterface::init( return return_type::SUCCESS; } +void ControllerInterface::assign_interfaces( + std::vector && command_interfaces, + std::vector && state_interfaces) +{ + command_interfaces_ = std::forward(command_interfaces); + state_interfaces_ = std::forward(state_interfaces); +} + +void ControllerInterface::release_interfaces() +{ + command_interfaces_.clear(); + state_interfaces_.clear(); +} + std::shared_ptr ControllerInterface::get_lifecycle_node() { diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 4dd37237d0..3da1eb1030 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -37,11 +37,25 @@ target_compile_definitions(controller_manager PRIVATE "CONTROLLER_MANAGER_BUILDI # prevent pluginlib from using boost target_compile_definitions(controller_manager PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +add_executable(ros2_control_node src/ros2_control_node.cpp) +target_include_directories(ros2_control_node PRIVATE include) +target_link_libraries(ros2_control_node controller_manager) +ament_target_dependencies(ros2_control_node + controller_interface + hardware_interface + rclcpp +) + install(TARGETS controller_manager RUNTIME DESTINATION bin LIBRARY DESTINATION lib ARCHIVE DESTINATION lib ) + +install(TARGETS ros2_control_node + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + install(DIRECTORY include/ DESTINATION include ) @@ -77,10 +91,6 @@ if(BUILD_TESTING) ) target_include_directories(test_controller_manager PRIVATE include) target_link_libraries(test_controller_manager controller_manager test_controller) - ament_target_dependencies( - test_controller_manager - test_robot_hardware - ) ament_add_gmock( test_load_controller @@ -89,10 +99,6 @@ if(BUILD_TESTING) ) target_include_directories(test_load_controller PRIVATE include) target_link_libraries(test_load_controller controller_manager) - ament_target_dependencies( - test_load_controller - test_robot_hardware - ) ament_add_gmock( test_controller_manager_srvs @@ -101,10 +107,6 @@ if(BUILD_TESTING) ) target_include_directories(test_controller_manager_srvs PRIVATE include) target_link_libraries(test_controller_manager_srvs controller_manager test_controller) - ament_target_dependencies( - test_controller_manager_srvs - test_robot_hardware - ) endif() ament_export_libraries( @@ -116,6 +118,8 @@ ament_export_include_directories( ament_export_dependencies( controller_interface controller_manager_msgs + hardware_interface pluginlib + rclcpp ) ament_package() diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 768e79ad4e..231abbb73e 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -26,12 +26,13 @@ #include "controller_manager/visibility_control.h" #include "controller_manager_msgs/srv/list_controllers.hpp" #include "controller_manager_msgs/srv/list_controller_types.hpp" +#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" #include "controller_manager_msgs/srv/load_controller.hpp" #include "controller_manager_msgs/srv/reload_controller_libraries.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" #include "controller_manager_msgs/srv/unload_controller.hpp" -#include "hardware_interface/robot_hardware.hpp" +#include "hardware_interface/resource_manager.hpp" #include "pluginlib/class_loader.hpp" @@ -44,14 +45,19 @@ namespace controller_manager class ControllerManager : public rclcpp::Node { public: - static constexpr bool WAIT_FOR_ALL_RESOURCES = false; - static constexpr double INFINITE_TIMEOUT = 0.0; + static constexpr bool kWaitForAllResources = false; + static constexpr double kInfiniteTimeout = 0.0; CONTROLLER_MANAGER_PUBLIC ControllerManager( - std::shared_ptr hw, + std::unique_ptr resource_manager, std::shared_ptr executor, - const std::string & name = "controller_manager"); + const std::string & manager_node_name = "controller_manager"); + + CONTROLLER_MANAGER_PUBLIC + ControllerManager( + std::shared_ptr executor, + const std::string & manager_node_name = "controller_manager"); CONTROLLER_MANAGER_PUBLIC virtual @@ -68,12 +74,10 @@ class ControllerManager : public rclcpp::Node */ CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceSharedPtr - load_controller( - const std::string & controller_name); + load_controller(const std::string & controller_name); CONTROLLER_MANAGER_PUBLIC - controller_interface::return_type unload_controller( - const std::string & controller_name); + controller_interface::return_type unload_controller(const std::string & controller_name); CONTROLLER_MANAGER_PUBLIC std::vector get_loaded_controllers() const; @@ -104,14 +108,30 @@ class ControllerManager : public rclcpp::Node const std::vector & start_controllers, const std::vector & stop_controllers, int strictness, - bool start_asap = WAIT_FOR_ALL_RESOURCES, - const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT)); + bool start_asap = kWaitForAllResources, + const rclcpp::Duration & timeout = rclcpp::Duration(kInfiniteTimeout)); + CONTROLLER_MANAGER_PUBLIC - controller_interface::return_type - update(); + void read(); + + CONTROLLER_MANAGER_PUBLIC + controller_interface::return_type update(); + + CONTROLLER_MANAGER_PUBLIC + void write(); + + /// Deterministic (real-time safe) callback group, e.g., update function. + /** + * Deterministic (real-time safe) callback group for the update function. Default behavior + * is read hardware, update controller and finally write new values to the hardware. + */ + rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_; protected: + CONTROLLER_MANAGER_PUBLIC + void init_services(); + CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceSharedPtr add_controller_impl(const ControllerSpec & controller); @@ -138,6 +158,11 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC + void list_hardware_interfaces_srv_cb( + const std::shared_ptr request, + std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC void load_controller_service_cb( const std::shared_ptr request, @@ -161,10 +186,19 @@ class ControllerManager : public rclcpp::Node private: std::vector get_controller_names(); - std::shared_ptr hw_; + std::unique_ptr resource_manager_; + std::shared_ptr executor_; + std::shared_ptr> loader_; + /// Best effort (non real-time safe) callback group, e.g., service callbacks. + /** + * Best effort (non real-time safe) callback group for callbacks that can possibly break + * real-time requirements, for example, service callbacks. + */ + rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_; + /** * @brief The RTControllerListWrapper class wraps a double-buffered list of controllers * to avoid needing to lock the real-time thread when switching controllers in @@ -245,6 +279,8 @@ class ControllerManager : public rclcpp::Node list_controllers_service_; rclcpp::Service::SharedPtr list_controller_types_service_; + rclcpp::Service::SharedPtr + list_hardware_interfaces_service_; rclcpp::Service::SharedPtr load_controller_service_; rclcpp::Service::SharedPtr diff --git a/controller_manager/package.xml b/controller_manager/package.xml index ea8081722a..0efe9ab703 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -21,7 +21,6 @@ ament_cmake_gtest ament_lint_auto ament_lint_common - test_robot_hardware ament_cmake diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 37baa82c26..4219b5eb91 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include "controller_interface/controller_interface.hpp" @@ -49,56 +50,103 @@ rclcpp::NodeOptions get_cm_node_options() rclcpp::NodeOptions node_options; // Required for getting types of controllers to be loaded via service call node_options.allow_undeclared_parameters(true); + node_options.automatically_declare_parameters_from_overrides(true); return node_options; } ControllerManager::ControllerManager( - std::shared_ptr hw, std::shared_ptr executor, const std::string & manager_node_name) : rclcpp::Node(manager_node_name, get_cm_node_options()), - hw_(hw), + resource_manager_(std::make_unique()), executor_(executor), loader_(std::make_shared>( kControllerInterfaceName, kControllerInterface)) { + std::string robot_description = ""; + get_parameter("robot_description", robot_description); + if (robot_description.empty()) { + throw std::runtime_error("unable to initialize resource manager, no robot description found."); + } + + resource_manager_->load_urdf(robot_description); + + init_services(); +} + +ControllerManager::ControllerManager( + std::unique_ptr resource_manager, + std::shared_ptr executor, + const std::string & manager_node_name) +: rclcpp::Node(manager_node_name, get_cm_node_options()), + resource_manager_(std::move(resource_manager)), + executor_(executor), + loader_(std::make_shared>( + kControllerInterfaceName, kControllerInterface)) +{ + init_services(); +} + +void ControllerManager::init_services() +{ + deterministic_callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + best_effort_callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + using namespace std::placeholders; list_controllers_service_ = create_service( - "~/list_controllers", std::bind( - &ControllerManager::list_controllers_srv_cb, this, _1, - _2)); + "~/list_controllers", + std::bind(&ControllerManager::list_controllers_srv_cb, this, _1, _2), + rmw_qos_profile_services_default, + best_effort_callback_group_); list_controller_types_service_ = create_service( - "~/list_controller_types", std::bind( - &ControllerManager::list_controller_types_srv_cb, this, _1, - _2)); + "~/list_controller_types", + std::bind(&ControllerManager::list_controller_types_srv_cb, this, _1, _2), + rmw_qos_profile_services_default, + best_effort_callback_group_); + list_hardware_interfaces_service_ = + create_service( + "~/list_hardware_interfaces", + std::bind(&ControllerManager::list_hardware_interfaces_srv_cb, this, _1, _2), + rmw_qos_profile_services_default, + best_effort_callback_group_); load_controller_service_ = create_service( - "~/load_controller", std::bind( - &ControllerManager::load_controller_service_cb, this, _1, - _2)); + "~/load_controller", + std::bind(&ControllerManager::load_controller_service_cb, this, _1, _2), + rmw_qos_profile_services_default, + best_effort_callback_group_); reload_controller_libraries_service_ = create_service( - "~/reload_controller_libraries", std::bind( - &ControllerManager::reload_controller_libraries_service_cb, this, _1, - _2)); + "~/reload_controller_libraries", + std::bind(&ControllerManager::reload_controller_libraries_service_cb, this, _1, _2), + rmw_qos_profile_services_default, + best_effort_callback_group_); switch_controller_service_ = create_service( - "~/switch_controller", std::bind( - &ControllerManager::switch_controller_service_cb, this, _1, - _2)); + "~/switch_controller", + std::bind(&ControllerManager::switch_controller_service_cb, this, _1, _2), + rmw_qos_profile_services_default, + best_effort_callback_group_); unload_controller_service_ = create_service( - "~/unload_controller", std::bind( - &ControllerManager::unload_controller_service_cb, this, _1, - _2)); + "~/unload_controller", + std::bind(&ControllerManager::unload_controller_service_cb, this, _1, _2), + rmw_qos_profile_services_default, + best_effort_callback_group_); } controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller( const std::string & controller_name, const std::string & controller_type) { - RCLCPP_INFO(get_logger(), "Loading controller '%s'\n", controller_name.c_str()); + RCLCPP_INFO(get_logger(), "Loading controller '%s'", controller_name.c_str()); if (!loader_->isClassAvailable(controller_type)) { - const std::string error_msg("Loader for controller '" + controller_name + "' not found\n"); + const std::string error_msg("Loader for controller '" + controller_name + "' not found"); + RCLCPP_ERROR(get_logger(), "available classes:"); + for (const auto & c : loader_->getDeclaredClasses()) { + RCLCPP_ERROR(get_logger(), "%s", c.c_str()); + } RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str()); throw std::runtime_error(error_msg); } @@ -442,7 +490,7 @@ ControllerManager::add_controller_impl( return nullptr; } - controller.c->init(hw_, controller.info.name); + controller.c->init(controller.info.name); // TODO(v-lopez) this should only be done if controller_manager is configured. // Probably the whole load_controller part should fail if the controller_manager @@ -517,32 +565,6 @@ void ControllerManager::stop_controllers() void ControllerManager::start_controllers() { -#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING - // start controllers - if (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::DONE) { - for (const auto & request : start_request_) { - request->startRequest(time); - } - - switch_params_.do_switch = false; - } else if (// NOLINT - (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::ERROR) || - (switch_params_.timeout > 0.0 && - (time - switch_params_.init_time).toSec() > switch_params_.timeout)) - { - // abort controllers in case of error or timeout (if set) - for (const auto & request : start_request_) { - request->abortRequest(time); - } - - switch_params_.do_switch = false; - } else { - // wait controllers - for (const auto & request : start_request_) { - request->waitRequest(time); - } - } -#else // Dummy implementation, replace with the code above when migrated std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); @@ -558,6 +580,79 @@ void ControllerManager::start_controllers() continue; } auto controller = found_it->c; + + bool assignment_successful = true; + // assign command interfaces to the controller + auto command_interface_config = controller->command_interface_configuration(); + // default to controller_interface::configuration_type::NONE + std::vector command_interface_names = {}; + if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { + command_interface_names = resource_manager_->command_interface_keys(); + } + if (command_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) + { + command_interface_names = command_interface_config.names; + } + std::vector command_loans; + command_loans.reserve(command_interface_names.size()); + for (const auto & command_interface : command_interface_names) { + if (resource_manager_->command_interface_is_claimed(command_interface)) { + RCLCPP_ERROR( + get_logger(), + "Resource conflict for controller %s. Command interface %s is already claimed", + request.c_str(), command_interface.c_str()); + assignment_successful = false; + break; + } + try { + command_loans.emplace_back(resource_manager_->claim_command_interface(command_interface)); + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_logger(), + "Can't activate controller %s.%s", + request.c_str(), e.what()); + assignment_successful = false; + break; + } + } + // something went wrong during command interfaces, go skip the controller + if (!assignment_successful) { + continue; + } + + // assign state interfaces to the controller + auto state_interface_config = controller->state_interface_configuration(); + // default to controller_interface::configuration_type::NONE + std::vector state_interface_names = {}; + if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) { + state_interface_names = resource_manager_->state_interface_keys(); + } + if (state_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) + { + state_interface_names = state_interface_config.names; + } + std::vector state_loans; + state_loans.reserve(state_interface_names.size()); + for (const auto & state_interface : state_interface_names) { + try { + state_loans.emplace_back(resource_manager_->claim_state_interface(state_interface)); + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_logger(), + "Can't activate controller %s.%s", + request.c_str(), e.what()); + assignment_successful = false; + break; + } + } + // something went wrong during state interfaces, go skip the controller + if (!assignment_successful) { + continue; + } + controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); + const auto new_state = controller->get_lifecycle_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_ERROR( @@ -569,7 +664,6 @@ void ControllerManager::start_controllers() } // All controllers started, switching done switch_params_.do_switch = false; -#endif } void ControllerManager::start_controllers_asap() @@ -671,12 +765,32 @@ void ControllerManager::list_controller_types_srv_cb( for (const auto & cur_type : cur_types) { response->types.push_back(cur_type); response->base_classes.push_back(kControllerInterface); - RCLCPP_INFO(get_logger(), cur_type); + RCLCPP_DEBUG(get_logger(), cur_type); } RCLCPP_DEBUG(get_logger(), "list types service finished"); } +void ControllerManager::list_hardware_interfaces_srv_cb( + const std::shared_ptr, + std::shared_ptr response) +{ + auto state_interface_names = resource_manager_->state_interface_keys(); + for (const auto & state_interface_name : state_interface_names) { + controller_manager_msgs::msg::HardwareInterface hwi; + hwi.name = state_interface_name; + hwi.is_claimed = false; + response->state_interfaces.push_back(hwi); + } + auto command_interface_names = resource_manager_->command_interface_keys(); + for (const auto & command_interface_name : command_interface_names) { + controller_manager_msgs::msg::HardwareInterface hwi; + hwi.name = command_interface_name; + hwi.is_claimed = resource_manager_->command_interface_is_claimed(command_interface_name); + response->command_interfaces.push_back(hwi); + } +} + void ControllerManager::load_controller_service_cb( const std::shared_ptr request, std::shared_ptr response) @@ -811,8 +925,12 @@ std::vector ControllerManager::get_controller_names() return names; } -controller_interface::return_type -ControllerManager::update() +void ControllerManager::read() +{ + resource_manager_->read(); +} + +controller_interface::return_type ControllerManager::update() { std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); @@ -833,9 +951,15 @@ ControllerManager::update() if (switch_params_.do_switch) { manage_switch(); } + return ret; } +void ControllerManager::write() +{ + resource_manager_->write(); +} + std::vector & ControllerManager::RTControllerListWrapper::update_and_get_used_by_rt_list() { @@ -843,8 +967,7 @@ ControllerManager::RTControllerListWrapper::update_and_get_used_by_rt_list() return controllers_lists_[used_by_realtime_controllers_index_]; } -std::vector & -ControllerManager::RTControllerListWrapper::get_unused_list( +std::vector & ControllerManager::RTControllerListWrapper::get_unused_list( const std::lock_guard &) { assert(controllers_lock_.try_lock()); @@ -881,9 +1004,7 @@ int ControllerManager::RTControllerListWrapper::get_other_list(int index) const } void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using( - int index, - std::chrono::microseconds sleep_period) -const + int index, std::chrono::microseconds sleep_period) const { while (used_by_realtime_controllers_index_ == index) { if (!rclcpp::ok()) { diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp new file mode 100644 index 0000000000..9a2597615f --- /dev/null +++ b/controller_manager/src/ros2_control_node.cpp @@ -0,0 +1,55 @@ +// Copyright 2020 ROS2-Control Development Team +// +// 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 "controller_manager/controller_manager.hpp" +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr executor = + std::make_shared(); + std::string manager_node_name = "controller_manager"; + + auto cm = std::make_shared( + executor, + manager_node_name); + + // load controller_manager update time parameter + int update_rate = 100; + if (!cm->get_parameter("update_rate", update_rate)) { + throw std::runtime_error("update_rate parameter not existing or empty"); + } + RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", update_rate); + + auto timer = cm->create_wall_timer( + std::chrono::milliseconds(1000 / update_rate), + [&cm]() { + cm->read(); + cm->update(); + cm->write(); + }, + cm->deterministic_callback_group_); + + executor->add_node(cm); + executor->spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 2e2378a016..128b3cf1f3 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -29,12 +29,102 @@ #include "rclcpp/utilities.hpp" #include "test_controller/test_controller.hpp" -#include "test_robot_hardware/test_robot_hardware.hpp" constexpr auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT; constexpr auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; -class TestControllerManager : public ::testing::Test +namespace controller_manager_test +{ +constexpr auto urdf = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + test_actuator + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + +)"; +} // namespace controller_manager_test + +class ControllerManagerFixture : public ::testing::Test { public: static void SetUpTestCase() @@ -42,16 +132,21 @@ class TestControllerManager : public ::testing::Test rclcpp::init(0, nullptr); } - void SetUp() + static void TearDownTestCase() { - robot_ = std::make_shared(); - robot_->init(); + rclcpp::shutdown(); + } + void SetUp() + { executor_ = std::make_shared(); + cm_ = std::make_shared( + std::make_unique(controller_manager_test::urdf), + executor_, "test_controller_manager"); } - std::shared_ptr robot_; std::shared_ptr executor_; + std::shared_ptr cm_; }; class ControllerMock : public controller_interface::ControllerInterface diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 195dcb6beb..697f45aae5 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -24,8 +24,11 @@ namespace test_controller { +// indicating the node name under which the controller node +// is being loaded. constexpr char TEST_CONTROLLER_NAME[] = "test_controller_name"; -constexpr char TEST_CONTROLLER_TYPE[] = "test_controller"; +// corresponds to the name listed within the pluginlib xml +constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_controller"; class TestController : public controller_interface::ControllerInterface { public: @@ -36,6 +39,18 @@ class TestController : public controller_interface::ControllerInterface virtual ~TestController() = default; + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + CONTROLLER_MANAGER_PUBLIC controller_interface::return_type update() override; diff --git a/controller_manager/test/test_controller/test_controller.xml b/controller_manager/test/test_controller/test_controller.xml index dbccf5453f..e4d1d61ca5 100644 --- a/controller_manager/test/test_controller/test_controller.xml +++ b/controller_manager/test/test_controller/test_controller.xml @@ -1,6 +1,6 @@ - + Controller used for testing diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index e3db44d203..cd247f01fd 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -21,23 +21,23 @@ #include "controller_manager_msgs/srv/list_controllers.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "test_robot_hardware/test_robot_hardware.hpp" #include "./test_controller/test_controller.hpp" -TEST_F(TestControllerManager, controller_lifecycle) { - auto cm = std::make_shared( - robot_, executor_, - "test_controller_manager"); +using ::testing::_; +using ::testing::Return; +class TestControllerManager : public ControllerManagerFixture +{}; +TEST_F(TestControllerManager, controller_lifecycle) { auto test_controller = std::make_shared(); - cm->add_controller( + cm_->add_controller( test_controller, test_controller::TEST_CONTROLLER_NAME, - test_controller::TEST_CONTROLLER_TYPE); - EXPECT_EQ(1u, cm->get_loaded_controllers().size()); + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ(2, test_controller.use_count()); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm_->update()); EXPECT_EQ( 0u, test_controller->internal_counter) << @@ -47,7 +47,7 @@ TEST_F(TestControllerManager, controller_lifecycle) { lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_lifecycle_node()->get_current_state().id()); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm_->update()); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; // Start controller, will take effect at the end of the update function @@ -55,7 +55,7 @@ TEST_F(TestControllerManager, controller_lifecycle) { std::vector stop_controllers = {}; auto switch_future = std::async( std::launch::async, - &controller_manager::ControllerManager::switch_controller, cm, + &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); @@ -64,7 +64,7 @@ TEST_F(TestControllerManager, controller_lifecycle) { switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm_->update()); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; EXPECT_EQ( controller_interface::return_type::SUCCESS, @@ -74,7 +74,7 @@ TEST_F(TestControllerManager, controller_lifecycle) { lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_node()->get_current_state().id()); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm_->update()); EXPECT_EQ(1u, test_controller->internal_counter); // Stop controller, will take effect at the end of the update function @@ -82,7 +82,7 @@ TEST_F(TestControllerManager, controller_lifecycle) { stop_controllers = {test_controller::TEST_CONTROLLER_NAME}; switch_future = std::async( std::launch::async, - &controller_manager::ControllerManager::switch_controller, cm, + &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); @@ -91,7 +91,7 @@ TEST_F(TestControllerManager, controller_lifecycle) { switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm_->update()); EXPECT_EQ( 2u, test_controller->internal_counter) << @@ -106,14 +106,14 @@ TEST_F(TestControllerManager, controller_lifecycle) { test_controller->get_lifecycle_node()->get_current_state().id()); auto unload_future = std::async( std::launch::async, - &controller_manager::ControllerManager::unload_controller, cm, + &controller_manager::ControllerManager::unload_controller, cm_, test_controller::TEST_CONTROLLER_NAME); ASSERT_EQ( std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100))) << "unload_controller should be blocking until next update cycle"; - cm->update(); + cm_->update(); EXPECT_EQ( controller_interface::return_type::SUCCESS, unload_future.get() diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index c43ef101fe..e9849bdb73 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -31,7 +31,7 @@ using ::testing::Return; using namespace std::chrono_literals; -class TestControllerManagerSrvs : public TestControllerManager +class TestControllerManagerSrvs : public ControllerManagerFixture { public: TestControllerManagerSrvs() @@ -40,13 +40,15 @@ class TestControllerManagerSrvs : public TestControllerManager void SetUp() override { - TestControllerManager::SetUp(); - cm_ = std::make_shared( - robot_, executor_, - "test_controller_manager"); + ControllerManagerFixture::SetUp(); + update_timer_ = cm_->create_wall_timer( - std::chrono::milliseconds(10), - std::bind(&controller_manager::ControllerManager::update, cm_.get())); + std::chrono::milliseconds(10), [&]() { + cm_->read(); + cm_->update(); + cm_->write(); + } + ); executor_->add_node(cm_); @@ -90,7 +92,6 @@ class TestControllerManagerSrvs : public TestControllerManager } protected: - std::shared_ptr cm_; rclcpp::TimerBase::SharedPtr update_timer_; std::future executor_spin_future_; }; @@ -114,7 +115,7 @@ TEST_F(TestControllerManagerSrvs, list_controller_types) ASSERT_EQ( controller_types, result->base_classes.size()); - ASSERT_THAT(result->types, ::testing::Contains("test_controller")); + ASSERT_THAT(result->types, ::testing::Contains(test_controller::TEST_CONTROLLER_CLASS_NAME)); ASSERT_THAT( result->base_classes, ::testing::Contains("controller_interface::ControllerInterface")); @@ -137,14 +138,14 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv) { auto test_controller = std::make_shared(); auto abstract_test_controller = cm_->add_controller( test_controller, test_controller::TEST_CONTROLLER_NAME, - test_controller::TEST_CONTROLLER_TYPE); + test_controller::TEST_CONTROLLER_CLASS_NAME); EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ( 1u, result->controller.size()); ASSERT_EQ(test_controller::TEST_CONTROLLER_NAME, result->controller[0].name); - ASSERT_EQ(test_controller::TEST_CONTROLLER_TYPE, result->controller[0].type); + ASSERT_EQ(test_controller::TEST_CONTROLLER_CLASS_NAME, result->controller[0].type); ASSERT_EQ("inactive", result->controller[0].state); cm_->switch_controller( @@ -206,7 +207,7 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { // Add a controller, but stopped auto test_controller = cm_->load_controller( test_controller::TEST_CONTROLLER_NAME, - test_controller::TEST_CONTROLLER_TYPE); + test_controller::TEST_CONTROLLER_CLASS_NAME); // weak_ptr so the only controller shared_ptr instance is owned by the controller_manager and // can be completely destroyed before reloading the library @@ -234,9 +235,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { test_controller = cm_->load_controller( test_controller::TEST_CONTROLLER_NAME, - test_controller::TEST_CONTROLLER_TYPE); + test_controller::TEST_CONTROLLER_CLASS_NAME); test_controller_weak = test_controller; - // Start Controller cm_->switch_controller( {test_controller::TEST_CONTROLLER_NAME}, {}, @@ -288,7 +288,7 @@ TEST_F(TestControllerManagerSrvs, load_controller_srv) { auto result = call_service_and_wait(*client, request, srv_executor); ASSERT_FALSE(result->ok) << "There's no param specifying the type for " << request->name; rclcpp::Parameter joint_parameters(std::string(test_controller::TEST_CONTROLLER_NAME) + ".type", - test_controller::TEST_CONTROLLER_TYPE); + test_controller::TEST_CONTROLLER_CLASS_NAME); cm_->set_parameter(joint_parameters); result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); @@ -310,7 +310,7 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) { auto test_controller = std::make_shared(); auto abstract_test_controller = cm_->add_controller( test_controller, test_controller::TEST_CONTROLLER_NAME, - test_controller::TEST_CONTROLLER_TYPE); + test_controller::TEST_CONTROLLER_CLASS_NAME); EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); result = call_service_and_wait(*client, request, srv_executor, true); diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 68acfda697..a5a3d1d486 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -31,89 +31,87 @@ using ::testing::_; using ::testing::Return; -TEST_F(TestControllerManager, load_unknown_controller) +class TestLoadController : public ControllerManagerFixture +{}; + +TEST_F(TestLoadController, load_unknown_controller) { - controller_manager::ControllerManager cm(robot_, executor_, "test_controller_manager"); ASSERT_THROW( - cm.load_controller("unknown_controller_name", "unknown_controller_type"), std::runtime_error); + cm_->load_controller("unknown_controller_name", "unknown_controller_type"), std::runtime_error); } -TEST_F(TestControllerManager, load1_known_controller) +TEST_F(TestLoadController, load1_known_controller) { - controller_manager::ControllerManager cm(robot_, executor_, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller("test_controller_01", test_controller::TEST_CONTROLLER_TYPE)); - EXPECT_EQ(1u, cm.get_loaded_controllers().size()); + ASSERT_NO_THROW( + cm_->load_controller( + "test_controller_01", + test_controller::TEST_CONTROLLER_CLASS_NAME)); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); controller_manager::ControllerSpec abstract_test_controller = - cm.get_loaded_controllers()[0]; + cm_->get_loaded_controllers()[0]; auto lifecycle_node = abstract_test_controller.c->get_lifecycle_node(); - lifecycle_node->configure(); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller.c->get_lifecycle_node()->get_current_state().id()); } -TEST_F(TestControllerManager, load2_known_controller) +TEST_F(TestLoadController, load2_known_controller) { - controller_manager::ControllerManager cm(robot_, executor_, "test_controller_manager"); - std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; + std::string controller_type = test_controller::TEST_CONTROLLER_CLASS_NAME; // load the controller with name1 std::string controller_name1 = "test_controller1"; - ASSERT_NO_THROW(cm.load_controller(controller_name1, controller_type)); - EXPECT_EQ(1u, cm.get_loaded_controllers().size()); + ASSERT_NO_THROW(cm_->load_controller(controller_name1, controller_type)); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); controller_manager::ControllerSpec abstract_test_controller1 = - cm.get_loaded_controllers()[0]; + cm_->get_loaded_controllers()[0]; EXPECT_STREQ( controller_name1.c_str(), abstract_test_controller1.c->get_lifecycle_node()->get_name()); - abstract_test_controller1.c->get_lifecycle_node()->configure(); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); // load the same controller again with a different name std::string controller_name2 = "test_controller2"; - ASSERT_NO_THROW(cm.load_controller(controller_name2, controller_type)); - EXPECT_EQ(2u, cm.get_loaded_controllers().size()); + ASSERT_NO_THROW(cm_->load_controller(controller_name2, controller_type)); + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); controller_manager::ControllerSpec abstract_test_controller2 = - cm.get_loaded_controllers()[1]; + cm_->get_loaded_controllers()[1]; EXPECT_STREQ( controller_name2.c_str(), abstract_test_controller2.c->get_lifecycle_node()->get_name()); EXPECT_STREQ( controller_name2.c_str(), abstract_test_controller2.info.name.c_str()); - abstract_test_controller2.c->get_lifecycle_node()->configure(); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); } -TEST_F(TestControllerManager, update) +TEST_F(TestLoadController, update) { - controller_manager::ControllerManager cm(robot_, executor_, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller("test_controller_01", test_controller::TEST_CONTROLLER_TYPE)); + ASSERT_NO_THROW( + cm_->load_controller( + "test_controller_01", + test_controller::TEST_CONTROLLER_CLASS_NAME)); controller_manager::ControllerSpec abstract_test_controller = - cm.get_loaded_controllers()[0]; + cm_->get_loaded_controllers()[0]; auto lifecycle_node = abstract_test_controller.c->get_lifecycle_node(); - lifecycle_node->configure(); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller.c->get_lifecycle_node()->get_current_state().id()); } -TEST_F(TestControllerManager, switch_controller_empty) +TEST_F(TestLoadController, switch_controller_empty) { - auto cm = std::make_shared( - robot_, executor_, - "test_controller_manager"); - std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; + std::string controller_type = test_controller::TEST_CONTROLLER_CLASS_NAME; // load the controller with name1 std::string controller_name1 = "test_controller1"; - ASSERT_NO_THROW(cm->load_controller(controller_name1, controller_type)); - EXPECT_EQ(1u, cm->get_loaded_controllers().size()); + ASSERT_NO_THROW(cm_->load_controller(controller_name1, controller_type)); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); const auto UNSPECIFIED = 0; @@ -122,44 +120,43 @@ TEST_F(TestControllerManager, switch_controller_empty) EXPECT_EQ( controller_interface::return_type::SUCCESS, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)) ) << "Switch with no controllers specified"; EXPECT_EQ( controller_interface::return_type::SUCCESS, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration(0, 0)) ) << "Switch with no controllers specified"; EXPECT_EQ( controller_interface::return_type::SUCCESS, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, UNSPECIFIED, true, rclcpp::Duration(0, 0)) ) << "Switch with no controllers specified, unspecified strictness defaults to BEST_EFFORT"; - start_controllers = {"nonexistent_controller"}; stop_controllers = {}; EXPECT_EQ( controller_interface::return_type::ERROR, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)) ) << "STRICT switch with nonexistent controller specified"; EXPECT_EQ( controller_interface::return_type::SUCCESS, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration(0, 0)) ) << "BEST_EFFORT switch with nonexistent controller specified"; EXPECT_EQ( controller_interface::return_type::SUCCESS, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, UNSPECIFIED, true, rclcpp::Duration(0, 0)) ) << "Unspecified switch with nonexistent controller specified, defaults to BEST_EFFORT"; @@ -170,81 +167,75 @@ TEST_F(TestControllerManager, switch_controller_empty) stop_controllers = {"nonexistent_controller"}; EXPECT_EQ( controller_interface::return_type::ERROR, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)) ) << "STRICT switch with nonexistent controller specified"; EXPECT_EQ( controller_interface::return_type::SUCCESS, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration(0, 0)) ) << "BEST_EFFORT switch with nonexistent controller specified"; - start_controllers = {"nonexistent_controller"}; stop_controllers = {"nonexistent_controller"}; EXPECT_EQ( controller_interface::return_type::ERROR, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)) ) << "STRICT switch with nonexistent controller specified"; EXPECT_EQ( controller_interface::return_type::SUCCESS, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration(0, 0)) ) << "BEST_EFFORT switch with nonexistent controller specified"; } -TEST_F(TestControllerManager, switch_controller) +TEST_F(TestLoadController, switch_controller) { - auto cm = std::make_shared( - robot_, executor_, - "test_controller_manager"); - std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; + std::string controller_type = test_controller::TEST_CONTROLLER_CLASS_NAME; // load the controller with name1 std::string controller_name1 = "test_controller1"; - ASSERT_NO_THROW(cm->load_controller(controller_name1, controller_type)); - EXPECT_EQ(1u, cm->get_loaded_controllers().size()); + ASSERT_NO_THROW(cm_->load_controller(controller_name1, controller_type)); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); controller_manager::ControllerSpec abstract_test_controller1 = - cm->get_loaded_controllers()[0]; + cm_->get_loaded_controllers()[0]; ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); - { // Test stopping an stopped controller std::vector start_controllers = {}; std::vector stop_controllers = {controller_name1}; EXPECT_EQ( controller_interface::return_type::ERROR, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)) ) << "STRICT switch with stopped controller specified"; EXPECT_EQ( controller_interface::return_type::SUCCESS, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration(0, 0)) ) << "BEST_EFFORT switch stopped controller specified"; } - { // STRICT Combination of valid controller + invalid controller std::vector start_controllers = {controller_name1, "nonexistent_controller"}; std::vector stop_controllers = {}; EXPECT_EQ( controller_interface::return_type::ERROR, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)) ) << "STRICT switch with nonexistent controller specified"; @@ -253,7 +244,7 @@ TEST_F(TestControllerManager, switch_controller) stop_controllers = {"nonexistent_controller"}; EXPECT_EQ( controller_interface::return_type::ERROR, - cm->switch_controller( + cm_->switch_controller( start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)) ) << "STRICT switch with nonexistent controller specified"; @@ -262,13 +253,13 @@ TEST_F(TestControllerManager, switch_controller) // Only testing with STRICT now for simplicity { // Test starting an stopped controller, and stopping afterwards RCLCPP_INFO( - cm->get_logger(), + cm_->get_logger(), "Starting stopped controller"); std::vector start_controllers = {controller_name1}; std::vector stop_controllers = {}; auto switch_future = std::async( std::launch::async, - &controller_manager::ControllerManager::switch_controller, cm, + &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); @@ -276,7 +267,7 @@ TEST_F(TestControllerManager, switch_controller) std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; - cm->update(); + cm_->update(); EXPECT_EQ( controller_interface::return_type::SUCCESS, switch_future.get() @@ -286,16 +277,15 @@ TEST_F(TestControllerManager, switch_controller) lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); - // Stop controller start_controllers = {}; stop_controllers = {controller_name1}; RCLCPP_INFO( - cm->get_logger(), + cm_->get_logger(), "Stopping started controller"); switch_future = std::async( std::launch::async, - &controller_manager::ControllerManager::switch_controller, cm, + &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); @@ -303,7 +293,7 @@ TEST_F(TestControllerManager, switch_controller) std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; - cm->update(); + cm_->update(); EXPECT_EQ( controller_interface::return_type::SUCCESS, switch_future.get() @@ -315,23 +305,20 @@ TEST_F(TestControllerManager, switch_controller) } } -TEST_F(TestControllerManager, switch_multiple_controllers) +TEST_F(TestLoadController, switch_multiple_controllers) { - auto cm = std::make_shared( - robot_, executor_, - "test_controller_manager"); - std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; + std::string controller_type = test_controller::TEST_CONTROLLER_CLASS_NAME; // load the controller with name1 std::string controller_name1 = "test_controller1"; std::string controller_name2 = "test_controller2"; - ASSERT_NO_THROW(cm->load_controller(controller_name1, controller_type)); - ASSERT_NO_THROW(cm->load_controller(controller_name2, controller_type)); - EXPECT_EQ(2u, cm->get_loaded_controllers().size()); + ASSERT_NO_THROW(cm_->load_controller(controller_name1, controller_type)); + ASSERT_NO_THROW(cm_->load_controller(controller_name2, controller_type)); + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); controller_manager::ControllerSpec abstract_test_controller1 = - cm->get_loaded_controllers()[0]; + cm_->get_loaded_controllers()[0]; controller_manager::ControllerSpec abstract_test_controller2 = - cm->get_loaded_controllers()[1]; + cm_->get_loaded_controllers()[1]; ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, @@ -343,13 +330,13 @@ TEST_F(TestControllerManager, switch_multiple_controllers) // Only testing with STRICT now for simplicity { // Test starting an stopped controller, and stopping afterwards RCLCPP_INFO( - cm->get_logger(), + cm_->get_logger(), "Starting stopped controller #1"); std::vector start_controllers = {controller_name1}; std::vector stop_controllers = {}; auto switch_future = std::async( std::launch::async, - &controller_manager::ControllerManager::switch_controller, cm, + &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); @@ -357,7 +344,7 @@ TEST_F(TestControllerManager, switch_multiple_controllers) std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; - cm->update(); + cm_->update(); EXPECT_EQ( controller_interface::return_type::SUCCESS, switch_future.get() @@ -374,11 +361,11 @@ TEST_F(TestControllerManager, switch_multiple_controllers) start_controllers = {controller_name2}; stop_controllers = {controller_name1}; RCLCPP_INFO( - cm->get_logger(), + cm_->get_logger(), "Stopping controller #1, starting controller #2"); switch_future = std::async( std::launch::async, - &controller_manager::ControllerManager::switch_controller, cm, + &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); @@ -386,7 +373,7 @@ TEST_F(TestControllerManager, switch_multiple_controllers) std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; - cm->update(); + cm_->update(); EXPECT_EQ( controller_interface::return_type::SUCCESS, switch_future.get() @@ -402,11 +389,11 @@ TEST_F(TestControllerManager, switch_multiple_controllers) start_controllers = {}; stop_controllers = {controller_name2}; RCLCPP_INFO( - cm->get_logger(), + cm_->get_logger(), "Stopping controller #1, starting controller #2"); switch_future = std::async( std::launch::async, - &controller_manager::ControllerManager::switch_controller, cm, + &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); @@ -414,13 +401,12 @@ TEST_F(TestControllerManager, switch_multiple_controllers) std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; - cm->update(); + cm_->update(); EXPECT_EQ( controller_interface::return_type::SUCCESS, switch_future.get() ); - ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index dba0452ecc..6f54882df9 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -16,10 +16,12 @@ find_package(rosidl_default_generators REQUIRED) set(msg_files msg/ControllerState.msg + msg/HardwareInterface.msg ) set(srv_files srv/ListControllers.srv srv/ListControllerTypes.srv + srv/ListHardwareInterfaces.srv srv/LoadController.srv srv/ReloadControllerLibraries.srv srv/SwitchController.srv diff --git a/controller_manager_msgs/msg/HardwareInterface.msg b/controller_manager_msgs/msg/HardwareInterface.msg new file mode 100644 index 0000000000..b6ce4bab24 --- /dev/null +++ b/controller_manager_msgs/msg/HardwareInterface.msg @@ -0,0 +1,2 @@ +string name +bool is_claimed diff --git a/controller_manager_msgs/srv/ListHardwareInterfaces.srv b/controller_manager_msgs/srv/ListHardwareInterfaces.srv new file mode 100644 index 0000000000..5bf1767cbd --- /dev/null +++ b/controller_manager_msgs/srv/ListHardwareInterfaces.srv @@ -0,0 +1,4 @@ + +--- +HardwareInterface[] command_interfaces +HardwareInterface[] state_interfaces diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 3c460859e5..56c5801006 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -25,9 +25,7 @@ add_library( src/components/sensor.cpp src/components/system.cpp src/component_parser.cpp - src/operation_mode_handle.cpp src/resource_manager.cpp - src/robot_hardware.cpp ) target_include_directories( hardware_interface @@ -69,22 +67,11 @@ if(BUILD_TESTING) target_include_directories(test_macros PRIVATE include) ament_target_dependencies(test_macros rcpputils) - ament_add_gmock(test_robot_hardware_interfaces test/test_robot_hardware_interface.cpp) - target_link_libraries(test_robot_hardware_interfaces hardware_interface) - - ament_add_gmock(test_register_actuators test/test_register_actuators.cpp) - target_link_libraries(test_register_actuators hardware_interface) - ament_target_dependencies(test_register_actuators rcpputils) - - ament_add_gmock(test_register_joints test/test_register_joints.cpp) - target_link_libraries(test_register_joints hardware_interface) - ament_target_dependencies(test_register_joints rcpputils) - ament_add_gmock(test_actuator_handle test/test_actuator_handle.cpp) target_link_libraries(test_actuator_handle hardware_interface) ament_target_dependencies(test_actuator_handle rcpputils) - ament_add_gmock(test_joint_handle test/test_joint_handle.cpp) + ament_add_gmock(test_joint_handle test/test_handle.cpp) target_link_libraries(test_joint_handle hardware_interface) ament_target_dependencies(test_joint_handle rcpputils) diff --git a/hardware_interface/include/hardware_interface/joint_handle.hpp b/hardware_interface/include/hardware_interface/joint_handle.hpp deleted file mode 100644 index ac61db4a35..0000000000 --- a/hardware_interface/include/hardware_interface/joint_handle.hpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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. - -#ifndef HARDWARE_INTERFACE__JOINT_HANDLE_HPP_ -#define HARDWARE_INTERFACE__JOINT_HANDLE_HPP_ - -#include "hardware_interface/handle.hpp" - -namespace hardware_interface -{ -/** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public ReadWriteHandle -{ -public: - using ReadWriteHandle::ReadWriteHandle; -}; -} // namespace hardware_interface - -#endif // HARDWARE_INTERFACE__JOINT_HANDLE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index d5a23c8fc2..7411a3c580 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ #include +#include #include #include "hardware_interface/handle.hpp" @@ -50,6 +51,16 @@ class LoanedCommandInterface } } + const std::string & get_name() const + { + return command_interface_.get_name(); + } + + const std::string & get_interface_name() const + { + return command_interface_.get_interface_name(); + } + void set_value(double val) { command_interface_.set_value(val); diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index ca3df8189c..7d6d4d5c1d 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ #include +#include #include #include "hardware_interface/handle.hpp" @@ -50,6 +51,16 @@ class LoanedStateInterface } } + const std::string & get_name() const + { + return state_interface_.get_name(); + } + + const std::string & get_interface_name() const + { + return state_interface_.get_interface_name(); + } + double get_value() const { return state_interface_.get_value(); diff --git a/hardware_interface/include/hardware_interface/operation_mode_handle.hpp b/hardware_interface/include/hardware_interface/operation_mode_handle.hpp deleted file mode 100644 index bd9f2053c5..0000000000 --- a/hardware_interface/include/hardware_interface/operation_mode_handle.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2017 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. - -#ifndef HARDWARE_INTERFACE__OPERATION_MODE_HANDLE_HPP_ -#define HARDWARE_INTERFACE__OPERATION_MODE_HANDLE_HPP_ - -#include - -#include "hardware_interface/visibility_control.h" - -namespace hardware_interface -{ -/// Enum for Operation Mode. -/** Operation can either be active or inactive */ -enum class HARDWARE_INTERFACE_PUBLIC_TYPE OperationMode: bool -{ - ACTIVE = true, - INACTIVE = false -}; - -/// A handle for Operation Mode. -/** Used to set status to active or inactive for operation such as read/write. */ -class OperationModeHandle -{ -public: - HARDWARE_INTERFACE_PUBLIC - OperationModeHandle(); - - /// Constructor of Operation Mode. - /** - * The mode handles are passive in terms of ownership for the mode pointer. - * This means the handle is allowed to read and write the respective mode, - * however is not allowed to reallocate or delete the memory storage. - * \param name The name of the joint - * \param mode A pointer to the storage for this hardware's operation mode - */ - HARDWARE_INTERFACE_PUBLIC - OperationModeHandle( - const std::string & name, - OperationMode * mode); - - HARDWARE_INTERFACE_PUBLIC - const std::string & - get_name() const; - - HARDWARE_INTERFACE_PUBLIC - void - set_mode(OperationMode mode); - - HARDWARE_INTERFACE_PUBLIC - bool valid_pointers() const; - -private: - std::string name_; - OperationMode * mode_; -}; - -} // namespace hardware_interface - -#endif // HARDWARE_INTERFACE__OPERATION_MODE_HANDLE_HPP_ diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index ad3486cbb0..dea9585f49 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -38,6 +38,9 @@ class ResourceStorage; class ResourceManager { public: + /// Default constructor for the Resource Manager. + ResourceManager(); + /// Constructor for the Resource Manager. /** * The implementation loads the specified urdf and initializes the @@ -58,6 +61,18 @@ class ResourceManager ~ResourceManager(); + /// Load resources from on a given URDF. + /** + * The resource manager can be post initialized with a given URDF. + * This is mainly used in conjunction with the default constructor + * in which the URDF might not be present at first initialization. + * + * \param urdf string containing the URDF. + * \param validate_interfaces boolean argument indicating whether the exported + * interfaces ought to be validated. Defaults to true. + */ + void load_urdf(const std::string & urdf, bool validate_interfaces = true); + /// Claim a state interface given its key. /** * The resource is claimed as long as being in scope. @@ -174,6 +189,12 @@ class ResourceManager */ void import_component(std::unique_ptr system); + /// Reads all loaded hardware components. + void read(); + + /// Write all loaded hardware components. + void write(); + private: void release_command_interface(const std::string & key); diff --git a/hardware_interface/include/hardware_interface/robot_hardware.hpp b/hardware_interface/include/hardware_interface/robot_hardware.hpp deleted file mode 100644 index b422b264c4..0000000000 --- a/hardware_interface/include/hardware_interface/robot_hardware.hpp +++ /dev/null @@ -1,114 +0,0 @@ -// Copyright 2017 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. - -#ifndef HARDWARE_INTERFACE__ROBOT_HARDWARE_HPP_ -#define HARDWARE_INTERFACE__ROBOT_HARDWARE_HPP_ - -#include -#include -#include - -#include "control_msgs/msg/dynamic_joint_state.hpp" -#include "hardware_interface/actuator_handle.hpp" -#include "hardware_interface/joint_handle.hpp" -#include "hardware_interface/operation_mode_handle.hpp" -#include "hardware_interface/robot_hardware_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/visibility_control.h" - -namespace hardware_interface -{ -class RobotHardware : public RobotHardwareInterface -{ -public: - HARDWARE_INTERFACE_PUBLIC - RobotHardware() = default; - - HARDWARE_INTERFACE_PUBLIC - virtual - ~RobotHardware() = default; - - HARDWARE_INTERFACE_PUBLIC - return_type - register_operation_mode_handle(OperationModeHandle * operation_mode); - - HARDWARE_INTERFACE_PUBLIC - return_type - get_operation_mode_handle(const std::string & name, OperationModeHandle ** operation_mode_handle); - - HARDWARE_INTERFACE_PUBLIC - std::vector - get_registered_write_op_names(); - - HARDWARE_INTERFACE_PUBLIC - std::vector - get_registered_operation_mode_handles(); - - HARDWARE_INTERFACE_PUBLIC - hardware_interface_ret_t register_actuator( - const std::string & actuator_name, const std::string & interface_name, - double default_value = 0.0); - - HARDWARE_INTERFACE_PUBLIC - hardware_interface_ret_t register_joint( - const std::string & joint_name, const std::string & interface_name, double default_value = 0.0); - - HARDWARE_INTERFACE_PUBLIC - hardware_interface_ret_t get_actuator_handle(ActuatorHandle & actuator_handle); - - HARDWARE_INTERFACE_PUBLIC - hardware_interface_ret_t get_joint_handle(JointHandle & joint_handle); - - HARDWARE_INTERFACE_PUBLIC - hardware_interface_ret_t get_actuator_handles( - std::vector & actuator_handles, - const std::string & interface_name); - - HARDWARE_INTERFACE_PUBLIC - hardware_interface_ret_t get_joint_handles( - std::vector & joint_handles, - const std::string & interface_name); - - HARDWARE_INTERFACE_PUBLIC - const std::vector & get_registered_actuator_names(); - - HARDWARE_INTERFACE_PUBLIC - const std::vector & get_registered_joint_names(); - - HARDWARE_INTERFACE_PUBLIC - const std::vector & get_registered_actuator_interface_names( - const std::string & actuator_name); - - HARDWARE_INTERFACE_PUBLIC - const std::vector & get_registered_joint_interface_names( - const std::string & joint_name); - - HARDWARE_INTERFACE_PUBLIC - std::vector get_registered_actuators(); - - HARDWARE_INTERFACE_PUBLIC - std::vector get_registered_joints(); - -private: - std::vector registered_operation_mode_handles_; - - control_msgs::msg::DynamicJointState registered_actuators_; - control_msgs::msg::DynamicJointState registered_joints_; -}; - -using RobotHardwareSharedPtr = std::shared_ptr; - -} // namespace hardware_interface - -#endif // HARDWARE_INTERFACE__ROBOT_HARDWARE_HPP_ diff --git a/hardware_interface/include/hardware_interface/robot_hardware_interface.hpp b/hardware_interface/include/hardware_interface/robot_hardware_interface.hpp deleted file mode 100644 index 5a2e57697c..0000000000 --- a/hardware_interface/include/hardware_interface/robot_hardware_interface.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2017 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. - -#ifndef HARDWARE_INTERFACE__ROBOT_HARDWARE_INTERFACE_HPP_ -#define HARDWARE_INTERFACE__ROBOT_HARDWARE_INTERFACE_HPP_ - -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/visibility_control.h" - -namespace hardware_interface -{ - -class RobotHardwareInterface -{ -public: - HARDWARE_INTERFACE_PUBLIC - RobotHardwareInterface() = default; - - HARDWARE_INTERFACE_PUBLIC - virtual - ~RobotHardwareInterface() = default; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type init() = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type read() = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type write() = 0; -}; - -} // namespace hardware_interface - -#endif // HARDWARE_INTERFACE__ROBOT_HARDWARE_INTERFACE_HPP_ diff --git a/hardware_interface/src/operation_mode_handle.cpp b/hardware_interface/src/operation_mode_handle.cpp deleted file mode 100644 index 25ee0b4290..0000000000 --- a/hardware_interface/src/operation_mode_handle.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2017 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 "hardware_interface/operation_mode_handle.hpp" - -#include -#include - -#include "hardware_interface/macros.hpp" - -namespace hardware_interface -{ - -OperationModeHandle::OperationModeHandle() -: name_(), - mode_(nullptr) -{} - -OperationModeHandle::OperationModeHandle( - const std::string & name, - OperationMode * mode) -: name_(name), - mode_(mode) -{ - THROW_ON_NULLPTR(mode) -} - -const std::string & -OperationModeHandle::get_name() const -{ - return name_; -} - -void -OperationModeHandle::set_mode(OperationMode mode) -{ - THROW_ON_NULLPTR(mode_) - - * mode_ = mode; -} - -bool OperationModeHandle::valid_pointers() const -{ - return mode_; -} - -} // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 200b6bd39c..1c870bccd1 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -110,7 +110,9 @@ class ResourceStorage { initialize_hardware( hardware_info, sensor_loader_, sensors_); - sensors_.back().configure(hardware_info); + if (return_type::OK != sensors_.back().configure(hardware_info)) { + throw std::runtime_error(std::string("failed to configure ") + hardware_info.name); + } import_state_interfaces(sensors_.back()); } @@ -120,7 +122,9 @@ class ResourceStorage { initialize_hardware( hardware_info, system_loader_, systems_); - systems_.back().configure(hardware_info); + if (return_type::OK != systems_.back().configure(hardware_info)) { + throw std::runtime_error(std::string("failed to configure ") + hardware_info.name); + } import_state_interfaces(systems_.back()); import_command_interfaces(systems_.back(), claimed_command_interface_map); } @@ -138,17 +142,25 @@ class ResourceStorage std::unordered_map command_interface_map_; }; +ResourceManager::ResourceManager() +: resource_storage_(std::make_unique()) +{} + ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager(const std::string & urdf, bool validate_interfaces) : resource_storage_(std::make_unique()) +{ + load_urdf(urdf, validate_interfaces); +} + +void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); - for (const auto & hardware : hardware_info) { if (hardware.type == actuator_type) { resource_storage_->initialize_actuator(hardware, claimed_command_interface_map_); @@ -321,4 +333,28 @@ size_t ResourceManager::system_components_size() const { return resource_storage_->systems_.size(); } + +void ResourceManager::read() +{ + for (auto & component : resource_storage_->actuators_) { + component.read(); + } + for (auto & component : resource_storage_->sensors_) { + component.read(); + } + for (auto & component : resource_storage_->systems_) { + component.read(); + } +} + +void ResourceManager::write() +{ + for (auto & component : resource_storage_->actuators_) { + component.write(); + } + for (auto & component : resource_storage_->systems_) { + component.write(); + } +} + } // namespace hardware_interface diff --git a/hardware_interface/src/robot_hardware.cpp b/hardware_interface/src/robot_hardware.cpp deleted file mode 100644 index 6f95200e08..0000000000 --- a/hardware_interface/src/robot_hardware.cpp +++ /dev/null @@ -1,364 +0,0 @@ -// Copyright 2017 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 "hardware_interface/robot_hardware.hpp" - -#include -#include -#include - -#include "hardware_interface/macros.hpp" -#include "hardware_interface/operation_mode_handle.hpp" -#include "rcutils/logging_macros.h" - -namespace -{ -constexpr auto kOperationModeLoggerName = "joint operation mode handle"; -constexpr auto kActuatorLoggerName = "actuator handle"; -constexpr auto kJointLoggerName = "joint handle"; -} - -namespace hardware_interface -{ -/// Register the handle to the handle list if the handle's name is not found. -/** - * \param[in] registered_handles The handle list. - * \param[in] handle The handle to be registered. - * \param[in] logger_name The name of the logger. - * \return The return code, one of `OK` or `ERROR`. - */ -template -return_type -register_handle(std::vector & registered_handles, T * handle, const std::string & logger_name) -{ - if (handle->get_name().empty()) { - RCUTILS_LOG_ERROR_NAMED(logger_name.c_str(), "cannot register handle! No name is specified"); - return return_type::ERROR; - } - - if (!handle->valid_pointers()) { - RCUTILS_LOG_ERROR_NAMED(logger_name.c_str(), "cannot register handle! Points to nullptr!"); - return return_type::ERROR; - } - - auto handle_pos = std::find_if( - registered_handles.begin(), registered_handles.end(), - [&](auto handle_ptr) -> bool { - return handle_ptr->get_name() == handle->get_name(); - }); - - // handle exists already - if (handle_pos != registered_handles.end()) { - RCUTILS_LOG_ERROR_NAMED(logger_name.c_str(), "cannot register handle! Handle exists already"); - return return_type::ERROR; - } - registered_handles.push_back(handle); - return return_type::OK; -} - -return_type -RobotHardware::register_operation_mode_handle(OperationModeHandle * operation_mode_handle) -{ - return register_handle( - registered_operation_mode_handles_, - operation_mode_handle, - kOperationModeLoggerName); -} - -/// Find the handle by name from the registered handle list. -/** - * \param[in] registered_handles The registered handle list. - * \param[in] name The handle's name. - * \param[in] logger_name The name of the logger. - * \param[out] handle the handle if found. - * \return The return code, one of `OK` or `ERROR`. - */ -template -return_type -get_handle( - std::vector & registered_handles, - const std::string & name, - const std::string & logger_name, - T ** handle) -{ - if (name.empty()) { - RCUTILS_LOG_ERROR_NAMED(logger_name.c_str(), "cannot get handle! No name given"); - return return_type::ERROR; - } - - auto handle_pos = std::find_if( - registered_handles.begin(), registered_handles.end(), - [&](auto handle_ptr) -> bool { - return handle_ptr->get_name() == name; - }); - - if (handle_pos == registered_handles.end()) { - RCUTILS_LOG_ERROR_NAMED( - logger_name.c_str(), "cannot get handle. No joint %s found.", - name.c_str()); - return return_type::ERROR; - } - - *handle = *handle_pos; - return return_type::OK; -} - -return_type -RobotHardware::get_operation_mode_handle( - const std::string & name, OperationModeHandle ** operation_mode_handle) -{ - THROW_ON_NOT_NULLPTR(*operation_mode_handle) - return get_handle( - registered_operation_mode_handles_, - name, - kOperationModeLoggerName, - operation_mode_handle); -} - -template -std::vector -get_registered_names(std::vector & registered_handles) -{ - std::vector names; - names.reserve(registered_handles.size()); - for (auto handle : registered_handles) { - names.push_back(handle->get_name()); - } - return names; -} - -std::vector -RobotHardware::get_registered_write_op_names() -{ - return get_registered_names(registered_operation_mode_handles_); -} - -std::vector -RobotHardware::get_registered_operation_mode_handles() -{ - return registered_operation_mode_handles_; -} - -hardware_interface_ret_t register_handle( - const std::string & handle_name, - const std::string & interface_name, - const double default_value, - control_msgs::msg::DynamicJointState & registered, - const std::string & logger_name) -{ - if (handle_name.empty() || interface_name.empty()) { - RCUTILS_LOG_ERROR_NAMED(logger_name.c_str(), "handle name or interface is empty!"); - return return_type::ERROR; - } - - const auto & names_list = registered.joint_names; - const auto it = std::find(names_list.cbegin(), names_list.cend(), handle_name); - if (it == names_list.cend()) { - registered.joint_names.push_back(handle_name); - control_msgs::msg::InterfaceValue iv; - iv.interface_names = {interface_name}; - iv.values = {default_value}; - registered.interface_values.push_back(iv); - return return_type::OK; - } else { - const auto index = std::distance(names_list.cbegin(), it); - auto & ivs = registered.interface_values[static_cast(index)]; - const auto interface_names = ivs.interface_names; - const auto it = std::find(interface_names.cbegin(), interface_names.cend(), interface_name); - if (it == interface_names.cend()) { - ivs.interface_names.push_back(interface_name); - ivs.values.push_back(default_value); - return return_type::OK; - } else { - RCUTILS_LOG_ERROR_NAMED( - logger_name.c_str(), "handle with interface (%s: %s) is already registered!", - handle_name.c_str(), interface_name.c_str()); - return return_type::ERROR; - } - } -} - -hardware_interface_ret_t RobotHardware::register_actuator( - const std::string & actuator_name, - const std::string & interface_name, - const double default_value) -{ - return register_handle( - actuator_name, interface_name, default_value, registered_actuators_, - kActuatorLoggerName); -} - -hardware_interface_ret_t RobotHardware::register_joint( - const std::string & joint_name, - const std::string & interface_name, - double default_value) -{ - return register_handle( - joint_name, interface_name, default_value, registered_joints_, - kJointLoggerName); -} - -template -hardware_interface_ret_t get_handle( - HandleType & handle, - control_msgs::msg::DynamicJointState & registered, - const std::string & logger_name) -{ - const auto & handle_name = handle.get_name(); - const auto & interface_name = handle.get_interface_name(); - - if (handle_name.empty() || interface_name.empty()) { - RCUTILS_LOG_ERROR_NAMED(logger_name.c_str(), "name or interface is ill-defined!"); - return return_type::ERROR; - } - - const auto & names_list = registered.joint_names; - const auto it = std::find(names_list.cbegin(), names_list.cend(), handle_name); - if (it == names_list.cend()) { - RCUTILS_LOG_ERROR_NAMED( - logger_name.c_str(), "handle with name %s not found!", - handle_name.c_str()); - return return_type::ERROR; - } - - const auto index = std::distance(names_list.cbegin(), it); - auto & ivs = registered.interface_values[static_cast(index)]; - const auto interface_names = ivs.interface_names; - const auto if_it = std::find(interface_names.cbegin(), interface_names.cend(), interface_name); - if (if_it != interface_names.cend()) { - const auto value_index = std::distance(interface_names.cbegin(), if_it); - handle = handle.with_value_ptr(&(ivs.values[static_cast(value_index)])); - return return_type::OK; - } else { - RCUTILS_LOG_ERROR_NAMED( - logger_name.c_str(), - "handle with interface (%s: %s) wasn't found!", handle_name.c_str(), interface_name.c_str()); - return return_type::ERROR; - } - - return return_type::ERROR; -} - -hardware_interface_ret_t RobotHardware::get_actuator_handle(ActuatorHandle & actuator_handle) -{ - return get_handle(actuator_handle, registered_actuators_, kActuatorLoggerName); -} - -hardware_interface_ret_t RobotHardware::get_joint_handle(JointHandle & joint_handle) -{ - return get_handle(joint_handle, registered_joints_, kJointLoggerName); -} - -template -hardware_interface_ret_t get_handles( - std::vector & handles, - std::vector && registered, - const std::string & interface_name) -{ - std::copy_if( - registered.begin(), registered.end(), std::back_inserter(handles), [&](const auto & handle) { - return handle.get_interface_name() == interface_name; - }); - return return_type::OK; -} - -hardware_interface_ret_t RobotHardware::get_actuator_handles( - std::vector & actuator_handles, const std::string & interface_name) -{ - return get_handles(actuator_handles, get_registered_actuators(), interface_name); -} - -hardware_interface_ret_t RobotHardware::get_joint_handles( - std::vector & joint_handles, - const std::string & interface_name) -{ - return get_handles(joint_handles, get_registered_joints(), interface_name); -} - -const std::vector & RobotHardware::get_registered_actuator_names() -{ - return registered_actuators_.joint_names; -} - -const std::vector & RobotHardware::get_registered_joint_names() -{ - return registered_joints_.joint_names; -} - -template -const std::vector & get_registered_interface_names( - const std::string & name, - control_msgs::msg::DynamicJointState & registered) -{ - const auto & it = std::find( - registered.joint_names.begin(), registered.joint_names.end(), name); - - if (it == registered.joint_names.end()) { - throw std::runtime_error(name + " not found"); - } - - // joint found, can safely cast here - const auto joint_index = - static_cast(std::distance(registered.joint_names.begin(), it)); - - return registered.interface_values[joint_index].interface_names; -} - -const std::vector & RobotHardware::get_registered_actuator_interface_names( - const std::string & actuator_name) -{ - return get_registered_interface_names(actuator_name, registered_actuators_); -} - -const std::vector & RobotHardware::get_registered_joint_interface_names( - const std::string & joint_name) -{ - return get_registered_interface_names(joint_name, registered_joints_); -} - -template -std::vector get_registered_handles(control_msgs::msg::DynamicJointState & registered) -{ - std::vector result; - result.reserve(registered.joint_names.size()); // rough estimate - - auto & handle_names = registered.joint_names; - auto & interface_values = registered.interface_values; - - assert(registered.joint_names.size() == registered.interface_values.size()); - for (auto i = 0u; i < handle_names.size(); ++i) { - auto & jointernal_interfaces = interface_values[i]; - assert(jointernal_interfaces.interface_names.size() == jointernal_interfaces.values.size()); - - for (auto j = 0u; j < jointernal_interfaces.interface_names.size(); ++j) { - result.emplace_back( - handle_names[i], jointernal_interfaces.interface_names[j], - &jointernal_interfaces.values[j]); - } - } - - return result; -} - -std::vector RobotHardware::get_registered_actuators() -{ - return get_registered_handles(registered_actuators_); -} - -std::vector RobotHardware::get_registered_joints() -{ - return get_registered_handles(registered_joints_); -} - -} // namespace hardware_interface diff --git a/hardware_interface/test/test_joint_handle.cpp b/hardware_interface/test/test_handle.cpp similarity index 75% rename from hardware_interface/test/test_joint_handle.cpp rename to hardware_interface/test/test_handle.cpp index 6400a6a4be..ae5c598b66 100644 --- a/hardware_interface/test/test_joint_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -13,9 +13,8 @@ // limitations under the License. #include -#include "hardware_interface/joint_handle.hpp" +#include "hardware_interface/handle.hpp" -using hardware_interface::JointHandle; using hardware_interface::CommandInterface; using hardware_interface::StateInterface; @@ -25,7 +24,7 @@ constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; } // namespace -TEST(TestJointHandle, command_interface) +TEST(TestHandle, command_interface) { double value = 1.337; CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; @@ -34,7 +33,7 @@ TEST(TestJointHandle, command_interface) EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); } -TEST(TestJointHandle, state_interface) +TEST(TestHandle, state_interface) { double value = 1.337; StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; @@ -42,33 +41,33 @@ TEST(TestJointHandle, state_interface) // interface.set_value(5); compiler error, no set_value function } -TEST(TestJointHandle, name_getters_work) +TEST(TestHandle, name_getters_work) { - JointHandle handle{JOINT_NAME, FOO_INTERFACE}; + StateInterface handle{JOINT_NAME, FOO_INTERFACE}; EXPECT_EQ(handle.get_name(), JOINT_NAME); EXPECT_EQ(handle.get_interface_name(), FOO_INTERFACE); } -TEST(TestJointHandle, value_methods_throw_for_nullptr) +TEST(TestHandle, value_methods_throw_for_nullptr) { - JointHandle handle{JOINT_NAME, FOO_INTERFACE}; + CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; EXPECT_ANY_THROW(handle.get_value()); EXPECT_ANY_THROW(handle.set_value(0.0)); } -TEST(TestJointHandle, value_methods_work_on_non_nullptr) +TEST(TestHandle, value_methods_work_on_non_nullptr) { double value = 1.337; - JointHandle handle{JOINT_NAME, FOO_INTERFACE, &value}; + CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value}; EXPECT_DOUBLE_EQ(handle.get_value(), value); EXPECT_NO_THROW(handle.set_value(0.0)); EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } -TEST(TestJointHandle, with_value_ptr_initializes_new_handle_correctly) +TEST(TestHandle, with_value_ptr_initializes_new_handle_correctly) { double value = 1.337; - JointHandle handle{JOINT_NAME, FOO_INTERFACE}; + StateInterface handle{JOINT_NAME, FOO_INTERFACE}; auto new_handle = handle.with_value_ptr(&value); EXPECT_ANY_THROW(handle.get_value()); EXPECT_DOUBLE_EQ(new_handle.get_value(), value); diff --git a/hardware_interface/test/test_register_actuators.cpp b/hardware_interface/test/test_register_actuators.cpp deleted file mode 100644 index 88b2ede31d..0000000000 --- a/hardware_interface/test/test_register_actuators.cpp +++ /dev/null @@ -1,191 +0,0 @@ -// Copyright 2020 ros2_control development team -// -// 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 "hardware_interface/robot_hardware.hpp" - -namespace hw = hardware_interface; -using testing::SizeIs; -using testing::IsEmpty; -using testing::Each; -using testing::NotNull; -using testing::UnorderedElementsAre; -using testing::ElementsAre; - -namespace -{ -constexpr auto ACTUATOR_NAME = "actuator_1"; -constexpr auto ACTUATOR2_NAME = "neck_tilt_motor"; -constexpr auto FOO_INTERFACE = "FooInterface"; -constexpr auto BAR_INTERFACE = "BarInterface"; -} // namespace - -class TestActuators : public testing::Test -{ - class DummyRobotHardware : public hw::RobotHardware - { - hw::return_type init() override - { - return hw::return_type::OK; - } - - hw::return_type read() override - { - return hw::return_type::OK; - } - - hw::return_type write() override - { - return hw::return_type::OK; - } - }; - -public: - TestActuators() - { - } - -protected: - DummyRobotHardware robot_hw_; -}; - -TEST_F(TestActuators, no_actuators_registered_return_empty_on_all_fronts) -{ - EXPECT_THAT(robot_hw_.get_registered_actuators(), IsEmpty()); - EXPECT_THAT(robot_hw_.get_registered_actuator_names(), IsEmpty()); - - hw::ActuatorHandle handle{"", ""}; - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.get_actuator_handle(handle)); -} - -TEST_F(TestActuators, can_register_actuator_interfaces) -{ - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR_NAME, FOO_INTERFACE)); - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR_NAME, BAR_INTERFACE)); -} - -TEST_F(TestActuators, can_not_double_register_actuator_interfaces) -{ - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR_NAME, FOO_INTERFACE)); - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR_NAME, BAR_INTERFACE)); - - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.register_actuator(ACTUATOR_NAME, FOO_INTERFACE)); - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.register_actuator(ACTUATOR_NAME, BAR_INTERFACE)); -} - -TEST_F(TestActuators, can_not_register_with_empty_fields) -{ - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.register_actuator("", "")); - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.register_actuator(ACTUATOR_NAME, "")); - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.register_actuator("", FOO_INTERFACE)); -} - -TEST_F(TestActuators, can_not_get_non_registered_actuators_or_interfaces) -{ - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR_NAME, FOO_INTERFACE)); - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR2_NAME, BAR_INTERFACE)); - - hw::ActuatorHandle handle1{ACTUATOR_NAME, BAR_INTERFACE}; - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.get_actuator_handle(handle1)); - hw::ActuatorHandle handle2{ACTUATOR2_NAME, FOO_INTERFACE}; - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.get_actuator_handle(handle2)); -} - -TEST_F(TestActuators, can_get_registered_actuators) -{ - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR_NAME, FOO_INTERFACE)); - EXPECT_THAT(robot_hw_.get_registered_actuator_names(), ElementsAre(ACTUATOR_NAME)); - const auto registered_actuators = robot_hw_.get_registered_actuators(); - EXPECT_THAT(registered_actuators, SizeIs(1)); - const auto & actuator_handle = registered_actuators[0]; - EXPECT_EQ(actuator_handle.get_name(), ACTUATOR_NAME); - EXPECT_EQ(actuator_handle.get_interface_name(), FOO_INTERFACE); - - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR_NAME, BAR_INTERFACE)); - EXPECT_THAT(robot_hw_.get_registered_actuator_names(), ElementsAre(ACTUATOR_NAME)); - - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR2_NAME, FOO_INTERFACE)); - EXPECT_THAT( - robot_hw_.get_registered_actuator_names(), - UnorderedElementsAre(ACTUATOR_NAME, ACTUATOR2_NAME)); - EXPECT_THAT( - robot_hw_.get_registered_actuator_interface_names(ACTUATOR_NAME), - UnorderedElementsAre(FOO_INTERFACE, BAR_INTERFACE)); - EXPECT_THAT( - robot_hw_.get_registered_actuator_interface_names(ACTUATOR2_NAME), - UnorderedElementsAre(FOO_INTERFACE)); - - std::vector handles = - {{ACTUATOR_NAME, FOO_INTERFACE}, {ACTUATOR_NAME, BAR_INTERFACE}, - {ACTUATOR2_NAME, FOO_INTERFACE}}; - - for (auto & handle : handles) { - EXPECT_EQ(hw::return_type::OK, robot_hw_.get_actuator_handle(handle)); - } -} - -TEST_F(TestActuators, set_get_works_on_registered_actuators) -{ - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR_NAME, FOO_INTERFACE)); - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR_NAME, BAR_INTERFACE)); - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR2_NAME, FOO_INTERFACE)); - auto actuator_handles = robot_hw_.get_registered_actuators(); - - for (auto & handle : actuator_handles) { - const auto new_value = handle.get_value() + 1.337; - EXPECT_NO_THROW(handle.set_value(new_value)); - EXPECT_DOUBLE_EQ(handle.get_value(), new_value); - } - - std::vector handles = - {{ACTUATOR_NAME, FOO_INTERFACE}, {ACTUATOR_NAME, BAR_INTERFACE}, - {ACTUATOR2_NAME, FOO_INTERFACE}}; - - for (auto & handle : handles) { - EXPECT_ANY_THROW(handle.get_value()); - EXPECT_ANY_THROW(handle.set_value(0.0)); - - EXPECT_EQ(hw::return_type::OK, robot_hw_.get_actuator_handle(handle)); - const auto new_value = handle.get_value() + 1.337; - EXPECT_NO_THROW(handle.set_value(new_value)); - EXPECT_DOUBLE_EQ(handle.get_value(), new_value); - } -} - -TEST_F(TestActuators, can_get_registered_actuators_of_interface) -{ - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR_NAME, FOO_INTERFACE)); - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR_NAME, BAR_INTERFACE)); - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_actuator(ACTUATOR2_NAME, FOO_INTERFACE)); - - std::vector handles1; - ASSERT_EQ(hw::return_type::OK, robot_hw_.get_actuator_handles(handles1, FOO_INTERFACE)); - ASSERT_EQ(handles1.size(), 2ul); - for (const auto & handle : handles1) { - ASSERT_EQ(handle.get_interface_name(), FOO_INTERFACE); - } - - std::vector handles2; - ASSERT_EQ(hw::return_type::OK, robot_hw_.get_actuator_handles(handles2, BAR_INTERFACE)); - ASSERT_EQ(handles2.size(), 1ul); - for (const auto & handle : handles2) { - ASSERT_EQ(handle.get_interface_name(), BAR_INTERFACE); - } - - std::vector handles3; - ASSERT_EQ(hw::return_type::OK, robot_hw_.get_actuator_handles(handles3, "NoInterface")); - ASSERT_TRUE(handles3.empty()); -} diff --git a/hardware_interface/test/test_register_joints.cpp b/hardware_interface/test/test_register_joints.cpp deleted file mode 100644 index dff8326579..0000000000 --- a/hardware_interface/test/test_register_joints.cpp +++ /dev/null @@ -1,191 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 "hardware_interface/robot_hardware.hpp" - -namespace hw = hardware_interface; -using testing::SizeIs; -using testing::IsEmpty; -using testing::Each; -using testing::NotNull; -using testing::UnorderedElementsAre; -using testing::ElementsAre; - -namespace -{ -constexpr auto JOINT_NAME = "joints_1"; -constexpr auto JOINT2_NAME = "neck_tilt_motor"; -constexpr auto FOO_INTERFACE = "FooInterface"; -constexpr auto BAR_INTERFACE = "BarInterface"; -} // namespace - -class TestJoints : public testing::Test -{ - class DummyRobotHardware : public hw::RobotHardware - { - hw::return_type init() override - { - return hw::return_type::OK; - } - - hw::return_type read() override - { - return hw::return_type::OK; - } - - hw::return_type write() override - { - return hw::return_type::OK; - } - }; - -public: - TestJoints() - { - } - -protected: - DummyRobotHardware robot_hw_; -}; - -TEST_F(TestJoints, no_jointss_registered_return_empty_on_all_fronts) -{ - EXPECT_THAT(robot_hw_.get_registered_joints(), IsEmpty()); - EXPECT_THAT(robot_hw_.get_registered_joint_names(), IsEmpty()); - - hw::JointHandle handle{"", ""}; - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.get_joint_handle(handle)); -} - -TEST_F(TestJoints, can_register_jointernal_interfaces) -{ - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, FOO_INTERFACE)); - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, BAR_INTERFACE)); -} - -TEST_F(TestJoints, can_not_double_register_jointernal_interfaces) -{ - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, FOO_INTERFACE)); - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, BAR_INTERFACE)); - - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.register_joint(JOINT_NAME, FOO_INTERFACE)); - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.register_joint(JOINT_NAME, BAR_INTERFACE)); -} - -TEST_F(TestJoints, can_not_register_with_empty_fields) -{ - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.register_joint("", "")); - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.register_joint(JOINT_NAME, "")); - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.register_joint("", FOO_INTERFACE)); -} - -TEST_F(TestJoints, can_not_get_non_registered_jointss_or_interfaces) -{ - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, FOO_INTERFACE)); - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT2_NAME, BAR_INTERFACE)); - - hw::JointHandle handle1{JOINT_NAME, BAR_INTERFACE}; - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.get_joint_handle(handle1)); - hw::JointHandle handle2{JOINT2_NAME, FOO_INTERFACE}; - EXPECT_EQ(hw::return_type::ERROR, robot_hw_.get_joint_handle(handle2)); -} - -TEST_F(TestJoints, can_get_registered_joints) -{ - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, FOO_INTERFACE)); - EXPECT_THAT(robot_hw_.get_registered_joint_names(), ElementsAre(JOINT_NAME)); - const auto registered_jointss = robot_hw_.get_registered_joints(); - EXPECT_THAT(registered_jointss, SizeIs(1)); - const auto & joints_handle = registered_jointss[0]; - EXPECT_EQ(joints_handle.get_name(), JOINT_NAME); - EXPECT_EQ(joints_handle.get_interface_name(), FOO_INTERFACE); - - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, BAR_INTERFACE)); - EXPECT_THAT(robot_hw_.get_registered_joint_names(), ElementsAre(JOINT_NAME)); - - EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT2_NAME, FOO_INTERFACE)); - EXPECT_THAT( - robot_hw_.get_registered_joint_names(), - UnorderedElementsAre(JOINT_NAME, JOINT2_NAME)); - EXPECT_THAT( - robot_hw_.get_registered_joint_interface_names(JOINT_NAME), - UnorderedElementsAre(FOO_INTERFACE, BAR_INTERFACE)); - EXPECT_THAT( - robot_hw_.get_registered_joint_interface_names(JOINT2_NAME), - UnorderedElementsAre(FOO_INTERFACE)); - - std::vector handles = - {{JOINT_NAME, FOO_INTERFACE}, {JOINT_NAME, BAR_INTERFACE}, - {JOINT2_NAME, FOO_INTERFACE}}; - - for (auto & handle : handles) { - EXPECT_EQ(hw::return_type::OK, robot_hw_.get_joint_handle(handle)); - } -} - -TEST_F(TestJoints, set_get_works_on_registered_jointss) -{ - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, FOO_INTERFACE)); - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, BAR_INTERFACE)); - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT2_NAME, FOO_INTERFACE)); - auto joints_handles = robot_hw_.get_registered_joints(); - - for (auto & handle : joints_handles) { - const auto new_value = handle.get_value() + 1.337; - EXPECT_NO_THROW(handle.set_value(new_value)); - EXPECT_DOUBLE_EQ(handle.get_value(), new_value); - } - - std::vector handles = - {{JOINT_NAME, FOO_INTERFACE}, {JOINT_NAME, BAR_INTERFACE}, - {JOINT2_NAME, FOO_INTERFACE}}; - - for (auto & handle : handles) { - EXPECT_ANY_THROW(handle.get_value()); - EXPECT_ANY_THROW(handle.set_value(0.0)); - - EXPECT_EQ(hw::return_type::OK, robot_hw_.get_joint_handle(handle)); - const auto new_value = handle.get_value() + 1.337; - EXPECT_NO_THROW(handle.set_value(new_value)); - EXPECT_DOUBLE_EQ(handle.get_value(), new_value); - } -} - -TEST_F(TestJoints, can_get_registered_joints_of_interface) -{ - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, FOO_INTERFACE)); - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, BAR_INTERFACE)); - ASSERT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT2_NAME, FOO_INTERFACE)); - - std::vector handles1; - ASSERT_EQ(hw::return_type::OK, robot_hw_.get_joint_handles(handles1, FOO_INTERFACE)); - ASSERT_EQ(handles1.size(), 2ul); - for (const auto & handle : handles1) { - ASSERT_EQ(handle.get_interface_name(), FOO_INTERFACE); - } - - std::vector handles2; - ASSERT_EQ(hw::return_type::OK, robot_hw_.get_joint_handles(handles2, BAR_INTERFACE)); - ASSERT_EQ(handles2.size(), 1ul); - for (const auto & handle : handles2) { - ASSERT_EQ(handle.get_interface_name(), BAR_INTERFACE); - } - - std::vector handles3; - ASSERT_EQ(hw::return_type::OK, robot_hw_.get_joint_handles(handles3, "NoInterface")); - ASSERT_TRUE(handles3.empty()); -} diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 7c2de79d1e..f9a4c8b14c 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -189,6 +189,12 @@ TEST_F(TestResourceManager, initialization_with_urdf) { ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); } +TEST_F(TestResourceManager, post_initialization_with_urdf) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + hardware_interface::ResourceManager rm; + ASSERT_NO_THROW(rm.load_urdf(urdf)); +} + TEST_F(TestResourceManager, initialization_with_urdf_manual_validation) { auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; // we validate the results manually @@ -284,6 +290,19 @@ TEST_F(TestResourceManager, resource_claiming) { } } } + + std::vector interfaces; + const auto interface_names = {"joint1/position", "joint2/velocity", "joint3/velocity"}; + for (const auto & key : interface_names) { + interfaces.emplace_back(rm.claim_command_interface(key)); + } + for (const auto & key : interface_names) { + EXPECT_TRUE(rm.command_interface_is_claimed(key)); + } + interfaces.clear(); + for (const auto & key : interface_names) { + EXPECT_FALSE(rm.command_interface_is_claimed(key)); + } } class ExternalComponent : public hardware_interface::components::ActuatorInterface diff --git a/hardware_interface/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp deleted file mode 100644 index 7844b5e10d..0000000000 --- a/hardware_interface/test/test_robot_hardware_interface.cpp +++ /dev/null @@ -1,143 +0,0 @@ -// Copyright 2020 ros2_control development team -// -// 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 "gmock/gmock.h" - -#include "hardware_interface/robot_hardware.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" - -namespace hw = hardware_interface; -using testing::SizeIs; -using testing::Each; -using testing::NotNull; -using testing::UnorderedElementsAre; -using testing::ElementsAre; - -namespace -{ -class MyTestRobotHardware : public hw::RobotHardware -{ -public: - hw::return_type init() override - { - return hw::return_type::OK; - } - - hw::return_type read() override - { - return hw::return_type::OK; - } - - hw::return_type write() override - { - return hw::return_type::OK; - } -}; - -constexpr auto JOINT_NAME = "joint_1"; -constexpr auto NEW_JOINT_NAME = "joint_2"; -} // namespace - -class TestRobotHardwareInterface : public testing::Test -{ -protected: - void SetUp() - { - robot_.init(); - } - - void SetUpHandles() - { - robot_.register_operation_mode_handle(&op_mode_handle_); - } - - void SetUpNewHandles() - { - robot_.register_operation_mode_handle(&new_op_mode_handle_); - } - - MyTestRobotHardware robot_; - - hw::OperationMode mode_ = hw::OperationMode::ACTIVE; - - hw::OperationModeHandle op_mode_handle_{JOINT_NAME, &mode_}; - hw::OperationModeHandle new_op_mode_handle_{NEW_JOINT_NAME, &mode_}; -}; - -TEST_F(TestRobotHardwareInterface, can_not_register_broken_handles) -{ - hw::OperationModeHandle broken_op_mode_handle; - EXPECT_EQ(hw::return_type::ERROR, robot_.register_operation_mode_handle(&broken_op_mode_handle)); -} - -TEST_F(TestRobotHardwareInterface, can_register_proper_handles) -{ - EXPECT_EQ(hw::return_type::OK, robot_.register_operation_mode_handle(&op_mode_handle_)); -} - -TEST_F(TestRobotHardwareInterface, cannot_double_register_handles) -{ - SetUpHandles(); - - EXPECT_EQ(hw::return_type::ERROR, robot_.register_operation_mode_handle(&op_mode_handle_)); -} - -TEST_F(TestRobotHardwareInterface, can_get_registered_joint_names) -{ - SetUpHandles(); - // EXPECT_THAT(robot_.get_registered_joint_names(), ElementsAre(JOINT_NAME)); - - SetUpNewHandles(); - // EXPECT_THAT( - // robot_.get_registered_joint_names(), - // UnorderedElementsAre(JOINT_NAME, NEW_JOINT_NAME)); -} - -TEST_F(TestRobotHardwareInterface, returned_joint_handles_are_not_null) -{ - SetUpHandles(); - SetUpNewHandles(); - - const auto op_mode_handles = robot_.get_registered_operation_mode_handles(); - EXPECT_THAT(op_mode_handles, SizeIs(2)); - EXPECT_THAT(op_mode_handles, Each(NotNull())); -} - -TEST_F(TestRobotHardwareInterface, can_get_registered_op_mode_handles) -{ - SetUpHandles(); - EXPECT_THAT(robot_.get_registered_operation_mode_handles(), SizeIs(1)); - - SetUpNewHandles(); - EXPECT_THAT(robot_.get_registered_operation_mode_handles(), SizeIs(2)); -} - -TEST_F(TestRobotHardwareInterface, can_get_handles_by_name) -{ - SetUpHandles(); - - hw::OperationModeHandle * op_mode_handle = nullptr; - EXPECT_EQ(hw::return_type::OK, robot_.get_operation_mode_handle(JOINT_NAME, &op_mode_handle)); - op_mode_handle = nullptr; - EXPECT_EQ( - hw::return_type::ERROR, - robot_.get_operation_mode_handle(NEW_JOINT_NAME, &op_mode_handle)); - - SetUpNewHandles(); - op_mode_handle = nullptr; - EXPECT_EQ(hw::return_type::OK, robot_.get_operation_mode_handle(NEW_JOINT_NAME, &op_mode_handle)); -} diff --git a/joint_limits_interface/COLCON_IGNORE b/joint_limits_interface/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ros2_control/package.xml b/ros2_control/package.xml index ee20a4dbbd..d59988b343 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -15,7 +15,7 @@ controller_manager hardware_interface transmission_interface - joint_limits_interface + test_robot_hardware diff --git a/ros2controlcli/ros2controlcli/api/__init__.py b/ros2controlcli/ros2controlcli/api/__init__.py index 8d589d7a82..c03c832c75 100644 --- a/ros2controlcli/ros2controlcli/api/__init__.py +++ b/ros2controlcli/ros2controlcli/api/__init__.py @@ -14,7 +14,8 @@ from controller_manager_msgs.srv import ListControllers, ListControllerTypes, \ - LoadController, ReloadControllerLibraries, SwitchController, UnloadController + ListHardwareInterfaces, LoadController, ReloadControllerLibraries, \ + SwitchController, UnloadController import rclpy from ros2cli.node.direct import DirectNode from ros2node.api import NodeNameCompleter @@ -62,6 +63,12 @@ def list_controller_types(controller_manager_name): '{}/list_controller_types'.format(controller_manager_name), ListControllerTypes, request) +def list_hardware_interfaces(controller_manager_name): + request = ListHardwareInterfaces.Request() + return service_caller('{}/list_hardware_interfaces'.format(controller_manager_name), + ListHardwareInterfaces, request) + + def reload_controller_libraries(controller_manager_name, force_kill): request = ReloadControllerLibraries.Request() request.force_kill = force_kill diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py new file mode 100644 index 0000000000..c973f20bfd --- /dev/null +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -0,0 +1,40 @@ +# Copyright 2020 PAL Robotics S.L. +# +# 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. + +from ros2cli.node.direct import add_arguments +from ros2cli.verb import VerbExtension +from ros2controlcli.api import add_controller_mgr_parsers, list_hardware_interfaces + + +class ListHardwareInterfacesVerb(VerbExtension): + """Output the list of loaded controllers, their type and status.""" + + def add_arguments(self, parser, cli_name): + add_arguments(parser) + add_controller_mgr_parsers(parser) + + def main(self, *, args): + hardware_interfaces = list_hardware_interfaces(args.controller_manager) + command_interfaces = sorted( + hardware_interfaces.command_interfaces, key=lambda hwi: hwi.name) + state_interfaces = sorted( + hardware_interfaces.state_interfaces, key=lambda hwi: hwi.name) + print('command interfaces') + for command_interface in command_interfaces: + print('\t%s [%s]' % + (command_interface.name, + 'claimed' if command_interface.is_claimed else 'unclaimed')) + print('state interfaces') + for state_interface in state_interfaces: + print('\t', state_interface.name) diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index ef67145897..d8e35b0699 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -37,6 +37,8 @@ ], 'ros2controlcli.verb': [ 'list = ros2controlcli.verb.list:ListVerb', + 'list_hardware_interfaces = \ + ros2controlcli.verb.list_hardware_interfaces:ListHardwareInterfacesVerb', 'list_types = ros2controlcli.verb.list_types:ListTypesVerb', 'load = ros2controlcli.verb.load:LoadVerb', 'reload_libraries = ros2controlcli.verb.reload_libraries:ReloadLibrariesVerb', diff --git a/test_robot_hardware/CMakeLists.txt b/test_robot_hardware/CMakeLists.txt index 4bffd8f328..feb8872755 100644 --- a/test_robot_hardware/CMakeLists.txt +++ b/test_robot_hardware/CMakeLists.txt @@ -14,45 +14,42 @@ find_package(ament_cmake REQUIRED) find_package(hardware_interface REQUIRED) find_package(rclcpp REQUIRED) -add_library(test_robot_hardware SHARED src/test_robot_hardware.cpp) -target_include_directories(test_robot_hardware PRIVATE include) -ament_target_dependencies( - test_robot_hardware +add_library(test_robot_hardware_components SHARED + src/test_single_joint_actuator.cpp + src/test_force_torque_sensor.cpp + src/test_two_joint_system.cpp +) +ament_target_dependencies(test_robot_hardware_components hardware_interface - rclcpp + pluginlib +) +pluginlib_export_plugin_description_file( + hardware_interface test_robot_components.xml ) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(test_robot_hardware PRIVATE "TEST_ROBOT_HARDWARE_BUILDING_DLL") install(DIRECTORY include/ - DESTINATION include) + DESTINATION include +) -install(TARGETS test_robot_hardware +install(TARGETS test_robot_hardware_components ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +install(FILES test_robot_hardware.urdf + DESTINATION share/${PROJECT_NAME}/ +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - - ament_add_gtest(test_robot_hardware_interface test/test_robot_hardware_interface.cpp) - if(TARGET test_robot_hardware_interface) - target_include_directories(test_robot_hardware_interface PRIVATE include) - target_link_libraries( - test_robot_hardware_interface - test_robot_hardware - ) - endif() endif() ament_export_dependencies( hardware_interface rclcpp ) -ament_export_libraries(${PROJECT_NAME}) +ament_export_libraries(test_robot_hardware_components) ament_export_include_directories(include) ament_package() diff --git a/test_robot_hardware/include/test_robot_hardware/test_robot_hardware.hpp b/test_robot_hardware/include/test_robot_hardware/test_robot_hardware.hpp deleted file mode 100644 index 56ec73e8af..0000000000 --- a/test_robot_hardware/include/test_robot_hardware/test_robot_hardware.hpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2017 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. - -#ifndef TEST_ROBOT_HARDWARE__TEST_ROBOT_HARDWARE_HPP_ -#define TEST_ROBOT_HARDWARE__TEST_ROBOT_HARDWARE_HPP_ - -#include -#include - -#include "hardware_interface/robot_hardware.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" - -#include "rclcpp/rclcpp.hpp" - -#include "test_robot_hardware/visibility_control.h" - -namespace test_robot_hardware -{ -// TODO(karsten1987): Maybe visibility macros on class level -// as all members are publically exposed -class TestRobotHardware : public hardware_interface::RobotHardware -{ -public: - TEST_ROBOT_HARDWARE_PUBLIC - hardware_interface::return_type - init(); - - TEST_ROBOT_HARDWARE_PUBLIC - hardware_interface::return_type - read(); - - TEST_ROBOT_HARDWARE_PUBLIC - hardware_interface::return_type - write(); - - std::string read_op_handle_name1 = "read1"; - std::string read_op_handle_name2 = "read2"; - std::string write_op_handle_name1 = "write1"; - std::string write_op_handle_name2 = "write2"; - - std::vector actuator_names = {"actuator1", "actuator2", "actuator3"}; - std::vector joint_names = {"joint1", "joint2", "joint3"}; - - std::vector pos_dflt_values = {1.1, 2.1, 3.1}; - std::vector vel_dflt_values = {1.2, 2.2, 3.2}; - std::vector eff_dflt_values = {1.3, 2.3, 3.3}; - - bool read1 = false; - bool read2 = false; - bool write1 = false; - bool write2 = false; - - hardware_interface::OperationModeHandle read_op_handle1; - hardware_interface::OperationModeHandle read_op_handle2; - - hardware_interface::OperationModeHandle write_op_handle1; - hardware_interface::OperationModeHandle write_op_handle2; - - const rclcpp::Logger logger = rclcpp::get_logger("test_robot_hardware"); -}; - -} // namespace test_robot_hardware -#endif // TEST_ROBOT_HARDWARE__TEST_ROBOT_HARDWARE_HPP_ diff --git a/test_robot_hardware/src/test_force_torque_sensor.cpp b/test_robot_hardware/src/test_force_torque_sensor.cpp new file mode 100644 index 0000000000..baf4304d33 --- /dev/null +++ b/test_robot_hardware/src/test_force_torque_sensor.cpp @@ -0,0 +1,135 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "hardware_interface/components/sensor_interface.hpp" + +using hardware_interface::status; +using hardware_interface::return_type; +using hardware_interface::StateInterface; + +namespace test_robot_hardware +{ + +class TestForceTorqueSensor : public hardware_interface::components::SensorInterface +{ + return_type configure(const hardware_interface::HardwareInfo & sensor_info) override + { + sensor_info_ = sensor_info; + + const auto & state_interfaces = sensor_info_.sensors[0].state_interfaces; + if (state_interfaces.size() != 6) {return return_type::ERROR;} + for (const auto & ft_key : {"fx", "fy", "fz", "tx", "ty", "tz"}) { + if (std::find_if( + state_interfaces.begin(), state_interfaces.end(), [&ft_key](const auto & interface_info) { + return interface_info.name == ft_key; + }) == state_interfaces.end()) + { + return return_type::ERROR; + } + } + + fprintf(stderr, "TestForceTorqueSensor configured successfully.\n"); + return return_type::OK; + } + + std::vector export_state_interfaces() override + { + std::vector state_interfaces; + + const auto & sensor_name = sensor_info_.sensors[0].name; + state_interfaces.emplace_back( + hardware_interface::StateInterface( + sensor_name, + "fx", + &values_.fx)); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + sensor_name, + "fy", + &values_.fy)); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + sensor_name, + "fz", + &values_.fz)); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + sensor_name, + "tx", + &values_.tx)); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + sensor_name, + "ty", + &values_.ty)); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + sensor_name, + "tz", + &values_.tz)); + + return state_interfaces; + } + + return_type start() override + { + return return_type::OK; + } + + return_type stop() override + { + return return_type::OK; + } + + status get_status() const override + { + return status::UNKNOWN; + } + + return_type read() override + { + values_.fx = fmod((values_.fx + 1.0), 10); + values_.fy = fmod((values_.fy + 1.0), 10); + values_.fz = fmod((values_.fz + 1.0), 10); + values_.tx = fmod((values_.tx + 1.0), 10); + values_.ty = fmod((values_.ty + 1.0), 10); + values_.tz = fmod((values_.tz + 1.0), 10); + return return_type::OK; + } + +private: + struct FTValues + { + double fx = 0.0; + double fy = 0.0; + double fz = 0.0; + double tx = 0.0; + double ty = 0.0; + double tz = 0.0; + }; + + FTValues values_; + hardware_interface::HardwareInfo sensor_info_; +}; + +} // namespace test_robot_hardware + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS( + test_robot_hardware::TestForceTorqueSensor, hardware_interface::components::SensorInterface) diff --git a/test_robot_hardware/src/test_robot_hardware.cpp b/test_robot_hardware/src/test_robot_hardware.cpp deleted file mode 100644 index 758bd81c15..0000000000 --- a/test_robot_hardware/src/test_robot_hardware.cpp +++ /dev/null @@ -1,124 +0,0 @@ -// Copyright 2017 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 "test_robot_hardware/test_robot_hardware.hpp" - -#include -#include -#include - -namespace test_robot_hardware -{ - -hardware_interface::return_type -TestRobotHardware::init() -{ - auto ret = hardware_interface::return_type::ERROR; - - read_op_handle1 = hardware_interface::OperationModeHandle( - read_op_handle_name1, - reinterpret_cast(&read1)); - ret = register_operation_mode_handle(&read_op_handle1); - if (ret != hardware_interface::return_type::OK) { - RCLCPP_WARN(logger, "can't register operation mode handle %s", read_op_handle_name1.c_str()); - return ret; - } - - read_op_handle2 = hardware_interface::OperationModeHandle( - read_op_handle_name2, - reinterpret_cast(&read2)); - ret = register_operation_mode_handle(&read_op_handle2); - if (ret != hardware_interface::return_type::OK) { - RCLCPP_WARN(logger, "can't register operation mode handle %s", read_op_handle_name2.c_str()); - return ret; - } - - write_op_handle1 = hardware_interface::OperationModeHandle( - write_op_handle_name1, - reinterpret_cast(&write1)); - ret = register_operation_mode_handle(&write_op_handle1); - if (ret != hardware_interface::return_type::OK) { - RCLCPP_WARN(logger, "can't register operation mode handle %s", write_op_handle_name1.c_str()); - return ret; - } - - write_op_handle2 = hardware_interface::OperationModeHandle( - write_op_handle_name2, - reinterpret_cast(&write2)); - ret = register_operation_mode_handle(&write_op_handle2); - if (ret != hardware_interface::return_type::OK) { - RCLCPP_WARN(logger, "can't register operation mode handle %s", write_op_handle_name2.c_str()); - return ret; - } - - // register actuators and joints - for (auto index = 0u; index < 3u; ++index) { - register_actuator(actuator_names[index], "position", pos_dflt_values[index]); - register_actuator(actuator_names[index], "velocity", vel_dflt_values[index]); - register_actuator(actuator_names[index], "effort", eff_dflt_values[index]); - register_actuator(actuator_names[index], "position_command", pos_dflt_values[index]); - register_actuator(actuator_names[index], "velocity_command", vel_dflt_values[index]); - register_actuator(actuator_names[index], "effort_command", eff_dflt_values[index]); - - register_joint(joint_names[index], "position", pos_dflt_values[index]); - register_joint(joint_names[index], "velocity", vel_dflt_values[index]); - register_joint(joint_names[index], "effort", eff_dflt_values[index]); - register_joint(joint_names[index], "position_command", pos_dflt_values[index]); - register_joint(joint_names[index], "velocity_command", vel_dflt_values[index]); - register_joint(joint_names[index], "effort_command", eff_dflt_values[index]); - } - - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type -TestRobotHardware::read() -{ - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type -TestRobotHardware::write() -{ - auto update_handle = [&](const std::string & joint_name, const std::string & interface_name) - { - auto get_handle = [&](const std::string & joint_name, const std::string & interface_name) - { - auto joint_handle = std::make_shared( - joint_name, - interface_name); - get_joint_handle(*joint_handle); - return joint_handle; - }; - - get_handle( - joint_name, - interface_name)->set_value( - get_handle( - joint_name, - interface_name + "_command")->get_value()); - }; - - // update all the joint state handles with their respectives command values - const std::vector interface_names = {"position", "velocity", "effort"}; - for (const auto & joint_name : joint_names) { - for (const auto & interface_name : interface_names) { - update_handle(joint_name, interface_name); - } - } - - return hardware_interface::return_type::OK; -} - -} // namespace test_robot_hardware diff --git a/test_robot_hardware/src/test_single_joint_actuator.cpp b/test_robot_hardware/src/test_single_joint_actuator.cpp new file mode 100644 index 0000000000..0bcccfd3f2 --- /dev/null +++ b/test_robot_hardware/src/test_single_joint_actuator.cpp @@ -0,0 +1,125 @@ +// Copyright 2020 ros2_control Development Team +// +// 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 "hardware_interface/components/actuator_interface.hpp" + +using hardware_interface::status; +using hardware_interface::return_type; +using hardware_interface::StateInterface; +using hardware_interface::CommandInterface; + +namespace test_robot_hardware +{ + +class TestSingleJointActuator : public hardware_interface::components::ActuatorInterface +{ + return_type configure(const hardware_interface::HardwareInfo & actuator_info) override + { + actuator_info_ = actuator_info; + + // can only control one joint + if (actuator_info_.joints.size() != 1) {return return_type::ERROR;} + // can only control in position + const auto & command_interfaces = actuator_info_.joints[0].command_interfaces; + if (command_interfaces.size() != 1) {return return_type::ERROR;} + if (command_interfaces[0].name != "position") {return return_type::ERROR;} + // can only give feedback state for position and velocity + const auto & state_interfaces = actuator_info_.joints[0].state_interfaces; + if (state_interfaces.size() < 1) {return return_type::ERROR;} + for (const auto & state_interface : state_interfaces) { + if ((state_interface.name != "position") && + (state_interface.name != "velocity")) + { + return return_type::ERROR; + } + } + fprintf(stderr, "TestSingleJointActuator configured successfully.\n"); + return return_type::OK; + } + + std::vector export_state_interfaces() override + { + std::vector state_interfaces; + + const auto & joint_name = actuator_info_.joints[0].name; + state_interfaces.emplace_back( + hardware_interface::StateInterface( + joint_name, + "position", + &position_state_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + joint_name, + "velocity", + &velocity_state_)); + + return state_interfaces; + } + + std::vector export_command_interfaces() override + { + std::vector command_interfaces; + + const auto & joint_name = actuator_info_.joints[0].name; + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + joint_name, + "position", + &position_command_)); + + return command_interfaces; + } + + return_type start() override + { + return return_type::OK; + } + + return_type stop() override + { + return return_type::OK; + } + + status get_status() const override + { + return status::UNKNOWN; + } + + return_type read() override + { + return return_type::OK; + } + + return_type write() override + { + velocity_state_ = position_command_ - position_state_; + position_state_ = position_command_; + return return_type::OK; + } + +private: + double position_state_ = 0.0; + double velocity_state_ = 0.0; + double position_command_ = 0.0; + hardware_interface::HardwareInfo actuator_info_; +}; + +} // namespace test_robot_hardware + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS( + test_robot_hardware::TestSingleJointActuator, hardware_interface::components::ActuatorInterface) diff --git a/test_robot_hardware/src/test_two_joint_system.cpp b/test_robot_hardware/src/test_two_joint_system.cpp new file mode 100644 index 0000000000..01d8d452ff --- /dev/null +++ b/test_robot_hardware/src/test_two_joint_system.cpp @@ -0,0 +1,111 @@ +// Copyright 2020 ros2_control Development Team +// +// 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 "hardware_interface/components/system_interface.hpp" + +using hardware_interface::status; +using hardware_interface::return_type; +using hardware_interface::StateInterface; +using hardware_interface::CommandInterface; + +namespace test_robot_hardware +{ + +class TestTwoJointSystem : public hardware_interface::components::SystemInterface +{ + return_type configure(const hardware_interface::HardwareInfo & system_info) override + { + system_info_ = system_info; + + // can only control two joint + if (system_info_.joints.size() != 2) {return return_type::ERROR;} + for (const auto & joint : system_info_.joints) { + // can only control in position + const auto & command_interfaces = joint.command_interfaces; + if (command_interfaces.size() != 1) {return return_type::ERROR;} + if (command_interfaces[0].name != "position") {return return_type::ERROR;} + // can only give feedback state for position and velocity + const auto & state_interfaces = joint.state_interfaces; + if (state_interfaces.size() != 1) {return return_type::ERROR;} + if (state_interfaces[0].name != "position") {return return_type::ERROR;} + } + + fprintf(stderr, "TestTwoJointSystem configured successfully.\n"); + return return_type::OK; + } + + std::vector export_state_interfaces() override + { + std::vector state_interfaces; + for (auto i = 0u; i < system_info_.joints.size(); ++i) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + system_info_.joints[i].name, "position", &position_state_[i])); + } + + return state_interfaces; + } + + std::vector export_command_interfaces() override + { + std::vector command_interfaces; + for (auto i = 0u; i < system_info_.joints.size(); ++i) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + system_info_.joints[i].name, "position", &position_command_[i])); + } + + return command_interfaces; + } + + return_type start() override + { + return return_type::OK; + } + + return_type stop() override + { + return return_type::OK; + } + + status get_status() const override + { + return status::UNKNOWN; + } + + return_type read() override + { + return return_type::OK; + } + + return_type write() override + { + return return_type::OK; + } + +private: + std::array position_command_ = {0.0, 0.0}; + std::array position_state_ = {0.0, 0.0}; + hardware_interface::HardwareInfo system_info_; +}; + +} // namespace test_robot_hardware + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS( + test_robot_hardware::TestTwoJointSystem, hardware_interface::components::SystemInterface) diff --git a/test_robot_hardware/test/test_robot_hardware_interface.cpp b/test_robot_hardware/test/test_robot_hardware_interface.cpp deleted file mode 100644 index c3f9983d62..0000000000 --- a/test_robot_hardware/test/test_robot_hardware_interface.cpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright 2017 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 "gtest/gtest.h" - -#include "hardware_interface/robot_hardware.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" - -#include "test_robot_hardware/test_robot_hardware.hpp" - -using hw_ret = hardware_interface::return_type; - -class TestRobotHardwareInterface : public ::testing::Test -{ -protected: - void SetUp() - { - joint_names_ = robot_.joint_names; - - joint_pos_values_ = robot_.pos_dflt_values; - joint_vel_values_ = robot_.vel_dflt_values; - joint_eff_values_ = robot_.eff_dflt_values; - - joint_pos_cmd_values_ = robot_.pos_dflt_values; - joint_vel_cmd_values_ = robot_.vel_dflt_values; - joint_eff_cmd_values_ = robot_.eff_dflt_values; - } - - test_robot_hardware::TestRobotHardware robot_; - - std::vector joint_names_; - - std::vector joint_pos_values_; - std::vector joint_vel_values_; - std::vector joint_eff_values_; - - std::vector joint_pos_cmd_values_; - std::vector joint_vel_cmd_values_; - std::vector joint_eff_cmd_values_; -}; - -TEST_F(TestRobotHardwareInterface, initialize) { - EXPECT_EQ(hw_ret::OK, robot_.init()); -} - -TEST_F(TestRobotHardwareInterface, get_registered_joint_handles) { - robot_.init(); - - auto ret = hw_ret::ERROR; - for (auto i = 0u; i < 3u; ++i) { - auto get_handle = [&](const std::string joint_name, const std::string interface_name) - { - hardware_interface::JointHandle joint_handle(joint_name, interface_name); - ret = robot_.get_joint_handle(joint_handle); - return joint_handle; - }; - - EXPECT_EQ(get_handle(joint_names_[i], "position").get_value(), joint_pos_values_[i]); - EXPECT_EQ(get_handle(joint_names_[i], "velocity").get_value(), joint_vel_values_[i]); - EXPECT_EQ(get_handle(joint_names_[i], "effort").get_value(), joint_eff_values_[i]); - - EXPECT_EQ(get_handle(joint_names_[i], "position_command").get_value(), joint_pos_values_[i]); - EXPECT_EQ(get_handle(joint_names_[i], "velocity_command").get_value(), joint_vel_values_[i]); - EXPECT_EQ(get_handle(joint_names_[i], "effort_command").get_value(), joint_eff_values_[i]); - } - - // 3 joints * 6 interfaces - EXPECT_EQ(robot_.get_registered_joints().size(), 3u * 6u); -} diff --git a/test_robot_hardware/test_robot_components.xml b/test_robot_hardware/test_robot_components.xml new file mode 100644 index 0000000000..f1c9e67200 --- /dev/null +++ b/test_robot_hardware/test_robot_components.xml @@ -0,0 +1,20 @@ + + + + + Test actuator component for a single position joint + + + + + + Test sensor component emulating a force torque sensor + + + + + + Test system component for a two degree of freedom position joint robot + + + diff --git a/test_robot_hardware/test_robot_hardware.urdf b/test_robot_hardware/test_robot_hardware.urdf new file mode 100644 index 0000000000..b99c860164 --- /dev/null +++ b/test_robot_hardware/test_robot_hardware.urdf @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + test_robot_hardware/TestSingleJointActuator + + + + + + + + + test_robot_hardware/TestForceTorqueSensor + + + + + + + + + + + + + test_robot_hardware/TestTwoJointSystem + + + + + + + + + + +