Initial ros2_control architecture

README.md

@@ -1,6 +1,6 @@
 # robotiq_hande_driver
 
-Package for controlling the [Robotiq Hand-E gripper](https://robotiq.com/products/adaptive-grippers#Hand-E) from the [ROS 2 Control](https://control.ros.org/humble/doc/getting_started/getting_started.html) framework.
+Package for controlling the [Robotiq Hand-E gripper](https://robotiq.com/products/adaptive-grippers#Hand-E) from the [ROS 2 Control](https://control.ros.org/humble/doc/getting_started/getting_started.html) framework. It uses the [robotiq_hande_description](https://github.com/macmacal/robotiq_hande_description) package for the URDF definitions.
 
 Work is still in progress.
 
@@ -14,13 +14,14 @@ Work is still in progress.
 ```bash
 cd ~/ceai_ws/src
 git clone git@github.com:AGH-CEAI/robotiq_hande_driver.git ./src
+vcs import src < src/robotiq_hande_driver/robotiq_hande_driver.repos &&
 colcon build --symlink-install --packages-select robotiq_hande_driver
 source ./install/local_setup.sh
 ```
 
-### Run
+### Launch preview
 ```bash
-ros2 run robotiq_hande_driver hande_driver
+ros2 launch robotiq_hande_driver gripper_controller_preview.launch.py use_fake_hardware:=true
 ```
 
 

robotiq_hande_driver/CMakeLists.txt

@@ -1,19 +1,75 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.16)
 project(robotiq_hande_driver)
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+set(HW_IF_INCLUDE_DEPENDS
+  fmt
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  rcpputils
+)
+
 find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${HW_IF_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+add_library(${PROJECT_NAME}
+    SHARED
+    hardware/src/hande_hardware_interface.cpp
+)
+
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+target_include_directories(robotiq_hande_driver PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/hardware/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(
+  ${PROJECT_NAME} PUBLIC
+  ${HW_IF_INCLUDE_DEPENDS}
+)
+
+pluginlib_export_plugin_description_file(hardware_interface ros2_control_plugin.xml)
+
+install(
+  DIRECTORY hardware/include/
+  DESTINATION include/
+)
+install(
+  DIRECTORY bringup
+  DESTINATION share/${PROJECT_NAME}
+)
 
-add_executable(hande_driver src/hande_driver.cpp)
-target_include_directories(hande_driver PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-target_compile_features(hande_driver PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
 
-install(TARGETS hande_driver
-  DESTINATION lib/${PROJECT_NAME})
+install(TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  # find_package(ament_cmake_pytest REQUIRED)
+  find_package(hardware_interface REQUIRED)
+
+  ament_add_gmock(test_load_hw_interface
+    test/test_load_hw_interface.cpp
+  )
+  target_link_libraries(test_load_hw_interface ${PROJECT_NAME})
+
+  # TODO: investigate
+  # ament_add_pytest_test(example_7_urdf_xacro test/test_urdf_xacro.py)
+  # ament_add_pytest_test(view_example_7_launch test/test_view_robot_launch.py)
+  # ament_add_pytest_test(run_example_7_launch test/test_r6bot_controller_launch.py)
+endif()
 
+## EXPORTS
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${HW_IF_INCLUDE_DEPENDS})
 ament_package()

robotiq_hande_driver/bringup/config/hande_controller.yaml

@@ -0,0 +1,22 @@
+/**:
+  controller_manager:
+    ros__parameters:
+      update_rate: 10 # Hz
+
+      joint_state_broadcaster:
+        type: joint_state_broadcaster/JointStateBroadcaster
+
+      gripper_action_controller:
+        type: position_controllers/GripperActionController
+
+  gripper_action_controller:
+    ros__parameters:
+      action_monitor_rate: 20.0 # Hz
+      joint: $(var tf_prefix)robotiq_hande_left_finger_joint
+      max_effort: 10.0 # N
+      goal_tolerance: 0.01 # m
+      allow_stalling: false
+      stall_velocity_threshold: 0.001 #m/s
+      stall_timeout: 1.0 # s
+      state_interfaces:
+        - position

robotiq_hande_driver/bringup/config/rviz_preview.rviz

@@ -0,0 +1,188 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 2233
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        robotiq_hande_coupler:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_hande_end:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        robotiq_hande_left_finger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_hande_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_hande_right_finger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: tool0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 0.5352108478546143
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.46039843559265137
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.508580684661865
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 2531
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001ac00000944fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000944000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000944fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e00000944000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a00000003efc0100000002fb0000000800540069006d00650100000000000005a00000027900fffffffb0000000800540069006d00650100000000000004500000000000000000000003ee0000094400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1440
+  X: 0
+  Y: 0