Add script

Dockerfile

@@ -12,6 +12,8 @@ ENV WS_INSTALL_DIR=${WS_DIR}/install
 ENV WS_LOG_DIR=${WS_DIR}/log
 WORKDIR ${WS_DIR}
 
+COPY config.yaml ${WS_DIR}/
+
 ### Install Gazebo
 ARG IGNITION_VERSION=fortress
 ENV IGNITION_VERSION=${IGNITION_VERSION}
@@ -25,6 +27,9 @@ RUN apt-get update && \
     apt-get install -yq python3-pil.imagetk && \
     apt-get install -yq ros-${ROS_DISTRO}-pilz-industrial-motion-planner && \
     apt-get install -yq tmux && \
+    apt-get install -yq nano && \
+    apt-get install -yq vim && \
+    apt-get install -yq less && \
     apt-get install -yq python3-pip && \
     apt-get install -yq ros-${ROS_DISTRO}-desktop && \
     apt-get install -yq ros-${ROS_DISTRO}-rclcpp-components
@@ -43,32 +48,39 @@ RUN apt-get update && \
 # Build interfaces and generic controller first
 COPY ./src/robot_interfaces ${WS_SRC_DIR}/robot_interfaces
 COPY ./src/robot_controller ${WS_SRC_DIR}/robot_controller
+RUN apt-get update
 RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
     colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/robot_interfaces ${WS_SRC_DIR}/robot_controller && \
     rm -rf ${WS_LOG_DIR}
 
 # Build packages
-COPY ./src/draw_svg ${WS_SRC_DIR}/draw_svg
 COPY ./src/drawing_controller ${WS_SRC_DIR}/drawing_controller
 COPY ./src/axidraw_controller ${WS_SRC_DIR}/axidraw_controller
+COPY ./src/virtual_drawing_surface ${WS_SRC_DIR}/virtual_drawing_surface
 RUN pip install -r ${WS_SRC_DIR}/drawing_controller/requirements.txt
 RUN pip install -r ${WS_SRC_DIR}/axidraw_controller/requirements.txt
+RUN pip install -r ${WS_SRC_DIR}/virtual_drawing_surface/requirements.txt
+RUN apt-get update
 RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
     source "${WS_INSTALL_DIR}/local_setup.bash" && \
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/draw_svg ${WS_SRC_DIR}/drawing_controller ${WS_SRC_DIR}/axidraw_controller && \
+    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/drawing_controller ${WS_SRC_DIR}/axidraw_controller ${WS_SRC_DIR}/virtual_drawing_surface && \
     rm -rf ${WS_LOG_DIR}
 
 # Build lite6 and xarm packages
 COPY ./src/lite6_controller ${WS_SRC_DIR}/lite6_controller
+COPY ./src/custom_xarm_description ${WS_SRC_DIR}/custom_xarm_description
+COPY ./src/custom_xarm_moveit_config ${WS_SRC_DIR}/custom_xarm_moveit_config
+COPY ./src/custom_xarm_gazebo ${WS_SRC_DIR}/custom_xarm_gazebo
+RUN apt-get update
 RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
     vcs import --recursive --shallow ${WS_SRC_DIR} < ${WS_SRC_DIR}/lite6_controller/lite6_controller.repos && \
     mv ${WS_SRC_DIR}/xarm_ros2/xarm* ${WS_SRC_DIR} && \
     rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR}/xarm_* && \
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/xarm_* ${WS_SRC_DIR}/lite6_controller && \
+    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/xarm_* ${WS_SRC_DIR}/lite6_controller ${WS_SRC_DIR}/custom_xarm_description ${WS_SRC_DIR}/custom_xarm_moveit_config ${WS_SRC_DIR}/custom_xarm_gazebo && \
     rm -rf ${WS_LOG_DIR}
 
 # Copy example svg images
-COPY ./svg svg
+COPY ./svg test-images
 
 ### Add workspace to the ROS entrypoint
 ### Source ROS workspace inside `~/.bashrc` to enable autocompletion

README.md

@@ -14,6 +14,16 @@ The simplest way to run the project currently is by building and running the doc
 bash .docker/build.bash
 ```
 
+If build fails, consider clearing build cache.
+Do not run this if you have other docker containers that you care about on your computer.
+``` sh
+podman builder prune --all --force
+```
+or 
+``` sh
+docker builder prune --all --force
+```
+
 ### Run built container
 ``` sh
 bash .docker/run.bash
@@ -25,6 +35,23 @@ bash .docker/devel.bash
 ```
 This will mount the host `drawing-robot-ros2` directory in the container at `src/drawing-robot-ros2`.
 
+Optionally you can pass a directory to the container with
+``` sh
+bash .docker/run.bash -v PATH_TO_SVG:/svg:rw
+```
+This will mount the given path to /svg in read-write mode in the container.
+
+
+#### Podman issues
+If using podman instead of docker, using the following will allow the container to access `/dev/` which is needed by the axidraw robot. 
+``` sh
+sudo bash .docker/build.bash
+```
+``` sh
+sudo bash .docker/devel.bash
+```
+Adding sudo may cause gazebo not to work, so it is recommended to use docker instead of podman.
+
 ## TODO Building locally
 
 Requirements:
@@ -50,26 +77,42 @@ DummyController echoes Motion messages to the terminal.
 ros2 run robot_controller dummy_controller
 ```
 
-AxidrawController draws on the axidraw robot
+### AxidrawController
+AxidrawController draws on the axidraw robot.
+Find the serial device in "/dev/", it is usually named "/dev/ttyACMX" where X is usually 0.
+Try a different serial port if the axidraw_controller continuously logs a message about failing to connect.
 ``` sh
-ros2 launch axidraw_controller axidraw_controller
+ros2 launch axidraw_controller axidraw_controller.launch.py serial_port:=/dev/ttyACM0 config:=./config.yaml
 ```
 
+### Lite6Controller
 This starts the simulated lite6
 ``` sh
-ros2 launch lite6_controller lite6_gazebo.launch.py
+ros2 launch lite6_controller lite6_gazebo.launch.py config:=./config.yaml
 ```
 
 This runs the real lite6
 ``` sh
-ros2 launch lite6_controller lite6_real.launch.py
+ros2 launch lite6_controller lite6_real.launch.py config:=./config.yaml
 ```
 
 This runs the real lite6 without Rviz (can be run on headless device over ssh)
 ``` sh
-ros2 launch lite6_controller lite6_real_no_gui.launch.py
+ros2 launch lite6_controller lite6_real_no_gui.launch.py config:=./config.yaml
 ```
 
+Before using the real lite6, it is recommended to run the calibration program.
+A lite6_controller must be running for calibration.
+This can be used to measure the Z height for a specific pen.
+The program also moves the arm to a known default position.
+``` sh
+ros2 run lite6_controller lite6_calibration
+```
+Follow the instructions, pressing enter when prompted.
+
+Change the Z-offset value accordingly.
+Restart the running lite6_controller after calibration.
+
 ### DrawingController
 Once a RobotController is running, simultaneously (using tmux or another terminal) run
 ``` sh
@@ -77,6 +120,23 @@ ros2 run drawing_controller drawing_controller svg/test.svg
 ```
 This will draw the svg image given as the last argument.
 
+### tmux workflow
+lite6 interface: http://192.168.1.150:18333
+
+#### Raspberry pi
+On the raspberry pi run 
+``` sh
+./setup_ros.sh
+```
+This will open a tmux session with the necessary ros2 packages sourced.
+#### Docker container
+``` sh
+tmux
+```
+
+If actively
+
+
 ## SVG compatibility info
 Tested with SVG from the following programs
 - Inkscape
@@ -88,6 +148,9 @@ Delimiter characters seem to vary somewhat.
 The following examples work:
 TODO ADD EXAMPLES OF SVG PATHS
 
+Make sure that all shapes in the SVG are within the bounds defined by height and width (or viewbox).
+Shapes outside of bounds will cause the robot to frequently visit the top left corner and edges of the paper and not draw the desired image.
+
 The following SVG primitives are supported:
 | Primitive                           | Support  |
 |-------------------------------------|----------|
@@ -164,7 +227,7 @@ And the following SVG path commands are supported:
 | MoveTo                 |  M, m             |             |
 | LineTo                 |  L, l, H, h, V, v |             |
 | Cubic Bézier Curve     |  C, c, S, s       |             |
-| Quadratic Bézier Curve |                   | Q, q, T, t  |
+| Quadratic Bézier Curve |  Q, q             | T, t        |
 | Elliptical Arc Curve   |                   | A, a        |
 | ClosePath              |  Z, z             |             |
 

config.yaml

@@ -0,0 +1,26 @@
+/robot_controller:
+  ros__parameters:
+    lite6_x_limit_lower: 0.1
+    lite6_x_limit_upper: 0.305
+    lite6_y_limit_lower: -0.1475
+    lite6_y_limit_upper: 0.1475
+
+    lite6_z_lift_amount: 0.01
+    lite6_z_offset: 0.201942
+
+    lite6_blend_radius: 0.0
+    lite6_max_velocity_scaling_factor: 1.0
+    lite6_max_acceleration_scaling_factor: 0.9
+
+/axidraw_serial:
+  ros__parameters:
+    axidraw_accel: 100
+    axidraw_speed_pendown: 50
+    axidraw_speed_penup: 50
+    axidraw_const_speed: false
+    axidraw_pen_delay_down: 0
+    axidraw_pen_delay_up: 0
+    axidraw_pen_pos_down: 40
+    axidraw_pen_pos_up: 60
+    axidraw_pen_rate_lower: 50
+    axidraw_pen_rate_raise: 75

scripts/log_drawing.sh

@@ -0,0 +1,12 @@
+#!/usr/bin/env sh
+# Logs drawn images
+# ./log_drawing.sh ROBOTNAME SVG
+# ROBOTNAME: robot name (logged before output)
+# SVG: svg path to be drawn
+
+date >> data.txt
+echo $1 >> data.txt
+ros2 run drawing_controller drawing_controller $2 2>&1 | grep 'Results' | tee -a data.txt
+echo '\n' >> data.txt
+
+#{'svg processing time': '00:00:02', 'failure': 65, 'total time': '00:00:09', 'drawing time': '00:00:06'}

scripts/plot_lite6_csv/plot_lite6_csv.py

@@ -1,4 +1,8 @@
 # 3D Heatmap in Python using matplotlib
+# Script expects a csv consisting of lines in the format:
+#   success, 10.0 14.2 14.4, 1.0 2.0 3.0, ...
+#   failure, 10.0 14.2 14.4, 1.0 2.0 3.0, ...
+# Usage: python plot_lite6_csv.py OUTPUT.csv
 
 # to make plot interactive 
 #%matplotlib

src/axidraw_controller/CMakeLists.txt

@@ -40,6 +40,11 @@ install(PROGRAMS
     DESTINATION lib/${PROJECT_NAME}
 )
 
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

src/axidraw_controller/config/config.yaml

@@ -0,0 +1,12 @@
+/axidraw_serial:
+  ros__parameters:
+    axidraw_accel: 100
+    axidraw_const_speed: false
+    axidraw_pen_delay_down: 0
+    axidraw_pen_delay_up: 0
+    axidraw_pen_pos_down: 40
+    axidraw_pen_pos_up: 60
+    axidraw_pen_rate_lower: 50
+    axidraw_pen_rate_raise: 75
+    axidraw_speed_pendown: 50
+    axidraw_speed_penup: 50

src/axidraw_controller/launch/axidraw_controller.launch.py

@@ -1,4 +1,5 @@
 import os
+import yaml
 from launch import LaunchDescription
 from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -18,6 +19,13 @@ def launch_setup(context, *args, **kwargs):
     serial_port = LaunchConfiguration('serial_port', default='/dev/ttyACM0')
     log_level = LaunchConfiguration("log_level", default='info')
 
+    config = LaunchConfiguration("config", default=PathJoinSubstitution([FindPackageShare('axidraw_controller'), 'config', 'config.yaml']))
+
+    robotcontroller_config = config.perform(context)
+    with open(robotcontroller_config, 'r') as f:
+        axidraw_config = yaml.safe_load(f)['/axidraw_serial']['ros__parameters']
+        print(f'Loaded configuration: {axidraw_config}')
+
     nodes = [
         Node(
             package="axidraw_controller",
@@ -34,6 +42,7 @@ def launch_setup(context, *args, **kwargs):
             arguments=["--ros-args", "--log-level", log_level],
             parameters=[
                 {"serial_port": serial_port},
+                axidraw_config,
             ],
         ),
     ]

src/axidraw_controller/src/cpp/axidraw_controller.cpp

@@ -162,7 +162,7 @@ class AxidrawController : public RobotController
         this->penup_pub->publish(std_msgs::msg::Empty());
       else
       {
-        this->pendown_pub->publish(std_msgs::msg::Empty());
+        //this->pendown_pub->publish(std_msgs::msg::Empty());
         while (i + count + 1 < points.size() && points[i + count + 1].z <= 0)
         {
           count++;

src/axidraw_controller/src/py/axidraw_serial.py

@@ -42,6 +42,7 @@ class AxidrawSerial(Node):
     status = {
         "serial": "not ready",
         "motion": "waiting",
+        "pen": "up",
     }
 
     def init_serial(self, port):
@@ -60,9 +61,22 @@ class AxidrawSerial(Node):
             return False
         self.ad.options.units = 2            # set working units to mm.
         self.ad.options.model = 2            # set model to AxiDraw V3/A3
+
+        self.ad.options.speed_pendown = self.get_parameter('axidraw_speed_pendown').get_parameter_value().integer_value # Maximum XY speed when the pen is down (plotting).
+        self.ad.options.speed_penup = self.get_parameter('axidraw_speed_penup').get_parameter_value().integer_value # Maximum XY speed when the pen is up.
+        self.ad.options.accel = self.get_parameter('axidraw_accel').get_parameter_value().integer_value  # Relative acceleration/deceleration speed.
+        #self.get_logger().error('accel:{}'.format(self.get_parameter('axidraw_accel').get_parameter_value().integer_value))
+        self.ad.options.pen_pos_down = self.get_parameter('axidraw_pen_pos_down').get_parameter_value().integer_value #Pen height when the pen is down (plotting).
+        self.ad.options.pen_pos_up = self.get_parameter('axidraw_pen_pos_up').get_parameter_value().integer_value #Pen height when the pen is up.
+        self.ad.options.pen_rate_lower = self.get_parameter('axidraw_pen_rate_lower').get_parameter_value().integer_value #  Speed of lowering the pen-lift motor.
+        self.ad.options.pen_rate_raise = self.get_parameter('axidraw_pen_rate_raise').get_parameter_value().integer_value #Speed of raising the pen-lift motor.
+        self.ad.options.const_speed = self.get_parameter('axidraw_const_speed').get_parameter_value().bool_value #Option: Use constant speed when pen is down.
+        self.ad.options.pen_delay_down = self.get_parameter('axidraw_pen_delay_down').get_parameter_value().integer_value #Added delay after lowering pen.
+        self.ad.options.pen_delay_up = self.get_parameter('axidraw_pen_delay_up').get_parameter_value().integer_value #Added delay after raising pen.
         self.ad.update()                     # Process changes to options
         self.status["serial"] = "ready"
         self.status["motion"] = "ready"
+        self.status["pen"] = "up"
         return True
 
     def __init__(self):
@@ -80,10 +94,22 @@ class AxidrawSerial(Node):
         self.declare_parameter('serial_port', '/dev/ttyACM0')
         port = self.get_parameter('serial_port').get_parameter_value().string_value
 
+        self.declare_parameter('axidraw_speed_pendown', 100)
+        self.declare_parameter('axidraw_speed_penup', 100)
+        self.declare_parameter('axidraw_accel', 100)
+        self.declare_parameter('axidraw_pen_pos_down', 40)
+        self.declare_parameter('axidraw_pen_pos_up', 60)
+        self.declare_parameter('axidraw_pen_rate_lower', 50)
+        self.declare_parameter('axidraw_pen_rate_raise', 75)
+        self.declare_parameter('axidraw_const_speed', False)
+        self.declare_parameter('axidraw_pen_delay_down', 0)
+        self.declare_parameter('axidraw_pen_delay_up', 0)
+
         self.status_srv = self.create_service(Status, 'axidraw_status', self.get_status)
 
         while not self.init_serial(port):
-            self.get_logger().error("Failed to connect to axidraw on port:{}".format(port))
+            self.get_logger().error("Failed to connect to axidraw on port:{}, retrying".format(port))
+        self.get_logger().info("Successfully connected to axidraw on port:{}".format(port))
 
         self.move_sub = self.create_subscription(Point, 'axidraw_move', self.move_callback, qos_profile=QoSProfile(depth=1))
         self.penup_sub = self.create_subscription(Empty, 'axidraw_penup', self.penup_callback, qos_profile=QoSProfile(depth=1))
@@ -165,7 +191,9 @@ class AxidrawSerial(Node):
 
         self.get_logger().info("Received penup: {}".format(msg))
 
+        if self.status['pen'] == "down":
             self.ad.penup()
+            self.status['pen'] = "up"
         self.set_ready()
 
     def pendown_callback(self, msg):
@@ -178,7 +206,9 @@ class AxidrawSerial(Node):
 
         self.get_logger().info("Received pendown: {}".format(msg))
 
+        if self.status['pen'] == "up":
             self.ad.pendown()
+            self.status['pen'] = "down"
         self.set_ready()
 
     def stroke_callback(self, msg):
@@ -193,6 +223,7 @@ class AxidrawSerial(Node):
 
         path = [ [p.x,p.y] for p in msg.points ]
         self.ad.draw_path(path)
+        self.status['pen'] = "up"
         self.set_ready()
 
 

src/custom_xarm_description/CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.5)
+project(custom_xarm_description)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(DIRECTORY
+  rviz
+  urdf
+  meshes
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

src/custom_xarm_description/README.md

@@ -0,0 +1 @@
+Taken from https://github.com/xArm-Developer/xarm_ros2/tree/humble 2023.03.21

src/custom_xarm_description/launch/_robot_description.launch.py

@@ -0,0 +1,104 @@
+#!/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
+from ament_index_python import get_package_share_directory
+from launch.launch_description_sources import load_python_launch_file_as_module
+from launch import LaunchDescription
+from launch.actions import OpaqueFunction, DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def launch_setup(context, *args, **kwargs):
+    prefix = LaunchConfiguration('prefix', default='')
+    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
+    limited = LaunchConfiguration('limited', default=False)
+    effort_control = LaunchConfiguration('effort_control', default=False)
+    velocity_control = LaunchConfiguration('velocity_control', default=False)
+    add_gripper = LaunchConfiguration('add_gripper', default=False)
+    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
+    dof = LaunchConfiguration('dof', default=7)
+    robot_type = LaunchConfiguration('robot_type', default='xarm')
+    ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotSystemHardware')
+    joint_states_remapping = LaunchConfiguration('joint_states_remapping', default='joint_states')
+    xacro_file = LaunchConfiguration('xacro_file', default=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']))
+
+    add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
+
+    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
+    geometry_type = LaunchConfiguration('geometry_type', default='box')
+    geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
+    geometry_height = LaunchConfiguration('geometry_height', default=0.1)
+    geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
+    geometry_length = LaunchConfiguration('geometry_length', default=0.1)
+    geometry_width = LaunchConfiguration('geometry_width', default=0.1)
+    geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
+    geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
+    geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
+    geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
+    geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
+
+    # robot_description
+    # xarm_description/launch/lib/robot_description_lib.py
+    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
+    get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
+    robot_description = {
+        'robot_description': get_xacro_file_content(
+            xacro_file=xacro_file, 
+            arguments={
+                'prefix': prefix,
+                'hw_ns': hw_ns.perform(context).strip('/'),
+                'limited': limited,
+                'effort_control': effort_control,
+                'velocity_control': velocity_control,
+                'add_gripper': add_gripper,
+                'add_vacuum_gripper': add_vacuum_gripper,
+                'dof': dof,
+                'robot_type': robot_type,
+                'ros2_control_plugin': ros2_control_plugin,
+                'add_realsense_d435i': add_realsense_d435i,
+                'add_other_geometry': add_other_geometry,
+                'geometry_type': geometry_type,
+                'geometry_mass': geometry_mass,
+                'geometry_height': geometry_height,
+                'geometry_radius': geometry_radius,
+                'geometry_length': geometry_length,
+                'geometry_width': geometry_width,
+                'geometry_mesh_filename': geometry_mesh_filename,
+                'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
+                'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
+                'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
+                'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
+            }
+        )
+    }
+
+    # robot state publisher node
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[robot_description],
+        remappings=[
+            # ('joint_states', joint_states_remapping),
+            ('/tf', 'tf'),
+            ('/tf_static', 'tf_static'),
+        ]
+    )
+    return [
+        robot_state_publisher_node
+    ]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=launch_setup)
+    ])

src/custom_xarm_description/launch/_robot_joint_state.launch.py

@@ -0,0 +1,95 @@
+#!/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.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def launch_setup(context, *args, **kwargs):
+    prefix = LaunchConfiguration('prefix', default='')
+    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
+    limited = LaunchConfiguration('limited', default=False)
+    effort_control = LaunchConfiguration('effort_control', default=False)
+    velocity_control = LaunchConfiguration('velocity_control', default=False)
+    add_gripper = LaunchConfiguration('add_gripper', default=False)
+    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
+    dof = LaunchConfiguration('dof', default=7)
+    robot_type = LaunchConfiguration('robot_type', default='xarm')
+    ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotSystemHardware')
+    joint_states_remapping = LaunchConfiguration('joint_states_remapping', default='joint_states')
+
+    add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
+
+    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
+    geometry_type = LaunchConfiguration('geometry_type', default='box')
+    geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
+    geometry_height = LaunchConfiguration('geometry_height', default=0.1)
+    geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
+    geometry_length = LaunchConfiguration('geometry_length', default=0.1)
+    geometry_width = LaunchConfiguration('geometry_width', default=0.1)
+    geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
+    geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
+    geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
+    geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
+    geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
+
+    # 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={
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': dof,
+            'robot_type': robot_type,
+            'ros2_control_plugin': ros2_control_plugin,
+            'joint_states_remapping': joint_states_remapping,
+            'add_realsense_d435i': add_realsense_d435i,
+            'add_other_geometry': add_other_geometry,
+            'geometry_type': geometry_type,
+            'geometry_mass': geometry_mass,
+            'geometry_height': geometry_height,
+            'geometry_radius': geometry_radius,
+            'geometry_length': geometry_length,
+            'geometry_width': geometry_width,
+            'geometry_mesh_filename': geometry_mesh_filename,
+            'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
+            'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
+            'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
+            'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
+        }.items(),
+    )
+
+    # joint state publisher node
+    joint_state_publisher_node = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        output='screen',
+        parameters=[{'source_list': ['{}{}/joint_states'.format(prefix.perform(context), hw_ns.perform(context))]}],
+    )
+
+    return [
+        robot_description_launch,
+        joint_state_publisher_node,
+    ]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=launch_setup)
+    ])

src/custom_xarm_description/launch/_robot_rviz_display.launch.py

@@ -0,0 +1,81 @@
+#!/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 IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+    prefix = LaunchConfiguration('prefix', default='')
+    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
+    limited = LaunchConfiguration('limited', default=False)
+    effort_control = LaunchConfiguration('effort_control', default=False)
+    velocity_control = LaunchConfiguration('velocity_control', default=False)
+    add_gripper = LaunchConfiguration('add_gripper', default=False)
+    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
+    dof = LaunchConfiguration('dof', default=7)
+    robot_type = LaunchConfiguration('robot_type', default='xarm')
+
+    add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
+
+    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
+    geometry_type = LaunchConfiguration('geometry_type', default='box')
+    geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
+    geometry_height = LaunchConfiguration('geometry_height', default=0.1)
+    geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
+    geometry_length = LaunchConfiguration('geometry_length', default=0.1)
+    geometry_width = LaunchConfiguration('geometry_width', default=0.1)
+    geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
+    geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
+    geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
+    geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
+    geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
+    
+    # robot joint state launch
+    # xarm_description/launch/_robot_joint_state.launch.py
+    robot_joint_state_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_joint_state.launch.py']),
+        launch_arguments={
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': dof,
+            'robot_type': robot_type,
+            'add_realsense_d435i': add_realsense_d435i,
+            'add_other_geometry': add_other_geometry,
+            'geometry_type': geometry_type,
+            'geometry_mass': geometry_mass,
+            'geometry_height': geometry_height,
+            'geometry_radius': geometry_radius,
+            'geometry_length': geometry_length,
+            'geometry_width': geometry_width,
+            'geometry_mesh_filename': geometry_mesh_filename,
+            'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
+            'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
+            'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
+            'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
+        }.items(),
+    )
+
+    # rviz2 display launch
+    # xarm_description/launch/_rviz_display.launch.py
+    rviz2_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_rviz_display.launch.py']),
+    )
+    
+    return LaunchDescription([
+        robot_joint_state_launch,
+        rviz2_launch
+    ])

src/custom_xarm_description/launch/_rviz_display.launch.py

@@ -0,0 +1,39 @@
+#!/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.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.actions import RegisterEventHandler, EmitEvent
+from launch.event_handlers import OnProcessExit
+from launch.events import Shutdown
+
+
+def generate_launch_description():
+    # rviz2 node
+    rviz2_params = PathJoinSubstitution([FindPackageShare('xarm_description'), 'rviz', 'display.rviz'])
+    rviz2_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', rviz2_params],
+        remappings=[
+            ('/tf', 'tf'),
+            ('/tf_static', 'tf_static'),
+        ]
+    )
+    
+    return LaunchDescription([
+        RegisterEventHandler(event_handler=OnProcessExit(
+            target_action=rviz2_node,
+            on_exit=[EmitEvent(event=Shutdown())]
+        )),
+        rviz2_node,
+    ])

src/custom_xarm_description/launch/lib/robot_description_lib.py

@@ -0,0 +1,29 @@
+#!/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.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+
+def get_xacro_file_content(
+    xacro_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), 
+    arguments={}):
+    command = [
+        PathJoinSubstitution([FindExecutable(name='xacro')]),
+        ' ',
+        xacro_file,
+        ' '
+    ]
+    if arguments and isinstance(arguments, dict):
+        for key, val in arguments.items():
+            command.extend([
+                '{}:='.format(key),
+                val,
+                ' '
+            ])
+    return Command(command)

src/custom_xarm_description/launch/lite6_rviz_display.launch.py

@@ -0,0 +1,42 @@
+#!/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 IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+def generate_launch_description():
+    prefix = LaunchConfiguration('prefix', default='')
+    hw_ns = LaunchConfiguration('hw_ns', default='ufactory')
+    limited = LaunchConfiguration('limited', default=False)
+    effort_control = LaunchConfiguration('effort_control', default=False)
+    velocity_control = LaunchConfiguration('velocity_control', default=False)
+    add_gripper = LaunchConfiguration('add_gripper', default=False)
+    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
+
+    # robot rviz launch
+    # xarm_description/launch/_robot_rviz_display.launch.py
+    robot_rviz_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_rviz_display.launch.py']),
+        launch_arguments={
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': '6',
+            'robot_type': 'lite',
+        }.items(),
+    )
+
+    return LaunchDescription([
+        robot_rviz_launch
+    ])

src/custom_xarm_description/launch/xarm5_rviz_display.launch.py

@@ -0,0 +1,42 @@
+#!/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 IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+def generate_launch_description():
+    prefix = LaunchConfiguration('prefix', default='')
+    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
+    limited = LaunchConfiguration('limited', default=False)
+    effort_control = LaunchConfiguration('effort_control', default=False)
+    velocity_control = LaunchConfiguration('velocity_control', default=False)
+    add_gripper = LaunchConfiguration('add_gripper', default=False)
+    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
+    
+    # robot rviz launch
+    # xarm_description/launch/_robot_rviz_display.launch.py
+    robot_rviz_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_rviz_display.launch.py']),
+        launch_arguments={
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': '5',
+            'robot_type': 'xarm',
+        }.items(),
+    )
+
+    return LaunchDescription([
+        robot_rviz_launch
+    ])

src/custom_xarm_description/launch/xarm6_rviz_display.launch.py

@@ -0,0 +1,42 @@
+#!/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 IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+def generate_launch_description():
+    prefix = LaunchConfiguration('prefix', default='')
+    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
+    limited = LaunchConfiguration('limited', default=False)
+    effort_control = LaunchConfiguration('effort_control', default=False)
+    velocity_control = LaunchConfiguration('velocity_control', default=False)
+    add_gripper = LaunchConfiguration('add_gripper', default=False)
+    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
+
+    # robot rviz launch
+    # xarm_description/launch/_robot_rviz_display.launch.py
+    robot_rviz_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_rviz_display.launch.py']),
+        launch_arguments={
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': '6',
+            'robot_type': 'xarm',
+        }.items(),
+    )
+
+    return LaunchDescription([
+        robot_rviz_launch
+    ])

src/custom_xarm_description/launch/xarm7_rviz_display.launch.py

@@ -0,0 +1,42 @@
+#!/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 IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+def generate_launch_description():
+    prefix = LaunchConfiguration('prefix', default='')
+    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
+    limited = LaunchConfiguration('limited', default=False)
+    effort_control = LaunchConfiguration('effort_control', default=False)
+    velocity_control = LaunchConfiguration('velocity_control', default=False)
+    add_gripper = LaunchConfiguration('add_gripper', default=False)
+    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
+    
+    # robot rviz launch
+    # xarm_description/launch/_robot_rviz_display.launch.py
+    robot_rviz_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_rviz_display.launch.py']),
+        launch_arguments={
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': '7',
+            'robot_type': 'xarm',
+        }.items(),
+    )
+
+    return LaunchDescription([
+        robot_rviz_launch
+    ])

src/custom_xarm_description/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>custom_xarm_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="a@a.com"></maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/custom_xarm_description/rviz/display.rviz

@@ -0,0 +1,195 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 617
+  - 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
+        link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_eef:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        world:
+          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: world
+    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: 3.1222100257873535
+      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: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6253979802131653
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 5.18356990814209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 60
+  Y: 60

src/custom_xarm_description/urdf/camera/realsense.gazebo.xacro

@@ -0,0 +1,127 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="realsense_gazebo" params="prefix">     
+    <gazebo reference="${prefix}camera_depth_frame">
+      <sensor name="cameradepth" type="depth">
+        <camera name="camera">
+          <horizontal_fov>1.57</horizontal_fov>
+          <image>
+            <width>1280</width>
+            <height>720</height>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.100</stddev>
+          </noise>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+        <visualize>0</visualize>
+      </sensor>
+    </gazebo>
+    <gazebo reference="${prefix}camera_color_frame">
+      <sensor name="cameracolor" type="camera">
+        <camera name="camera">
+          <horizontal_fov>1.57</horizontal_fov>
+          <image>
+            <width>1280</width>
+            <height>720</height>
+            <format>RGB_INT8</format>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+        <visualize>1</visualize>
+      </sensor>
+    </gazebo>
+    <gazebo reference="${prefix}camera_left_ir_frame">
+      <sensor name="cameraired1" type="camera">
+        <camera name="camera">
+          <horizontal_fov>1.57</horizontal_fov>
+          <image>
+            <width>1280</width>
+            <height>720</height>
+            <format>L_INT8</format>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.05</stddev>
+          </noise>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>1</update_rate>
+        <visualize>0</visualize>
+      </sensor>
+    </gazebo>
+    <gazebo reference="${prefix}camera_right_ir_frame">
+      <sensor name="cameraired2" type="camera">
+        <camera name="camera">
+          <horizontal_fov>1.57</horizontal_fov>
+          <image>
+            <width>1280</width>
+            <height>720</height>
+            <format>L_INT8</format>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.05</stddev>
+          </noise>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>1</update_rate>
+        <visualize>0</visualize>
+      </sensor>
+    </gazebo>
+    <gazebo>
+      <plugin name="realsense_gazebo_camera" filename="librealsense_gazebo_plugin.so">
+        <prefix>camera</prefix>
+        <depthUpdateRate>30.0</depthUpdateRate>
+        <colorUpdateRate>30.0</colorUpdateRate>
+        <infraredUpdateRate>1.0</infraredUpdateRate>
+        <depthTopicName>aligned_depth_to_color/image_raw</depthTopicName>
+        <depthCameraInfoTopicName>depth/camera_info</depthCameraInfoTopicName>
+        <colorTopicName>color/image_raw</colorTopicName>
+        <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
+        <infrared1TopicName>infra1/image_raw</infrared1TopicName>
+        <infrared1CameraInfoTopicName>infra1/camera_info</infrared1CameraInfoTopicName>
+        <infrared2TopicName>infra2/image_raw</infrared2TopicName>
+        <infrared2CameraInfoTopicName>infra2/camera_info</infrared2CameraInfoTopicName>
+        <colorOpticalframeName>camera_color_optical_frame</colorOpticalframeName>
+        <depthOpticalframeName>camera_depth_optical_frame</depthOpticalframeName>
+        <infrared1OpticalframeName>camera_left_ir_optical_frame</infrared1OpticalframeName>
+        <infrared2OpticalframeName>camera_right_ir_optical_frame</infrared2OpticalframeName>
+        <rangeMinDepth>0.3</rangeMinDepth>
+        <rangeMaxDepth>3.0</rangeMaxDepth>
+        <pointCloud>true</pointCloud>
+        <pointCloudTopicName>depth/color/points</pointCloudTopicName>
+        <pointCloudCutoff>0.3</pointCloudCutoff>
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/camera/realsense_d435i.urdf.xacro

@@ -0,0 +1,95 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="d435i_urdf" params="prefix:=''">
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+    <link name="${prefix}link_eef">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/camera/realsense/visual/d435_with_cam_stand.STL"/>
+        </geometry>
+        <material name="${prefix}Silver" />
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/camera/realsense/collision/d435_with_cam_stand.STL"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name="${prefix}camera_link"></link>
+
+    <link name="${prefix}camera_depth_frame"></link>
+
+    <link name="${prefix}camera_depth_optical_frame"></link>
+
+    <link name="${prefix}camera_color_frame"></link>
+
+    <link name="${prefix}camera_color_optical_frame"></link>
+
+    <link name="${prefix}camera_left_ir_frame"></link>
+
+    <link name="${prefix}camera_left_ir_optical_frame"></link>
+
+    <link name="${prefix}camera_right_ir_frame"></link>
+
+    <link name="${prefix}camera_right_ir_optical_frame"></link>
+
+    <joint name="${prefix}camera_link_joint" type="fixed">
+      <parent link="${prefix}link_eef" />
+      <child link="${prefix}camera_link" />
+      <!-- <origin xyz="0.067985 0 0.02725" rpy="0 ${-M_PI/2} 0" /> -->
+      <origin xyz="0.06746 0 0.0205" rpy="0 ${-M_PI/2} 0" />
+    </joint>
+
+    <joint name="${prefix}camera_depth_joint" type="fixed">
+      <parent link="${prefix}camera_link" />
+      <child link="${prefix}camera_depth_frame" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </joint>
+
+    <joint name="${prefix}camera_depth_optical_joint" type="fixed">
+      <parent link="${prefix}camera_depth_frame" />
+      <child link="${prefix}camera_depth_optical_frame" />
+      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
+    </joint>
+
+    <joint name="${prefix}camera_color_joint" type="fixed">
+      <parent link="${prefix}camera_link" />
+      <child link="${prefix}camera_color_frame" />
+      <origin xyz="0 0.015 0" rpy="0 0 0" />
+    </joint>
+
+    <joint name="${prefix}camera_color_optical_joint" type="fixed">
+      <parent link="${prefix}camera_color_frame" />
+      <child link="${prefix}camera_color_optical_frame" />
+      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
+    </joint>
+
+    <joint name="${prefix}camera_left_ir_joint" type="fixed">
+      <parent link="${prefix}camera_link" />
+      <child link="${prefix}camera_left_ir_frame" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </joint>
+
+    <joint name="${prefix}camera_left_ir_optical_joint" type="fixed">
+      <parent link="${prefix}camera_left_ir_frame" />
+      <child link="${prefix}camera_left_ir_optical_frame" />
+      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
+    </joint>
+
+    <joint name="${prefix}camera_right_ir_joint" type="fixed">
+      <parent link="${prefix}camera_link" />
+      <child link="${prefix}camera_right_ir_frame" />
+      <origin xyz="0 -0.050 0" rpy="0 0 0" />
+    </joint>
+
+    <joint name="${prefix}camera_right_ir_optical_joint" type="fixed">
+      <parent link="${prefix}camera_right_ir_frame" />
+      <child link="${prefix}camera_right_ir_optical_frame" />
+      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
+    </joint>
+  
+  </xacro:macro>
+</robot>

src/custom_xarm_description/urdf/common/common.gazebo.xacro

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="gazebo_ros_control_plugin" params="prefix:='' ros2_control_params:=''">
+
+    <gazebo>
+      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+        <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
+        <xacro:if value="${ros2_control_params != ''}">
+          <parameters>${ros2_control_params}</parameters>
+        </xacro:if>
+      </plugin>
+    </gazebo>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/dual_xarm_device.urdf.xacro

@@ -0,0 +1,102 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="dual_xarm">
+  <xacro:arg name="prefix_1" default="L_"/>
+  <xacro:arg name="prefix_2" default="R_"/>
+  <xacro:arg name="dof_1" default="7"/>
+  <xacro:arg name="dof_2" default="7"/>
+  <xacro:arg name="robot_type_1" default="xarm"/>
+  <xacro:arg name="robot_type_2" default="xarm"/>
+  <xacro:arg name="add_gripper_1" default="false"/>
+  <xacro:arg name="add_gripper_2" default="false"/>
+  <xacro:arg name="add_vacuum_gripper_1" default="false"/>
+  <xacro:arg name="add_vacuum_gripper_2" default="false"/>
+  <xacro:arg name="hw_ns" default="xarm"/>
+  <xacro:arg name="limited" default="false"/>
+  <xacro:arg name="effort_control" default="false"/>
+  <xacro:arg name="velocity_control" default="false"/>
+  <xacro:arg name="ros2_control_plugin" default="uf_robot_hardware/UFRobotSystemHardware"/>
+  <xacro:arg name="ros2_control_params" default="$(find xarm_controller)/config/xarm$(arg dof_1)_controllers.yaml"/>
+
+  <xacro:arg name="add_realsense_d435i_1" default="false"/>
+  <xacro:arg name="add_realsense_d435i_2" default="false"/>
+
+  <xacro:arg name="add_other_geometry_1" default="false"/>
+  <xacro:arg name="geometry_type_1" default="box"/>
+  <xacro:arg name="geometry_mass_1" default="0.1"/>
+  <xacro:arg name="geometry_height_1" default="0.1"/>
+  <xacro:arg name="geometry_radius_1" default="0.1"/>
+  <xacro:arg name="geometry_length_1" default="0.1"/>
+  <xacro:arg name="geometry_width_1" default="0.1"/>
+  <xacro:arg name="geometry_mesh_filename_1" default=""/>
+  <xacro:arg name="geometry_mesh_origin_xyz_1" default="0 0 0"/>
+  <xacro:arg name="geometry_mesh_origin_rpy_1" default="0 0 0"/>
+  <xacro:arg name="geometry_mesh_tcp_xyz_1" default="0 0 0"/>
+  <xacro:arg name="geometry_mesh_tcp_rpy_1" default="0 0 0"/>
+
+  <xacro:arg name="add_other_geometry_2" default="false"/>
+  <xacro:arg name="geometry_type_2" default="box"/>
+  <xacro:arg name="geometry_mass_2" default="0.1"/>
+  <xacro:arg name="geometry_height_2" default="0.1"/>
+  <xacro:arg name="geometry_radius_2" default="0.1"/>
+  <xacro:arg name="geometry_length_2" default="0.1"/>
+  <xacro:arg name="geometry_width_2" default="0.1"/>
+  <xacro:arg name="geometry_mesh_filename_2" default=""/>
+  <xacro:arg name="geometry_mesh_origin_xyz_2" default="0 0 0"/>
+  <xacro:arg name="geometry_mesh_origin_rpy_2" default="0 0 0"/>
+  <xacro:arg name="geometry_mesh_tcp_xyz_2" default="0 0 0"/>
+  <xacro:arg name="geometry_mesh_tcp_rpy_2" default="0 0 0"/>
+
+  <xacro:arg name="robot_ip_1" default=""/>
+  <xacro:arg name="robot_ip_2" default=""/>
+  <xacro:arg name="report_type_1" default="normal"/>
+  <xacro:arg name="report_type_2" default="normal"/>
+  <xacro:arg name="baud_checkset_1" default="true"/>
+  <xacro:arg name="baud_checkset_2" default="true"/>
+  <xacro:arg name="default_gripper_baud_1" default="2000000"/>
+  <xacro:arg name="default_gripper_baud_2" default="2000000"/>
+  
+  <!-- load xarm device -->
+  <xacro:include filename="$(find xarm_description)/urdf/xarm_device_macro.xacro" />
+  
+  <link name="ground" />
+
+  <!-- gazebo_plugin -->
+  <xacro:include filename="$(find xarm_description)/urdf/common/common.gazebo.xacro" />
+  <xacro:gazebo_ros_control_plugin ros2_control_params="$(arg ros2_control_params)"/>
+  
+  <xacro:xarm_device prefix="$(arg prefix_1)" namespace="$(arg hw_ns)" limited="$(arg limited)" 
+    effort_control="$(arg effort_control)" velocity_control="$(arg velocity_control)"
+    add_gripper="$(arg add_gripper_1)" add_vacuum_gripper="$(arg add_vacuum_gripper_1)" dof="$(arg dof_1)"
+    ros2_control_plugin="$(arg ros2_control_plugin)" robot_type="$(arg robot_type_1)"
+    load_gazebo_plugin="false" ros2_control_params="$(arg ros2_control_params)"
+    attach_to="ground" xyz="0 0 0" rpy="0 0 0"
+    add_realsense_d435i="$(arg add_realsense_d435i_1)" 
+    add_other_geometry="$(arg add_other_geometry_1)" 
+    geometry_type="$(arg geometry_type_1)" geometry_mass="$(arg geometry_mass_1)"
+    geometry_height="$(arg geometry_height_1)" geometry_radius="$(arg geometry_radius_1)"
+    geometry_length="$(arg geometry_length_1)" geometry_width="$(arg geometry_width_1)"
+    geometry_mesh_filename="$(arg geometry_mesh_filename_1)"
+    geometry_mesh_origin_xyz="$(arg geometry_mesh_origin_xyz_1)" geometry_mesh_origin_rpy="$(arg geometry_mesh_origin_rpy_1)"
+    geometry_mesh_tcp_xyz="$(arg geometry_mesh_tcp_xyz_1)" geometry_mesh_tcp_rpy="$(arg geometry_mesh_tcp_rpy_1)"
+    robot_ip="$(arg robot_ip_1)" report_type="$(arg report_type_1)"
+    baud_checkset="$(arg baud_checkset_1)" default_gripper_baud="$(arg default_gripper_baud_1)"
+    />
+  <xacro:xarm_device prefix="$(arg prefix_2)" namespace="$(arg hw_ns)" limited="$(arg limited)" 
+    effort_control="$(arg effort_control)" velocity_control="$(arg velocity_control)"
+    add_gripper="$(arg add_gripper_2)" add_vacuum_gripper="$(arg add_vacuum_gripper_2)" dof="$(arg dof_2)"
+    ros2_control_plugin="$(arg ros2_control_plugin)" robot_type="$(arg robot_type_2)"
+    load_gazebo_plugin="false" ros2_control_params="$(arg ros2_control_params)"
+    attach_to="ground" xyz="0 1 0" rpy="0 0 0"
+    add_realsense_d435i="$(arg add_realsense_d435i_2)" 
+    add_other_geometry="$(arg add_other_geometry_2)" 
+    geometry_type="$(arg geometry_type_2)" geometry_mass="$(arg geometry_mass_2)"
+    geometry_height="$(arg geometry_height_2)" geometry_radius="$(arg geometry_radius_2)"
+    geometry_length="$(arg geometry_length_2)" geometry_width="$(arg geometry_width_2)"
+    geometry_mesh_filename="$(arg geometry_mesh_filename_2)"
+    geometry_mesh_origin_xyz="$(arg geometry_mesh_origin_xyz_2)" geometry_mesh_origin_rpy="$(arg geometry_mesh_origin_rpy_2)"
+    geometry_mesh_tcp_xyz="$(arg geometry_mesh_tcp_xyz_2)" geometry_mesh_tcp_rpy="$(arg geometry_mesh_tcp_rpy_2)"
+    robot_ip="$(arg robot_ip_2)" report_type="$(arg report_type_2)"
+    baud_checkset="$(arg baud_checkset_2)" default_gripper_baud="$(arg default_gripper_baud_2)"
+    />
+
+</robot>

src/custom_xarm_description/urdf/gripper/lite_gripper.urdf.xacro

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="uflite_gripper">
+<!--
+  Author: Jason Peng <jason@ufactory.cc>
+-->
+  <xacro:macro name="uflite_gripper_urdf" params="prefix:='' attach_to:='' rpy:='0 0 0' xyz:='0 0 0' ">
+  <xacro:unless value="${attach_to == ''}">
+    <joint name="${prefix}gripper_fix" type="fixed">
+      <parent link="${attach_to}"/>
+      <child link="${prefix}uflite_gripper_link"/>
+      <origin xyz="${xyz}" rpy="${rpy}"/>
+    </joint>
+  </xacro:unless>
+  <link
+    name="${prefix}uflite_gripper_link">
+    <inertial>
+      <origin
+        xyz="0.0 0.0 0.030"
+        rpy="0 0 0" />
+      <mass
+        value="0.28" />
+      <inertia
+        ixx="0.00047106"
+        ixy="3.9292E-07"
+        ixz="2.6537E-06"
+        iyy="0.00033072"
+        iyz="-1.0975E-05"
+        izz="0.00025642" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/gripper_lite.stl"/>
+      </geometry>
+      <material name="White">
+        <color rgba="1.0 1.0 1.0 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="file:///$(find xarm_description)/meshes/lite6/collision/gripper_lite.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="${prefix}link_tcp" />
+
+  <joint
+    name="${prefix}joint_tcp"
+    type="fixed">
+    <origin
+      xyz="0 0 0.0836"
+      rpy="0 0 0" />
+    <parent
+      link="${prefix}uflite_gripper_link" />
+    <child
+      link="${prefix}link_tcp" />
+  </joint>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/gripper/xarm_gripper.gazebo.xacro

@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- from mimic_joint_gazebo_tutorial by @mintar, refer: https://github.com/mintar/mimic_joint_gazebo_tutorial -->
+  <xacro:macro name="mimic_joint_plugin_gazebo" params="name_prefix following_joint mimic_joint has_pid:=false multiplier:=1.0 offset:=0 sensitiveness:=0.0 max_effort:=1.0">
+    <gazebo>
+      <plugin name="${name_prefix}mimic_joint_plugin" filename="libgazebo_mimic_joint_plugin.so">
+        <joint>${following_joint}</joint>
+        <mimicJoint>${mimic_joint}</mimicJoint>
+        <xacro:if value="${has_pid}">                     <!-- if set to true, PID parameters from "/gazebo_ros_control/pid_gains/${mimic_joint}" are loaded -->
+          <hasPID />
+        </xacro:if>
+        <multiplier>${multiplier}</multiplier>
+        <offset>${offset}</offset>
+        <sensitiveness>${sensitiveness}</sensitiveness>   <!-- if absolute difference between setpoint and process value is below this threshold, do nothing; 0.0 = disable [rad] -->
+        <maxEffort>${max_effort}</maxEffort>              <!-- only taken into account if has_pid:=true [Nm] -->
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:macro name="xarm_gripper_gazebo" params="prefix">
+
+    <gazebo reference="${prefix}xarm_gripper_base_link">
+      <selfCollide>false</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}left_outer_knuckle">
+      <selfCollide>false</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}left_finger">
+      <selfCollide>false</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}left_inner_knuckle">
+      <selfCollide>false</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}right_outer_knuckle">
+      <selfCollide>false</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}right_finger">
+      <selfCollide>false</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}right_inner_knuckle">
+      <selfCollide>false</selfCollide>
+    </gazebo>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/gripper/xarm_gripper.ros2_control.xacro

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="xarm_gripper_ros2_control" params="
+    prefix:='' ros2_control_plugin:='uf_robot_hardware/UFRobotFakeSystemHardware'
+  ">
+    <ros2_control name="XArmGripperSystem" type="system">
+      <hardware>
+        <plugin>${ros2_control_plugin}</plugin>
+      </hardware>
+      <joint name="${prefix}drive_joint">
+        <command_interface name="position">
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+    </ros2_control>
+  </xacro:macro>
+</robot>

src/custom_xarm_description/urdf/gripper/xarm_gripper.transmission.xacro

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="xarm_gripper_transmission"
+    params="prefix hard_interface:=PositionJointInterface reduction:=1">
+
+    <transmission name="${prefix}drive_joint_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}drive_joint">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}drive_joint_motor">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+  </xacro:macro>
+</robot>

src/custom_xarm_description/urdf/gripper/xarm_gripper.urdf.xacro

@@ -0,0 +1,415 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="xarm_gripper">
+<!--
+  Author: Jason Peng <jason@ufactory.cc>
+-->
+  <xacro:macro name="xarm_gripper_urdf" params="prefix:='' attach_to:='' rpy:='0 0 0' xyz:='0 0 0' ">
+  <xacro:unless value="${attach_to == ''}">
+    <joint name="${prefix}gripper_fix" type="fixed">
+      <parent link="${attach_to}"/>
+      <child link="${prefix}xarm_gripper_base_link"/>
+      <origin xyz="${xyz}" rpy="${rpy}"/>
+    </joint>
+  </xacro:unless>
+  <link
+    name="${prefix}xarm_gripper_base_link">
+    <inertial>
+      <origin
+        xyz="-0.00065489 -0.0018497 0.048028"
+        rpy="0 0 0" />
+      <mass
+        value="0.54156" />
+      <inertia
+        ixx="0.00047106"
+        ixy="3.9292E-07"
+        ixz="2.6537E-06"
+        iyy="0.00033072"
+        iyz="-1.0975E-05"
+        izz="0.00025642" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/base_link.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/base_link.STL"/>
+      </geometry>
+      <!-- <material
+        name="">
+        <color
+          rgba="0.89804 0.91765 0.92941 1" />
+      </material> -->
+      <material name="White">
+        <color rgba="1.0 1.0 1.0 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/base_link.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/base_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="${prefix}left_outer_knuckle">
+    <inertial>
+      <origin
+        xyz="2.9948E-14 0.021559 0.015181"
+        rpy="0 0 0" />
+      <mass
+        value="0.033618" />
+      <inertia
+        ixx="1.9111E-05"
+        ixy="-1.8803E-17"
+        ixz="-1.1002E-17"
+        iyy="6.6256E-06"
+        iyz="-7.3008E-06"
+        izz="1.3185E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/left_outer_knuckle.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/left_outer_knuckle.STL"/>
+      </geometry>
+      <material name="Silver">
+        <color rgba="0.753 0.753 0.753 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/left_outer_knuckle.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/left_outer_knuckle.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="${prefix}drive_joint"
+    type="revolute">
+    <origin
+      xyz="0 0.035 0.059098"
+      rpy="0 0 0" />
+    <parent
+      link="${prefix}xarm_gripper_base_link" />
+    <child
+      link="${prefix}left_outer_knuckle" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="0"
+      upper="0.85"
+      effort="50"
+      velocity="2" />
+  </joint>
+  <link
+    name="${prefix}left_finger">
+    <inertial>
+      <origin
+        xyz="-2.4536E-14 -0.016413 0.029258"
+        rpy="0 0 0" />
+      <mass
+        value="0.048304" />
+      <inertia
+        ixx="1.7493E-05"
+        ixy="-4.2156E-19"
+        ixz="6.9164E-18"
+        iyy="1.7225E-05"
+        iyz="4.6433E-06"
+        izz="5.1466E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/left_finger.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/left_finger.STL"/>
+      </geometry>
+      <material name="Silver">
+        <color rgba="0.753 0.753 0.753 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/left_finger.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/left_finger.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="${prefix}left_finger_joint"
+    type="revolute">
+    <origin
+      xyz="0 0.035465 0.042039"
+      rpy="0 0 0" />
+    <parent
+      link="${prefix}left_outer_knuckle" />
+    <child
+      link="${prefix}left_finger" />
+    <axis
+      xyz="-1 0 0" />
+    <limit
+      lower="0"
+      upper="0.85"
+      effort="50"
+      velocity="2" />
+    <mimic joint="${prefix}drive_joint" multiplier="1" offset="0" />
+  </joint>
+  <link
+    name="${prefix}left_inner_knuckle">
+    <inertial>
+      <origin
+        xyz="1.86600784687907E-06 0.0220467847633621 0.0261334672830885"
+        rpy="0 0 0" />
+      <mass
+        value="0.0230125781256706" />
+      <inertia
+        ixx="6.09490024271906E-06"
+        ixy="6.06651326160071E-11"
+        ixz="7.19102670500635E-11"
+        iyy="6.01955084375188E-06"
+        iyz="-2.75316812991721E-06"
+        izz="5.07862004479903E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/left_inner_knuckle.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/left_inner_knuckle.STL"/>
+      </geometry>
+      <material name="Silver">
+        <color rgba="0.753 0.753 0.753 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/left_inner_knuckle.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/left_inner_knuckle.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="${prefix}left_inner_knuckle_joint"
+    type="revolute">
+    <origin
+      xyz="0 0.02 0.074098"
+      rpy="0 0 0" />
+    <parent
+      link="${prefix}xarm_gripper_base_link" />
+    <child
+      link="${prefix}left_inner_knuckle" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="0"
+      upper="0.85"
+      effort="50"
+      velocity="2" />
+    <mimic joint="${prefix}drive_joint" multiplier="1" offset="0" />
+  </joint>
+  <link
+    name="${prefix}right_outer_knuckle">
+    <inertial>
+      <origin
+        xyz="-3.1669E-14 -0.021559 0.015181"
+        rpy="0 0 0" />
+      <mass
+        value="0.033618" />
+      <inertia
+        ixx="1.9111E-05"
+        ixy="-1.8789E-17"
+        ixz="1.0986E-17"
+        iyy="6.6256E-06"
+        iyz="7.3008E-06"
+        izz="1.3185E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/right_outer_knuckle.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/right_outer_knuckle.STL"/>
+      </geometry>
+      <material name="Silver">
+        <color rgba="0.753 0.753 0.753 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/right_outer_knuckle.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/right_outer_knuckle.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="${prefix}right_outer_knuckle_joint"
+    type="revolute">
+    <origin
+      xyz="0 -0.035 0.059098"
+      rpy="0 0 0" />
+    <parent
+      link="${prefix}xarm_gripper_base_link" />
+    <child
+      link="${prefix}right_outer_knuckle" />
+    <axis
+      xyz="-1 0 0" />
+    <limit
+      lower="0"
+      upper="0.85"
+      effort="50"
+      velocity="2" />
+    <mimic joint="${prefix}drive_joint" multiplier="1" offset="0" />
+  </joint>
+  <link
+    name="${prefix}right_finger">
+    <inertial>
+      <origin
+        xyz="2.5618E-14 0.016413 0.029258"
+        rpy="0 0 0" />
+      <mass
+        value="0.048304" />
+      <inertia
+        ixx="1.7493E-05"
+        ixy="-5.0014E-19"
+        ixz="-7.5079E-18"
+        iyy="1.7225E-05"
+        iyz="-4.6435E-06"
+        izz="5.1466E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/right_finger.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/right_finger.STL"/>
+      </geometry>
+      <material name="Silver">
+        <color rgba="0.753 0.753 0.753 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/right_finger.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/right_finger.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="${prefix}right_finger_joint"
+    type="revolute">
+    <origin
+      xyz="0 -0.035465 0.042039"
+      rpy="0 0 0" />
+    <parent
+      link="${prefix}right_outer_knuckle" />
+    <child
+      link="${prefix}right_finger" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="0"
+      upper="0.85"
+      effort="50"
+      velocity="2" />
+    <mimic joint="${prefix}drive_joint" multiplier="1" offset="0" />
+  </joint>
+  <link
+    name="${prefix}right_inner_knuckle">
+    <inertial>
+      <origin
+        xyz="1.866E-06 -0.022047 0.026133"
+        rpy="0 0 0" />
+      <mass
+        value="0.023013" />
+      <inertia
+        ixx="6.0949E-06"
+        ixy="-6.0665E-11"
+        ixz="7.191E-11"
+        iyy="6.0197E-06"
+        iyz="2.7531E-06"
+        izz="5.0784E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/right_inner_knuckle.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/right_inner_knuckle.STL"/>
+      </geometry>
+      <material name="Silver">
+        <color rgba="0.753 0.753 0.753 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/gripper/right_inner_knuckle.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/gripper/right_inner_knuckle.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="${prefix}right_inner_knuckle_joint"
+    type="revolute">
+    <origin
+      xyz="0 -0.02 0.074098"
+      rpy="0 0 0" />
+    <parent
+      link="${prefix}xarm_gripper_base_link" />
+    <child
+      link="${prefix}right_inner_knuckle" />
+    <axis
+      xyz="-1 0 0" />
+    <limit
+      lower="0"
+      upper="0.85"
+      effort="50"
+      velocity="2" />
+    <mimic joint="${prefix}drive_joint" multiplier="1" offset="0" />
+  </joint>
+
+  <link name="${prefix}link_tcp" />
+
+  <joint
+    name="${prefix}joint_tcp"
+    type="fixed">
+    <origin
+      xyz="0 0 0.172"
+      rpy="0 0 0" />
+    <parent
+      link="${prefix}xarm_gripper_base_link" />
+    <child
+      link="${prefix}link_tcp" />
+  </joint>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/gripper/xarm_gripper_macro.xacro

@@ -0,0 +1,51 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="xarm_gripper" >  
+
+  <!-- xarm_gripper -->
+  <xacro:include filename="$(find xarm_description)/urdf/gripper/xarm_gripper.ros2_control.xacro" />
+  <xacro:include filename="$(find xarm_description)/urdf/gripper/xarm_gripper.urdf.xacro" />
+  <xacro:include filename="$(find xarm_description)/urdf/gripper/xarm_gripper.transmission.xacro" />
+  <xacro:include filename="$(find xarm_description)/urdf/gripper/xarm_gripper.gazebo.xacro" />
+
+  <xacro:macro name="xarm_gripper_macro" params="prefix:='' attach_to:='' ns:='xarm' xyz:='0 0 0' rpy:='0 0 0' effort_control:='false' velocity_control:='false' 
+    load_gazebo_plugin:='false' ros2_control_plugin:='uf_robot_hardware/UFRobotFakeSystemHardware' ros2_control_params:='' ">
+
+    <!-- gazebo_plugin -->
+    <xacro:if value="${load_gazebo_plugin}">
+      <xacro:include filename="$(find xarm_description)/urdf/common/common.gazebo.xacro" />
+      <xacro:gazebo_ros_control_plugin prefix="${prefix}" ros2_control_params="${ros2_control_params}"/>
+    </xacro:if>
+
+    <xacro:if value="${ros2_control_plugin != 'uf_robot_hardware/UFRobotSystemHardware'}">
+      <xacro:xarm_gripper_ros2_control prefix="${prefix}" ros2_control_plugin="${ros2_control_plugin}" />
+    </xacro:if>
+
+    <xacro:xarm_gripper_urdf prefix="${prefix}" attach_to="${attach_to}" xyz="${xyz}" rpy="${rpy}" />
+
+    <xacro:xarm_gripper_transmission prefix="${prefix}" hard_interface="${'EffortJointInterface' if effort_control else 'VelocityJointInterface' if velocity_control else 'PositionJointInterface'}" />
+
+    <xacro:xarm_gripper_gazebo prefix="${prefix}" />
+
+    <!-- mimic_joint_plugin has to be installed: -->
+    <xacro:mimic_joint_plugin_gazebo name_prefix="${prefix}left_finger_joint"
+      following_joint="${prefix}drive_joint" mimic_joint="${prefix}left_finger_joint"
+      has_pid="true" multiplier="1.0" max_effort="10.0" />
+
+    <xacro:mimic_joint_plugin_gazebo name_prefix="${prefix}left_inner_knuckle_joint"
+      following_joint="${prefix}drive_joint" mimic_joint="${prefix}left_inner_knuckle_joint"
+      has_pid="true" multiplier="1.0" max_effort="10.0" />
+
+    <xacro:mimic_joint_plugin_gazebo name_prefix="${prefix}right_outer_knuckle_joint"
+      following_joint="${prefix}drive_joint" mimic_joint="${prefix}right_outer_knuckle_joint"
+      has_pid="true" multiplier="1.0" max_effort="10.0" />
+
+    <xacro:mimic_joint_plugin_gazebo name_prefix="${prefix}right_finger_joint"
+      following_joint="${prefix}drive_joint" mimic_joint="${prefix}right_finger_joint"
+      has_pid="true" multiplier="1.0" max_effort="10.0" />
+
+    <xacro:mimic_joint_plugin_gazebo name_prefix="${prefix}right_inner_knuckle_joint"
+      following_joint="${prefix}drive_joint" mimic_joint="${prefix}right_inner_knuckle_joint"
+      has_pid="true" multiplier="1.0" max_effort="10.0" />
+
+  </xacro:macro>
+</robot>

src/custom_xarm_description/urdf/lite6/lite6.gazebo.xacro

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="lite6_gazebo" params="prefix">
+
+    <gazebo reference="${prefix}link_base">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link1">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link2">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link3">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link4">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link5">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link6">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/lite6/lite6.ros2_control.xacro

@@ -0,0 +1,110 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="lite6_ros2_control" params="prefix
+    velocity_control:='false'
+    ros2_control_plugin:='uf_robot_hardware/UFRobotSystemHardware'
+    hw_ns:='xarm' add_gripper:='false'
+    robot_ip:='' report_type:='normal' baud_checkset:='true' default_gripper_baud:=2000000
+    joint1_lower_limit:=${-2.0*pi}  joint1_upper_limit:=${2.0*pi}
+    joint2_lower_limit:=${-2.61799}  joint2_upper_limit:=${2.61799}
+    joint3_lower_limit:=${-0.061087}  joint3_upper_limit:=${5.235988}
+    joint4_lower_limit:=${-2.0*pi}  joint4_upper_limit:=${2.0*pi}
+    joint5_lower_limit:=${-2.1642}  joint5_upper_limit:=${2.1642}
+    joint6_lower_limit:=${-2.0*pi}  joint6_upper_limit:=${2.0*pi}">
+    <ros2_control name="${prefix}${ros2_control_plugin}" type="system">
+      <hardware>
+        <plugin>${ros2_control_plugin}</plugin>
+        <xacro:if value="${ros2_control_plugin == 'uf_robot_hardware/UFRobotSystemHardware'}">
+          <param name="hw_ns">${prefix}${hw_ns}</param>
+          <param name="velocity_control">${velocity_control}</param>
+          <param name="prefix">P${prefix}</param>
+          <param name="robot_ip">R${robot_ip}</param>
+          <param name="report_type">${report_type}</param>
+          <param name="dof">6</param>
+          <param name="baud_checkset">${baud_checkset}</param>
+          <param name="default_gripper_baud">${default_gripper_baud}</param>
+          <param name="robot_type">lite</param>
+          <param name="add_gripper">${add_gripper}</param>
+        </xacro:if>
+      </hardware>
+      <joint name="${prefix}joint1">
+        <command_interface name="position">
+          <param name="min">${joint1_lower_limit}</param>
+          <param name="max">${joint1_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint2">
+        <command_interface name="position">
+          <param name="min">${joint2_lower_limit}</param>
+          <param name="max">${joint2_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint3">
+        <command_interface name="position">
+          <param name="min">${joint3_lower_limit}</param>
+          <param name="max">${joint3_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint4">
+        <command_interface name="position">
+          <param name="min">${joint4_lower_limit}</param>
+          <param name="max">${joint4_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint5">
+        <command_interface name="position">
+          <param name="min">${joint5_lower_limit}</param>
+          <param name="max">${joint5_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint6">
+        <command_interface name="position">
+          <param name="min">${joint6_lower_limit}</param>
+          <param name="max">${joint6_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+    </ros2_control>
+  </xacro:macro>
+</robot>

src/custom_xarm_description/urdf/lite6/lite6.transmission.xacro

@@ -0,0 +1,75 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="lite6_transmission"
+    params="prefix hard_interface:=EffortJointInterface reduction:=100">
+
+    <transmission name="${prefix}tran1">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint1">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor1">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran2">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint2">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor2">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran3">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint3">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor3">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran4">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint4">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor3">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran5">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint5">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor5">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran6">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint6">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor6">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/lite6/lite6.urdf.xacro

@@ -0,0 +1,412 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="lite6_urdf" params="prefix
+    joint1_lower_limit:=${-2.0*pi}  joint1_upper_limit:=${2.0*pi}
+    joint2_lower_limit:=${-2.61799}  joint2_upper_limit:=${2.61799}
+    joint3_lower_limit:=${-0.061087}  joint3_upper_limit:=${5.235988}
+    joint4_lower_limit:=${-2.0*pi}  joint4_upper_limit:=${2.0*pi}
+    joint5_lower_limit:=${-2.1642}  joint5_upper_limit:=${2.1642}
+    joint6_lower_limit:=${-2.0*pi}  joint6_upper_limit:=${2.0*pi}">
+
+    <material name="${prefix}White">
+      <color rgba="1.0 1.0 1.0 1.0"/>
+    </material>
+
+    <material name="${prefix}Silver">
+      <color rgba="0.753 0.753 0.753 1.0"/>
+    </material>
+    
+    <link name="${prefix}link_base">
+      <inertial>
+        <origin
+          xyz="-0.00829544579053192 3.26357432323433E-05 0.0631194584987089"
+          rpy="0 0 0" />
+        <mass
+          value="1.65393501783165" />
+        <inertia
+          ixx="0"
+          ixy="0"
+          ixz="0"
+          iyy="0"
+          iyz="0"
+          izz="0" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/base.stl"/>
+        </geometry>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/base.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link1">
+      <inertial>
+        <origin
+          xyz="-0.00036 0.03788 -0.0027"
+          rpy="0 0 0" />
+        <mass
+          value="1.169" />
+        <inertia
+          ixx="1.45164E-03"
+          ixy="1.24E-05"
+          ixz="-6.7E-06"
+          iyy="8.873E-04"
+          iyz="1.255E-04"
+          izz="1.31993E-03" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link1.stl"/>
+        </geometry>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link1.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${prefix}joint1" type="revolute">
+      <origin
+        xyz="0 0 0.2435"
+        rpy="0 0 0" />
+      <parent
+        link="${prefix}link_base" />
+      <child
+        link="${prefix}link1" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="${joint1_lower_limit}"
+        upper="${joint1_upper_limit}"
+        effort="50.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+    <link name="${prefix}link2">
+      <inertial>
+        <origin
+          xyz="0.178 0.0 0.0576"
+          rpy="0 0 0" />
+        <mass
+          value="1.192" />
+        <inertia
+          ixx="1.5854E-03"
+          ixy="-6.766E-06"
+          ixz="-1.15136E-03"
+          iyy="5.6097E-03"
+          iyz="1.14E-06"
+          izz="4.85E-03" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link2.stl"/>
+        </geometry>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link2.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${prefix}joint2" type="revolute">
+      <origin
+        xyz="0 0 0"
+        rpy="1.5708 -1.5708 3.1416" />
+      <parent
+        link="${prefix}link1" />
+      <child
+        link="${prefix}link2" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="${joint2_lower_limit}"
+        upper="${joint2_upper_limit}"
+        effort="50.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+    <link name="${prefix}link3">
+      <inertial>
+        <origin
+          xyz="0.07285 -0.030 -0.0009"
+          rpy="0 0 0" />
+        <mass
+          value="0.930" />
+        <inertia
+          ixx="8.861E-04"
+          ixy="-3.9287E-04"
+          ixz="7.066E-05"
+          iyy="1.5785E-03"
+          iyz="-2.445E-05"
+          izz="1.84677E-03" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link3.stl"/>
+        </geometry>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link3.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${prefix}joint3" type="revolute">
+      <origin
+        xyz="0.2002 0 0"
+        rpy="-3.1416 0 1.5708" />
+      <parent
+        link="${prefix}link2" />
+      <child
+        link="${prefix}link3" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="${joint3_lower_limit}"
+        upper="${joint3_upper_limit}"
+        effort="32.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+    <link name="${prefix}link4">
+      <inertial>
+        <origin
+          xyz="-0.0004 -0.0275 -0.0817"
+          rpy="0 0 0" />
+        <mass
+          value="1.31" />
+        <inertia
+          ixx="3.705E-03"
+          ixy="-2.0E-06"
+          ixz="7.17E-06"
+          iyy="3.0455E-03"
+          iyz="-9.3188E-04"
+          izz="1.5413E-03" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link4.stl"/>
+        </geometry>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link4.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${prefix}joint4" type="revolute">
+      <origin
+        xyz="0.087 -0.22761 0"
+        rpy="1.5708 0 0" />
+      <parent
+        link="${prefix}link3" />
+      <child
+        link="${prefix}link4" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="${joint4_lower_limit}"
+        upper="${joint4_upper_limit}"
+        effort="32.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+    <link name="${prefix}link5">
+      <inertial>
+        <origin
+          xyz="0.0 0.010 0.0019"
+          rpy="0 0 0" />
+        <mass
+          value="0.784" />
+        <inertia
+          ixx="5.668E-04"
+          ixy="6E-07"
+          ixz="-5.3E-06"
+          iyy="5.077E-04"
+          iyz="-4.8E-07"
+          izz="5.3E-04" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link5.stl"/>
+        </geometry>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link5.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${prefix}joint5" type="revolute">
+      <origin
+        xyz="0 0 0"
+        rpy="1.5708 0 0" />
+      <parent
+        link="${prefix}link4" />
+      <child
+        link="${prefix}link5" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="${joint5_lower_limit}"
+        upper="${joint5_upper_limit}"
+        effort="32.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+    <link name="${prefix}link6">
+      <inertial>
+        <origin
+          xyz="0.0 -0.00194 -0.0102"
+          rpy="0 0 0" />
+        <mass
+          value="0.180" />
+        <inertia
+          ixx="7.726E-05"
+          ixy="1E-06"
+          ixz="4E-07"
+          iyy="8.5665E-05"
+          iyz="-6E-07"
+          izz="1.4814E-04" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link6.stl"/>
+        </geometry>
+        <material name="${prefix}Silver" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/link6.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${prefix}joint6" type="revolute">
+      <origin
+        xyz="0 0.0625 0"
+        rpy="-1.5708 0 0" />
+      <parent
+        link="${prefix}link5" />
+      <child
+        link="${prefix}link6" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="${joint6_lower_limit}"
+        upper="${joint6_upper_limit}"
+        effort="20.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+
+    <!-- <link name="${prefix}link_eef" />
+
+    <joint
+      name="${prefix}joint_eef"
+      type="fixed">
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <parent
+        link="${prefix}link6" />
+      <child
+        link="${prefix}link_eef" />
+    </joint> -->
+
+
+    <link name="${prefix}pen_link">
+    <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+            <cylinder radius="0.005" length="0.2"/>
+        </geometry>
+        <material name="Cyan">
+            <color rgba="0.0 1.0 1.0 1.0"/>
+        </material>
+    </visual>
+
+    <collision>
+        <geometry>
+            <box size="0 0 0"/>
+        </geometry>
+    </collision>
+    </link>
+
+    <joint name="${prefix}pen_joint" type="fixed">
+      <parent link="${prefix}link6"/>
+      <child link="${prefix}pen_link"/>
+    </joint>
+
+  <!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
+  <gazebo>
+    <plugin name="pen_position" filename="libgazebo_ros_p3d.so">
+        <!-- <alwaysOn>true</alwaysOn> -->
+      <ros>
+        <remapping>odom:=pen_position</remapping>
+      </ros>
+      <frame_name>world</frame_name>
+      <!-- <body_name>pen_link</body_name> -->
+      <body_name>${prefix}link6</body_name>
+      <!-- <body_name>${prefix}pen_link</body_name> -->
+      <!--       topic name is always /odom -->
+      <!-- <topic_name>pen_position</topic_name> -->
+      <!--       Update rate in Hz -->
+      <update_rate>1000</update_rate>
+      <!-- <xyzOffsets>0 0 0</xyzOffsets> -->
+      <!-- <rpyOffsets>0 0 0</rpyOffsets> -->
+    </plugin>
+  </gazebo>
+
+  </xacro:macro>
+</robot>

src/custom_xarm_description/urdf/lite6/lite6_robot_macro.xacro

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="lite6" >
+  <xacro:macro name="lite6_robot" params="prefix:='' namespace:='xarm' limited:='false' effort_control:='false' 
+    velocity_control:='false' attach_to:='world' xyz:='0 0 0' rpy:='0 0 0' load_gazebo_plugin:='false' 
+    ros2_control_plugin:='uf_robot_hardware/UFRobotSystemHardware' ros2_control_params:='' add_gripper:='false' 
+    robot_ip:='' report_type:='normal' baud_checkset:='true' default_gripper_baud:=2000000 ">
+
+    <!-- include lite6 relative macros: -->
+    <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.ros2_control.xacro" />
+    <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.urdf.xacro" />
+    <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.transmission.xacro" />
+    <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.gazebo.xacro" />
+
+    <!-- gazebo_plugin -->
+    <xacro:if value="${load_gazebo_plugin}">
+      <xacro:include filename="$(find custom_xarm_description)/urdf/common/common.gazebo.xacro" />
+      <xacro:gazebo_ros_control_plugin  prefix="${prefix}" ros2_control_params="${ros2_control_params}"/>
+    </xacro:if>
+
+    <!-- add one world link if no 'attach_to' specified  -->
+    <xacro:if value="${attach_to == 'world'}">
+      <link name="world" />
+    </xacro:if>
+
+    <joint name="${prefix}world_joint" type="fixed">
+      <parent link="${attach_to}" />
+      <child link = "${prefix}link_base" />
+      <origin xyz="${xyz}" rpy="${rpy}" />
+    </joint>
+
+    <xacro:if value="${limited}">
+      <xacro:lite6_ros2_control prefix="${prefix}"
+        velocity_control="${velocity_control}"
+        ros2_control_plugin="${ros2_control_plugin}"
+        hw_ns="${namespace}" add_gripper="${add_gripper}"
+        robot_ip="${robot_ip}" report_type="${report_type}"
+        baud_checkset="${baud_checkset}" default_gripper_baud="${default_gripper_baud}"
+        joint1_lower_limit="${-pi}"   joint1_upper_limit="${pi}"
+        joint2_lower_limit="${-2.61799}"  joint2_upper_limit="${2.61799}"
+        joint3_lower_limit="${-0.061087}" joint3_upper_limit="${5.235988}"
+        joint4_lower_limit="${-pi}"   joint4_upper_limit="${pi}"
+        joint5_lower_limit="${-2.1642}"   joint5_upper_limit="${2.1642}"
+        joint6_lower_limit="${-pi}"   joint6_upper_limit="${pi}"/>
+      <xacro:lite6_urdf prefix="${prefix}"
+        joint1_lower_limit="${-pi}"   joint1_upper_limit="${pi}"
+        joint2_lower_limit="${-2.61799}"  joint2_upper_limit="${2.61799}"
+        joint3_lower_limit="${-0.061087}" joint3_upper_limit="${5.235988}"
+        joint4_lower_limit="${-pi}"   joint4_upper_limit="${pi}"
+        joint5_lower_limit="${-2.1642}"   joint5_upper_limit="${2.1642}"
+        joint6_lower_limit="${-pi}"   joint6_upper_limit="${pi}"/>
+    </xacro:if>
+    <xacro:unless value="${limited}">
+      <xacro:lite6_ros2_control prefix="${prefix}" velocity_control="${velocity_control}"
+        ros2_control_plugin="${ros2_control_plugin}"
+        hw_ns="${namespace}" add_gripper="${add_gripper}"
+        robot_ip="${robot_ip}" report_type="${report_type}"
+        baud_checkset="${baud_checkset}" default_gripper_baud="${default_gripper_baud}" />
+      <xacro:lite6_urdf prefix="${prefix}"/>
+    </xacro:unless>
+
+    <xacro:lite6_transmission prefix="${prefix}" hard_interface="${'EffortJointInterface' if effort_control else 'VelocityJointInterface' if velocity_control else 'PositionJointInterface'}" />
+    
+    <xacro:lite6_gazebo prefix="${prefix}" />
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/other/other_geometry.urdf.xacro

@@ -0,0 +1,122 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="xarm_vacuum_gripper">
+<!--
+  Author: Jason Peng <jason@ufactory.cc>
+-->
+  <xacro:macro name="other_geometry" params="prefix:='' attach_to:='' rpy:='0 0 0' xyz:='0 0 0' 
+    geometry_type:='box' 
+    geometry_mass:='0.1' 
+    geometry_height:='0.1'
+    geometry_radius:='0.1'
+    geometry_length:='0.1'
+    geometry_width:='0.1'
+    geometry_mesh_filename:=''
+    geometry_mesh_origin_xyz:='0 0 0'
+    geometry_mesh_origin_rpy:='0 0 0'
+    geometry_mesh_tcp_xyz:='0 0 0'
+    geometry_mesh_tcp_rpy:='0 0 0'
+    ">
+    <xacro:unless value="${attach_to == ''}">
+      <joint name="${prefix}other_geometry_fix" type="fixed">
+        <parent link="${attach_to}"/>
+        <child link="${prefix}other_geometry_link"/>
+        <origin xyz="${xyz}" rpy="${rpy}"/>
+      </joint>
+    </xacro:unless>
+
+    <xacro:if value="${geometry_type == 'mesh'}">
+      <xacro:property name="_origin_xyz" value="${geometry_mesh_origin_xyz}"/>
+      <xacro:property name="_origin_rpy" value="${geometry_mesh_origin_rpy}"/>
+      <xacro:property name="_tcp_xyz" value="${geometry_mesh_tcp_xyz}"/>
+      <xacro:property name="_tcp_rpy" value="${geometry_mesh_tcp_rpy}"/>
+    </xacro:if>
+    <xacro:unless value="${geometry_type == 'mesh'}">
+      <xacro:property name="_origin_xyz" value="0 0 ${geometry_height / 2 if geometry_type != 'sphere' else geometry_radius}"/>
+      <xacro:property name="_origin_rpy" value="0 0 0"/>
+      <xacro:property name="_tcp_xyz" value="0 0 ${geometry_height if geometry_type != 'sphere' else geometry_radius * 2}"/>
+      <xacro:property name="_tcp_rpy" value="0 0 0"/>
+    </xacro:unless>
+
+    <xacro:if value="${geometry_mesh_filename.startswith('file:///') or geometry_mesh_filename.startswith('package://')}">
+      <xacro:property name="_mesh_filename" value="${geometry_mesh_filename}"/>
+    </xacro:if>
+    <xacro:unless value="${geometry_mesh_filename.startswith('file:///') or geometry_mesh_filename.startswith('package://')}">
+      <xacro:property name="_mesh_filename" value="file:///$(find xarm_description)/meshes/other/${geometry_mesh_filename}"/>
+    </xacro:unless>
+
+    <link
+      name="${prefix}other_geometry_link">
+      <inertial>
+        <origin
+          xyz="0.0 0.0 0.055"
+          rpy="0 0 0" />
+        <mass
+          value="${geometry_mass}" />
+        <inertia
+          ixx="0.00047106"
+          ixy="3.9292E-07"
+          ixz="2.6537E-06"
+          iyy="0.00033072"
+          iyz="-1.0975E-05"
+          izz="0.00025642" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="${_origin_xyz}"
+          rpy="${_origin_rpy}" />
+        <geometry>
+          <xacro:if value="${geometry_type == 'mesh'}">
+            <mesh filename="${_mesh_filename}"/>
+          </xacro:if>
+          <xacro:if value="${geometry_type == 'sphere'}">
+            <sphere radius="${geometry_radius}"/>
+          </xacro:if>
+          <xacro:if value="${geometry_type == 'cylinder'}">
+            <cylinder length="${geometry_height}" radius="${geometry_radius}"/>
+          </xacro:if>
+          <xacro:if value="${geometry_type != 'mesh' and geometry_type != 'sphere' and geometry_type != 'cylinder'}">
+            <box size="${geometry_length} ${geometry_width} ${geometry_height}"/>
+          </xacro:if>
+        </geometry>
+        <material name="White">
+          <color rgba="1.0 1.0 1.0 1.0"/>
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="${_origin_xyz}"
+          rpy="${_origin_rpy}" />
+        <geometry>
+          <xacro:if value="${geometry_type == 'mesh'}">
+            <mesh filename="${_mesh_filename}"/>
+          </xacro:if>
+          <xacro:if value="${geometry_type == 'sphere'}">
+            <sphere radius="${geometry_radius}"/>
+          </xacro:if>
+          <xacro:if value="${geometry_type == 'cylinder'}">
+            <cylinder length="${geometry_height}" radius="${geometry_radius}"/>
+          </xacro:if>
+          <xacro:if value="${geometry_type != 'mesh' and geometry_type != 'sphere' and geometry_type != 'cylinder'}">
+            <box size="${geometry_length} ${geometry_width} ${geometry_height}"/>
+          </xacro:if>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name="${prefix}link_tcp" />
+
+    <joint
+      name="${prefix}joint_tcp"
+      type="fixed">
+      <origin
+        xyz="${_tcp_xyz}"
+        rpy="${_tcp_rpy}" />
+      <parent
+        link="${prefix}other_geometry_link" />
+      <child
+        link="${prefix}link_tcp" />
+    </joint>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/vacuum_gripper/lite_vacuum_gripper.urdf.xacro

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="uflite_vacuum_gripper">
+<!--
+  Author: Jason Peng <jason@ufactory.cc>
+-->
+  <xacro:macro name="uflite_vacuum_gripper_urdf" params="prefix:='' attach_to:='' rpy:='0 0 0' xyz:='0 0 0' ">
+  <xacro:unless value="${attach_to == ''}">
+    <joint name="${prefix}vacuum_gripper_fix" type="fixed">
+      <parent link="${attach_to}"/>
+      <child link="${prefix}uflite_vacuum_gripper_link"/>
+      <origin xyz="${xyz}" rpy="${rpy}"/>
+    </joint>
+  </xacro:unless>
+  <link
+    name="${prefix}uflite_vacuum_gripper_link">
+    <inertial>
+      <origin
+        xyz="0.0 0.0 0.030"
+        rpy="0 0 0" />
+      <mass
+        value="0.14" />
+      <inertia
+        ixx="0.00047106"
+        ixy="3.9292E-07"
+        ixz="2.6537E-06"
+        iyy="0.00033072"
+        iyz="-1.0975E-05"
+        izz="0.00025642" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="file:///$(find xarm_description)/meshes/lite6/visual/vacuum_gripper_lite.stl"/>
+      </geometry>
+      <material name="White">
+        <color rgba="1.0 1.0 1.0 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="file:///$(find xarm_description)/meshes/lite6/collision/vacuum_gripper_lite.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="${prefix}link_tcp" />
+
+  <joint
+    name="${prefix}joint_tcp"
+    type="fixed">
+    <origin
+      xyz="0 0 0.061"
+      rpy="0 0 0" />
+    <parent
+      link="${prefix}uflite_vacuum_gripper_link" />
+    <child
+      link="${prefix}link_tcp" />
+  </joint>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/vacuum_gripper/xarm_vacuum_gripper.urdf.xacro

@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="xarm_vacuum_gripper">
+<!--
+  Author: Jason Peng <jason@ufactory.cc>
+-->
+  <xacro:macro name="xarm_vacuum_gripper_urdf" params="prefix:='' attach_to:='' rpy:='0 0 0' xyz:='0 0 0' effort_control:='false' velocity_control:='false' ">
+  <xacro:unless value="${attach_to == ''}">
+    <joint name="${prefix}vacuum_gripper_fix" type="fixed">
+      <parent link="${attach_to}"/>
+      <child link="${prefix}xarm_vacuum_gripper_link"/>
+      <origin xyz="${xyz}" rpy="${rpy}"/>
+    </joint>
+  </xacro:unless>
+  <link
+    name="${prefix}xarm_vacuum_gripper_link">
+    <inertial>
+      <origin
+        xyz="0.0 0.0 0.055"
+        rpy="0 0 0" />
+      <mass
+        value="0.656" />
+      <inertia
+        ixx="0.00047106"
+        ixy="3.9292E-07"
+        ixz="2.6537E-06"
+        iyy="0.00033072"
+        iyz="-1.0975E-05"
+        izz="0.00025642" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/vacuum_gripper/visual/vacuum_gripper.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/vacuum_gripper/visual/vacuum_gripper.STL"/>
+      </geometry>
+      <material name="White">
+        <color rgba="1.0 1.0 1.0 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <!-- <mesh filename="package://xarm_description/meshes/vacuum_gripper/collision/vacuum_gripper.STL" /> -->
+        <mesh filename="file:///$(find xarm_description)/meshes/vacuum_gripper/collision/vacuum_gripper.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="${prefix}link_tcp" />
+
+  <joint
+    name="${prefix}joint_tcp"
+    type="fixed">
+    <origin
+      xyz="0 0 0.126"
+      rpy="0 0 0" />
+    <parent
+      link="${prefix}xarm_vacuum_gripper_link" />
+    <child
+      link="${prefix}link_tcp" />
+  </joint>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/xarm5/xarm5.gazebo.xacro

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="xarm5_gazebo" params="prefix">
+
+    <gazebo reference="${prefix}link_base">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link1">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link2">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link3">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link4">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link5">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+    
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/xarm5/xarm5.ros2_control.xacro

@@ -0,0 +1,96 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="xarm5_ros2_control" params="prefix
+    velocity_control:='false'
+    ros2_control_plugin:='uf_robot_hardware/UFRobotSystemHardware'
+    hw_ns:='xarm' add_gripper:='false'
+    robot_ip:='' report_type:='normal' baud_checkset:='true' default_gripper_baud:=2000000
+    joint1_lower_limit:=${-2.0*pi}  joint1_upper_limit:=${2.0*pi}
+    joint2_lower_limit:=${-2.059}  joint2_upper_limit:=${2.0944}
+    joint3_lower_limit:=${-3.927}  joint3_upper_limit:=${0.19198}
+    joint4_lower_limit:=${-1.69297}  joint4_upper_limit:=${pi}
+    joint5_lower_limit:=${-2.0*pi}  joint5_upper_limit:=${2.0*pi}">
+    <ros2_control name="${prefix}${ros2_control_plugin}" type="system">
+      <hardware>
+        <plugin>${ros2_control_plugin}</plugin>
+        <xacro:if value="${ros2_control_plugin == 'uf_robot_hardware/UFRobotSystemHardware'}">
+          <param name="hw_ns">${prefix}${hw_ns}</param>
+          <param name="velocity_control">${velocity_control}</param>
+          <param name="prefix">P${prefix}</param>
+          <param name="robot_ip">R${robot_ip}</param>
+          <param name="report_type">${report_type}</param>
+          <param name="dof">5</param>
+          <param name="baud_checkset">${baud_checkset}</param>
+          <param name="default_gripper_baud">${default_gripper_baud}</param>
+          <param name="robot_type">xarm</param>
+          <param name="add_gripper">${add_gripper}</param>
+        </xacro:if>
+      </hardware>
+      <joint name="${prefix}joint1">
+        <command_interface name="position">
+          <param name="min">${joint1_lower_limit}</param>
+          <param name="max">${joint1_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint2">
+        <command_interface name="position">
+          <param name="min">${joint2_lower_limit}</param>
+          <param name="max">${joint2_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint3">
+        <command_interface name="position">
+          <param name="min">${joint3_lower_limit}</param>
+          <param name="max">${joint3_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint4">
+        <command_interface name="position">
+          <param name="min">${joint4_lower_limit}</param>
+          <param name="max">${joint4_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint5">
+        <command_interface name="position">
+          <param name="min">${joint5_lower_limit}</param>
+          <param name="max">${joint5_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+    </ros2_control>
+  </xacro:macro>
+</robot>

src/custom_xarm_description/urdf/xarm5/xarm5.transmission.xacro

@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="xarm5_transmission"
+    params="prefix hard_interface:=EffortJointInterface reduction:=100">
+
+    <transmission name="${prefix}tran1">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint1">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor1">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran2">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint2">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor2">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran3">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint3">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor3">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran4">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint4">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor3">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran5">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint5">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor5">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/xarm5/xarm5.urdf.xacro

@@ -0,0 +1,364 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="xarm5_urdf" params="prefix
+    joint1_lower_limit:=${-2.0*pi}  joint1_upper_limit:=${2.0*pi}
+    joint2_lower_limit:=${-2.059}  joint2_upper_limit:=${2.0944}
+    joint3_lower_limit:=${-3.927}  joint3_upper_limit:=${0.19198}
+    joint4_lower_limit:=${-1.69297}  joint4_upper_limit:=${pi}
+    joint5_lower_limit:=${-2.0*pi}  joint5_upper_limit:=${2.0*pi}">
+
+    <material name="${prefix}Black">
+      <color rgba="0.0 0.0 0.0 1.0"/>
+    </material>
+
+    <material name="${prefix}Red">
+      <color rgba="0.8 0.0 0.0 1.0"/>
+    </material>
+
+    <material name="${prefix}White">
+      <color rgba="1.0 1.0 1.0 1.0"/>
+    </material>
+
+    <material name="${prefix}Silver">
+      <color rgba="0.753 0.753 0.753 1.0"/>
+    </material>
+
+    <link
+      name="${prefix}link_base">
+      <inertial>
+        <origin
+          xyz="-0.00218 -0.00023 0.08604"
+          rpy="0 0 0" />
+        <mass
+          value="1.95" />
+        <inertia
+          ixx="0.004805"
+          ixy="-8.33E-06"
+          ixz="0.00026847"
+          iyy="0.0050349"
+          iyz="7.23E-06"
+          izz="0.0025418" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/base_link.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/base_link.STL"/>
+        </geometry>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/base_link.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/base_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <link
+      name="${prefix}link1">
+      <inertial>
+        <origin
+          xyz="0.00016 0.02064 -0.01556"
+          rpy="0 0 0" />
+        <mass
+          value="1.85" />
+        <inertia
+          ixx="0.0046885"
+          ixy="-8.67E-06"
+          ixz="2.439E-05"
+          iyy="0.0041688"
+          iyz="0.0006114"
+          izz="0.0024758" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/link1.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/link1.STL"/>
+        </geometry>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/link1.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/link1.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="${prefix}joint1"
+      type="revolute">
+      <origin
+        xyz="0 0 0.267"
+        rpy="0 0 0" />
+      <parent
+        link="${prefix}link_base" />
+      <child
+        link="${prefix}link1" />
+      <axis
+        xyz="0 0 1" />
+     <limit
+        lower="${joint1_lower_limit}"
+        upper="${joint1_upper_limit}"
+        effort="50"
+        velocity="3.14" />
+      <dynamics
+        damping="10"
+        friction="1" />
+    </joint>
+    <link
+      name="${prefix}link2">
+      <inertial>
+        <origin
+          xyz="0.0351 -0.21375 0.02835"
+          rpy="0 0 0" />
+        <mass
+          value="1.71" />
+        <inertia
+          ixx="0.024904"
+          ixy="-0.004271"
+          ixz="-0.0008356"
+          iyy="0.004901"
+          iyz="0.0052393"
+          izz="0.0238188" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/link2.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/link2.STL"/>
+        </geometry>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/link2.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/link2.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="${prefix}joint2"
+      type="revolute">
+      <origin
+        xyz="0 0 0"
+        rpy="-1.5708 0 0" />
+      <parent
+        link="${prefix}link1" />
+      <child
+        link="${prefix}link2" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="${joint2_lower_limit}"
+        upper="${joint2_upper_limit}"
+        effort="50"
+        velocity="3.14" />
+      <dynamics
+        damping="10"
+        friction="1" />
+    </joint>
+    <link
+      name="${prefix}link3">
+      <inertial>
+        <origin
+          xyz="0.06716 0.2299 -0.00249"
+          rpy="0 0 0" />
+        <mass
+          value="1.74" />
+        <inertia
+          ixx="0.0335922"
+          ixy="0.0040788"
+          ixz="-0.0014658"
+          iyy="0.0055857"
+          iyz="-0.0080609"
+          izz="0.0318905" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/link3.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/link3.STL"/>
+        </geometry>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/link3.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/link3.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="${prefix}joint3"
+      type="revolute">
+      <origin
+        xyz="0.0535 -0.2845 0"
+        rpy="0 0 0" />
+      <parent
+        link="${prefix}link2" />
+      <child
+        link="${prefix}link3" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="${joint3_lower_limit}"
+        upper="${joint3_upper_limit}"
+        effort="30"
+        velocity="3.14" />
+      <dynamics
+        damping="5"
+        friction="1" />
+    </joint>
+    <link
+      name="${prefix}link4">
+      <inertial>
+        <origin
+          xyz="0.0636 0.02203 0.00355"
+          rpy="0 0 0" />
+        <mass
+          value="1.13" />
+        <inertia
+          ixx="0.0010927"
+          ixy="0.0003076"
+          ixz="-0.0002076"
+          iyy="0.0016058"
+          iyz="-8.863E-05"
+          izz="0.0019148" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/link4.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/link4.STL"/>
+        </geometry>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/link4.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/link4.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="${prefix}joint4"
+      type="revolute">
+      <origin
+        xyz="0.0775 0.3425 0"
+        rpy="0 0 0" />
+      <parent
+        link="${prefix}link3" />
+      <child
+        link="${prefix}link4" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="${joint4_lower_limit}"
+        upper="${joint4_upper_limit}"
+        effort="20"
+        velocity="3.14" />
+      <dynamics
+        damping="3"
+        friction="1" />
+    </joint>
+    <link
+      name="${prefix}link5">
+      <inertial>
+        <origin
+          xyz="-3E-05 -0.00677 -0.01098"
+          rpy="0 0 0" />
+        <mass
+          value="0.16" />
+        <inertia
+          ixx="9.341E-05"
+          ixy="0.0"
+          ixz="0.0"
+          iyy="5.87E-05"
+          iyz="3.57E-06"
+          izz="1.33E-05" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/link5.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/link5.STL"/>
+        </geometry>
+        <material name="${prefix}Silver" />
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm5/visual/link5.STL" /> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm5/visual/link5.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="${prefix}joint5"
+      type="revolute">
+      <origin
+        xyz="0.076 0.097 0"
+        rpy="-1.5708 0 0" />
+      <parent
+        link="${prefix}link4" />
+      <child
+        link="${prefix}link5" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="${joint5_lower_limit}"
+        upper="${joint5_upper_limit}"
+        effort="20"
+        velocity="3.14" />
+      <dynamics
+        damping="3"
+        friction="1" />
+    </joint>
+
+    <!-- <link name="${prefix}link_eef" />
+
+    <joint
+      name="${prefix}joint_eef"
+      type="fixed">
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <parent
+        link="${prefix}link5" />
+      <child
+        link="${prefix}link_eef" />
+    </joint> -->
+  
+  </xacro:macro>
+</robot>

src/custom_xarm_description/urdf/xarm5/xarm5_robot_macro.xacro

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="xarm5" >
+  <xacro:macro name="xarm5_robot" params="prefix:='' namespace:='xarm' limited:='false' effort_control:='false' 
+    velocity_control:='false' attach_to:='world' xyz:='0 0 0' rpy:='0 0 0' load_gazebo_plugin:='false' 
+    ros2_control_plugin:='uf_robot_hardware/UFRobotSystemHardware' ros2_control_params:='' add_gripper:='false' 
+    robot_ip:='' report_type:='normal' baud_checkset:='true' default_gripper_baud:=2000000 ">
+
+    <!-- include xarm5 relative macros: -->
+    <xacro:include filename="$(find xarm_description)/urdf/xarm5/xarm5.ros2_control.xacro" />
+    <xacro:include filename="$(find xarm_description)/urdf/xarm5/xarm5.urdf.xacro" />
+    <xacro:include filename="$(find xarm_description)/urdf/xarm5/xarm5.transmission.xacro" />
+    <xacro:include filename="$(find xarm_description)/urdf/xarm5/xarm5.gazebo.xacro" />
+
+    <!-- gazebo_plugin -->
+    <xacro:if value="${load_gazebo_plugin}">
+      <xacro:include filename="$(find xarm_description)/urdf/common/common.gazebo.xacro" />
+      <xacro:gazebo_ros_control_plugin prefix="${prefix}" ros2_control_params="${ros2_control_params}"/>
+    </xacro:if>
+
+    <!-- add one world link if no 'attach_to' specified  -->
+    <xacro:if value="${attach_to == 'world'}">
+      <link name="world" />
+    </xacro:if>
+
+    <joint name="${prefix}world_joint" type="fixed">
+      <parent link="${attach_to}" />
+      <child link = "${prefix}link_base" />
+      <origin xyz="${xyz}" rpy="${rpy}" />
+    </joint>
+
+    <xacro:if value="${limited}">
+      <xacro:xarm5_ros2_control prefix="${prefix}"
+        velocity_control="${velocity_control}"
+        ros2_control_plugin="${ros2_control_plugin}"
+        hw_ns="${namespace}" add_gripper="${add_gripper}"
+        robot_ip="${robot_ip}" report_type="${report_type}"
+        baud_checkset="${baud_checkset}" default_gripper_baud="${default_gripper_baud}"
+        joint1_lower_limit="${-pi*0.99}" joint1_upper_limit="${pi*0.99}"
+        joint2_lower_limit="${-2.059}" joint2_upper_limit="${2.0944}"
+        joint3_lower_limit="${-pi*0.99}" joint3_upper_limit="${0.19198}"
+        joint4_lower_limit="${-1.69297}" joint4_upper_limit="${pi*0.99}"
+        joint5_lower_limit="${-pi*0.99}" joint5_upper_limit="${pi*0.99}"/>
+      <xacro:xarm5_urdf prefix="${prefix}"
+        joint1_lower_limit="${-pi*0.99}" joint1_upper_limit="${pi*0.99}"
+        joint2_lower_limit="${-2.059}" joint2_upper_limit="${2.0944}"
+        joint3_lower_limit="${-pi*0.99}" joint3_upper_limit="${0.19198}"
+        joint4_lower_limit="${-1.69297}" joint4_upper_limit="${pi*0.99}"
+        joint5_lower_limit="${-pi*0.99}" joint5_upper_limit="${pi*0.99}"/>
+    </xacro:if>
+    <xacro:unless value="${limited}">
+      <xacro:xarm5_ros2_control prefix="${prefix}" velocity_control="${velocity_control}"
+        ros2_control_plugin="${ros2_control_plugin}"
+        hw_ns="${namespace}" add_gripper="${add_gripper}"
+        robot_ip="${robot_ip}" report_type="${report_type}"
+        baud_checkset="${baud_checkset}" default_gripper_baud="${default_gripper_baud}" />
+      <xacro:xarm5_urdf prefix="${prefix}"/>
+    </xacro:unless>
+
+    <xacro:xarm5_transmission prefix="${prefix}" hard_interface="${'EffortJointInterface' if effort_control else 'VelocityJointInterface' if velocity_control else 'PositionJointInterface'}" />
+
+    <xacro:xarm5_gazebo prefix="${prefix}" />
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/xarm6/xarm6.gazebo.xacro

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="xarm6_gazebo" params="prefix">
+
+    <gazebo reference="${prefix}link_base">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link1">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link2">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link3">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link4">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link5">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+    <gazebo reference="${prefix}link6">
+      <selfCollide>true</selfCollide>
+    </gazebo>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/xarm6/xarm6.ros2_control.xacro

@@ -0,0 +1,118 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="xarm6_ros2_control" params="prefix
+    velocity_control:='false'
+    ros2_control_plugin:='uf_robot_hardware/UFRobotSystemHardware'
+    hw_ns:='xarm' add_gripper:='false'
+    robot_ip:='' report_type:='normal' baud_checkset:='true' default_gripper_baud:=2000000
+    joint1_lower_limit:=${-2.0*pi}  joint1_upper_limit:=${2.0*pi}
+    joint2_lower_limit:=${-2.059}  joint2_upper_limit:=${2.0944}
+    joint3_lower_limit:=${-3.927}  joint3_upper_limit:=${0.19198}
+    joint4_lower_limit:=${-2.0*pi}  joint4_upper_limit:=${2.0*pi}
+    joint5_lower_limit:=${-1.69297}  joint5_upper_limit:=${pi}
+    joint6_lower_limit:=${-2.0*pi}  joint6_upper_limit:=${2.0*pi}">
+    <ros2_control name="${prefix}${ros2_control_plugin}" type="system">
+      <hardware>
+        <plugin>${ros2_control_plugin}</plugin>
+        <xacro:if value="${ros2_control_plugin == 'uf_robot_hardware/UFRobotSystemHardware'}">
+          <param name="hw_ns">${prefix}${hw_ns}</param>
+          <param name="velocity_control">${velocity_control}</param>
+          <param name="prefix">P${prefix}</param>
+          <param name="robot_ip">R${robot_ip}</param>
+          <param name="report_type">${report_type}</param>
+          <param name="dof">6</param>
+          <param name="baud_checkset">${baud_checkset}</param>
+          <param name="default_gripper_baud">${default_gripper_baud}</param>
+          <param name="robot_type">xarm</param>
+          <param name="add_gripper">${add_gripper}</param>
+        </xacro:if>
+        <!-- fake -->
+        <!-- <plugin>fake_components/GenericSystem</plugin> -->
+
+        <!-- gazebo -->
+        <!-- <plugin>gazebo_ros2_control/GazeboSystem</plugin> -->
+
+        <!-- real xarm -->
+        <!-- <plugin>uf_robot_hardware/UFRobotSystemHardware</plugin> -->
+      </hardware>
+      <joint name="${prefix}joint1">
+        <command_interface name="position">
+          <param name="min">${joint1_lower_limit}</param>
+          <param name="max">${joint1_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint2">
+        <command_interface name="position">
+          <param name="min">${joint2_lower_limit}</param>
+          <param name="max">${joint2_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint3">
+        <command_interface name="position">
+          <param name="min">${joint3_lower_limit}</param>
+          <param name="max">${joint3_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint4">
+        <command_interface name="position">
+          <param name="min">${joint4_lower_limit}</param>
+          <param name="max">${joint4_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint5">
+        <command_interface name="position">
+          <param name="min">${joint5_lower_limit}</param>
+          <param name="max">${joint5_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+      <joint name="${prefix}joint6">
+        <command_interface name="position">
+          <param name="min">${joint6_lower_limit}</param>
+          <param name="max">${joint6_upper_limit}</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <param name="min">-3.14</param>
+          <param name="max">3.14</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <!-- <state_interface name="effort"/> -->
+      </joint>
+    </ros2_control>
+  </xacro:macro>
+</robot>

src/custom_xarm_description/urdf/xarm6/xarm6.transmission.xacro

@@ -0,0 +1,75 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="xarm6_transmission"
+    params="prefix hard_interface:=EffortJointInterface reduction:=100">
+
+    <transmission name="${prefix}tran1">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint1">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor1">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran2">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint2">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor2">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran3">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint3">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor3">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran4">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint4">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor3">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran5">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint5">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor5">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}tran6">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}joint6">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}motor6">
+        <hardwareInterface>hardware_interface/${hard_interface}</hardwareInterface>
+        <mechanicalReduction>${reduction}</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+  </xacro:macro>
+
+</robot>

src/custom_xarm_description/urdf/xarm6/xarm6.urdf.xacro

@@ -0,0 +1,335 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="xarm6_urdf" params="prefix
+    joint1_lower_limit:=${-2.0*pi}  joint1_upper_limit:=${2.0*pi}
+    joint2_lower_limit:=${-2.059}  joint2_upper_limit:=${2.0944}
+    joint3_lower_limit:=${-3.927}  joint3_upper_limit:=${0.19198}
+    joint4_lower_limit:=${-2.0*pi}  joint4_upper_limit:=${2.0*pi}
+    joint5_lower_limit:=${-1.69297}  joint5_upper_limit:=${pi}
+    joint6_lower_limit:=${-2.0*pi}  joint6_upper_limit:=${2.0*pi}">
+
+    <material name="${prefix}Black">
+      <color rgba="0.0 0.0 0.0 1.0"/>
+    </material>
+
+    <material name="${prefix}Red">
+      <color rgba="0.8 0.0 0.0 1.0"/>
+    </material>
+
+    <material name="${prefix}White">
+      <color rgba="1.0 1.0 1.0 1.0"/>
+    </material>
+
+    <material name="${prefix}Silver">
+      <color rgba="0.753 0.753 0.753 1.0"/>
+    </material>
+
+    <link name="${prefix}link_base">
+      <visual>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/base.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/base.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/base.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/base.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+      </collision>
+      <inertial>
+        <origin xyz="0.0 0.0 0.09103" rpy="0 0 0" />
+        <mass value="2.7" />
+        <inertia
+          ixx="0.00494875"
+          ixy="-3.5E-06"
+          ixz="1.25E-05"
+          iyy="0.00494174"
+          iyz="1.67E-06"
+          izz="0.002219" />
+      </inertial>
+    </link>
+
+    <link name="${prefix}link1">
+      <visual>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link1.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link1.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link1.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link1.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+      </collision>
+      <inertial>
+        <origin xyz="-0.002 0.02692 -0.01332" rpy="0 0 0"/>
+        <mass value="2.16"/>
+        <inertia
+          ixx="0.00539427"
+          ixy="1.095E-05"
+          ixz="1.635E-06"
+          iyy="0.0048979"
+          iyz="0.000793"
+          izz="0.00311573"/>
+      </inertial>
+    </link>
+
+    <joint name="${prefix}joint1" type="revolute">
+      <parent link="${prefix}link_base"/>
+      <child  link="${prefix}link1"/>
+      <origin xyz="0 0 0.267" rpy="0 0 0"/>
+      <axis   xyz="0 0 1"/>
+      <limit
+        lower="${joint1_lower_limit}"
+        upper="${joint1_upper_limit}"
+        effort="50.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+
+    <link name="${prefix}link2">
+      <visual>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link2.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link2.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link2.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link2.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+      </collision>
+      <inertial>
+        <origin
+          xyz="0.03531 -0.21398 0.03386"
+          rpy="0 0 0" />
+        <mass
+          value="1.71" />
+        <inertia
+          ixx="0.0248674"
+          ixy="-0.00430651"
+          ixz="-0.00067797"
+          iyy="0.00485548"
+          iyz="0.00457245"
+          izz="0.02387827" />
+      </inertial>
+    </link>
+
+    <joint name="${prefix}joint2" type="revolute">
+      <parent link="${prefix}link1"/>
+      <child link="${prefix}link2"/>
+      <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+      <axis xyz="0 0 1"/>
+      <limit
+        lower="${joint2_lower_limit}"
+        upper="${joint2_upper_limit}"
+        effort="50.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+
+    <link name="${prefix}link3">
+      <visual>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link3.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link3.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link3.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link3.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+      </collision>
+      <inertial>
+        <origin
+          xyz="0.06781 0.10749 0.01457"
+          rpy="0 0 0" />
+        <mass
+          value="1.384" />
+        <inertia
+          ixx="0.0053694"
+          ixy="0.0014185"
+          ixz="-0.00092094"
+          iyy="0.0032423"
+          iyz="-0.00169178"
+          izz="0.00501731" />
+      </inertial>
+    </link>
+
+    <joint name="${prefix}joint3" type="revolute">
+      <parent link="${prefix}link2"/>
+      <child link="${prefix}link3"/>
+      <origin xyz="0.0535 -0.2845 0" rpy="0 0 0"/>
+      <axis xyz="0 0 1"/>
+      <limit
+        lower="${joint3_lower_limit}"
+        upper="${joint3_upper_limit}"
+        effort="32.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+
+    <link name="${prefix}link4">
+      <visual>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link4.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link4.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link4.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link4.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+      </collision>
+      <inertial>
+        <origin xyz="-0.00021 0.02578 -0.02538" rpy="0 0 0"/>
+        <mass value="1.115"/>
+        <inertia
+          ixx="0.00439263"
+          ixy="5.028E-05"
+          ixz="1.374E-05"
+          iyy="0.0040077"
+          iyz="0.00045338"
+          izz="0.00110321"/>
+      </inertial>
+    </link>
+
+    <joint name="${prefix}joint4" type="revolute">
+      <parent link="${prefix}link3"/>
+      <child link="${prefix}link4"/>
+      <origin xyz="0.0775 0.3425 0" rpy="-1.5708 0 0"/>
+      <axis xyz="0 0 1"/>
+      <limit
+        lower="${joint4_lower_limit}"
+        upper="${joint4_upper_limit}"
+        effort="32.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+
+    <link name="${prefix}link5">
+      <visual>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link5.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link5.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}White" />
+      </visual>
+      <collision>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link5.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link5.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+      </collision>
+      <inertial>
+        <origin
+          xyz="0.05428 0.01781 0.00543"
+          rpy="0 0 0" />
+        <mass
+          value="1.275" />
+        <inertia
+          ixx="0.001202758"
+          ixy="0.000492428"
+          ixz="-0.00039147"
+          iyy="0.0022876"
+          iyz="-1.235E-04"
+          izz="0.0026866" />
+      </inertial>
+    </link>
+
+    <joint name="${prefix}joint5" type="revolute">
+      <parent link="${prefix}link4"/>
+      <child link="${prefix}link5"/>
+      <origin xyz="0 0 0" rpy="1.5708 0 0"/>
+      <axis xyz="0 0 1"/>
+      <limit
+        lower="${joint5_lower_limit}"
+        upper="${joint5_upper_limit}"
+        effort="32.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+
+    <link name="${prefix}link6">
+      <visual>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link6.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link6.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}Silver" />
+      </visual>
+      <collision>
+        <geometry>
+          <!-- <mesh filename="package://xarm_description/meshes/xarm6/visual/link6.stl"/> -->
+          <mesh filename="file:///$(find xarm_description)/meshes/xarm6/visual/link6.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+      </collision>
+      <inertial>
+        <origin
+          xyz="0 0.00064 -0.00952"
+          rpy="0 0 0" />
+        <mass
+          value="0.1096" />
+        <inertia
+          ixx="4.5293E-05"
+          ixy="0"
+          ixz="0"
+          iyy="4.8111E-05"
+          iyz="0"
+          izz="7.9715E-05" />
+      </inertial>
+    </link>
+
+    <joint name="${prefix}joint6" type="revolute">
+      <parent link="${prefix}link5"/>
+      <child link="${prefix}link6"/>
+      <origin xyz="0.076 0.097 0" rpy="-1.5708 0 0"/>
+      <axis xyz="0 0 1"/>
+      <limit
+        lower="${joint6_lower_limit}"
+        upper="${joint6_upper_limit}"
+        effort="20.0"
+        velocity="3.14"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+
+    <!-- <link name="${prefix}link_eef" />
+
+    <joint
+      name="${prefix}joint_eef"
+      type="fixed">
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <parent
+        link="${prefix}link6" />
+      <child
+        link="${prefix}link_eef" />
+    </joint> -->
+  
+  </xacro:macro>
+</robot>