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
@@ -14,9 +14,14 @@ if [ "${#}" -gt "0" ]; then
     fi
 fi
 
-#docker build
+CONTAINER_CMD="podman build --format docker"
+if ! [ -x "$(command -v podman)" ]; then
+  echo 'podman not installed, using docker' >&2
+  CONTAINER_CMD="docker build"
+fi
+
 DOCKER_BUILD_CMD=(
-    podman build --format docker
+    "${CONTAINER_CMD}"
     "${PROJECT_DIR}"
     --tag "${TAG}"
     "${BUILD_ARGS}"

.gitignore

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

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(ign_moveit2_examples)
+project(drawing-robot-ros2)
 
 # Default to C11
 if(NOT CMAKE_C_STANDARD)
@@ -27,31 +27,9 @@ find_package(rclcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 
-# Install C++ examples
-set(EXAMPLES_CPP_DIR examples/cpp)
-# Example 0 - Follow target
-set(EXECUTABLE_0 ex_follow_target)
-add_executable(${EXECUTABLE_0} ${EXAMPLES_CPP_DIR}/${EXECUTABLE_0}.cpp)
-ament_target_dependencies(${EXECUTABLE_0}
-    rclcpp
-    geometry_msgs
-    moveit_ros_planning_interface
-)
-install(TARGETS
-    ${EXECUTABLE_0}
-    DESTINATION lib/${PROJECT_NAME}
-)
-
-# Install Python examples
-set(EXAMPLES_PY_DIR examples/py)
-install(PROGRAMS
-    ${EXAMPLES_PY_DIR}/ex_follow_target.py
-    ${EXAMPLES_PY_DIR}/ex_throw_object.py
-    DESTINATION lib/${PROJECT_NAME}
-)
 
 # Install directories
-install(DIRECTORY launch rviz worlds DESTINATION share/${PROJECT_NAME})
+#install(DIRECTORY launch rviz worlds DESTINATION share/${PROJECT_NAME})
 
 # Setup the project
 ament_package()

Dockerfile

@@ -1,4 +1,4 @@
-ARG ROS_DISTRO=galactic
+ARG ROS_DISTRO=humble
 FROM ros:${ROS_DISTRO}-ros-base
 
 ### Use bash by default
@@ -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}
@@ -22,46 +24,64 @@ RUN apt-get update && \
 
 ### Install extra dependencies
 RUN apt-get update && \
-    apt-get install -yq python3-pil.imagetk
+    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 && \
+#RUN apt-get update && \
-    apt-get install -yq python3-pip && \
+#    apt-get install -yq python3-pip && \
-    pip install --upgrade --upgrade-strategy eager packaging && \
+#    pip install --upgrade --upgrade-strategy eager packaging && \
-    pip install https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip --upgrade --upgrade-strategy eager
+#    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)
+### Install splipy
-COPY ./drawing_robot_ros2.repos ${WS_SRC_DIR}/ign_moveit2_examples/drawing_robot_ros2.repos
+#RUN apt-get update && \
-RUN vcs import --recursive --shallow ${WS_SRC_DIR} < ${WS_SRC_DIR}/ign_moveit2_examples/drawing_robot_ros2.repos && \
+#    apt-get install -yq python3-pip && \
-    rosdep update && \
+#    pip install --upgrade --upgrade-strategy eager splipy
-    apt-get update && \
+
-    rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \
+# Build interfaces and generic controller first
-    rm -rf /var/lib/apt/lists/* && \
+COPY ./src/robot_interfaces ${WS_SRC_DIR}/robot_interfaces
-    source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
+COPY ./src/robot_controller ${WS_SRC_DIR}/robot_controller
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
+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
+# Build packages
-COPY ./ ${WS_SRC_DIR}/ign_moveit2_examples/
+COPY ./src/drawing_controller ${WS_SRC_DIR}/drawing_controller
-RUN rosdep update && \
+COPY ./src/axidraw_controller ${WS_SRC_DIR}/axidraw_controller
-    apt-get update && \
+COPY ./src/virtual_drawing_surface ${WS_SRC_DIR}/virtual_drawing_surface
-    rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \
+RUN pip install -r ${WS_SRC_DIR}/drawing_controller/requirements.txt
-    rm -rf /var/lib/apt/lists/* && \
+RUN pip install -r ${WS_SRC_DIR}/axidraw_controller/requirements.txt
-    source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
+RUN pip install -r ${WS_SRC_DIR}/virtual_drawing_surface/requirements.txt
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
+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
+# Build lite6 and xarm packages
-# TODO clean build process
+COPY ./src/lite6_controller ${WS_SRC_DIR}/lite6_controller
-COPY ./src/* ${WS_SRC_DIR}/
+COPY ./src/custom_xarm_description ${WS_SRC_DIR}/custom_xarm_description
-RUN rosdep update && \
+COPY ./src/custom_xarm_moveit_config ${WS_SRC_DIR}/custom_xarm_moveit_config
-    apt-get update && \
+COPY ./src/custom_xarm_gazebo ${WS_SRC_DIR}/custom_xarm_gazebo
-    rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \
+RUN apt-get update
-    rm -rf /var/lib/apt/lists/* && \
+RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
-    source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
+    vcs import --recursive --shallow ${WS_SRC_DIR} < ${WS_SRC_DIR}/lite6_controller/lite6_controller.repos && \
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
+    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,17 +1,248 @@
 # drawing-robot-ros2
 
+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
-source install/local_setup.bash
+bash .docker/build.bash
-cd src/ign_moveit2_examples/src/draw_svg/
-rosdep install --from-paths . --ignore-src -r -y
-colcon build
-source install/local_setup.bash
-ros2 launch draw_svg draw_svg.launch.py
 ```
 
-## xArm lite6
+If build fails, consider clearing build cache.
-- web interface: http://192.168.1.150:18333
+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 
+- python3-pil.imagetk
+- ros-humble-moveit
+- ros-humble-ros-gz
+- ignition-fortress
+
+``` sh
+./rebuild.sh
+```
+``` sh
+source src/install/local_setup.bash
+```
+
+## Running
+### RobotController
+One of the following RobotControllers should be started:
+
+DummyController echoes Motion messages to the terminal.
+``` sh
+ros2 run robot_controller dummy_controller
+```
+
+### 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.launch.py serial_port:=/dev/ttyACM0 config:=./config.yaml
+```
+
+### Lite6Controller
+This starts the simulated lite6
+``` sh
+ros2 launch lite6_controller lite6_gazebo.launch.py config:=./config.yaml
+```
+
+This runs the real lite6
+``` sh
+ros2 launch lite6_controller lite6_real.launch.py config:=./config.yaml
+```
+
+This runs the real lite6 without Rviz (can be run on headless device over ssh)
+``` sh
+ros2 launch lite6_controller lite6_real_no_gui.launch.py config:=./config.yaml
+```
+
+Before using the real lite6, it is recommended to run the calibration program.
+A lite6_controller must be running for calibration.
+This can be used to measure the Z height for a specific pen.
+The program also moves the arm to a known default position.
+``` sh
+ros2 run lite6_controller lite6_calibration
+```
+Follow the instructions, pressing enter when prompted.
+
+Change the Z-offset value accordingly.
+Restart the running lite6_controller after calibration.
+
+### DrawingController
+Once a RobotController is running, simultaneously (using tmux or another terminal) run
+``` sh
+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
 
@@ -27,4 +258,84 @@ apt-mark hold libraspberrypi-bin libraspberrypi-dev libraspberrypi-doc libraspbe
 apt-mark hold raspberrypi-bootloader raspberrypi-kernel raspberrypi-kernel-headers
 
 apt update && apt upgrade
+
+
+apt install ros-dev-tools
+```
+``` sh
+adduser ubuntu dialout #give access to serial devices (axidraw)
+``` 
+
+### Misc commands
+``` sh
+apt update
+apt install git tmux python3-colcon-ros python3-pip ros-humble-moveit
+``` 
+
+``` sh
+  apt install colcon
+  apt search colcon
+  apt install ros-dev-tools
+  vi /etc/issue
+  systemctl stop wpa_supplicant
+  wpa_supplicant -B -i wlan0 -c /etc/wpa_supplicant/wpa_supplicant.conf
+  wpa_cli
+  ip link set wlan0 up
+  wpa_cli
+  dhclient wlan0
+  ping google.com
+  apt install ros-galactic-moveit
+  apt install xauth
+  vim /etc/ssh/sshd_config
+  systemctl restart sshd
+  colcon build --packages-select robot_interfaces robot_controller
+```
+
+sets priority for wlan0; uses it as gateway if connected.
+/etc/netplan/50-cloud-init.yaml
+``` sh
+network:
+    wifis:
+        wlan0:
+            dhcp4: true
+            dhcp4-overrides:
+              route-metric: 100
+            optional: true
+            access-points:
+              "SSID":
+                password: "PSK"
+    ethernets:
+        eth0:
+            dhcp4: true
+            dhcp4-overrides:
+              route-metric: 200
+            optional: true
+    version: 2
+```
+/etc/ssh/sshd:
+```
+X11Forwarding yes
+X11UseLocalhost no
+```
+
+### Access xarm webUI from different network
+If connected to the pi on 192.168.22.199, one can forward the webUI to localhost:8080 with the following:
+``` sh
+ssh -L 8080:192.168.1.150:18333 ubuntu@192.168.22.199
+```
+
+## Moveit2 docs
+
+``` sh
+
+git clone https://github.com/ros-planning/moveit2.git
+cd moveit2
+git checkout humble
+
+sudo apt-get install doxygen graphviz
+
+DOXYGEN_OUTPUT_DIRECTORY=docs doxygen
+
+firefox docs/index.html
+
 ```

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

drawing_robot_ros2.repos

@@ -1,25 +1,29 @@
 repositories:
-  pymoveit2:
+  #pymoveit2:
-    type: git
+  #  type: git
-    url: https://github.com/AndrejOrsula/pymoveit2.git
+  #  url: https://github.com/AndrejOrsula/pymoveit2.git
-    version: master
+  #  version: master
-  panda_ign_moveit2:
+  #panda_ign_moveit2:
-    type: git
+  #  type: git
-    url: https://github.com/AndrejOrsula/panda_ign_moveit2.git
+  #  url: https://github.com/AndrejOrsula/panda_ign_moveit2.git
-    version: master
+  #  version: master
-  ros_ign:
+  #ros_ign:
-    type: git
+  #  type: git
-    url: https://github.com/ignitionrobotics/ros_ign.git
+  #  url: https://github.com/ignitionrobotics/ros_ign.git
-    version: galactic
+  #  version: galactic
-  ign_ros2_control:
+  #ign_ros2_control:
-    type: git
+  #  type: git
-    url: https://github.com/AndrejOrsula/ign_ros2_control.git
+  #  url: https://github.com/AndrejOrsula/ign_ros2_control.git
-    version: devel
+  #  version: devel
-  ros2_controllers:
+  #ros2_controllers:
-    type: git
+  #  type: git
-    url: https://github.com/AndrejOrsula/ros2_controllers.git
+  #  url: https://github.com/AndrejOrsula/ros2_controllers.git
-    version: jtc_effort
+  #  version: jtc_effort
   xarm_ros2:
     type: git
     url: https://github.com/xArm-Developer/xarm_ros2
-    version: galactic
+    version: humble
+  #moveit_visual_tools:
+  #  type: git
+  #  url: https://github.com/ros-planning/moveit_visual_tools
+  #  version: ros2

examples/cpp/ex_follow_target.cpp

@@ -1,71 +0,0 @@
-/// Example that uses MoveIt 2 to follow a target inside Ignition Gazebo
-
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <rclcpp/qos.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-const std::string MOVE_GROUP = "arm";
-
-class MoveItFollowTarget : public rclcpp::Node
-{
-public:
-  /// Constructor
-  MoveItFollowTarget();
-
-  /// Move group interface for the robot
-  moveit::planning_interface::MoveGroupInterface move_group_;
-  /// Subscriber for target pose
-  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_sub_;
-  /// Target pose that is used to detect changes
-  geometry_msgs::msg::Pose previous_target_pose_;
-
-private:
-  /// Callback that plans and executes trajectory each time the target pose is changed
-  void target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
-};
-
-MoveItFollowTarget::MoveItFollowTarget() : Node("ex_follow_target"),
-                                           move_group_(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
-{
-  // Use upper joint velocity and acceleration limits
-  this->move_group_.setMaxAccelerationScalingFactor(1.0);
-  this->move_group_.setMaxVelocityScalingFactor(1.0);
-
-  // Subscribe to target pose
-  target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
-
-  RCLCPP_INFO(this->get_logger(), "Initialization successful.");
-}
-
-void MoveItFollowTarget::target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
-{
-  // Return if target pose is unchanged
-  if (msg->pose == previous_target_pose_)
-  {
-    return;
-  }
-
-  RCLCPP_INFO(this->get_logger(), "Target pose has changed. Planning and executing...");
-
-  // Plan and execute motion
-  this->move_group_.setPoseTarget(msg->pose);
-  this->move_group_.move();
-
-  // Update for next callback
-  previous_target_pose_ = msg->pose;
-}
-
-int main(int argc, char *argv[])
-{
-  rclcpp::init(argc, argv);
-
-  auto target_follower = std::make_shared<MoveItFollowTarget>();
-
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.add_node(target_follower);
-  executor.spin();
-
-  rclcpp::shutdown();
-  return EXIT_SUCCESS;
-}

examples/py/ex_follow_target.py

@@ -1,83 +0,0 @@
-#!/usr/bin/env python3
-"""Example that uses MoveIt 2 to follow a target inside Ignition Gazebo"""
-
-import rclpy
-from geometry_msgs.msg import Pose, PoseStamped
-from pymoveit2 import MoveIt2
-from pymoveit2.robots import panda
-from rclpy.callback_groups import ReentrantCallbackGroup
-from rclpy.node import Node
-from rclpy.qos import QoSProfile
-
-
-class MoveItFollowTarget(Node):
-    def __init__(self):
-
-        super().__init__("ex_follow_target_py")
-
-        # Create callback group that allows execution of callbacks in parallel without restrictions
-        self._callback_group = ReentrantCallbackGroup()
-
-        # Create MoveIt 2 interface
-        self._moveit2 = MoveIt2(
-            node=self,
-            joint_names=panda.joint_names(),
-            base_link_name=panda.base_link_name(),
-            end_effector_name=panda.end_effector_name(),
-            group_name=panda.MOVE_GROUP_ARM,
-            execute_via_moveit=True,
-            callback_group=self._callback_group,
-        )
-        # Use upper joint velocity and acceleration limits
-        self._moveit2.max_velocity = 1.0
-        self._moveit2.max_acceleration = 1.0
-
-        # Create a subscriber for target pose
-        self.__previous_target_pose = Pose()
-        self.create_subscription(
-            msg_type=PoseStamped,
-            topic="/target_pose",
-            callback=self.target_pose_callback,
-            qos_profile=QoSProfile(depth=1),
-            callback_group=self._callback_group,
-        )
-
-        self.get_logger().info("Initialization successful.")
-
-    def target_pose_callback(self, msg: PoseStamped):
-        """
-        Plan and execute trajectory each time the target pose is changed
-        """
-
-        # Return if target pose is unchanged
-        if msg.pose == self.__previous_target_pose:
-            return
-
-        self.get_logger().info("Target pose has changed. Planning and executing...")
-
-        # Plan and execute motion
-        self._moveit2.move_to_pose(
-            position=msg.pose.position,
-            quat_xyzw=msg.pose.orientation,
-        )
-
-        # Update for next callback
-        self.__previous_target_pose = msg.pose
-
-
-def main(args=None):
-
-    rclpy.init(args=args)
-
-    target_follower = MoveItFollowTarget()
-
-    executor = rclpy.executors.MultiThreadedExecutor(2)
-    executor.add_node(target_follower)
-    executor.spin()
-
-    rclpy.shutdown()
-    exit(0)
-
-
-if __name__ == "__main__":
-    main()

examples/py/ex_throw_object.py

@@ -1,151 +0,0 @@
-#!/usr/bin/env python3
-
-from copy import deepcopy
-from threading import Thread
-
-import rclpy
-from geometry_msgs.msg import Point, Quaternion
-from pymoveit2 import MoveIt2, MoveIt2Gripper
-from pymoveit2.robots import panda
-from rclpy.callback_groups import ReentrantCallbackGroup
-from rclpy.node import Node
-
-
-class MoveItThrowObject(Node):
-    def __init__(self):
-
-        super().__init__("ex_throw_object")
-
-        # Create callback group that allows execution of callbacks in parallel without restrictions
-        self._callback_group = ReentrantCallbackGroup()
-
-        # Create MoveIt 2 interface
-        self._moveit2 = MoveIt2(
-            node=self,
-            joint_names=panda.joint_names(),
-            base_link_name=panda.base_link_name(),
-            end_effector_name=panda.end_effector_name(),
-            group_name=panda.MOVE_GROUP_ARM,
-            callback_group=self._callback_group,
-        )
-        # Use upper joint velocity and acceleration limits
-        self._moveit2.max_velocity = 1.0
-        self._moveit2.max_acceleration = 1.0
-
-        # Create MoveIt 2 interface for gripper
-        self._moveit2_gripper = MoveIt2Gripper(
-            node=self,
-            gripper_joint_names=panda.gripper_joint_names(),
-            open_gripper_joint_positions=panda.OPEN_GRIPPER_JOINT_POSITIONS,
-            closed_gripper_joint_positions=panda.CLOSED_GRIPPER_JOINT_POSITIONS,
-            gripper_group_name=panda.MOVE_GROUP_GRIPPER,
-            callback_group=self._callback_group,
-        )
-
-        self.get_logger().info("Initialization successful.")
-
-    def throw(self):
-        """
-        Plan and execute hard-coded trajectory with intention to throw an object
-        """
-
-        self.get_logger().info("Throwing... Wish me luck!")
-
-        throwing_object_pos = Point(x=0.5, y=0.0, z=0.015)
-        default_quat = Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)
-
-        # Open gripper
-        self._moveit2_gripper.open()
-        self._moveit2_gripper.wait_until_executed()
-
-        # Move above object
-        position_above_object = deepcopy(throwing_object_pos)
-        position_above_object.z += 0.15
-        self._moveit2.move_to_pose(
-            position=position_above_object,
-            quat_xyzw=default_quat,
-        )
-        self._moveit2.wait_until_executed()
-
-        # Move to grasp position
-        self._moveit2.move_to_pose(
-            position=throwing_object_pos,
-            quat_xyzw=default_quat,
-        )
-        self._moveit2.wait_until_executed()
-
-        # Close gripper
-        self._moveit2_gripper.close()
-        self._moveit2_gripper.wait_until_executed()
-
-        # Decrease speed
-        self._moveit2.max_velocity = 0.25
-        self._moveit2.max_acceleration = 0.1
-
-        # Move above object (again)
-        self._moveit2.move_to_pose(
-            position=position_above_object,
-            quat_xyzw=default_quat,
-        )
-        self._moveit2.wait_until_executed()
-
-        # Move to pre-throw configuration
-        joint_configuration_pre_throw = [0.0, -1.75, 0.0, -0.1, 0.0, 3.6, 0.8]
-        self._moveit2.move_to_configuration(joint_configuration_pre_throw)
-        self._moveit2.wait_until_executed()
-
-        # Increase speed
-        self._moveit2.max_velocity = 1.0
-        self._moveit2.max_acceleration = 1.0
-
-        # Throw itself
-        joint_configuration_throw = [0.0, 1.0, 0.0, -1.1, 0.0, 1.9, 0.8]
-        self._moveit2.move_to_configuration(joint_configuration_throw)
-
-        # Release object while executing motion
-        sleep_duration_s = 1.2
-        if rclpy.ok():
-            self.create_rate(1 / sleep_duration_s).sleep()
-        self._moveit2_gripper.open()
-        self._moveit2_gripper.wait_until_executed()
-        self._moveit2.wait_until_executed()
-
-        # Return to default configuration
-        joint_configuration_default = [
-            0.0,
-            -0.7853981633974483,
-            0.0,
-            -2.356194490192345,
-            0.0,
-            1.5707963267948966,
-            0.7853981633974483,
-        ]
-        self._moveit2.move_to_configuration(joint_configuration_default)
-        self._moveit2.wait_until_executed()
-
-
-def main(args=None):
-
-    rclpy.init(args=args)
-
-    object_thrower = MoveItThrowObject()
-
-    # Spin the node in background thread(s)
-    executor = rclpy.executors.MultiThreadedExecutor(3)
-    executor.add_node(object_thrower)
-    executor_thread = Thread(target=executor.spin, daemon=True, args=())
-    executor_thread.start()
-
-    # Wait for everything to setup
-    sleep_duration_s = 2.0
-    if rclpy.ok():
-        object_thrower.create_rate(1 / sleep_duration_s).sleep()
-
-    object_thrower.throw()
-
-    rclpy.shutdown()
-    exit(0)
-
-
-if __name__ == "__main__":
-    main()

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`

launch/default.launch.py

@@ -1,150 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch default world with the default robot (configurable)"""
-
-from os import path
-from typing import List
-
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    world_type = LaunchConfiguration("world_type")
-    robot_type = LaunchConfiguration("robot_type")
-    rviz_config = LaunchConfiguration("rviz_config")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    ign_verbosity = LaunchConfiguration("ign_verbosity")
-    log_level = LaunchConfiguration("log_level")
-
-    # Determine what world/robot combination to launch
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "__world_launch_basename",
-            default_value=["world_", world_type, ".launch.py"],
-        ),
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "__robot_launch_basename",
-            default_value=["robot_", robot_type, ".launch.py"],
-        ),
-    )
-
-    # List of included launch descriptions
-    launch_descriptions = [
-        # Launch Ignition Gazebo with the required ROS<->IGN bridges
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("ign_moveit2_examples"),
-                        "launch",
-                        "worlds",
-                        LaunchConfiguration("__world_launch_basename"),
-                    ]
-                )
-            ),
-            launch_arguments=[
-                ("use_sim_time", use_sim_time),
-                ("ign_verbosity", ign_verbosity),
-                ("log_level", log_level),
-            ],
-        ),
-        # Spawn robot
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("ign_moveit2_examples"),
-                        "launch",
-                        "robots",
-                        LaunchConfiguration("__robot_launch_basename"),
-                    ]
-                )
-            ),
-            launch_arguments=[
-                ("use_sim_time", use_sim_time),
-                ("ign_verbosity", ign_verbosity),
-                ("log_level", log_level),
-            ],
-        ),
-        # Launch move_group of MoveIt 2
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare([robot_type, "_moveit_config"]),
-                        "launch",
-                        "move_group.launch.py",
-                    ]
-                )
-            ),
-            launch_arguments=[
-                ("ros2_control_plugin", "ign"),
-                ("ros2_control_command_interface", "effort"),
-                # TODO: Re-enable colligion geometry for manipulator arm once spawning with specific joint configuration is enabled
-                ("collision_arm", "false"),
-                ("rviz_config", rviz_config),
-                ("use_sim_time", use_sim_time),
-                ("log_level", log_level),
-            ],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + launch_descriptions)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # World selection
-        DeclareLaunchArgument(
-            "world_type",
-            default_value="default",
-            description="Name of the world configuration to load.",
-        ),
-        # Robot selection
-        DeclareLaunchArgument(
-            "robot_type",
-            default_value="panda",
-            description="Name of the robot type to use.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "rviz_config",
-            default_value=path.join(
-                get_package_share_directory("ign_moveit2_examples"),
-                "rviz",
-                "ign_moveit2_examples.rviz",
-            ),
-            description="Path to configuration for RViz2.",
-        ),
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "ign_verbosity",
-            default_value="2",
-            description="Verbosity level for Ignition Gazebo (0~4).",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

launch/ex_cpp_follow_target.launch.py

@@ -1,187 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch C++ example for following a target"""
-
-from os import path
-from typing import List
-
-import yaml
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import (
-    Command,
-    FindExecutable,
-    LaunchConfiguration,
-    PathJoinSubstitution,
-)
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    description_package = LaunchConfiguration("description_package")
-    description_filepath = LaunchConfiguration("description_filepath")
-    moveit_config_package = "panda_moveit_config"
-    robot_type = LaunchConfiguration("robot_type")
-    rviz_config = LaunchConfiguration("rviz_config")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    ign_verbosity = LaunchConfiguration("ign_verbosity")
-    log_level = LaunchConfiguration("log_level")
-
-    # URDF
-    _robot_description_xml = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [FindPackageShare(description_package), description_filepath]
-            ),
-            " ",
-            "name:=",
-            robot_type,
-        ]
-    )
-    robot_description = {"robot_description": _robot_description_xml}
-
-    # SRDF
-    _robot_description_semantic_xml = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [
-                    FindPackageShare(moveit_config_package),
-                    "srdf",
-                    "panda.srdf.xacro",
-                ]
-            ),
-            " ",
-            "name:=",
-            robot_type,
-        ]
-    )
-    robot_description_semantic = {
-        "robot_description_semantic": _robot_description_semantic_xml
-    }
-
-    # List of included launch descriptions
-    launch_descriptions = [
-        # Launch world with robot (configured for this example)
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("ign_moveit2_examples"),
-                        "launch",
-                        "default.launch.py",
-                    ]
-                )
-            ),
-            launch_arguments=[
-                ("world_type", "follow_target"),
-                ("robot_type", robot_type),
-                ("rviz_config", rviz_config),
-                ("use_sim_time", use_sim_time),
-                ("ign_verbosity", ign_verbosity),
-                ("log_level", log_level),
-            ],
-        ),
-    ]
-
-    # List of nodes to be launched
-    nodes = [
-        # Run the example node (C++)
-        Node(
-            package="ign_moveit2_examples",
-            executable="ex_follow_target",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[
-                robot_description,
-                robot_description_semantic,
-                {"use_sim_time": use_sim_time},
-            ],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
-
-
-def load_yaml(package_name: str, file_path: str):
-    """
-    Load yaml configuration based on package name and file path relative to its share.
-    """
-
-    package_path = get_package_share_directory(package_name)
-    absolute_file_path = path.join(package_path, file_path)
-    return parse_yaml(absolute_file_path)
-
-
-def parse_yaml(absolute_file_path: str):
-    """
-    Parse yaml from file, given its absolute file path.
-    """
-
-    try:
-        with open(absolute_file_path, "r") as file:
-            return yaml.safe_load(file)
-    except EnvironmentError:
-        return None
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # Locations of robot resources
-        DeclareLaunchArgument(
-            "description_package",
-            default_value="panda_description",
-            description="Custom package with robot description.",
-        ),
-        DeclareLaunchArgument(
-            "description_filepath",
-            default_value=path.join("urdf", "panda.urdf.xacro"),
-            description="Path to xacro or URDF description of the robot, relative to share of `description_package`.",
-        ),
-        # Robot selection
-        DeclareLaunchArgument(
-            "robot_type",
-            default_value="panda",
-            description="Name of the robot to use.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "rviz_config",
-            default_value=path.join(
-                get_package_share_directory("ign_moveit2_examples"),
-                "rviz",
-                "ign_moveit2_examples.rviz",
-            ),
-            description="Path to configuration for RViz2.",
-        ),
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "ign_verbosity",
-            default_value="2",
-            description="Verbosity level for Ignition Gazebo (0~4).",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

launch/ex_py_follow_target.launch.py

@@ -1,105 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch Python example for following a target"""
-
-from os import path
-from typing import List
-
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    robot_type = LaunchConfiguration("robot_type")
-    rviz_config = LaunchConfiguration("rviz_config")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    ign_verbosity = LaunchConfiguration("ign_verbosity")
-    log_level = LaunchConfiguration("log_level")
-
-    # List of included launch descriptions
-    launch_descriptions = [
-        # Launch world with robot (configured for this example)
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("ign_moveit2_examples"),
-                        "launch",
-                        "default.launch.py",
-                    ]
-                )
-            ),
-            launch_arguments=[
-                ("world_type", "follow_target"),
-                ("robot_type", robot_type),
-                ("rviz_config", rviz_config),
-                ("use_sim_time", use_sim_time),
-                ("ign_verbosity", ign_verbosity),
-                ("log_level", log_level),
-            ],
-        ),
-    ]
-
-    # List of nodes to be launched
-    nodes = [
-        # Run the example node (Python)
-        Node(
-            package="ign_moveit2_examples",
-            executable="ex_follow_target.py",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # Robot selection
-        DeclareLaunchArgument(
-            "robot_type",
-            default_value="panda",
-            description="Name of the robot to use.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "rviz_config",
-            default_value=path.join(
-                get_package_share_directory("ign_moveit2_examples"),
-                "rviz",
-                "ign_moveit2_examples.rviz",
-            ),
-            description="Path to configuration for RViz2.",
-        ),
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "ign_verbosity",
-            default_value="2",
-            description="Verbosity level for Ignition Gazebo (0~4).",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

launch/ex_py_throw_object.launch.py

@@ -1,105 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch Python example for throwing an object"""
-
-from os import path
-from typing import List
-
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    robot_type = LaunchConfiguration("robot_type")
-    rviz_config = LaunchConfiguration("rviz_config")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    ign_verbosity = LaunchConfiguration("ign_verbosity")
-    log_level = LaunchConfiguration("log_level")
-
-    # List of included launch descriptions
-    launch_descriptions = [
-        # Launch world with robot (configured for this example)
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("ign_moveit2_examples"),
-                        "launch",
-                        "default.launch.py",
-                    ]
-                )
-            ),
-            launch_arguments=[
-                ("world_type", "throw_object"),
-                ("robot_type", robot_type),
-                ("rviz_config", rviz_config),
-                ("use_sim_time", use_sim_time),
-                ("ign_verbosity", ign_verbosity),
-                ("log_level", log_level),
-            ],
-        ),
-    ]
-
-    # List of nodes to be launched
-    nodes = [
-        # Run the example node (Python)
-        Node(
-            package="ign_moveit2_examples",
-            executable="ex_throw_object.py",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # Robot selection
-        DeclareLaunchArgument(
-            "robot_type",
-            default_value="panda",
-            description="Name of the robot to use.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "rviz_config",
-            default_value=path.join(
-                get_package_share_directory("ign_moveit2_examples"),
-                "rviz",
-                "ign_moveit2_examples.rviz",
-            ),
-            description="Path to configuration for RViz2.",
-        ),
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "ign_verbosity",
-            default_value="2",
-            description="Verbosity level for Ignition Gazebo (0~4).",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

launch/robots/robot_panda.launch.py

@@ -1,61 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch script for spawning Franka Emika Panda into Ignition Gazebo world"""
-
-from typing import List
-
-from launch_ros.actions import Node
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    model = LaunchConfiguration("model")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    log_level = LaunchConfiguration("log_level")
-
-    # List of nodes to be launched
-    nodes = [
-        # ros_ign_gazebo_create
-        Node(
-            package="ros_ign_gazebo",
-            executable="create",
-            output="log",
-            arguments=["-file", model, "--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + nodes)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # Model for Ignition Gazebo
-        DeclareLaunchArgument(
-            "model",
-            default_value="panda",
-            description="Name or filepath of model to load.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

launch/worlds/world_default.launch.py

@@ -1,91 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch default.sdf and the required ROS<->IGN bridges"""
-
-from typing import List
-
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    world = LaunchConfiguration("world")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    ign_verbosity = LaunchConfiguration("ign_verbosity")
-    log_level = LaunchConfiguration("log_level")
-
-    # List of included launch descriptions
-    launch_descriptions = [
-        # Launch Ignition Gazebo
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("ros_ign_gazebo"),
-                        "launch",
-                        "ign_gazebo.launch.py",
-                    ]
-                )
-            ),
-            launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])],
-        ),
-    ]
-
-    # List of nodes to be launched
-    nodes = [
-        # ros_ign_bridge (clock -> ROS 2)
-        Node(
-            package="ros_ign_bridge",
-            executable="parameter_bridge",
-            output="log",
-            arguments=[
-                "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
-                "--ros-args",
-                "--log-level",
-                log_level,
-            ],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # World for Ignition Gazebo
-        DeclareLaunchArgument(
-            "world",
-            default_value="default.sdf",
-            description="Name or filepath of world to load.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "ign_verbosity",
-            default_value="2",
-            description="Verbosity level for Ignition Gazebo (0~4).",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

launch/worlds/world_follow_target.launch.py

@@ -1,113 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch worlds/follow_target.sdf and the required ROS<->IGN bridges"""
-
-from os import path
-from typing import List
-
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    world = LaunchConfiguration("world")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    ign_verbosity = LaunchConfiguration("ign_verbosity")
-    log_level = LaunchConfiguration("log_level")
-
-    # List of included launch descriptions
-    launch_descriptions = [
-        # Launch Ignition Gazebo
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("ros_ign_gazebo"),
-                        "launch",
-                        "ign_gazebo.launch.py",
-                    ]
-                )
-            ),
-            launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])],
-        ),
-    ]
-
-    # List of nodes to be launched
-    nodes = [
-        # ros_ign_bridge (clock -> ROS 2)
-        Node(
-            package="ros_ign_bridge",
-            executable="parameter_bridge",
-            output="log",
-            arguments=[
-                "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
-                "--ros-args",
-                "--log-level",
-                log_level,
-            ],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-        # ros_ign_bridge (target pose -> ROS 2)
-        Node(
-            package="ros_ign_bridge",
-            executable="parameter_bridge",
-            output="log",
-            arguments=[
-                "/model/target/pose"
-                + "@"
-                + "geometry_msgs/msg/PoseStamped[ignition.msgs.Pose",
-                "--ros-args",
-                "--log-level",
-                log_level,
-            ],
-            parameters=[{"use_sim_time": use_sim_time}],
-            remappings=[("/model/target/pose", "/target_pose")],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # World for Ignition Gazebo
-        DeclareLaunchArgument(
-            "world",
-            default_value=path.join(
-                get_package_share_directory("ign_moveit2_examples"),
-                "worlds",
-                "follow_target.sdf",
-            ),
-            description="Name or filepath of world to load.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "ign_verbosity",
-            default_value="2",
-            description="Verbosity level for Ignition Gazebo (0~4).",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

launch/worlds/world_throw_object.launch.py

@@ -1,97 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch worlds/throw_object.sdf and the required ROS<->IGN bridges"""
-
-from os import path
-from typing import List
-
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    world = LaunchConfiguration("world")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    ign_verbosity = LaunchConfiguration("ign_verbosity")
-    log_level = LaunchConfiguration("log_level")
-
-    # List of included launch descriptions
-    launch_descriptions = [
-        # Launch Ignition Gazebo
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("ros_ign_gazebo"),
-                        "launch",
-                        "ign_gazebo.launch.py",
-                    ]
-                )
-            ),
-            launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])],
-        ),
-    ]
-
-    # List of nodes to be launched
-    nodes = [
-        # ros_ign_bridge (clock -> ROS 2)
-        Node(
-            package="ros_ign_bridge",
-            executable="parameter_bridge",
-            output="log",
-            arguments=[
-                "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
-                "--ros-args",
-                "--log-level",
-                log_level,
-            ],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # World for Ignition Gazebo
-        DeclareLaunchArgument(
-            "world",
-            default_value=path.join(
-                get_package_share_directory("ign_moveit2_examples"),
-                "worlds",
-                "throw_object.sdf",
-            ),
-            description="Name or filepath of world to load.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "ign_verbosity",
-            default_value="2",
-            description="Verbosity level for Ignition Gazebo (0~4).",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

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

package.xml

@@ -1,11 +1,11 @@
 <?xml version="1.0"?>
 <package format="3">
-  <name>ign_moveit2_examples</name>
+  <name>drawing-robot-ros2</name>
-  <version>1.0.0</version>
+  <version>0.0.0</version>
-  <description>C++ and Python examples of using MoveIt 2 for planning motions that are executed inside Ignition Gazebo simulation environment</description>
+  <description>drawing-robot-ros2</description>
-  <maintainer email="orsula.andrej@gmail.com">Andrej Orsula</maintainer>
+  <maintainer email="a@a.com">a</maintainer>
-  <author>Andrej Orsula</author>
+  <author>a</author>
-  <license>BSD</license>
+  <license>GPL</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
@@ -13,17 +13,11 @@
   <depend>geometry_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
 
-  <exec_depend>ign_ros2_control</exec_depend>
   <exec_depend>moveit</exec_depend>
   <exec_depend>pymoveit2</exec_depend>
   <exec_depend>rclpy</exec_depend>
-  <exec_depend>ros_ign_bridge</exec_depend>
-  <exec_depend>ros_ign_gazebo</exec_depend>
   <exec_depend>rviz2</exec_depend>
 
-  <exec_depend>panda_description</exec_depend>
-  <exec_depend>panda_moveit_config</exec_depend>
-
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

rebuild.sh

@@ -0,0 +1,20 @@
+#!/bin/bash
+
+mkdir import
+vcs import --recursive import < drawing_robot_ros2.repos
+sudo rosdep init
+rosdep update
+rosdep install -y -r -i --rosdistro "humble" --from-paths import
+source "/opt/ros/humble/setup.bash"
+
+pip install https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip --upgrade --upgrade-strategy eager
+
+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 --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

@@ -10,6 +10,40 @@ find_package(ament_cmake REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
+#
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(robot_interfaces REQUIRED)
+find_package(robot_controller REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+
+find_package(tf2_ros REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+add_executable(axidraw_controller src/cpp/axidraw_controller.cpp)
+ament_target_dependencies(axidraw_controller
+    "rclcpp"
+    "geometry_msgs"
+    "robot_controller"
+    "robot_interfaces"
+)
+
+
+install(TARGETS
+  axidraw_controller
+  DESTINATION lib/${PROJECT_NAME})
+
+install(PROGRAMS
+    src/py/axidraw_serial.py
+    DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -22,4 +56,8 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+# Install directories
+#install(DIRECTORY launch rviz urdf worlds DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
 ament_package()

src/axidraw_controller/README.md

@@ -0,0 +1,8 @@
+# axidraw controller
+
+Implements robot_controller for the Axidraw robot.
+
+`axidraw_serial.py` is used to communicate with the robot using the [python API](https://axidraw.com/doc/py_api).
+If more direct control is desired, this can be implemented by sending serial commands directly to the [EBB control board](http://evil-mad.github.io/EggBot/ebb.html) of the Axidraw.
+
+On linux systems the board appears on `/dev/ttyACM0`.

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

@@ -0,0 +1,56 @@
+import os
+import yaml
+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.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+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 RegisterEventHandler, EmitEvent
+from launch.event_handlers import OnProcessExit
+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='info')
+
+    config = LaunchConfiguration("config", default=PathJoinSubstitution([FindPackageShare('axidraw_controller'), 'config', 'config.yaml']))
+
+    robotcontroller_config = config.perform(context)
+    with open(robotcontroller_config, 'r') as f:
+        axidraw_config = yaml.safe_load(f)['/axidraw_serial']['ros__parameters']
+        print(f'Loaded configuration: {axidraw_config}')
+
+    nodes = [
+        Node(
+            package="axidraw_controller",
+            executable="axidraw_controller",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[
+            ],
+        ),
+        Node(
+            package="axidraw_controller",
+            executable="axidraw_serial.py",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[
+                {"serial_port": serial_port},
+                axidraw_config,
+            ],
+        ),
+    ]
+
+    return nodes
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=launch_setup)
+    ])

src/axidraw_controller/package.xml

@@ -12,6 +12,13 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <!-- <depend>robot_interfaces</depend> -->
+  <!-- <depend>robot_controller</depend> -->
+  <depend>geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

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

@@ -0,0 +1,227 @@
+#include <cstdio>
+#include <functional>
+#include <memory>
+#include <thread>
+#include <vector>
+#include <chrono>
+
+#include "robot_controller/robot_controller.hpp"
+#include "robot_interfaces/action/execute_motion.hpp"
+#include "robot_interfaces/msg/points.hpp"
+#include "robot_interfaces/srv/status.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+#include "std_msgs/msg/empty.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+using namespace std::chrono_literals;
+
+/**
+ * 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;
+  rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr move_pub;
+  rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr penup_pub;
+  rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr pendown_pub;
+  rclcpp::Publisher<robot_interfaces::msg::Points>::SharedPtr path_pub;
+
+  public:
+  AxidrawController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : RobotController(options)
+  {
+    status_client = this->create_client<robot_interfaces::srv::Status>("axidraw_status");
+
+    while (!status_client->wait_for_service(5s))
+    {
+      if (!rclcpp::ok())
+      {
+        RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for axidraw_status");
+        //return 0;
+      }
+      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);
+    penup_pub = this->create_publisher<std_msgs::msg::Empty>("axidraw_penup", 10);
+    pendown_pub = this->create_publisher<std_msgs::msg::Empty>("axidraw_pendown", 10);
+    path_pub = this->create_publisher<robot_interfaces::msg::Points>("axidraw_path", 10);
+  }
+
+  /**
+   * Return true if axidraw is ready
+   */
+  bool is_ready()
+  {
+    auto request = std::make_shared<robot_interfaces::srv::Status::Request>();
+    request->resource = "motion";
+    auto result = status_client->async_send_request(request);
+
+    result.wait();
+    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;
+    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)
+   */
+  geometry_msgs::msg::Point translate_pose(geometry_msgs::msg::PoseStamped ps)
+  {
+    auto pose = ps.pose;
+    auto point = geometry_msgs::msg::Point();
+
+    point.x = translate(pose.position.x, 0, 1, 0, xlim);
+    point.y = translate(pose.position.y, 0, 1, 0, ylim);
+
+    // Axidraw z-axis has 2 states, pen up is 1, pen down is 0.
+    point.z = pose.position.z > 0 ? 1 : 0;
+
+    return point;
+  }
+
+  // Translate all poses in a vector
+  std::vector<geometry_msgs::msg::Point> translate_poses(std::vector<geometry_msgs::msg::PoseStamped> ps)
+  {
+    std::vector<geometry_msgs::msg::Point> points;
+    for (auto p : ps)
+      points.push_back(translate_pose(p));
+    return points;
+  }
+
+
+  /**
+   * 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");
+    rclcpp::Rate loop_rate(20);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
+    auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
+
+    //for (auto p : goal->motion.path)
+    //{
+    //  auto pp = p.pose;
+    //  RCLCPP_INFO(this->get_logger(), "Position x:%f y:%f z:%f",pp.position.x,pp.position.y,pp.position.z);
+    //  RCLCPP_INFO(this->get_logger(), "Orientation w:%f x:%f y:%f z:%f", pp.orientation.w, pp.orientation.x,pp.orientation.y,pp.orientation.z);
+    //  //  W:%f X:%f Y:%f Z:%f
+    //}
+
+    auto points = translate_poses(goal->motion.path);
+
+    for (long unsigned int i = 0; (i < points.size()) && rclcpp::ok(); ++i)
+    {
+      // Check if there is a cancel request
+      if (goal_handle->is_canceling())
+      {
+        result->result = feedback->status;
+        goal_handle->canceled(result);
+        RCLCPP_INFO(this->get_logger(), "Goal canceled");
+        return;
+      }
+
+      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());
+        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";
+      // Publish feedback
+      goal_handle->publish_feedback(feedback);
+      RCLCPP_INFO(this->get_logger(), feedback->status.c_str());
+
+    }
+
+    // Check if goal is done
+    if (rclcpp::ok())
+    {
+      result->result = "success";
+      goal_handle->succeed(result);
+      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+    }
+  }
+};
+
+/**
+ *
+ */
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting axidraw_controller");
+  auto robot = std::make_shared<AxidrawController>();
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(robot);
+  executor.spin();
+
+  rclcpp::shutdown();
+  return EXIT_SUCCESS;
+}

src/axidraw_controller/src/py/axidraw_serial.py

@@ -0,0 +1,242 @@
+#!/usr/bin/env python3
+from pyaxidraw import axidraw
+
+from robot_interfaces.srv import Status
+from robot_interfaces.msg import Points
+from geometry_msgs.msg import Point
+from std_msgs.msg import Empty
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+
+#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
+        if not self.ad.connect():            # Open serial port to AxiDraw;
+            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:{}, 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))
+        self.pendown_sub = self.create_subscription(Empty, 'axidraw_pendown', self.pendown_callback, qos_profile=QoSProfile(depth=1))
+        self.path_sub = self.create_subscription(Points, 'axidraw_path', self.stroke_callback, qos_profile=QoSProfile(depth=1))
+
+    def get_status(self, request, response):
+        '''
+        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):
+        '''
+        Sets the robot motion to "busy"
+            Parameters:
+            Returns:
+        '''
+        self.status["motion"] = "busy"
+
+    def set_ready(self):
+        '''
+        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))
+
+        self.ad.goto(msg.x,msg.y)
+        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.points[:6]))
+
+        path = [ [p.x,p.y] for p in msg.points ]
+        self.ad.draw_path(path)
+        self.status['pen'] = "up"
+        self.set_ready()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    axidraw_serial = AxidrawSerial()
+    try:
+        rclpy.spin(axidraw_serial)
+    finally:
+        axidraw_serial.go_home()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/axidraw_controller/src/test.py

@@ -1,13 +0,0 @@
-#!/usr/bin/env python3
-from pyaxidraw import axidraw   # import module
-ad = axidraw.AxiDraw()          # Initialize class
-ad.interactive()                # Enter interactive context
-ad.options.port = "/dev/ttyAXI"
-if not ad.connect():            # Open serial port to AxiDraw;
-    quit()                      #   Exit, if no connection.
-ad.options.units = 1            # set working units to cm.
-                                # Absolute moves follow:
-ad.moveto(1, 1)                 # Pen-up move to (1 inch, 1 inch)
-ad.lineto(2, 1)                 # Pen-down move, to (2 inch, 1 inch)
-ad.moveto(0, 0)                 # Pen-up move, back to origin.
-ad.disconnect()                 # Close serial port to AxiDraw

src/custom_xarm_description/CMakeLists.txt

@@ -1,5 +1,15 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.5)
-project(drawing_controller)
+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)
@@ -22,4 +32,12 @@ if(BUILD_TESTING)
   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
+    ])