Version 1 shipping review

xarm_controller/config/xarm6_controllers.yaml

@@ -4,8 +4,18 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
+    position_controller:
+      type: position_controllers/JointGroupPositionController
+
+    velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    effort_controller:
+      type: effort_controllers/JointGroupEffortController
+
     traj_controller:
       type: joint_trajectory_controller/JointTrajectoryController
+
     # Original comment: only for fake control (?)
     gripper_traj_controller:
       type: joint_trajectory_controller/JointTrajectoryController
@@ -14,6 +24,42 @@ controller_manager:
     bio_gripper_traj_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
+position_controller:
+  ros__parameters:
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+
+velocity_controller:
+  ros__parameters:
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+    interface_name: velocity
+    command_interfaces:
+      - velocity
+    state_interfaces:
+      - position
+      - velocity
+
+effort_controller:
+  ros__parameters:
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+
 traj_controller:
   ros__parameters:
     command_interfaces:

xarm_controller/launch/_ros2_control.launch.py

@@ -1,10 +1,5 @@
 #!/usr/bin/env python3
 # Software License Agreement (BSD License)
-#
-# Copyright (c) 2021, UFACTORY, Inc.
-# All rights reserved.
-#
-# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
 
 import os
 from ament_index_python import get_package_share_directory
@@ -19,21 +14,21 @@
 def launch_setup(context, *args, **kwargs):
     prefix = LaunchConfiguration('prefix', default='')
     hw_ns = LaunchConfiguration('hw_ns', default='xarm')
+    kinematics_suffix = LaunchConfiguration('hw_ns', default='')
 
     ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotSystemHardware')
     xacro_file = LaunchConfiguration('xacro_file', default=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']))
 
+    xarm_type = 'xarm6' #TODO - fix
+
     # ros2 control params
     # xarm_controller/launch/lib/robot_controller_lib.py
     mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_controller'), 'launch', 'lib', 'robot_controller_lib.py'))
     generate_ros2_control_params_temp_file = getattr(mod, 'generate_ros2_control_params_temp_file')
     ros2_control_params = generate_ros2_control_params_temp_file(
-        os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}{}_controllers.yaml'.format(robot_type.perform(context), dof.perform(context) if robot_type.perform(context) in ('xarm', 'lite') else '')),
+        os.path.join(get_package_share_directory('xarm_controller'), 'config', f'{xarm_type}_controllers.yaml'),
         prefix=prefix.perform(context), 
-        add_gripper=False,
-        add_bio_gripper=False,
         ros_namespace=LaunchConfiguration('ros_namespace', default='').perform(context),
-        robot_type='xarm'
     )
 
     # robot_description
@@ -46,10 +41,27 @@ def launch_setup(context, *args, **kwargs):
                 'hw_ns': hw_ns.perform(context).strip('/'),
                 'ros2_control_plugin': ros2_control_plugin,
                 'ros2_control_params': ros2_control_params,
+                'kinematics_suffix': kinematics_suffix, 
             }
         )
     }
 
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[robot_description],
+        # BL: don't remap /tf to custom topics. 
+        # I.e., multiple robots publish to same /tf topic 
+        # Instead, use prefix to distinguish different robots
+        # remappings=[
+        #     ('/tf', 'tf'),
+        #     ('/tf_static', 'tf_static'),
+        # ]
+        # namespace=this_robot_namespace
+    )
+
+
     mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_api'), 'launch', 'lib', 'robot_api_lib.py'))
     generate_robot_api_params = getattr(mod, 'generate_robot_api_params')
     robot_params = generate_robot_api_params(
@@ -63,19 +75,23 @@ def launch_setup(context, *args, **kwargs):
         package='controller_manager',
         executable='ros2_control_node',
         parameters=[
-            robot_description,
             ros2_control_params,
             robot_params,
         ],
+        remappings=[
+            ('~/robot_description', '/robot_description'),
+        ],
         output='screen',
     )
 
     return [
-        ros2_control_node
+        ros2_control_node,
+        robot_state_publisher_node
     ]
 
-
 def generate_launch_description():
     return LaunchDescription([
         OpaqueFunction(function=launch_setup)
     ])
+
+

xarm_controller/launch/lib/robot_controller_lib.py

@@ -1,17 +1,11 @@
 #!/usr/bin/env python3
 # Software License Agreement (BSD License)
-#
-# Copyright (c) 2021, UFACTORY, Inc.
-# All rights reserved.
-#
-# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
 
 import os
 import yaml
 from tempfile import NamedTemporaryFile
 from ament_index_python import get_package_share_directory
 
-
 def add_prefix_to_ros2_control_params(prefix, ros2_control_params):
     if not prefix:
         return
@@ -33,9 +27,8 @@ def add_prefix_to_ros2_control_params(prefix, ros2_control_params):
         if name in controller_manager_ros__parameters:
             controller_manager_ros__parameters[new_name] = controller_manager_ros__parameters.pop(name)
 
-
 def generate_ros2_control_params_temp_file(ros2_control_params_path, prefix='', add_gripper=False, add_bio_gripper=False, ros_namespace='', update_rate=None, robot_type='xarm'):
-    if ros_namespace or prefix or add_gripper or add_bio_gripper or update_rate:
+    if ros_namespace or prefix or update_rate:
         with open(ros2_control_params_path, 'r') as f:
             ros2_control_params_yaml = yaml.safe_load(f)
         if update_rate is not None:
@@ -63,7 +56,7 @@ def generate_ros2_control_params_temp_file(ros2_control_params_path, prefix='',
 
         add_prefix_to_ros2_control_params(prefix, ros2_control_params_yaml)
 
-        if not ros_namespace.startswith('/'):
+        if ros_namespace and not ros_namespace.startswith('/'):
             ros_namespace = f'/{ros_namespace}'
 
         # add namespace, possibly just slash to all nodes

xarm_description/config/default_urdf_arguments/end_effector.yaml

@@ -1,5 +1,5 @@
 # end-effector
-gripper: false
+gripper: true
 vacuum_gripper: false
 bio_gripper: false
 realsense_d435i: false

xarm_description/config/default_urdf_arguments/xarm6.yaml

@@ -2,7 +2,7 @@
 # hw_ns: 'xarm' # changes at runtime
 # ros2_control_plugin: 'uf_robot_hardware/UFRobotSystemHardware' # changes at runtime
 # ros2_control_params: '' # changes at runtime
-robot_ip: ''
+robot_ip: '192.168.1.242'
 dof: 6
 robot_type: 'xarm'
 report_type: 'normal'
@@ -13,9 +13,9 @@ robot_sn: ''
 # control-related
 limited: false
 effort_control: false 
-velocity_control: false
+velocity_control: true
 # urdf-related
-attach_to: 'world'
+attach_to: 'xarm_device' #TODO: this currently has to match the robot name in the URDF. Fix in gazebo to change frame id.
 attach_xyz: '0 0 0'
 attach_rpy: '0 0 0'
 create_attach_link: true

xarm_description/config/kinematics/default/xarm6_default_kinematics.yaml

@@ -11,15 +11,15 @@ kinematics:
     y: 0
     z: 0
     roll: -1.5708
-    pitch: 0
+    pitch: 0.1
     yaw: 0
   joint3:
     x: 0.0535
     y: -0.2845
     z: 0
     roll: 0
     pitch: 0
-    yaw: 0
+    yaw: -0.1
   joint4:
     x: 0.0775
     y: 0.3425

xarm_description/config/kinematics/default/xarm6_kinematics_sim.yaml

@@ -0,0 +1,43 @@
+kinematics:
+  joint1:
+    x: 0
+    y: 0
+    z: 0.267
+    roll: 0
+    pitch: 0
+    yaw: 0
+  joint2:
+    x: 0
+    y: 0
+    z: 0
+    roll: -1.5708
+    pitch: -0.5
+    yaw: 0
+  joint3:
+    x: 0.0535
+    y: -0.2845
+    z: 0
+    roll: 0
+    pitch: 0
+    yaw: -0.5
+  joint4:
+    x: 0.0775
+    y: 0.3425
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
+  joint5:
+    x: 0
+    y: 0
+    z: 0
+    roll: 1.5708
+    pitch: -0.95
+    yaw: 0
+  joint6:
+    x: 0.076
+    y: 0.097
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0

xarm_description/config/kinematics/user/xarm6_kinematics_sim.yaml

@@ -0,0 +1,43 @@
+kinematics:
+  joint1:
+    x: 0
+    y: 0
+    z: 0.267
+    roll: 0
+    pitch: 0
+    yaw: 0
+  joint2:
+    x: 0
+    y: 0
+    z: 0
+    roll: -1.5708
+    pitch: -0.5
+    yaw: 0
+  joint3:
+    x: 0.0535
+    y: -0.2845
+    z: 0
+    roll: 0
+    pitch: 0
+    yaw: -0.5
+  joint4:
+    x: 0.0775
+    y: 0.3425
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
+  joint5:
+    x: 0
+    y: 0
+    z: 0
+    roll: 1.5708
+    pitch: -0.95
+    yaw: 0
+  joint6:
+    x: 0.076
+    y: 0.097
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0

xarm_description/launch/_robot_description.launch.py

@@ -1,10 +1,5 @@
 #!/usr/bin/env python3
 # Software License Agreement (BSD License)
-#
-# Copyright (c) 2021, UFACTORY, Inc.
-# All rights reserved.
-#
-# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
 
 import os
 import sys

xarm_description/launch/_robot_joint_state.launch.py

@@ -1,26 +1,19 @@
 #!/usr/bin/env python3
 # Software License Agreement (BSD License)
-#
-# Copyright (c) 2021, UFACTORY, Inc.
-# All rights reserved.
-#
-# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
 
 from launch import LaunchDescription
-from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
+from launch.actions import OpaqueFunction, IncludeLaunchDescription
+from launch_ros.actions import Node
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
-
-def generate_launch_description(context, *args, **kwargs):
+def launch_setup(context, *args, **kwargs):
     prefix = LaunchConfiguration('prefix', default='')
     hw_ns = LaunchConfiguration('hw_ns', default='xarm')
     ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotSystemHardware')
 
     # robot description launch
-    # xarm_description/launch/_robot_description.launch.py
     robot_description_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
         launch_arguments={
@@ -39,7 +32,15 @@ def generate_launch_description(context, *args, **kwargs):
         parameters=[{'source_list': ['{}{}/joint_states'.format(prefix.perform(context), hw_ns.perform(context))]}],
     )
 
-    return LaunchDescription([
+    return [
         robot_description_launch,
-        joint_state_publisher_node,
-    ])
+        joint_state_publisher_node
+    ]
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=launch_setup)
+    ])
+
+
+