dignakov · destogl · Jan 13, 2021
diff --git a/README.md b/README.md
@@ -1,3 +1,81 @@
+# Starting ROS2 Demo
+
+## Install foxy version of ros2_control or check its sourc
+
+## Checkout repositories
+**Note**: you can use relese of the ros2_control repository
+
+  ```
+  git clone https://github.com/ros-controls/ros2_control.git
+  ```
+
+Checkout changed version of `JointTrajectoryController`
+  ```
+  git clone https://github.com/ros-controls/ros2_controllers.git
+  cd ros2_controllers
+  git remote add denis https://github.com/destogl/ros2_controllers.git
+  git fetch denis
+  git checkout denis/jtc_versions
+  ```
+
+Checkout changed version of ros2_demos repository
+  ```
+  git clone -b add_demo_nodes https://github.com/ros-controls/ros2_control_demos.git
+  ```
+
+Checkout KUKA demo driver and changed `kuka_experimental` repositories
+  ```
+  git clone https://github.com/dignakov/kuka_experimental.git
+  git clone https://github.com/dignakov/ros2_control_demo_drivers.git
+  ```
+**Attention**: remove `ros2_control_abb_demo_driver` package for `ros2_control_demo_drivers` if needed.
+
+Compile everything using:
+  ```
+  colcon build --symlink-install
+  ```
+
+## Starting Robot
+1. In a terminal start ros2_control framework with KUKA RSI system hardware:
+  ```
+  ros2 launch kuka_kr5_support kr5_rsi.launch.py
+  ```
+
+2. In the second terminal start KUKA RSI simulator:
+  ```
+  ros2 run kuka_rsi_simulator kuka_rsi_simulator
+  ```
+
+3. In the third terminal start `rviz2`.
+   And add RobotModel from the `/robot_description` topic.
+   You can also just open rviz config from `kuka_kr5_support`/config/test_controllers.rviz
+
+4. Follow the test case for one of the controllers.
+
+
+### Test with ForwardCommandController (it will not work on the hardware if positions are not interpolated)
+1. Load and start controllers:
+  ```
+  ros2 control load_start_controller joint_state_controller
+  ros2 control load_start_controller forward_position_controller
+  ```
+2. Start a test node which sends commmands in a loop to the controller:
+  ```
+  ros2 launch kuka_kr5_support test_forward_position_controller.launch.py
+  ```
+
+### Test with JointTrajectoryController
+1. Load and start controllers:
+  ```
+  ros2 control load_start_controller joint_state_controller
+  ros2 control load_start_controller position_trajectory_controller
+  ```
+
+2. Start a test node which sends commmands in a loop to the controller:
+  ```
+  ros2 launch kuka_kr5_support test_position_trajectory_controller.launch.py
+  ```
+
 # Kuka experimental
 
 [![Build Status](http://build.ros.org/job/Kdev__kuka_experimental__ubuntu_xenial_amd64/badge/icon)](http://build.ros.org/job/Kdev__kuka_experimental__ubuntu_xenial_amd64)

diff --git a/kuka_experimental/package.xml b/kuka_experimental/package.xml
@@ -13,6 +13,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <exec_depend>kuka_kr5_support</exec_depend>
   <exec_depend>kuka_kr6_support</exec_depend>
   <exec_depend>kuka_resources</exec_depend>
   <!--<exec_depend>kuka_rsi_hw_interface</exec_depend>-->

diff --git a/kuka_kr5_support/CMakeLists.txt b/kuka_kr5_support/CMakeLists.txt
@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(kuka_kr5_support)
+
+find_package(ament_cmake REQUIRED)
+
+if (CATKIN_ENABLE_TESTING)
+endif()
+
+install(DIRECTORY config launch meshes urdf
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/kuka_kr5_support/config/hardware_controllers.yaml b/kuka_kr5_support/config/hardware_controllers.yaml
@@ -0,0 +1,41 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 250  # Hz
+
+    joint_state_controller:
+      type: joint_state_controller/JointStateController
+
+    forward_position_controller:
+      type: forward_command_controller/ForwardCommandController
+
+    position_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+forward_position_controller:
+  ros__parameters:
+    joints:
+      - joint_a1
+      - joint_a2
+      - joint_a3
+      - joint_a4
+      - joint_a5
+      - joint_a6
+    interface_name: position
+
+position_trajectory_controller:
+  ros__parameters:
+    joints:
+      - joint_a1
+      - joint_a2
+      - joint_a3
+      - joint_a4
+      - joint_a5
+      - joint_a6
+
+    state_publish_rate: 50.0 # Defaults to 50
+    action_monitor_rate: 20.0 # Defaults to 20
+
+    allow_partial_joints_goal: false # Defaults to false
+    constraints:
+      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
+      goal_time: 0.0 # Defaults to 0.0 (start immediatelly)
diff --git a/kuka_kr5_support/config/joint_names_kr5.yaml b/kuka_kr5_support/config/joint_names_kr5.yaml
@@ -0,0 +1 @@
+controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6']
diff --git a/kuka_kr5_support/config/test_controllers.rviz b/kuka_kr5_support/config/test_controllers.rviz
@@ -0,0 +1,190 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 87
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 780
+  - 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
+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
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base
+    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
+      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: 2.50931453704834
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.41824325919151306
+        Y: -0.2059020698070526
+        Z: 0.9586235284805298
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5403985381126404
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.988577365875244
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1051
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000003b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000003b0000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000003b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000044000003b0000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000504000003b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 5120
+  Y: 180
diff --git a/kuka_kr5_support/config/test_forward_position_publisher.yaml b/kuka_kr5_support/config/test_forward_position_publisher.yaml
@@ -0,0 +1,10 @@
+publisher_forward_position_controller:
+  ros__parameters:
+
+    wait_sec_between_publish: 1
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1: [0.0, -1.571, 0.758, -0.758, 0.758, 0.0]
+    pos2: [0.0, -1.571, 1.571, 0.0, 1.571, 0.0]
+    pos3: [0.0, -1.571, 2.329, 0.758, 0.758, 0.0]
+    pos4: [0.0, -1.571, 1.571, 0.0, 1.571, 0.0]
diff --git a/kuka_kr5_support/config/test_position_trajectory_publisher.yaml b/kuka_kr5_support/config/test_position_trajectory_publisher.yaml
@@ -0,0 +1,18 @@
+publisher_joint_trajectory_controller:
+  ros__parameters:
+
+    controller_name: position_trajectory_controller
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1: [0.0, -1.571, 0.758, -0.758, 0.758, 0.0]
+    pos2: [0.0, -1.571, 1.571, 0.0, 1.571, 0.0]
+    pos3: [0.0, -1.571, 2.329, 0.758, 0.758, 0.0]
+    pos4: [0.0, -1.571, 1.571, 0.0, 1.571, 0.0]
+
+    joints:
+      - joint_a1
+      - joint_a2
+      - joint_a3
+      - joint_a4
+      - joint_a5
+      - joint_a6
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1 @@
		controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6']