Add parallel_gripper_controller, configure gripper speed and effort w…

…ith hardware interface (#1002)

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>

gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp

@@ -40,6 +40,11 @@ void GripperActionController<HardwareInterface>::preempt_active_goal()
 template <const char * HardwareInterface>
 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::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<ParamListener>(get_node());

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
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/parallel_gripper_action_controller>
+)
+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()

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 <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters.
+
+.. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml

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 <cassert>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+// 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<Commands> 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<GripperCommandAction>;
+  using ActionServerPtr = ActionServer::SharedPtr;
+  using GoalHandle = rclcpp_action::ServerGoalHandle<GripperCommandAction>;
+  using RealtimeGoalHandle =
+    realtime_tools::RealtimeServerGoalHandle<control_msgs::action::ParallelGripperCommand>;
+  using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
+  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
+
+  bool update_hold_position_;
+
+  bool verbose_ = false;  ///< Hard coded verbose flag to help in debugging
+  std::string name_;      ///< Controller name.
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    joint_command_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    effort_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    speed_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+    joint_position_state_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+    joint_velocity_state_interface_;
+
+  std::shared_ptr<ParamListener> 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<const GripperCommandAction::Goal> goal);
+
+  rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<GoalHandle> goal_handle);
+
+  void accepted_callback(std::shared_ptr<GoalHandle> 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_