diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 6fd3b159b2..235800711b 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -51,7 +51,6 @@ The controllers are using `common hardware interface definitions`_, and may use Admittance Controller <../admittance_controller/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> - Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Parallel Gripper Controller <../parallel_gripper_controller/doc/userdoc.rst> PID Controller <../pid_controller/doc/userdoc.rst> diff --git a/doc/migration.rst b/doc/migration.rst index f38e440820..db48cc9355 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -4,6 +4,10 @@ Migration Guides: Jazzy to Kilted ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes important changes between Jazzy (previous) and Kilted (current) releases, where changes to user code might be necessary. +GripperActionController +***************************** +The ``effort_controllers/GripperActionController`` and ``position_controllers/GripperActionController`` have been removed. The ``parallel_gripper_action_controller/GripperActionController`` should be used instead. `(#1652 `__). + diff_drive_controller ***************************** * Parameters ``has_velocity_limits``, ``has_acceleration_limits``, and ``has_jerk_limits`` are removed. Instead, set the respective limits to ``.NAN``. (`#1653 `_). diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst deleted file mode 100644 index 8c0b5cd98e..0000000000 --- a/gripper_controllers/CHANGELOG.rst +++ /dev/null @@ -1,333 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package gripper_controllers -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -4.24.0 (2025-04-27) -------------------- - -4.23.0 (2025-04-10) -------------------- -* Use global cmake macros and fix gcc-10 build (`#1527 `_) -* Replace RCLCPP\_*_STREAM macros with RCLCPP\_* (`#1600 `_) -* Contributors: Christoph Fröhlich, Vedant Randive - -4.22.0 (2025-03-17) -------------------- - -4.21.0 (2025-03-01) -------------------- -* Update API of PID class (`#1437 `_) -* Cleanup wrong lifecycle transitions in tests and unnecessary checks (`#1534 `_) -* Bump version of pre-commit hooks (`#1514 `_) -* Contributors: Christoph Fröhlich, github-actions[bot] - -4.20.0 (2025-01-29) -------------------- -* Update paths of GPL includes (`#1487 `_) -* Contributors: Christoph Fröhlich - -4.19.0 (2025-01-13) -------------------- -* Remove custom logic to skip configuration of gripper_controllers on Windows or macOS (`#1471 `_) -* Remove visibility macros (`#1451 `_) -* Contributors: Bence Magyar, Silvio Traversaro - -4.18.0 (2024-12-19) -------------------- - -4.17.0 (2024-12-07) -------------------- -* Use the .hpp headers from `realtime_tools` package (`#1406 `_) -* Add explicit cast to period.count() (`#1404 `_) -* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) -* Update maintainers and add url tags (`#1363 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.16.0 (2024-11-08) -------------------- - -4.15.0 (2024-10-07) -------------------- - -4.14.0 (2024-09-11) -------------------- - -4.13.0 (2024-08-22) -------------------- - -4.12.1 (2024-08-14) -------------------- - -4.12.0 (2024-07-23) -------------------- -* Unused header cleanup (`#1199 `_) -* Fix parallel gripper controller CI (`#1202 `_) -* Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (`#1002 `_) -* Contributors: Henry Moore, Paul Gesel, Sai Kishor Kothakota - -4.11.0 (2024-07-09) -------------------- -* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) -* Contributors: Sai Kishor Kothakota - -4.10.0 (2024-07-01) -------------------- - -4.9.0 (2024-06-05) ------------------- - -4.8.0 (2024-05-14) ------------------- - -4.7.0 (2024-03-22) ------------------- -* Fix usage of visibility macros (`#1039 `_) -* Contributors: Silvio Traversaro - -4.6.0 (2024-02-12) ------------------- -* Add test_depend on `hardware_interface_testing` (`#1018 `_) -* Fix tests for using new `get_node_options` API (`#840 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.5.0 (2024-01-31) ------------------- -* Let sphinx add parameter description with nested structures to documentation (`#652 `_) -* Contributors: Christoph Fröhlich - -4.4.0 (2024-01-11) ------------------- - -4.3.0 (2024-01-08) ------------------- -* Add few warning flags to error (`#961 `_) -* Contributors: Sai Kishor Kothakota - -4.2.0 (2023-12-12) ------------------- - -4.1.0 (2023-12-01) ------------------- -* Increase test coverage of interface configuration getters (`#856 `_) -* Contributors: Christoph Fröhlich - -4.0.0 (2023-11-21) ------------------- -* fix tests for API break of passing controller manager update rate in init method (`#854 `_) -* Adjust tests after passing URDF to controllers (`#817 `_) -* Contributors: Bence Magyar, Sai Kishor Kothakota - -3.17.0 (2023-10-31) -------------------- - -3.16.0 (2023-09-20) -------------------- - -3.15.0 (2023-09-11) -------------------- -* Add test for effort gripper controller (`#769 `_) -* Fixed implementation so that effort_controllers/GripperActionController works. (`#756 `_) -* Contributors: chama1176 - -3.14.0 (2023-08-16) -------------------- - -3.13.0 (2023-08-04) -------------------- - -3.12.0 (2023-07-18) -------------------- - -3.11.0 (2023-06-24) -------------------- -* Added -Wconversion flag and fix warnings (`#667 `_) -* Let sphinx add parameter description to documentation (`#651 `_) -* Contributors: Christoph Fröhlich, gwalck - -3.10.1 (2023-06-06) -------------------- - -3.10.0 (2023-06-04) -------------------- - -3.9.0 (2023-05-28) ------------------- -* Fix compilation warnings (`#621 `_) -* Contributors: Noel Jiménez García, Mathias Lüdtke - -3.8.0 (2023-05-14) ------------------- - -3.7.0 (2023-05-02) ------------------- - -3.6.0 (2023-04-29) ------------------- -* Renovate load controller tests (`#569 `_) -* Contributors: Bence Magyar - -3.5.0 (2023-04-14) ------------------- -* [Parameters] Use `gt_eq` instead of deprecated `lower_bounds` in validators (`#561 `_) -* Contributors: Dr. Denis - -3.4.0 (2023-04-02) ------------------- - -3.3.0 (2023-03-07) ------------------- -* Add comments about auto-generated header files (`#539 `_) -* Fix Segfault in GripperActionController (`#527 `_) -* Contributors: AndyZe, Erik Holum - -3.2.0 (2023-02-10) ------------------- -* Fix overriding of install (`#510 `_) -* Contributors: Tyler Weaver, Chris Thrasher - -3.1.0 (2023-01-26) ------------------- -* Changing to_chrono to use nanoseconds & Reset gripper action goal timer to match JTC impl (`#507 `_) -* Contributors: Dan Wahl - -3.0.0 (2023-01-19) ------------------- -* Add backward_ros to all controllers (`#489 `_) -* Contributors: Bence Magyar - -2.15.0 (2022-12-06) -------------------- -* Add basic gripper controller tests (`#459 `_) -* Contributors: Bence Magyar - -2.14.0 (2022-11-18) -------------------- -* Use optional from C++17 (`#460 `_) -* Generate parameters for Gripper Action (`#398 `_) -* Contributors: Bence Magyar, Tyler Weaver - -2.13.0 (2022-10-05) -------------------- - -2.12.0 (2022-09-01) -------------------- -* Add an initialization of the gripper action command for safe startup. (`#425 `_) -* Fix formatting CI job (`#418 `_) -* Contributors: Shota Aoki, Tyler Weaver - -2.11.0 (2022-08-04) -------------------- - -2.10.0 (2022-08-01) -------------------- -* Formatting changes from pre-commit (`#400 `_) -* Parameter loading fixup in diff_drive and gripper controllers (`#385 `_) -* Contributors: Andy Zelenak, Tyler Weaver - -2.9.0 (2022-07-14) ------------------- -* Allow gripper stalling when moving to goal (`#355 `_) -* Contributors: Marq Rasmussen - -2.8.0 (2022-07-09) ------------------- - -2.7.0 (2022-07-03) ------------------- -* Update controllers with new get_name hardware interfaces (`#369 `_) -* Contributors: Lucas Schulze - -2.6.0 (2022-06-18) ------------------- -* CMakeLists cleanup (`#362 `_) -* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) - * Default C++ version to 17 - * Replace explicit use of declare_paremeter with auto_declare -* Contributors: Andy Zelenak, Jafar Abdi - -2.5.0 (2022-05-13) ------------------- - -2.4.0 (2022-04-29) ------------------- -* updated to use node getter functions (`#329 `_) -* Contributors: Bence Magyar, Denis Štogl, Jack Center - -2.3.0 (2022-04-21) ------------------- -* Use CallbackReturn from controller_interface namespace (`#333 `_) -* Contributors: Bence Magyar, Denis Štogl - -2.2.0 (2022-03-25) ------------------- -* Use lifecycle node as base for controllers (`#244 `_) -* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar - -2.1.0 (2022-02-23) ------------------- - -2.0.1 (2022-02-01) ------------------- - -2.0.0 (2022-01-28) ------------------- - -1.3.0 (2022-01-11) ------------------- - -1.2.0 (2021-12-29) ------------------- - -1.1.0 (2021-10-25) ------------------- - -1.0.0 (2021-09-29) ------------------- -* Remove compile warnings. (`#245 `_) -* Add time and period to update function (`#241 `_) -* Unify style of controllers. (`#236 `_) -* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) -* Removing Boost from controllers. (`#235 `_) -* Contributors: Bence Magyar, Denis Štogl, bailaC - -0.5.0 (2021-08-30) ------------------- -* Add auto declaration of parameters. (`#224 `_) -* Bring precommit config up to speed with ros2_control (`#227 `_) -* Contributors: Bence Magyar, Lovro Ivanov - -0.4.1 (2021-07-08) ------------------- -* Fix test dependencies (`#213 `_) -* Contributors: Bence Magyar - -0.4.0 (2021-06-28) ------------------- -* Force torque sensor broadcaster (`#152 `_) - * Add rclcpp::shutdown(); to all standalone test functions -* Fixes for Windows (`#205 `_) - * Disable gripper on Windows too -* disable gripper on OSX (`#192 `_) -* Port gripper action controller to ROS2 (`#162 `_) -* Contributors: Bence Magyar, Denis Štogl, Jafar Abdi - -0.3.1 (2021-05-23) ------------------- - -0.3.0 (2021-05-21) ------------------- - -0.2.1 (2021-05-03) ------------------- - -0.2.0 (2021-02-06) ------------------- - -0.1.2 (2021-01-07) ------------------- - -0.1.1 (2021-01-06) ------------------- - -0.1.0 (2020-12-23) ------------------- diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt deleted file mode 100644 index 4ee1c4bb7c..0000000000 --- a/gripper_controllers/CMakeLists.txt +++ /dev/null @@ -1,83 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(gripper_controllers) - -find_package(ros2_control_cmake REQUIRED) -set_compiler_options() -export_windows_symbols() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs - control_toolbox - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_action - realtime_tools -) - -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -generate_parameter_library(gripper_action_controller_parameters - src/gripper_action_controller_parameters.yaml -) - -add_library(gripper_action_controller SHARED - src/gripper_action_controller.cpp -) -target_compile_features(gripper_action_controller PUBLIC cxx_std_17) -target_include_directories(gripper_action_controller PUBLIC - $ - $ -) -target_link_libraries(gripper_action_controller PUBLIC - gripper_action_controller_parameters -) -ament_target_dependencies(gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - ament_add_gmock(test_load_gripper_action_controllers - test/test_load_gripper_action_controllers.cpp - ) - ament_target_dependencies(test_load_gripper_action_controllers - controller_manager - ros2_control_test_assets - ) - - ament_add_gmock(test_gripper_controllers - test/test_gripper_controllers.cpp - ) - target_link_libraries(test_gripper_controllers - gripper_action_controller - ) -endif() - -install( - DIRECTORY include/ - DESTINATION include/gripper_action_controller -) -install( - TARGETS - gripper_action_controller - gripper_action_controller_parameters - EXPORT export_gripper_action_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - INCLUDES DESTINATION include -) - -ament_export_targets(export_gripper_action_controller HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/gripper_controllers/doc/userdoc.rst b/gripper_controllers/doc/userdoc.rst deleted file mode 100644 index 7f51c8f4ac..0000000000 --- a/gripper_controllers/doc/userdoc.rst +++ /dev/null @@ -1,27 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/gripper_controllers/doc/userdoc.rst - -.. _gripper_controllers_userdoc: - -Gripper Action Controller --------------------------------- - -Controllers for executing a gripper command action for simple single-dof grippers: - -- ``position_controllers/GripperActionController`` -- ``effort_controllers/GripperActionController`` - -Parameters -^^^^^^^^^^^ -These controllers use the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. - -List of parameters -========================= - -.. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml - - -An example parameter file -========================= - -.. generate_parameter_library_default:: - ../src/gripper_action_controller_parameters.yaml diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp deleted file mode 100644 index cf00b6c158..0000000000 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ /dev/null @@ -1,168 +0,0 @@ -// Copyright 2014, SRI International -// -// 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. - -/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian - -#ifndef GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_HPP_ -#define GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_HPP_ - -// C++ standard -#include -#include -#include -#include - -// ROS -#include "rclcpp/rclcpp.hpp" - -// ROS messages -#include "control_msgs/action/gripper_command.hpp" - -// rclcpp_action -#include "rclcpp_action/create_server.hpp" - -// ros_controls -#include "controller_interface/controller_interface.hpp" -#include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/loaned_state_interface.hpp" -#include "realtime_tools/realtime_buffer.hpp" -#include "realtime_tools/realtime_server_goal_handle.hpp" - -// Project -#include "gripper_controllers/hardware_interface_adapter.hpp" - -// auto-generated by generate_parameter_library -#include "gripper_controllers/gripper_action_controller_parameters.hpp" - -namespace gripper_action_controller -{ -/** - * \brief Controller for executing a gripper command action for simple - * single-dof grippers. - * - * \tparam HardwareInterface Controller hardware interface. Currently \p - * hardware_interface::HW_IF_POSITION and \p - * hardware_interface::HW_IF_EFFORT are supported out-of-the-box. - */ -template -class GripperActionController : public controller_interface::ControllerInterface -{ -public: - /** - * \brief Store position and max effort in struct to allow easier realtime - * buffer usage - */ - struct Commands - { - double position_; // Last commanded position - double max_effort_; // Max allowed effort - }; - GripperActionController(); - - /** - * @brief command_interface_configuration This controller requires the - * position command interfaces for the controlled joints - */ - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - /** - * @brief command_interface_configuration This controller requires the - * position and velocity state interfaces for the controlled joints - */ - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - controller_interface::CallbackReturn on_init() override; - - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - realtime_tools::RealtimeBuffer command_; - // pre-allocated memory that is reused to set the realtime buffer - Commands command_struct_, command_struct_rt_; - -protected: - using GripperCommandAction = control_msgs::action::GripperCommand; - using ActionServer = rclcpp_action::Server; - using ActionServerPtr = ActionServer::SharedPtr; - using GoalHandle = rclcpp_action::ServerGoalHandle; - using RealtimeGoalHandle = - realtime_tools::RealtimeServerGoalHandle; - using RealtimeGoalHandlePtr = std::shared_ptr; - using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; - - using HwIfaceAdapter = HardwareInterfaceAdapter; - - bool update_hold_position_; - - bool verbose_ = false; ///< Hard coded verbose flag to help in debugging - std::string name_; ///< Controller name. - std::optional> - joint_command_interface_; - std::optional> - joint_position_state_interface_; - std::optional> - joint_velocity_state_interface_; - - std::shared_ptr param_listener_; - Params params_; - - HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired goal state to HW interface. - - RealtimeGoalHandleBuffer - rt_active_goal_; ///< Container for the currently active action goal, if any. - control_msgs::action::GripperCommand::Result::SharedPtr pre_alloc_result_; - - rclcpp::Duration action_monitor_period_; - - // ROS API - ActionServerPtr action_server_; - - rclcpp::TimerBase::SharedPtr goal_handle_timer_; - - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle); - - void accepted_callback(std::shared_ptr goal_handle); - - void preempt_active_goal(); - - void set_hold_position(); - - rclcpp::Time last_movement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); ///< Store stall time - double computed_command_; ///< Computed command - - /** - * \brief Check for success and publish appropriate result and feedback. - **/ - void check_for_success( - const rclcpp::Time & time, double error_position, double current_position, - double current_velocity); -}; - -} // namespace gripper_action_controller - -#include "gripper_controllers/gripper_action_controller_impl.hpp" - -#endif // GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_HPP_ diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp deleted file mode 100644 index a94e163d7b..0000000000 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ /dev/null @@ -1,348 +0,0 @@ -// Copyright 2014, SRI International -// -// 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. - -/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian, Stu Glaser - -#ifndef GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ -#define GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ - -#include "gripper_controllers/gripper_action_controller.hpp" - -#include -#include - -namespace gripper_action_controller -{ -template -void GripperActionController::preempt_active_goal() -{ - // Cancels the currently active goal - const auto active_goal = *rt_active_goal_.readFromNonRT(); - if (active_goal) - { - // Marks the current goal as canceled - active_goal->setCanceled(std::make_shared()); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - } -} - -template -controller_interface::CallbackReturn GripperActionController::on_init() -{ - RCLCPP_WARN( - get_node()->get_logger(), - "[Deprecated]: the `position_controllers/GripperActionController` and " - "`effort_controllers::GripperActionController` controllers are replaced by " - "'parallel_gripper_controllers/GripperActionController' controller"); - try - { - param_listener_ = std::make_shared(get_node()); - params_ = param_listener_->get_params(); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -template -controller_interface::return_type GripperActionController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - command_struct_rt_ = *(command_.readFromRT()); - - const double current_position = joint_position_state_interface_->get().get_value(); - const double current_velocity = joint_velocity_state_interface_->get().get_value(); - - const double error_position = command_struct_rt_.position_ - current_position; - const double error_velocity = -current_velocity; - - check_for_success(get_node()->now(), error_position, current_position, current_velocity); - - // Hardware interface adapter: Generate and send commands - computed_command_ = hw_iface_adapter_.updateCommand( - command_struct_rt_.position_, 0.0, error_position, error_velocity, - command_struct_rt_.max_effort_); - return controller_interface::return_type::OK; -} - -template -rclcpp_action::GoalResponse GripperActionController::goal_callback( - const rclcpp_action::GoalUUID &, std::shared_ptr) -{ - RCLCPP_INFO(get_node()->get_logger(), "Received & accepted new action goal"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -template -void GripperActionController::accepted_callback( - std::shared_ptr goal_handle) // Try to update goal -{ - auto rt_goal = std::make_shared(goal_handle); - - // Accept new goal - preempt_active_goal(); - - // This is the non-realtime command_struct - // We use command_ for sharing - command_struct_.position_ = goal_handle->get_goal()->command.position; - command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; - command_.writeFromNonRT(command_struct_); - - pre_alloc_result_->reached_goal = false; - pre_alloc_result_->stalled = false; - - last_movement_time_ = get_node()->now(); - rt_goal->execute(); - rt_active_goal_.writeFromNonRT(rt_goal); - - // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list - goal_handle_timer_.reset(); - - // Setup goal status checking timer - goal_handle_timer_ = get_node()->create_wall_timer( - action_monitor_period_.to_chrono(), - std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); -} - -template -rclcpp_action::CancelResponse GripperActionController::cancel_callback( - const std::shared_ptr goal_handle) -{ - RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); - - // Check that cancel request refers to currently active goal (if any) - const auto active_goal = *rt_active_goal_.readFromNonRT(); - if (active_goal && active_goal->gh_ == goal_handle) - { - // Enter hold current position mode - set_hold_position(); - - RCLCPP_INFO( - get_node()->get_logger(), "Canceling active action goal because cancel callback received."); - - // Mark the current goal as canceled - auto action_res = std::make_shared(); - active_goal->setCanceled(action_res); - // Reset current goal - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - } - return rclcpp_action::CancelResponse::ACCEPT; -} - -template -void GripperActionController::set_hold_position() -{ - command_struct_.position_ = joint_position_state_interface_->get().get_value(); - command_struct_.max_effort_ = params_.max_effort; - command_.writeFromNonRT(command_struct_); -} - -template -void GripperActionController::check_for_success( - const rclcpp::Time & time, double error_position, double current_position, - double current_velocity) -{ - const auto active_goal = *rt_active_goal_.readFromNonRT(); - if (!active_goal) - { - return; - } - - if (fabs(error_position) < params_.goal_tolerance) - { - pre_alloc_result_->effort = computed_command_; - pre_alloc_result_->position = current_position; - pre_alloc_result_->reached_goal = true; - pre_alloc_result_->stalled = false; - RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); - active_goal->setSucceeded(pre_alloc_result_); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - } - else - { - if (fabs(current_velocity) > params_.stall_velocity_threshold) - { - last_movement_time_ = time; - } - else if ((time - last_movement_time_).seconds() > params_.stall_timeout) - { - pre_alloc_result_->effort = computed_command_; - pre_alloc_result_->position = current_position; - pre_alloc_result_->reached_goal = false; - pre_alloc_result_->stalled = true; - - if (params_.allow_stalling) - { - RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); - active_goal->setSucceeded(pre_alloc_result_); - } - else - { - RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); - active_goal->setAborted(pre_alloc_result_); - } - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - } - } -} - -template -controller_interface::CallbackReturn GripperActionController::on_configure( - const rclcpp_lifecycle::State &) -{ - const auto logger = get_node()->get_logger(); - params_ = param_listener_->get_params(); - - // Action status checking update rate - action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); - RCLCPP_INFO( - logger, "Action status changes will be monitored at %f Hz.", params_.action_monitor_rate); - - // Controlled joint - if (params_.joint.empty()) - { - RCLCPP_ERROR(logger, "Joint name cannot be empty"); - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; -} -template -controller_interface::CallbackReturn GripperActionController::on_activate( - const rclcpp_lifecycle::State &) -{ - auto command_interface_it = std::find_if( - command_interfaces_.begin(), command_interfaces_.end(), - [](const hardware_interface::LoanedCommandInterface & command_interface) - { return command_interface.get_interface_name() == HardwareInterface; }); - if (command_interface_it == command_interfaces_.end()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 %s command interface", HardwareInterface); - return controller_interface::CallbackReturn::ERROR; - } - if (command_interface_it->get_prefix_name() != params_.joint) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Command interface is different than joint name `%s` != `%s`", - command_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); - return controller_interface::CallbackReturn::ERROR; - } - const auto position_state_interface_it = std::find_if( - state_interfaces_.begin(), state_interfaces_.end(), - [](const hardware_interface::LoanedStateInterface & state_interface) - { return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); - if (position_state_interface_it == state_interfaces_.end()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); - return controller_interface::CallbackReturn::ERROR; - } - if (position_state_interface_it->get_prefix_name() != params_.joint) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Position state interface is different than joint name `%s` != `%s`", - position_state_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); - return controller_interface::CallbackReturn::ERROR; - } - const auto velocity_state_interface_it = std::find_if( - state_interfaces_.begin(), state_interfaces_.end(), - [](const hardware_interface::LoanedStateInterface & state_interface) - { return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; }); - if (velocity_state_interface_it == state_interfaces_.end()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); - return controller_interface::CallbackReturn::ERROR; - } - if (velocity_state_interface_it->get_prefix_name() != params_.joint) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Velocity command interface is different than joint name `%s` != `%s`", - velocity_state_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); - return controller_interface::CallbackReturn::ERROR; - } - - joint_command_interface_ = *command_interface_it; - joint_position_state_interface_ = *position_state_interface_it; - joint_velocity_state_interface_ = *velocity_state_interface_it; - - // Hardware interface adapter - hw_iface_adapter_.init(joint_command_interface_, get_node()); - - // Command - non RT version - command_struct_.position_ = joint_position_state_interface_->get().get_value(); - command_struct_.max_effort_ = params_.max_effort; - command_.initRT(command_struct_); - - // Result - pre_alloc_result_ = std::make_shared(); - pre_alloc_result_->position = command_struct_.position_; - pre_alloc_result_->reached_goal = false; - pre_alloc_result_->stalled = false; - - // Action interface - action_server_ = rclcpp_action::create_server( - get_node(), "~/gripper_cmd", - std::bind( - &GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&GripperActionController::cancel_callback, this, std::placeholders::_1), - std::bind(&GripperActionController::accepted_callback, this, std::placeholders::_1)); - - return controller_interface::CallbackReturn::SUCCESS; -} - -template -controller_interface::CallbackReturn GripperActionController::on_deactivate( - const rclcpp_lifecycle::State &) -{ - joint_command_interface_ = std::nullopt; - joint_position_state_interface_ = std::nullopt; - joint_velocity_state_interface_ = std::nullopt; - release_interfaces(); - return controller_interface::CallbackReturn::SUCCESS; -} - -template -controller_interface::InterfaceConfiguration -GripperActionController::command_interface_configuration() const -{ - return { - controller_interface::interface_configuration_type::INDIVIDUAL, - {params_.joint + "/" + HardwareInterface}}; -} - -template -controller_interface::InterfaceConfiguration -GripperActionController::state_interface_configuration() const -{ - return { - controller_interface::interface_configuration_type::INDIVIDUAL, - {params_.joint + "/" + hardware_interface::HW_IF_POSITION, - params_.joint + "/" + hardware_interface::HW_IF_VELOCITY}}; -} - -template -GripperActionController::GripperActionController() -: controller_interface::ControllerInterface(), - action_monitor_period_(rclcpp::Duration::from_seconds(0)) -{ -} - -} // namespace gripper_action_controller - -#endif // GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp deleted file mode 100644 index 66da3d869a..0000000000 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ /dev/null @@ -1,183 +0,0 @@ -// Copyright 2014, SRI International -// Copyright 2013, 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. - -/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian - -#ifndef GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_ -#define GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_ - -#include -#include -#include -#include - -#include "control_toolbox/pid.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -/** - * \brief Helper class to simplify integrating the GripperActionController with - * different hardware interfaces. - * - * The GripperActionController outputs position while - * it is supposed to work with either position or effort commands. - * - */ -template -class HardwareInterfaceAdapter -{ -public: - bool init( - std::optional< - std::reference_wrapper> /* joint_handle */, - std::shared_ptr & /* node */) - { - return false; - } - - void starting(const rclcpp::Time & /* time */) {} - void stopping(const rclcpp::Time & /* time */) {} - - double updateCommand( - double /* desired_position */, double /* desired_velocity */, double /* error_position */, - double /* error_velocity */, double /* max_allowed_effort */) - { - return 0.0; - } -}; - -/** - * \brief Adapter for a position-controlled hardware interface. Forwards desired - * positions as commands. - */ -template <> -class HardwareInterfaceAdapter -{ -public: - bool init( - std::optional> joint_handle, - const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */) - { - joint_handle_ = joint_handle; - return true; - } - - void starting(const rclcpp::Time & /* time */) {} - void stopping(const rclcpp::Time & /* time */) {} - - double updateCommand( - double desired_position, double /* desired_velocity */, double /* error_position */, - double /* error_velocity */, double max_allowed_effort) - { - // Forward desired position to command - joint_handle_->get().set_value(desired_position); - return max_allowed_effort; - } - -private: - std::optional> joint_handle_; -}; - -/** - * \brief Adapter for an effort-controlled hardware interface. Maps position and - * velocity errors to effort commands through a position PID loop. - * - * The following is an example configuration of a controller that uses this - * adapter. Notice the \p gains entry: \code gripper_controller: type: - * "gripper_action_controller/GripperActionController" joints: gripper_joint - * goal_tolerance: 0.01 - * stalled_velocity_threshold: 0.01 - * stall_timeout: 0.2 - * gains: - * gripper_joint: {p: 200, d: 1, i: 5, i_clamp: 1} - * \endcode - */ -template <> -class HardwareInterfaceAdapter -{ -public: - template - auto auto_declare( - const std::shared_ptr & node, const std::string & name, - const ParameterT & default_value) - { - if (!node->has_parameter(name)) - { - return node->declare_parameter(name, default_value); - } - else - { - return node->get_parameter(name).get_value(); - } - } - - bool init( - std::optional> joint_handle, - const std::shared_ptr & node) - { - joint_handle_ = joint_handle; - // Init PID gains from ROS parameter server - const std::string prefix = "gains." + joint_handle_->get().get_prefix_name(); - const auto k_p = auto_declare(node, prefix + ".p", 0.0); - const auto k_i = auto_declare(node, prefix + ".i", 0.0); - const auto k_d = auto_declare(node, prefix + ".d", 0.0); - const auto i_clamp = auto_declare(node, prefix + ".i_clamp", 0.0); - // Initialize PID - pid_ = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); - return true; - } - - void starting(const rclcpp::Time & /* time */) - { - if (!joint_handle_) - { - return; - } - // Reset PIDs, zero effort commands - pid_->reset(); - joint_handle_->get().set_value(0.0); - } - - void stopping(const rclcpp::Time & /* time */) {} - - double updateCommand( - double /* desired_position */, double /* desired_velocity */, double error_position, - double error_velocity, double max_allowed_effort) - { - // Preconditions - if (!joint_handle_) - { - return 0.0; - } - // Time since the last call to update - const auto period = std::chrono::steady_clock::now() - last_update_time_; - // Update PIDs - double command = pid_->compute_command(error_position, error_velocity, period); - command = std::min( - fabs(max_allowed_effort), std::max(-fabs(max_allowed_effort), command)); - joint_handle_->get().set_value(command); - last_update_time_ = std::chrono::steady_clock::now(); - return command; - } - -private: - using PidPtr = std::shared_ptr; - PidPtr pid_; - std::optional> joint_handle_; - std::chrono::steady_clock::time_point last_update_time_; -}; - -#endif // GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml deleted file mode 100644 index d7decaac37..0000000000 --- a/gripper_controllers/package.xml +++ /dev/null @@ -1,47 +0,0 @@ - - - - gripper_controllers - 4.24.0 - The gripper_controllers package - - Bence Magyar - Denis Štogl - Christoph Froehlich - Sai Kishor Kothakota - - Apache License 2.0 - - https://control.ros.org - https://github.com/ros-controls/ros2_controllers/issues - https://github.com/ros-controls/ros2_controllers/ - - Sachin Chitta - Jafar Abdi - - ament_cmake - - ros2_control_cmake - - backward_ros - control_msgs - control_toolbox - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_action - realtime_tools - - ament_cmake_gmock - controller_manager - hardware_interface_testing - ros2_control_test_assets - - - ament_cmake - - diff --git a/gripper_controllers/ros_control_plugins.xml b/gripper_controllers/ros_control_plugins.xml deleted file mode 100644 index cbb7e9f93f..0000000000 --- a/gripper_controllers/ros_control_plugins.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - diff --git a/gripper_controllers/src/gripper_action_controller.cpp b/gripper_controllers/src/gripper_action_controller.cpp deleted file mode 100644 index a41e41000a..0000000000 --- a/gripper_controllers/src/gripper_action_controller.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2014, SRI International -// -// 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. - -/// \author Sachin Chitta - -// Project -#include -#include -namespace position_controllers -{ -/** - * \brief Gripper action controller that sends - * commands to a \b position interface. - */ -using GripperActionController = - gripper_action_controller::GripperActionController; -} // namespace position_controllers - -namespace effort_controllers -{ -/** - * \brief Gripper action controller that sends - * commands to a \b effort interface. - */ -using GripperActionController = - gripper_action_controller::GripperActionController; -} // namespace effort_controllers - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - position_controllers::GripperActionController, controller_interface::ControllerInterface) -PLUGINLIB_EXPORT_CLASS( - effort_controllers::GripperActionController, controller_interface::ControllerInterface) diff --git a/gripper_controllers/src/gripper_action_controller_parameters.yaml b/gripper_controllers/src/gripper_action_controller_parameters.yaml deleted file mode 100644 index a847643b9f..0000000000 --- a/gripper_controllers/src/gripper_action_controller_parameters.yaml +++ /dev/null @@ -1,43 +0,0 @@ -gripper_action_controller: - action_monitor_rate: { - type: double, - default_value: 20.0, - description: "Hz", - validation: { - gt_eq: [0.1] - }, - } - joint: { - type: string, - default_value: "", - } - goal_tolerance: { - type: double, - default_value: 0.01, - validation: { - gt_eq: [0.0] - }, - } - max_effort: { - type: double, - default_value: 0.0, - description: "Max allowable effort", - validation: { - gt_eq: [0.0] - }, - } - allow_stalling: { - type: bool, - description: "Allow stalling will make the action server return success if the gripper stalls when moving to the goal", - default_value: false, - } - stall_velocity_threshold: { - type: double, - description: "stall velocity threshold", - default_value: 0.001, - } - stall_timeout: { - type: double, - description: "stall timeout", - default_value: 1.0, - } diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp deleted file mode 100644 index 97fef24eaf..0000000000 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ /dev/null @@ -1,182 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 "gmock/gmock.h" - -#include "test_gripper_controllers.hpp" - -#include "hardware_interface/loaned_command_interface.hpp" - -using hardware_interface::LoanedCommandInterface; -using hardware_interface::LoanedStateInterface; -using GripperCommandAction = control_msgs::action::GripperCommand; -using GoalHandle = rclcpp_action::ServerGoalHandle; -using testing::SizeIs; -using testing::UnorderedElementsAre; - -template -void GripperControllerTest::SetUpTestCase() -{ - rclcpp::init(0, nullptr); -} - -template -void GripperControllerTest::TearDownTestCase() -{ - rclcpp::shutdown(); -} - -template -void GripperControllerTest::SetUp() -{ - // initialize controller - controller_ = std::make_unique>(); -} - -template -void GripperControllerTest::TearDown() -{ - controller_.reset(nullptr); -} - -template -void GripperControllerTest::SetUpController() -{ - const auto result = - controller_->init("gripper_controller", "", 0, "", controller_->define_custom_node_options()); - ASSERT_EQ(result, controller_interface::return_type::OK); - - std::vector command_ifs; - command_ifs.emplace_back(this->joint_1_cmd_); - std::vector state_ifs; - state_ifs.emplace_back(this->joint_1_pos_state_); - state_ifs.emplace_back(this->joint_1_vel_state_); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); -} - -using TestTypes = ::testing::Types< - std::integral_constant, - std::integral_constant>; -TYPED_TEST_SUITE(GripperControllerTest, TestTypes); - -TYPED_TEST(GripperControllerTest, ParametersNotSet) -{ - this->SetUpController(); - - // configure failed, 'joints' parameter not set - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::ERROR); -} - -TYPED_TEST(GripperControllerTest, JointParameterIsEmpty) -{ - this->SetUpController(); - - this->controller_->get_node()->set_parameter({"joint", ""}); - - // configure failed, 'joints' is empty - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::ERROR); -} - -TYPED_TEST(GripperControllerTest, ConfigureParamsSuccess) -{ - this->SetUpController(); - - this->controller_->get_node()->set_parameter({"joint", "joint_1"}); - - // configure successful - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - - // check interface configuration - auto cmd_if_conf = this->controller_->command_interface_configuration(); - ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); - ASSERT_THAT(cmd_if_conf.names, UnorderedElementsAre(std::string("joint_1/") + TypeParam::value)); - EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_if_conf = this->controller_->state_interface_configuration(); - ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); - ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); - EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); -} - -TYPED_TEST(GripperControllerTest, ActivateWithWrongJointsNamesFails) -{ - this->SetUpController(); - - this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); - - // activate failed, 'joint4' is not a valid joint name for the hardware - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - this->controller_->on_activate(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::ERROR); -} - -TYPED_TEST(GripperControllerTest, ActivateSuccess) -{ - this->SetUpController(); - - this->controller_->get_node()->set_parameter({"joint", "joint1"}); - - // activate successful - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - this->controller_->on_activate(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); -} - -TYPED_TEST(GripperControllerTest, ActivateDeactivateActivateSuccess) -{ - this->SetUpController(); - - this->controller_->get_node()->set_parameter({"joint", "joint1"}); - - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - this->controller_->on_activate(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - this->controller_->on_deactivate(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - - // re-assign interfaces - std::vector command_ifs; - command_ifs.emplace_back(this->joint_1_cmd_); - std::vector state_ifs; - state_ifs.emplace_back(this->joint_1_pos_state_); - state_ifs.emplace_back(this->joint_1_vel_state_); - this->controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - this->controller_->on_activate(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); -} diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp deleted file mode 100644 index cb92bc9dcd..0000000000 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright 2022 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. - -#ifndef TEST_GRIPPER_CONTROLLERS_HPP_ -#define TEST_GRIPPER_CONTROLLERS_HPP_ - -#include - -#include -#include -#include - -#include "gripper_controllers/gripper_action_controller.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" - -using hardware_interface::CommandInterface; -using hardware_interface::HW_IF_EFFORT; -using hardware_interface::HW_IF_POSITION; -using hardware_interface::HW_IF_VELOCITY; -using hardware_interface::StateInterface; - -namespace -{ -// subclassing and friending so we can access member variables -template -class FriendGripperController -: public gripper_action_controller::GripperActionController -{ - FRIEND_TEST(GripperControllerTest, CommandSuccessTest); -}; - -template -class GripperControllerTest : public ::testing::Test -{ -public: - static void SetUpTestCase(); - static void TearDownTestCase(); - - void SetUp(); - void TearDown(); - - void SetUpController(); - void SetUpHandles(); - -protected: - std::unique_ptr> controller_; - - // dummy joint state values used for tests - const std::string joint_name_ = "joint1"; - std::vector joint_states_ = {1.1, 2.1}; - std::vector joint_commands_ = {3.1}; - - StateInterface joint_1_pos_state_{joint_name_, HW_IF_POSITION, &joint_states_[0]}; - StateInterface joint_1_vel_state_{joint_name_, HW_IF_VELOCITY, &joint_states_[1]}; - CommandInterface joint_1_cmd_{joint_name_, T::value, &joint_commands_[0]}; -}; - -} // anonymous namespace - -#endif // TEST_GRIPPER_CONTROLLERS_HPP_ diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp deleted file mode 100644 index b607fb6fe7..0000000000 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadGripperActionControllers, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); - - ASSERT_NE( - cm.load_controller( - "test_gripper_action_position_controller", "position_controllers/GripperActionController"), - nullptr); - ASSERT_NE( - cm.load_controller( - "test_gripper_action_effort_controller", "effort_controllers/GripperActionController"), - nullptr); - - rclcpp::shutdown(); -} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index a9cb03e85c..7a440c6625 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -26,7 +26,6 @@ forward_command_controller gpio_controllers gps_sensor_broadcaster - gripper_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller