Add script

.docker/build.bash

@@ -3,7 +3,7 @@
 SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)"
 PROJECT_DIR="$(dirname "${SCRIPT_DIR}")"
 
-TAG="cacdar/$(basename "${PROJECT_DIR}")"
+TAG="cacdar/drawing-robot-ros2"
 
 if [ "${#}" -gt "0" ]; then
     if [[ "${1}" != "-"* ]]; then

.gitignore

@@ -5,3 +5,11 @@
 
 # Python
 **/__pycache__
+
+# Sphinx
+**/_build
+
+# Doxygen
+**/html
+**/latex
+**/xml

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,47 +27,61 @@ 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
 
 ### Install AxiDraw
-RUN apt-get update && \
-    apt-get install -yq python3-pip && \
-    pip install --upgrade --upgrade-strategy eager packaging && \
-    pip install https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip --upgrade --upgrade-strategy eager
+#RUN apt-get update && \
+#    apt-get install -yq python3-pip && \
+#    pip install --upgrade --upgrade-strategy eager packaging && \
+#    pip install https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip --upgrade --upgrade-strategy eager
 
-### Import and install dependencies, then build these dependencies (not ign_moveit2_examples yet)
-COPY ./drawing_robot_ros2.repos ${WS_SRC_DIR}/ign_moveit2_examples/drawing_robot_ros2.repos
-RUN vcs import --recursive --shallow ${WS_SRC_DIR} < ${WS_SRC_DIR}/ign_moveit2_examples/drawing_robot_ros2.repos && \
-    rosdep update && \
-    apt-get update && \
-    rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \
-    rm -rf /var/lib/apt/lists/* && \
-    source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
+### Install splipy
+#RUN apt-get update && \
+#    apt-get install -yq python3-pip && \
+#    pip install --upgrade --upgrade-strategy eager splipy
+
+# 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}
 
-### Copy over the rest of ign_moveit2_examples, then install dependencies and build
-COPY ./ ${WS_SRC_DIR}/ign_moveit2_examples/
-RUN rosdep update && \
-    apt-get update && \
-    rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \
-    rm -rf /var/lib/apt/lists/* && \
-    source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
+# Build packages
+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}/drawing_controller ${WS_SRC_DIR}/axidraw_controller ${WS_SRC_DIR}/virtual_drawing_surface && \
     rm -rf ${WS_LOG_DIR}
 
-### Copy code built on top of example ign_moveit2_examples
-# TODO clean build process
-COPY ./src/* ${WS_SRC_DIR}/
-RUN rosdep update && \
-    apt-get update && \
-    rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \
-    rm -rf /var/lib/apt/lists/* && \
-    source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
+# 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 ${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 test-images
+
 ### Add workspace to the ROS entrypoint
 ### Source ROS workspace inside `~/.bashrc` to enable autocompletion
 RUN sed -i '$i source "${WS_INSTALL_DIR}/local_setup.bash" --' /ros_entrypoint.sh && \

Makefile

@@ -0,0 +1,20 @@
+# Minimal makefile for Sphinx documentation
+#
+
+# You can set these variables from the command line, and also
+# from the environment for the first two.
+SPHINXOPTS    ?=
+SPHINXBUILD   ?= sphinx-build
+SOURCEDIR     = .
+BUILDDIR      = _build
+
+# Put it first so that "make" without argument is like "make help".
+help:
+	@$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
+
+.PHONY: help Makefile
+
+# Catch-all target: route all unknown targets to Sphinx using the new
+# "make mode" option.  $(O) is meant as a shortcut for $(SPHINXOPTS).
+%: Makefile
+	@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)

README.md

@@ -1,6 +1,58 @@
 # drawing-robot-ros2
 
-## Building
+This repository contains ROS2 packages which make up a system used for drawing SVG images on different robots.
+These packages are in 'src/'.
+
+Documentation and build scripts for the entire project are at the top level.
+
+The simplest way to run the project currently is by building and running the docker container.
+
+## Docker
+### Build container
+
+``` sh
+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
+```
+
+If active changes are being made, run:
+``` sh
+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:
 - python3-pip 
@@ -17,48 +69,180 @@ source src/install/local_setup.bash
 ```
 
 ## Running
-Run
+### RobotController
+One of the following RobotControllers should be started:
+
+DummyController echoes Motion messages to the terminal.
 ``` sh
 ros2 run robot_controller dummy_controller
 ```
-or
+
+### 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
-```
-or
-``` sh
-ros2 launch lite6_controller lite6_gazebo.launch.py
-```
-or
-``` sh
-ros2 launch lite6_controller lite6_real.launch.py
-```
-or
-``` sh
-ros2 launch lite6_controller lite6_real_no_gui.launch.py
+ros2 launch axidraw_controller axidraw_controller.launch.py serial_port:=/dev/ttyACM0 config:=./config.yaml
 ```
 
-And simultaneously (using tmux or another terminal) run
+### Lite6Controller
+This starts the simulated lite6
 ``` sh
-ros2 run drawing_controller drawing_controller src/draw_svg/svg/test.svg
-```
-## Docker
-### Build container
-
-``` sh
-bash .docker/build.bash
+ros2 launch lite6_controller lite6_gazebo.launch.py config:=./config.yaml
 ```
 
-### Run built container
+This runs the real lite6
 ``` sh
-bash .docker/run.bash
+ros2 launch lite6_controller lite6_real.launch.py config:=./config.yaml
 ```
 
-If active changes are being made, run:
+This runs the real lite6 without Rviz (can be run on headless device over ssh)
 ``` sh
-bash .docker/devel.bash
+ros2 launch lite6_controller lite6_real_no_gui.launch.py config:=./config.yaml
 ```
-This will mount the host `drawing-robot-ros2` directory in the container at `src/drawing-robot-ros2`.
+
+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
+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
+- Inkpad
+- Affinitydraw
+- vtracer
+
+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  |
+|-------------------------------------|----------|
+| a                                  | no       |
+| animate                            | no       |
+| animateMotion                      | no       |
+| animateTransform                   | no       |
+| circle                             | no       |
+| clipPath                           | no       |
+| defs                               | no       |
+| desc                               | no       |
+| discard                            | no       |
+| ellipse                            | no       |
+| feBlend                            | no       |
+| feColorMatrix                      | no       |
+| feComponentTransfer                | no       |
+| feComposite                        | no       |
+| feConvolveMatrix                   | no       |
+| feDiffuseLighting                  | no       |
+| feDisplacementMap                  | no       |
+| feDistantLight                     | no       |
+| feDropShadow                       | no       |
+| feFlood                            | no       |
+| feFuncA                            | no       |
+| feFuncB                            | no       |
+| feFuncG                            | no       |
+| feFuncR                            | no       |
+| feGaussianBlur                     | no       |
+| feImage                            | no       |
+| feMerge                            | no       |
+| feMergeNode                        | no       |
+| feMorphology                       | no       |
+| feOffset                           | no       |
+| fePointLight                       | no       |
+| feSpecularLighting                 | no       |
+| feSpotLight                        | no       |
+| feTile                             | no       |
+| feTurbulence                       | no       |
+| filter                             | no       |
+| foreignObject                      | no       |
+| g                                  | yes      |
+| hatch                              | no       |
+| hatchpath                          | no       |
+| image                              | no       |
+| line                               | yes      |
+| linearGradient                     | no       |
+| marker                             | no       |
+| mask                               | no       |
+| metadata                           | no       |
+| mpath                              | no       |
+| path                               | partial  |
+| pattern                            | no       |
+| polygon                            | yes      |
+| polyline                           | yes      |
+| radialGradient                     | no       |
+| rect                               | no       |
+| script                             | no       |
+| set                                | no       |
+| stop                               | no       |
+| style                              | no       |
+| svg                                | no       |
+| switch                             | no       |
+| symbol                             | no       |
+| text                               | no       |
+| textPath                           | no       |
+| title                              | no       |
+| tspan                              | no       |
+| use                                | no       |
+| view                               | no       |
+
+And the following SVG path commands are supported:
+| Command type           | Supported         | Unsupported |
+|------------------------|-------------------|-------------|
+| 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        |
+| Elliptical Arc Curve   |                   | A, a        |
+| ClosePath              |  Z, z             |             |
+
+
+## Axidraw concerns
+## xArm concerns
+TODO make TCP height diagram
+
+The following paths work, notic
+
+## Creating compatible SVG images
+https://github.com/visioncortex/vtracer
+
+Use single layer (g) SVGs
+
 ## ROS2 rpi4
 https://github.com/ros-realtime/ros-realtime-rpi4-image/releases
 

conf.py

@@ -0,0 +1,33 @@
+# Configuration file for the Sphinx documentation builder.
+#
+# For the full list of built-in configuration values, see the documentation:
+# https://www.sphinx-doc.org/en/master/usage/configuration.html
+
+# -- Project information -----------------------------------------------------
+# https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information
+
+project = 'drawing_robot_ros2'
+copyright = '2023, Nicolas Hiillos'
+author = 'Nicolas Hiillos'
+
+# -- General configuration ---------------------------------------------------
+# https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration
+
+#extensions = ['sphinx.ext.pngmath', 'sphinx.ext.todo', 'breathe' ]
+extensions = [ 'breathe' ]
+
+templates_path = ['_templates']
+exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store']
+
+breathe_projects = {
+    "robot_controller": "src/robot_controller/xml",
+    "lite6_controller": "src/lite6_controller/xml",
+    "axidraw_controller": "src/axidraw_controller/xml",
+}
+
+
+# -- Options for HTML output -------------------------------------------------
+# https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output
+
+html_theme = 'alabaster'
+html_static_path = ['_static']

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

index.rst

@@ -0,0 +1,23 @@
+.. drawing_robot_ros2 documentation master file, created by
+   sphinx-quickstart on Fri Jan 20 12:29:28 2023.
+   You can adapt this file completely to your liking, but it should at least
+   contain the root `toctree` directive.
+
+Welcome to drawing_robot_ros2's documentation!
+==============================================
+
+.. toctree::
+   :maxdepth: 2
+   :caption: Contents:
+
+.. doxygenclass:: robot_controller
+   :project: robot_controller
+   :members:
+
+
+Indices and tables
+==================
+
+* :ref:`genindex`
+* :ref:`modindex`
+* :ref:`search`

make.bat

@@ -0,0 +1,35 @@
+@ECHO OFF
+
+pushd %~dp0
+
+REM Command file for Sphinx documentation
+
+if "%SPHINXBUILD%" == "" (
+	set SPHINXBUILD=sphinx-build
+)
+set SOURCEDIR=.
+set BUILDDIR=_build
+
+%SPHINXBUILD% >NUL 2>NUL
+if errorlevel 9009 (
+	echo.
+	echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
+	echo.installed, then set the SPHINXBUILD environment variable to point
+	echo.to the full path of the 'sphinx-build' executable. Alternatively you
+	echo.may add the Sphinx directory to PATH.
+	echo.
+	echo.If you don't have Sphinx installed, grab it from
+	echo.https://www.sphinx-doc.org/
+	exit /b 1
+)
+
+if "%1" == "" goto help
+
+%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
+goto end
+
+:help
+%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
+
+:end
+popd

rebuild.sh

@@ -13,7 +13,8 @@ rosdep install -y -r -i --rosdistro "humble" --from-paths src
 cd src
 rm -r install build log
 #colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
-colcon build --packages-select robot_interfaces robot_controller
+#colcon build --packages-select robot_interfaces robot_controller
+colcon build --paths robot_interfaces robot_controller
 source install/local_setup.bash
 colcon build
 source install/local_setup.bash

scripts/convertlogtimestamp.sh

@@ -0,0 +1,25 @@
+#!/usr/bin/env sh
+# Converts ROS2 log data timestamps from stdin to more readable format
+# Reads lines from stdin, pipe input to this script
+
+
+while IFS= read -r string; do
+    timestr=`echo $string | egrep -o "\[[0-9]+\.[0-9]+\]"`
+    if [[ ! $timestr ]] then
+        continue
+    fi
+    timestamp=${timestr#"["}
+    timestamp=${timestamp%"]"}
+    sec=`echo $timestamp | cut -d "." -f 1`
+    nano=`echo $timestamp | cut -d "." -f 2`
+    #nanos=$(( $nano /  1000000000 ))
+    #sec=$(( $nanos + $sec ))
+    timestamp=`date -d @"$sec" +"%Y-%m-%d-%H:%M:%S"`
+    #printf 'Got sec:"%s"\n' "$sec"
+    #printf 'Got nano:"%s"\n' "$nano"
+    #printf 'Got timestamp:"%s"\n' "$timestamp"
+    #head=`echo $string | awk -F'$timestr' '{print $1}'`
+    #tail=`echo $string | awk -F'$timestr' '{print $2}'`
+    #printf '%s[%s]%s\n' "$head" "$timestamp" "$tail"
+    echo "${string/"$timestr"/"[${timestamp}.${nano}]"}"
+done

scripts/gen-docs.sh

@@ -0,0 +1,20 @@
+#!/usr/bin/env sh
+# generates docs
+
+pushd ..
+
+pushd src/robot_controller/
+doxygen
+popd
+
+pushd src/lite6_controller/
+doxygen
+popd
+
+pushd src/axidraw_controller/
+doxygen
+popd
+
+make html
+
+

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

@@ -0,0 +1,86 @@
+# 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
+  
+# importing required libraries
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+import numpy as np
+from pylab import *
+
+import sys
+  
+print('arg:', sys.argv)
+if len(sys.argv) <= 1:
+    print('Give file path as arg')
+    exit()
+
+filepath = sys.argv[1]
+f = open(filepath, "r")
+data = f.read()
+
+# creating a dataset
+x_succ = []
+y_succ = []
+z_succ = []
+
+x_fail = []
+y_fail = []
+z_fail = []
+
+for l in data.split("\n"):
+    d = l.split(",")
+    if len(d) == 0:
+        continue
+    s = d[0]
+    for p in d[1:]:
+        p = p.split(" ")
+        if len(p) < 3:
+            continue
+        if s == 'success':
+           x_succ.append(float(p[0])) 
+           y_succ.append(float(p[1]))
+           z_succ.append(float(p[2]))
+        else:
+           x_fail.append(float(p[0]))
+           y_fail.append(float(p[1]))
+           z_fail.append(float(p[2]))
+
+x_succ = np.array(x_succ)
+y_succ = np.array(y_succ)
+z_succ = np.array(z_succ)
+
+x_fail = np.array(x_fail)
+y_fail = np.array(y_fail)
+z_fail = np.array(z_fail)
+  
+# creating figures
+fig = plt.figure(figsize=(10, 10))
+ax = fig.add_subplot(111, projection='3d')
+
+# setting color bar
+color_map1 = cm.ScalarMappable(cmap=cm.Greens_r)
+color_map1.set_array([x_succ + y_succ + z_succ])
+  
+color_map2 = cm.ScalarMappable(cmap=cm.Reds_r)
+color_map2.set_array([x_fail + y_fail + z_fail])
+
+# creating the heatmap
+img1 = ax.scatter(x_succ, y_succ, z_succ, marker='s',
+                 s=2, color='green')
+  
+img2 = ax.scatter(x_fail, y_fail, z_fail, marker='s',
+                 s=1, color='red')
+# adding title and labels
+ax.set_title("3D Heatmap")
+ax.set_xlabel('X-axis')
+ax.set_ylabel('Y-axis')
+ax.set_zlabel('Z-axis')
+  
+# displaying plot
+plt.show()

scripts/plot_lite6_csv/shell.nix

@@ -0,0 +1,11 @@
+# shell.nix
+{ pkgs ? import <nixpkgs> {} }:
+let
+  my-python-packages = p: with p; [
+    matplotlib
+    numpy
+    # other python packages
+  ];
+  my-python = pkgs.python3.withPackages my-python-packages;
+in my-python.env
+

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
@@ -16,7 +17,14 @@ from launch.events import Shutdown
 
 def launch_setup(context, *args, **kwargs):
     serial_port = LaunchConfiguration('serial_port', default='/dev/ttyACM0')
-    log_level = LaunchConfiguration("log_level", default='warn')
+    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(
@@ -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/requirements.txt

@@ -0,0 +1 @@
+axicli @ https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip

src/axidraw_controller/src/cpp/axidraw_controller.cpp

@@ -17,7 +17,10 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
 using namespace std::chrono_literals;
-// Controller for the axidraw robot. Calls axidraw python API over ROS2 topics
+
+/**
+ * Controller for the axidraw robot. Calls axidraw python API over ROS2 topics
+ */
 class AxidrawController : public RobotController
 {
   rclcpp::Client<robot_interfaces::srv::Status>::SharedPtr status_client;
@@ -31,15 +34,16 @@ class AxidrawController : public RobotController
   {
     status_client = this->create_client<robot_interfaces::srv::Status>("axidraw_status");
 
-    while (!status_client->wait_for_service(1s))
+    while (!status_client->wait_for_service(5s))
     {
       if (!rclcpp::ok())
       {
         RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for axidraw_status");
         //return 0;
       }
-      RCLCPP_ERROR(this->get_logger(), "Failed to connect to axidraw_status");
+      RCLCPP_WARN(this->get_logger(), "Failed to connect to axidraw_status, retrying");
     }
+    RCLCPP_INFO(this->get_logger(), "Successfully connected to axidraw_status, Axidraw is ready");
 
     // Create publishers for axidraw_serial.py topics
     move_pub = this->create_publisher<geometry_msgs::msg::Point>("axidraw_move", 10);
@@ -48,7 +52,9 @@ class AxidrawController : public RobotController
     path_pub = this->create_publisher<robot_interfaces::msg::Points>("axidraw_path", 10);
   }
 
-  // Return true if axidraw is ready
+  /**
+   * Return true if axidraw is ready
+   */
   bool is_ready()
   {
     auto request = std::make_shared<robot_interfaces::srv::Status::Request>();
@@ -56,22 +62,34 @@ class AxidrawController : public RobotController
     auto result = status_client->async_send_request(request);
 
     result.wait();
-    return result.get()->status == "waiting";
+    std::string res_string = result.get()->status;
+    RCLCPP_INFO(this->get_logger(), "Called is_ready(), status: %s", res_string.c_str());
+    return res_string == "ready";
   }
 
   // Set limits for A4 paper: 297x210mm
   float xlim = 297;
   float ylim = 210;
 
+  /**
+   * Function that translates an input value with a given range to a value within another range.
+   */
   float translate(float val, float lmin, float lmax, float rmin, float rmax)
   {
     float lspan = lmax - lmin;
     float rspan = rmax - rmin;
-    val = (val - lmin) / lspan;
-    return rmin + (val * rspan);
+    float out = (val - lmin) / lspan;
+    out = rmin + (val * rspan);
+
+    // Ensure that output is within bounds
+    out = std::max(rmin, out);
+    out = std::min(rmax, out);
+    return out;
   }
 
-  // Translate a pose to axidraw coordinates (mm)
+  /**
+   * Translate a pose to axidraw coordinates (mm)
+   */
   geometry_msgs::msg::Point translate_pose(geometry_msgs::msg::PoseStamped ps)
   {
     auto pose = ps.pose;
@@ -96,7 +114,9 @@ class AxidrawController : public RobotController
   }
 
 
-  /// Callback that executes path on robot
+  /**
+   * Callback that executes path on robot
+   */
   virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
   {
     RCLCPP_INFO(this->get_logger(), "Executing goal");
@@ -128,20 +148,47 @@ class AxidrawController : public RobotController
 
       auto p = points[i];
 
+      long unsigned int count = 0;
+
       // Ensure axidraw is not busy
       while (!is_ready())
+      {
+        feedback->status = std::to_string(i+1) + "/" + std::to_string(points.size()) + " waiting for previous motion";
+        goal_handle->publish_feedback(feedback);
         loop_rate.sleep();
+      }
 
       if (p.z > 0)
         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++;
+        }
+      }
 
       // Wait for pen movement to complete
       while (!is_ready())
+      {
+        feedback->status = std::to_string(i+1) + "/" + std::to_string(points.size()) + " waiting for current motion";
+        goal_handle->publish_feedback(feedback);
         loop_rate.sleep();
+      }
 
+      if (count == 0)
         this->move_pub->publish(p);
+      else
+      {
+        std::vector<geometry_msgs::msg::Point> ps(&points[i],&points[i+count]);
+        robot_interfaces::msg::Points msg;
+        msg.points = ps;
+        std::string log = "Publishing path with " + std::to_string(ps.size()) + " points";
+        RCLCPP_INFO(this->get_logger(), log.c_str());
+        this->path_pub->publish(msg);
+      }
+      i += count;
 
       // Update status
       feedback->status = std::to_string(i+1) + "/" + std::to_string(points.size()) + " complete";
@@ -161,6 +208,9 @@ class AxidrawController : public RobotController
   }
 };
 
+/**
+ *
+ */
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);

src/axidraw_controller/src/py/axidraw_serial.py

@@ -10,32 +10,50 @@ import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile
 
-
-#TODO delete this
-class Test():
-    def __init__():
-        ad = axidraw.AxiDraw()          # Initialize class
-        ad.interactive()                # Enter interactive context
-        ad.options.port = "/dev/ttyACM0"
-        if not ad.connect():            # Open serial port to AxiDraw;
-            quit()                      #   Exit, if no connection.
-        ad.options.units = 2            # set working units to mm.
-        ad.options.model = 2            # set model to AxiDraw V3/A3
-        # Absolute moves follow:
-        ad.moveto(10, 10)               # Pen-up move to (10mm, 10mm)
-        ad.lineto(20, 10)               # Pen-down move, to (20mm, 10)
-        ad.moveto(0, 0)                 # Pen-up move, back to origin.
-        ad.disconnect()                 # Close serial port to AxiDraw
-
-
 #TODO handle serial disconnect
+
 class AxidrawSerial(Node):
+    """
+    Class for forwarding axidraw python API functions to ROS2 topics.
+
+    ...
+
+    Attributes
+    ----------
+    status : dict
+        contains the robot's status
+
+    Methods
+    -------
+    init_serial(port)
+
+    Services
+    -------
+    Status, 'axidraw_status'
+
+    Topics
+    -------
+    Point, 'axidraw_move'
+    Empty, 'axidraw_penup'
+    Empty, 'axidraw_pendown'
+    Points, 'axidraw_path'
+    """
+
     status = {
         "serial": "not ready",
         "motion": "waiting",
+        "pen": "up",
     }
 
     def init_serial(self, port):
+        '''
+        Initiates connection to axidraw over serial.
+
+            Parameters:
+                    port (string): The serial port or path to serial device (example: "/dev/ttyACM0")
+            Returns:
+                    False if connection failed and True if it succeeded.
+        '''
         self.ad = axidraw.AxiDraw()          # Initialize class
         self.ad.interactive()                # Enter interactive context
         self.ad.options.port = port
@@ -43,21 +61,55 @@ 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):
+        """
+        Sets up a connection to the axidraw robot and starts the 'axidraw_***' services and topic subscriptions.
+        Retries connection to axidraw if it fails.
+        Fetches port from ROS2 parameter 'serial_port', defaulting to '/dev/ttyACM0'.
+
+        Parameters
+        ----------
+        """
+
         super().__init__('axidraw_serial')
 
         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))
@@ -65,16 +117,63 @@ class AxidrawSerial(Node):
         self.path_sub = self.create_subscription(Points, 'axidraw_path', self.stroke_callback, qos_profile=QoSProfile(depth=1))
 
     def get_status(self, request, response):
-        response.status = status.get(request.resource, "Resource '{}' not found.".format(request.resource))
+        '''
+        Looks up the status of the requested resource, sets it as the response status and returns it.
+        Used directly by the "axidraw_status" service.
+
+            Parameters:
+                    request (robot_interfaces.srv.Status.request): The request
+                    response (robot_interfaces.srv.Status.response): The response
+            Returns:
+                    response (robot_interfaces.srv.Status.response): The response with the data added. If not found returns "Resource 'X' not found.".
+        '''
+        response.status = self.status.get(request.resource, "Resource '{}' not found.".format(request.resource))
         return response
 
+    def go_home(self):
+        '''
+        Moves the robot to (0,0).
+            Parameters:
+            Returns:
+        '''
+        self.status["motion"] = "busy"
+        if self.status["serial"] == "ready":
+            self.ad.moveto(0,0)
+        self.status["motion"] = "ready"
+
     def set_busy(self):
-        self["motion"] = "busy"
+        '''
+        Sets the robot motion to "busy"
+            Parameters:
+            Returns:
+        '''
+        self.status["motion"] = "busy"
 
     def set_ready(self):
-        self["motion"] = "ready"
+        '''
+        Sets the robot motion to "ready"
+            Parameters:
+            Returns:
+        '''
+        self.status["motion"] = "ready"
+
+    def wait_ready(self):
+        '''
+        Sets the robot motion to "ready"
+            Parameters:
+            Returns:
+        '''
+        rate = self.create_rate(2) #2Hz
+        while self.status["motion"] != "ready":
+            rate.sleep()
+            pass
 
     def move_callback(self, msg):
+        '''
+        Callback for axidraw_move topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
         self.get_logger().info("Received move: {}".format(msg))
@@ -83,28 +182,48 @@ class AxidrawSerial(Node):
         self.set_ready()
 
     def penup_callback(self, msg):
+        '''
+        Callback for axidraw_penup topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
         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):
+        '''
+        Callback for axidraw_pendown topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
         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):
+        '''
+        Callback for axidraw_stroke topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
-        self.get_logger().info("Received path: {}".format(msg))
+        self.get_logger().info("Received path: {}...".format(msg.points[:6]))
 
         path = [ [p.x,p.y] for p in msg.points ]
         self.ad.draw_path(path)
+        self.status['pen'] = "up"
         self.set_ready()
 
 
@@ -112,9 +231,10 @@ def main(args=None):
     rclpy.init(args=args)
 
     axidraw_serial = AxidrawSerial()
-
+    try:
         rclpy.spin(axidraw_serial)
-
+    finally:
+        axidraw_serial.go_home()
         rclpy.shutdown()
 
 

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>