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,100 @@
+: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 gripper using IOs. It provides functionalities like open, close and reconfigure which can be used either though action server or service server and also publishes ``joint_states`` of gripper and also ``dynamic_interfaces`` for all command and state interfaces.
+
+Description of controller's interfaces
+---------------------------------------
+
+- ``joint_states`` [``sensor_msgs::msg::JointState``]: Publishes the state of gripper joint and configuration joint
+- ``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: [EL2008/Bremse_WQG7]
+          low: []
+        command:
+          high: [EL2008/Greiferteil_Oeffnen_WQG1]
+          low: [EL2008/Greiferteil_Schliessen_WQG2]
+        state:
+          high: [EL1008/Greifer_Geoeffnet_BG01]
+          low: [EL1008/Greifer_Geschloschen_BG02]
+        set_after_command:
+          high: []
+          low: [EL2008/Bremse_WQG7]
+
+      possible_closed_states: ['empty_close', 'full_close']
+
+      close:
+        set_before_command:
+          high: [EL2008/Bremse_WQG7]
+          low: [EL2008/Greiferteil_Oeffnen_WQG1]
+        command:
+          high: [EL2008/Greiferteil_Schliessen_WQG2]
+          low: [EL2008/Greiferteil_Oeffnen_WQG1]
+        state:
+          empty_close:
+            joint_states: [0.08]
+            high: [EL1008/Greifer_Geschloschen_BG02]
+            low: [EL1008/Bauteilabfrage_BG06]
+            set_after_command_high: [EL2008/Bremse_WQG7]
+            set_after_command_low: [EL2008/Bremse_WQG7]
+          full_close:
+            joint_states: [0.08]
+            high: [EL1008/Bauteilabfrage_BG06]
+            low: [EL1008/Greifer_Geschloschen_BG02]
+            set_after_command_high: [EL2008/Bremse_WQG7]
+            set_after_command_low: [EL2008/Bremse_WQG7]
+
+      configurations: ["stichmass_125", "stichmass_250"]
+      configuration_joints: [gripper_gripper_distance_joint]
+
+      configuration_setup:
+        stichmass_125:
+          joint_states: [0.125]
+          command_high: [EL2008/Stichmass_125_WQG5]
+          command_low: [EL2008/Stichmass_250_WQG6]
+          state_high: [EL1008/Stichmass_125mm_BG03]
+          state_low: [EL1008/Stichmass_250mm_BG04]
+        stichmass_250:
+          joint_states: [0.250]
+          command_high: [EL2008/Stichmass_250_WQG6]
+          command_low: [EL2008/Stichmass_125_WQG5]
+          state_high: [EL1008/Stichmass_250mm_BG04]
+          state_low: [EL1008/Stichmass_125mm_BG03]
+
+      gripper_specific_sensors: ["hohenabfrage", "bauteilabfrage"]
+      sensors_interfaces:
+        hohenabfrage:
+          input: "EL1008/Hohenabfrage_BG5"
+        bauteilabfrage:
+          input: "EL1008/Bauteilabfrage_BG06"