diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 2cb55150ce..78abe5d21f 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -52,6 +52,7 @@ The controllers are using `common hardware interface definitions`_, and may use 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> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 77598591ae..2c115091af 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -40,6 +40,11 @@ void GripperActionController::preempt_active_goal() 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()); diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt new file mode 100644 index 0000000000..56aa623a6f --- /dev/null +++ b/parallel_gripper_controller/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.16) +project(parallel_gripper_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +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(parallel_gripper_action_controller_parameters + src/gripper_action_controller_parameters.yaml +) + +add_library(parallel_gripper_action_controller SHARED + src/parallel_gripper_action_controller.cpp +) +target_compile_features(parallel_gripper_action_controller PUBLIC cxx_std_17) +target_include_directories(parallel_gripper_action_controller PUBLIC + $ + $ +) +target_link_libraries(parallel_gripper_action_controller PUBLIC + parallel_gripper_action_controller_parameters +) +ament_target_dependencies(parallel_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_parallel_gripper_action_controllers + test/test_load_parallel_gripper_action_controller.cpp + ) + ament_target_dependencies(test_load_parallel_gripper_action_controllers + controller_manager + ros2_control_test_assets + ) + + ament_add_gmock(test_parallel_gripper_controller + test/test_parallel_gripper_controller.cpp + ) + target_link_libraries(test_parallel_gripper_controller + parallel_gripper_action_controller + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/parallel_gripper_action_controller +) +install( + TARGETS + parallel_gripper_action_controller + parallel_gripper_action_controller_parameters + EXPORT export_parallel_gripper_action_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_parallel_gripper_action_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/parallel_gripper_controller/doc/userdoc.rst b/parallel_gripper_controller/doc/userdoc.rst new file mode 100644 index 0000000000..5c04eb5f90 --- /dev/null +++ b/parallel_gripper_controller/doc/userdoc.rst @@ -0,0 +1,19 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/parallel_gripper_controller/doc/userdoc.rst + +.. _parallel_gripper_controller_userdoc: + +Parallel Gripper Action Controller +----------------------------------- + +Controller for executing a ``ParallelGripperCommand`` action for simple parallel grippers. +This controller supports grippers that offer position only control as well as grippers that allow configuring the velocity and effort. +By default, the controller will only claim the ``{joint}/position`` interface for control. +The velocity and effort interfaces can be optionally claimed by setting the ``max_velocity_interface`` and ``max_effort_interface`` parameter, respectively. +By default, the controller will try to claim position and velocity state interfaces. +The claimed state interfaces can be configured by setting the ``state_interfaces`` parameter. + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp new file mode 100644 index 0000000000..d303990b89 --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -0,0 +1,174 @@ +// 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 PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ + +// C++ standard +#include +#include +#include +#include + +// ROS +#include "rclcpp/rclcpp.hpp" + +// ROS messages +#include "control_msgs/action/parallel_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 "parallel_gripper_controller/visibility_control.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_server_goal_handle.h" + +// Project +#include "parallel_gripper_action_controller_parameters.hpp" + +namespace parallel_gripper_action_controller +{ +/** + * \brief Controller for executing a `ParallelGripperCommand` action. + * + * \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. + */ + +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_cmd_; // Commanded position + double max_velocity_; // Desired max gripper velocity + double max_effort_; // Desired max allowed effort + }; + + GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); + + /** + * @brief command_interface_configuration This controller requires the + * position command interfaces for the controlled joints + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + 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 + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + realtime_tools::RealtimeBuffer command_; + // pre-allocated memory that is re-used to set the realtime buffer + Commands command_struct_, command_struct_rt_; + +protected: + using GripperCommandAction = control_msgs::action::ParallelGripperCommand; + 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; + + 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> + effort_interface_; + std::optional> + speed_interface_; + std::optional> + joint_position_state_interface_; + std::optional> + joint_velocity_state_interface_; + + std::shared_ptr param_listener_; + Params params_; + + RealtimeGoalHandleBuffer + rt_active_goal_; ///< Container for the currently active action goal, if any. + control_msgs::action::ParallelGripperCommand::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 parallel_gripper_action_controller + +#include "parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp" + +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp new file mode 100644 index 0000000000..b6fccf1bbf --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -0,0 +1,396 @@ +// 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 PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ + +#include +#include +#include + +#include +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" + +namespace parallel_gripper_action_controller +{ + +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()); + } +} + +controller_interface::CallbackReturn GripperActionController::on_init() +{ + 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; +} + +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_cmd_ - current_position; + + check_for_success(get_node()->now(), error_position, current_position, current_velocity); + + joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_); + if (speed_interface_.has_value()) + { + speed_interface_->get().set_value(command_struct_rt_.max_velocity_); + } + if (effort_interface_.has_value()) + { + effort_interface_->get().set_value(command_struct_rt_.max_effort_); + } + + return controller_interface::return_type::OK; +} + +rclcpp_action::GoalResponse GripperActionController::goal_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr goal_handle) +{ + if (goal_handle->command.position.size() != 1) + { + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + RCLCPP_ERROR( + get_node()->get_logger(), + "Received action goal with wrong number of position values, expects 1, got %zu", + goal_handle->command.position.size()); + return rclcpp_action::GoalResponse::REJECT; + } + + RCLCPP_INFO(get_node()->get_logger(), "Received & accepted new action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +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_cmd_ = goal_handle->get_goal()->command.position[0]; + if (!params_.max_velocity_interface.empty() && !goal_handle->get_goal()->command.velocity.empty()) + { + command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0]; + } + else + { + command_struct_.max_velocity_ = params_.max_velocity; + } + if (!params_.max_effort_interface.empty() && !goal_handle->get_goal()->command.effort.empty()) + { + command_struct_.max_effort_ = goal_handle->get_goal()->command.effort[0]; + } + else + { + command_struct_.max_effort_ = params_.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)); +} + +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; +} + +void GripperActionController::set_hold_position() +{ + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_effort_ = params_.max_effort; + command_struct_.max_velocity_ = params_.max_velocity; + command_.writeFromNonRT(command_struct_); +} + +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_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = 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_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = 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()); + } + } +} + +controller_interface::CallbackReturn GripperActionController::on_configure( + const rclcpp_lifecycle::State &) +{ + const auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + 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_STREAM( + logger, "Action status changes will be monitored at " << params_.action_monitor_rate << "Hz."); + + // Controlled joint + if (params_.joint.empty()) + { + RCLCPP_ERROR(logger, "Joint name cannot be empty"); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +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() == hardware_interface::HW_IF_POSITION; }); + if (command_interface_it == command_interfaces_.end()) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Expected 1 " << hardware_interface::HW_IF_POSITION << " command interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (command_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Command interface is different than joint name `" + << command_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + 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_STREAM( + get_node()->get_logger(), "Position state interface is different than joint name `" + << position_state_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + 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_STREAM( + get_node()->get_logger(), "Velocity command interface is different than joint name `" + << velocity_state_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + 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; + + for (auto & interface : command_interfaces_) + { + if (interface.get_interface_name() == "set_gripper_max_effort") + { + effort_interface_ = interface; + } + else if (interface.get_interface_name() == "set_gripper_max_velocity") + { + speed_interface_ = interface; + } + } + + + // Command - non RT version + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_effort_ = params_.max_effort; + command_struct_.max_velocity_ = params_.max_velocity; + command_.initRT(command_struct_); + + // Result + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + pre_alloc_result_->state.position[0] = command_struct_.position_cmd_; + 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; +} + +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; +} + +controller_interface::InterfaceConfiguration +GripperActionController::command_interface_configuration() const +{ + std::vector names = {params_.joint + "/" + hardware_interface::HW_IF_POSITION}; + if (!params_.max_effort_interface.empty()) + { + names.push_back({params_.max_effort_interface}); + } + if (!params_.max_velocity_interface.empty()) + { + names.push_back({params_.max_velocity_interface}); + } + + return {controller_interface::interface_configuration_type::INDIVIDUAL, names}; +} + +controller_interface::InterfaceConfiguration +GripperActionController::state_interface_configuration() const +{ + std::vector interface_names; + for (const auto & interface : params_.state_interfaces) + { + interface_names.push_back(params_.joint + "/" + interface); + } + return {controller_interface::interface_configuration_type::INDIVIDUAL, interface_names}; +} + +GripperActionController::GripperActionController() +: controller_interface::ControllerInterface(), + action_monitor_period_(rclcpp::Duration::from_seconds(0)) +{ +} + +} // namespace parallel_gripper_action_controller + +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp new file mode 100644 index 0000000000..63e5c8e1c7 --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp @@ -0,0 +1,56 @@ +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec(dllexport) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml new file mode 100644 index 0000000000..f189071617 --- /dev/null +++ b/parallel_gripper_controller/package.xml @@ -0,0 +1,35 @@ + + + + parallel_gripper_controller + 2.32.0 + The parallel_gripper_controller package + Bence Magyar + + Apache License 2.0 + + Sachin Chitta + + ament_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 + ros2_control_test_assets + + + ament_cmake + + diff --git a/parallel_gripper_controller/ros_control_plugins.xml b/parallel_gripper_controller/ros_control_plugins.xml new file mode 100644 index 0000000000..dcd9609ba7 --- /dev/null +++ b/parallel_gripper_controller/ros_control_plugins.xml @@ -0,0 +1,10 @@ + + + + + + + + diff --git a/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml new file mode 100644 index 0000000000..9b0dbb8a05 --- /dev/null +++ b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml @@ -0,0 +1,69 @@ +parallel_gripper_action_controller: + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Hz", + read_only: true, + validation: { + gt_eq: [ 0.1 ] + }, + } + joint: { + type: string, + read_only: true, + } + state_interfaces: { + type: string_array, + default_value: [position, velocity], + } + goal_tolerance: { + type: double, + default_value: 0.01, + 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, + validation: { + gt_eq: [ 0.0 ] + }, + } + max_effort_interface: { + type: string, + description: "Controller will claim the specified effort interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", + } + max_effort: { + type: double, + default_value: 10.0, + description: "Default effort used for grasping (Newtons)", + validation: { + gt_eq: [ 0.0 ] + }, + } + max_velocity_interface: { + type: string, + description: "Controller will claim the speed specified interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", + } + max_velocity: { + type: double, + default_value: 0.1, + description: "Default target velocity used for grasping (meters/second)", + validation: { + gt_eq: [ 0.0 ] + }, + } diff --git a/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp new file mode 100644 index 0000000000..3a6d028ddf --- /dev/null +++ b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp @@ -0,0 +1,25 @@ +// 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 + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + parallel_gripper_action_controller::GripperActionController, + controller_interface::ControllerInterface) diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp new file mode 100644 index 0000000000..130b12e0bb --- /dev/null +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -0,0 +1,45 @@ +// 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/executors/single_threaded_executor.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( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "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/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp new file mode 100644 index 0000000000..d9a5ae958b --- /dev/null +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -0,0 +1,169 @@ +// 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 + +#include "gmock/gmock.h" + +#include "test_parallel_gripper_controller.hpp" + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using GripperCommandAction = control_msgs::action::ParallelGripperCommand; +using GoalHandle = rclcpp_action::ServerGoalHandle; +using testing::SizeIs; +using testing::UnorderedElementsAre; + +void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } + +void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } + +void GripperControllerTest::SetUp() +{ + // initialize controller + controller_ = std::make_unique(); +} + +void GripperControllerTest::TearDown() { controller_.reset(nullptr); } + +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)); +} + +TEST_F(GripperControllerTest, ParametersNotSet) +{ + this->SetUpController(); + + // configure failed, 'joints' parameter not set + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(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); +} + +TEST_F(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/") + hardware_interface::HW_IF_POSITION)); + 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); +} + +TEST_F(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); +} + +TEST_F(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); +} + +TEST_F(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/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp new file mode 100644 index 0000000000..8bc6ab954c --- /dev/null +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -0,0 +1,66 @@ +// 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_PARALLEL_GRIPPER_CONTROLLER_HPP_ +#define TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" + +namespace +{ +// subclassing and friending so we can access member variables +class FriendGripperController : public parallel_gripper_action_controller::GripperActionController +{ + FRIEND_TEST(GripperControllerTest, CommandSuccessTest); +}; + +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}; + + hardware_interface::StateInterface joint_1_pos_state_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]}; + hardware_interface::StateInterface joint_1_vel_state_{ + joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]}; + hardware_interface::CommandInterface joint_1_cmd_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]}; +}; + +} // anonymous namespace + +#endif // TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_