Adding Io gripper controller

io_gripper_controller/CMakeLists.txt

@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 3.8)
+project(io_gripper_controller)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow)
+endif()
+
+# using this instead of visibility macros
+# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
+set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+
+# find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  ament_cmake
+  ament_cmake_gmock
+  control_msgs
+  controller_interface
+  controller_manager
+  generate_parameter_library
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  ros2_control_test_assets
+  sensor_msgs
+  std_msgs
+  std_srvs
+)
+
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(io_gripper_controller_parameters
+  src/io_gripper_controller.yaml
+)
+add_library(
+  io_gripper_controller
+  SHARED
+  src/io_gripper_controller.cpp
+)
+target_include_directories(io_gripper_controller PUBLIC
+  "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(io_gripper_controller io_gripper_controller_parameters)
+ament_target_dependencies(io_gripper_controller ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_compile_definitions(io_gripper_controller PRIVATE "io_gripper_controller_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(
+  controller_interface io_gripper_controller.xml)
+
+install(
+  TARGETS
+  io_gripper_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+
+  ament_add_gmock(test_load_io_gripper_controller test/test_load_io_gripper_controller.cpp)
+  target_include_directories(test_load_io_gripper_controller PRIVATE include)
+  ament_target_dependencies(
+    test_load_io_gripper_controller
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+
+  ament_add_gmock(test_io_gripper_controller
+    test/test_io_gripper_controller.cpp
+    test/test_io_gripper_controller_open.cpp
+    test/test_io_gripper_controller_close.cpp
+    test/test_io_gripper_controller_all_param_set.cpp
+    test/test_io_gripper_controller_open_close_action.cpp
+    test/test_io_gripper_controller_reconfigure.cpp
+    test/test_io_gripper_controller_reconfigure_action.cpp
+  )
+  target_include_directories(test_io_gripper_controller PRIVATE include)
+  target_link_libraries(test_io_gripper_controller io_gripper_controller)
+  ament_target_dependencies(
+    test_io_gripper_controller
+    controller_interface
+    hardware_interface
+  )
+
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_dependencies(
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+ament_export_libraries(
+  io_gripper_controller
+)
+
+ament_package()

io_gripper_controller/README.md

@@ -0,0 +1,5 @@
+# io_gripper_controller package
+
+The package implements controllers to control the gripper using IOs.
+
+For detailed documentation check the `docs` folder or [ros2_control documentation](https://control.ros.org/).

io_gripper_controller/doc/userdoc.rst

@@ -0,0 +1,102 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/io_gripper_controller/doc/userdoc.rst
+
+.. _io_gripper_controller_userdoc:
+
+io_gripper_controller
+=============================
+
+The IO Gripper Controller provides implementation to control the grippers that are commanded using IOs.
+This is often the case for pneumatic gripper in the industy, that can range from simple parallel gripper up to custom, multi-dof grippers for manipulating specific parts.
+ It provides functionalities like open, close and reconfigure which can be used either though action server or service server and also publishes gripper's joint values if any and provides output for all gripper's command and state interfaces.
+
+Description of controller's interfaces
+---------------------------------------
+
+- ``joint_states`` [``sensor_msgs::msg::JointState``]: Publishes the state of gripper's open/close joint if any and configuration joints that might influece the geometry and kinematics of the gripper.
+- ``dynamic_interfaces`` [``control_msgs::msg::DynamicInterfaceValues``]: Publishes all command and state interface of the IOs and sensors of gripper.
+
+
+Parameters
+,,,,,,,,,,,
+
+This controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters.
+
+This controller adds the following parameters:
+
+.. generate_parameter_library_details:: ../src/io_gripper_controller.yaml
+
+
+Example parameters
+....................
+
+.. code-block:: yaml
+
+  io_gripper_controller:
+
+    ros__parameters:
+
+      use_action: true
+
+      open_close_joints: [gripper_clamp_jaw]
+
+      open:
+        joint_states: [0.0]
+        set_before_command:
+          high: [Release_Break_valve]
+          low: []
+        command:
+          high: [Open_valve]
+          low: [Close_valve]
+        state:
+          high: [Opened_signal]
+          low: [Closed_signal]
+        set_after_command:
+          high: []
+          low: [Release_Break_valve]
+
+      possible_closed_states: ['empty_close', 'full_close']
+
+      close:
+        set_before_command:
+          high: [Release_Break_valve]
+          low: []
+        command:
+          high: [Close_valve]
+          low: [Open_valve]
+        state:
+          empty_close:
+            joint_states: [0.16]
+            high: [Closed_signal]
+            low: [Part_Grasped_signal]
+            set_after_command_high: []
+            set_after_command_low: [Release_Break_valve]
+          full_close:
+            joint_states: [0.08]
+            high: [Part_Grasped_signal]
+            low: [Closed_signal]
+            set_after_command_high: []
+            set_after_command_low: [Release_Break_valve]
+
+      configurations: ["narrow_objects", "wide_objects"]
+      configuration_joints: [gripper_gripper_distance_joint]
+
+      configuration_setup:
+        narrow_objects:
+          joint_states: [0.1]
+          command_high: [Narrow_Configuration_Cmd]
+          command_low: [Wide_Configuration_Cmd]
+          state_high: [Narrow_Configuraiton_Signal]
+          state_low: [Wide_Configuration_Signal]
+        wide_objects:
+          joint_states: [0.2]
+          command_high: [Wide_Configuration_Cmd]
+          command_low: [Narrow_Configuration_Cmd]
+          state_high: [Wide_Configuration_Signal]
+          state_low: [Narrow_Configuraiton_Signal]
+
+      gripper_specific_sensors: ["part_sensor_top", "part_sensor"]
+      sensors_interfaces:
+        part_sensor_top:
+          input: "Part_Sensor_Top_signal"
+        part_sensor:
+          input: "Part_Grasped_signal"