add gripper controller with optional velocity/effort

Signed-off-by: Paul Gesel <[email protected]>

antipodal_gripper_controller/CMakeLists.txt

@@ -0,0 +1,88 @@
+cmake_minimum_required(VERSION 3.16)
+project(antipodal_gripper_controller LANGUAGES CXX)
+
+if(APPLE OR WIN32)
+  message(WARNING "antipodal gripper controllers are not available on OSX or Windows")
+  return()
+endif()
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra)
+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(antipodal_gripper_action_controller_parameters
+  src/antipodal_gripper_action_controller_parameters.yaml
+)
+
+add_library(antipodal_gripper_action_controller SHARED
+        src/antipodal_gripper_action_controller.cpp
+)
+target_compile_features(antipodal_gripper_action_controller PUBLIC cxx_std_17)
+target_include_directories(antipodal_gripper_action_controller PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/antipodal_gripper_action_controller>
+)
+target_link_libraries(antipodal_gripper_action_controller PUBLIC
+        antipodal_gripper_action_controller_parameters
+)
+ament_target_dependencies(antipodal_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_antipodal_gripper_action_controllers
+          test/test_load_antipodal_gripper_action_controller.cpp
+  )
+  ament_target_dependencies(test_load_antipodal_gripper_action_controllers
+    controller_manager
+    ros2_control_test_assets
+  )
+
+  ament_add_gmock(test_antipodal_gripper_controller
+    test/test_antipodal_gripper_controller.cpp
+  )
+  target_link_libraries(test_antipodal_gripper_controller
+    antipodal_gripper_action_controller
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/antipodal_gripper_action_controller
+)
+install(
+  TARGETS
+    antipodal_gripper_action_controller
+        antipodal_gripper_action_controller_parameters
+  EXPORT export_antipodal_gripper_action_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  INCLUDES DESTINATION include
+)
+
+ament_export_targets(export_antipodal_gripper_action_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

antipodal_gripper_controller/doc/userdoc.rst

@@ -0,0 +1,14 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/antipodal_gripper_controller/doc/userdoc.rst
+
+.. _antipodal_gripper_controller_userdoc:
+
+Gripper Action Controller
+--------------------------------
+
+Controller for executing a gripper command action for simple single-dof antipodal grippers.
+
+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

antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp

@@ -0,0 +1,175 @@
+// 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 ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_
+#define ANTIPODAL_GRIPPER_CONTROLLER__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/gripper_command.hpp"
+
+// rclcpp_action
+#include "rclcpp_action/create_server.hpp"
+
+// ros_controls
+#include "controller_interface/controller_interface.hpp"
+#include "antipodal_gripper_controller/visibility_control.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "realtime_tools/realtime_server_goal_handle.h"
+
+// Project
+#include "antipodal_gripper_action_controller_parameters.hpp"
+
+namespace antipodal_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.
+ */
+
+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 target_velocity_;  // Desired gripper velocity
+    double max_effort_;       // 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::GripperCommand;
+  using ActionServer = rclcpp_action::Server<GripperCommandAction>;
+  using ActionServerPtr = ActionServer::SharedPtr;
+  using GoalHandle = rclcpp_action::ServerGoalHandle<GripperCommandAction>;
+  using RealtimeGoalHandle =
+    realtime_tools::RealtimeServerGoalHandle<control_msgs::action::GripperCommand>;
+  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::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<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 antipodal_gripper_action_controller
+
+#include "antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp"
+
+#endif  // ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_