Manipulator setup (#15)

- added manipulation package for ROSbot XL
- choosing the configuraiton with `sudo snap set rosbot-xl configuraiton=manipulation` (available options: `basic`, `manipulation`)
- added support for a USB gamepad (started with `sudo snap start rosbot-xl.joy`
- Group `ros-domain-id`, `ros-localhost-only`, `tranport` and `namespace` behind the `ros` key

demo/compose.yaml

@@ -0,0 +1,38 @@
+# Usage:
+# xhost +local:docker && docker compose up
+
+x-net-config:
+  &net-config
+  network_mode: host
+  ipc: host
+  env_file: net.env
+
+x-gpu-config:
+  &gpu-config
+  runtime: nvidia
+  environment:
+    - DISPLAY=${DISPLAY:?err}
+    - NVIDIA_VISIBLE_DEVICES=all
+    - NVIDIA_DRIVER_CAPABILITIES=all
+
+services:
+  rviz:
+    # Launching rviz with moveit2 requires all the configs, using manipulation simulation image is easiest option
+    image: husarion/rosbot-xl-manipulation-gazebo:humble
+    <<: [ *net-config, *gpu-config ]
+    container_name: rviz
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix:rw
+      - ./config/rosbot_xl.rviz:/ros2_ws/install/rosbot_xl_manipulation_moveit/share/rosbot_xl_manipulation_moveit/config/moveit.rviz
+    command: ros2 launch rosbot_xl_manipulation_moveit rviz.launch.py
+
+  joy2twist:
+    image: husarion/joy2twist:humble-1.0.0-20230204-stable
+    <<: *net-config
+    devices:
+      - /dev/input
+    volumes: 
+      - ./config/joy2twist.yaml:/joy2twist.yaml
+    command: > 
+      ros2 launch joy2twist gamepad_controller.launch.py
+        joy2twist_params_file:=/joy2twist.yaml

demo/config/joy2twist.yaml

@@ -0,0 +1,22 @@
+/**:
+  ros__parameters:
+    linear_velocity_factor:
+      fast: 1.0
+      regular: 0.5
+      slow: 0.2
+
+    angular_velocity_factor:
+      fast: 1.5
+      regular: 0.8
+      slow: 0.4
+
+    # This button mapping should be adjusted to the specific controller
+    # The following map is suited for Logitech F710
+    button_index_map:
+      axis:
+        angular_z: 2 # Right joystick
+        linear_x: 1 # Left joystick
+        linear_y: 0 # Left joystick
+      dead_man_switch: 4 # LB
+      fast_mode: 7 # RT
+      slow_mode: 5 # RB

demo/net.env

@@ -0,0 +1,18 @@
+# =======================================
+# Network config options (uncomment one)
+# =======================================
+
+# 1. Fast DDS + LAN
+# RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+
+# 2. Cyclone DDS + LAN
+RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+
+# 3. Fast DDS + VPN
+# RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+# FASTRTPS_DEFAULT_PROFILES_FILE=/husarnet-fastdds.xml
+
+# 4. Cyclone DDS + VPN
+# RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+# FASTRTPS_DEFAULT_PROFILES_FILE=/husarnet-fastdds.xml
+# CYCLONEDDS_URI=file:///husarnet-cyclonedds.xml

justfile

@@ -158,6 +158,7 @@ iterate:
     sudo snap try squashfs-root/
     sudo snap connect rosbot-xl:raw-usb
     sudo snap connect rosbot-xl:shm-plug rosbot-xl:shm-slot
+    sudo snap connect rosbot-xl:shutdown
 
     end_time=$(date +%s)
     duration=$(( end_time - start_time ))

snap/hooks/configure

@@ -7,51 +7,28 @@
 
 source $SNAP/usr/bin/utils.sh
 
-$SNAP/usr/bin/configure_hook_ros.sh
-
-# Function to validate the option values
-validate_option() {
-  local OPT=$1
-  local VALID_OPTIONS=("${!2}")
-
-  VALUE="$(snapctl get driver.${OPT})"
+# Define the top-level key and the list of valid keys
+VALID_DRIVER_KEYS=("mecanum" "include-camera-mount" "camera-model" "lidar-model" "db-serial-port" "manipulator-serial-port" )
+VALID_WEBUI_KEYS=("layout" "port")
 
-  # Create an associative array to check valid options
-  declare -A valid_options_map
-  for option in "${VALID_OPTIONS[@]}"; do
-    valid_options_map["$option"]=1
-  done
-
-  # Join the valid options with newlines
-  JOINED_OPTIONS=$(printf "%s\n" "${VALID_OPTIONS[@]}")
-
-  if [ -n "${VALUE}" ]; then
-    if [[ -z "${valid_options_map[$VALUE]}" ]]; then
-      log_and_echo "'${VALUE}' is not a supported value for '${OPT}'. Possible values are:\n${JOINED_OPTIONS}"
-      exit 1
-    fi
-  fi
-}
+# Call the validation function
+validate_keys "driver" VALID_DRIVER_KEYS[@]
+validate_keys "webui" VALID_WEBUI_KEYS[@]
 
 # Define the valid options for camera model and lidar model
 VALID_CAMERA_OPTIONS=("None" "intel_realsense_d435" "orbbec_astra" "stereolabs_zed" "stereolabs_zedm" "stereolabs_zed2" "stereolabs_zed2i" "stereolabs_zedx" "stereolabs_zedxm")
 VALID_LIDAR_OPTIONS=("None" "slamtec_rplidar_a2" "slamtec_rplidar_a3" "slamtec_rplidar_s1" "slamtec_rplidar_s2" "slamtec_rplidar_s3" "velodyne_puck")
 VALID_BOOLEAN_OPTIONS=("True" "False")
 
 # Validate parameters with array of possible values
-validate_option "camera-model" VALID_CAMERA_OPTIONS[@]
-validate_option "lidar-model" VALID_LIDAR_OPTIONS[@]
-validate_option "mecanum" VALID_BOOLEAN_OPTIONS[@]
-validate_option "include-camera-mount" VALID_BOOLEAN_OPTIONS[@]
-
-# Make sure the serial-port value is valid
-OPT="serial-port"
-VALUE="$(snapctl get ${OPT})"
+validate_option "driver.camera-model" VALID_CAMERA_OPTIONS[@]
+validate_option "driver.lidar-model" VALID_LIDAR_OPTIONS[@]
+validate_option "driver.mecanum" VALID_BOOLEAN_OPTIONS[@]
+validate_option "driver.include-camera-mount" VALID_BOOLEAN_OPTIONS[@]
 
-if [ "${VALUE}" != "auto" ] && [ ! -e "${VALUE}" ]; then
-  log_and_echo "'${VALUE}' is not a valid value for '${OPT}'. It must be 'auto' or a valid serial port in the /dev/ directory."
-  exit 1
-fi
+# Validate specific serial ports
+validate_serial_port "driver.db-serial-port"
+validate_serial_port "driver.manipulator-serial-port"
 
 # Get the transport setting using snapctl
 OPT="webui.layout"
@@ -64,17 +41,32 @@ if [ ! -f "${SNAP_COMMON}/foxglove-${LAYOUT}.json" ]; then
 fi
 
 # Make sure WEBUI_PORT is valid
-OPT="webui.port"
-WEBUI_PORT="$(snapctl get ${OPT})"
+EXCLUDED_PORTS=(3000 8888)
+SUPPORTED_RANGE=(0 65535)
 
-if ! is_integer "${WEBUI_PORT}" || [ "${WEBUI_PORT}" -lt 0 ] || [ "${WEBUI_PORT}" -gt 65535 ] || [ "${WEBUI_PORT}" -eq 3000 ] || [ "${WEBUI_PORT}" -eq 8888 ]; then
-  log_and_echo "'${WEBUI_PORT}' is not a supported value for '${OPT}'. Possible values are integers between 0 and 65535, excluding 3000 and 8888."
-  exit 1
+# Validate a specific port, for example, webui.port
+validate_number "webui.port" SUPPORTED_RANGE[@] EXCLUDED_PORTS[@]
+
+VALID_CONFIGURATION_OPTIONS=("basic" "manipulation")
+
+# Validate parameters with array of possible values
+validate_option "configuration" VALID_CONFIGURATION_OPTIONS[@]
+
+OPT="configuration"
+VALUE="$(snapctl get ${OPT})"
+if [ "${VALUE}" == "manipulation" ]; then
+  log_and_echo "Currently the 'ros.namespace', 'driver.include-camera-mount' and 'driver.camera-model' are not supported for the 'manipulation' option. Unsetting them."
+  snapctl set ros.namespace!
+  snapctl set driver.camera-model=None
+  snapctl set driver.include-camera-mount=False
 fi
 
+# moved to the end because for manipulator we usnet ros.namespace
+$SNAP/usr/bin/configure_hook_ros.sh
+
 # restart services with new ROS 2 config
-for service in daemon web-ui web-ws; do
-  if snapctl services ${SNAP_NAME}.${service} | grep -qw active; then
+for service in daemon web-ui web-ws joy; do
+  if snapctl services ${SNAP_NAME}.${service} | grep -qw enabled; then
     snapctl restart ${SNAP_NAME}.${service}
     log "Restarted ${SNAP_NAME}.${service}"
   fi

snap/hooks/install

@@ -11,12 +11,13 @@ snapctl set driver.mecanum=True
 snapctl set driver.include-camera-mount=True
 snapctl set driver.camera-model=None
 snapctl set driver.lidar-model=None
-snapctl set driver.namespace! # unset
+snapctl set driver.db-serial-port=auto
+snapctl set driver.manipulator-serial-port=auto
 
 snapctl set webui.layout=default
 snapctl set webui.port=8080
 
-snapctl set serial-port="auto"
+snapctl set configuration=basic
 
 if ! snapctl is-connected raw-usb; then
   log "Plug 'raw-usb' isn't connected, please run:"
@@ -32,5 +33,10 @@ cp -r $SNAP/opt/ros/snap/share/ros_components_description ${SNAP_COMMON}/ros2_ws
 # copy joy params
 cp -r $SNAP/usr/share/rosbot-xl/config/teleop_twist_joy_params.yaml ${SNAP_COMMON}/
 
+# copy joy params
+cp -r $SNAP/usr/share/rosbot-xl/config/joy_servo.yaml ${SNAP_COMMON}/
+cp -r $SNAP/usr/share/rosbot-xl/config/joy_teleop.config.yaml ${SNAP_COMMON}/
+cp -r $SNAP/usr/share/rosbot-xl/config/joy_params.yaml ${SNAP_COMMON}/
+
 # copy webui layouts
 cp -r $SNAP/usr/share/$SNAP_NAME/config/foxglove-*.json ${SNAP_COMMON}/

snap/local/arm_activate_launcher.sh

@@ -0,0 +1,7 @@
+#!/bin/bash -e
+
+STATE=$1
+
+ros2 service call /controller_manager/set_hardware_component_state \
+    controller_manager_msgs/srv/SetHardwareComponentState \
+    "{name: 'manipulator', target_state: {id: 0, label: '${STATE}'}}"

snap/local/bridge_launcher.sh

@@ -1,10 +1,13 @@
 #!/bin/bash -e
 
-NAMESPACE="$(snapctl get driver.namespace)"
+LAUNCH_OPTIONS=""
 
-# Check if the namespace is set and not empty
+# Retrieve the namespace using snapctl
+NAMESPACE="$(snapctl get ros.namespace)"
+
+# Check if NAMESPACE is not set or is empty
 if [ -n "$NAMESPACE" ]; then
-  ros2 launch $SNAP/usr/bin/bridge_launch.py namespace:=${NAMESPACE}
-else
-  ros2 launch $SNAP/usr/bin/bridge_launch.py
+    LAUNCH_OPTIONS+="namespace:=${NAMESPACE} "
 fi
+
+ros2 launch $SNAP/usr/bin/bridge_launch.py ${LAUNCH_OPTIONS}

snap/local/caddy_launcher.sh

@@ -29,8 +29,8 @@ echo "${index_html_content/"$replace_pattern"/$replace_value}" > $index_html_pat
 sed -i "s|<div id=\"root\"></div>|<script>\nlocalStorage.clear();sessionStorage.clear();\n</script>\n&|" $index_html_path
 
 # Define a function to log and echo messages
-# Retrieve the driver.namespace value
-namespace=$(snapctl get driver.namespace)
+# Retrieve the ros.namespace value
+namespace=$(snapctl get ros.namespace)
 
 # Check if the namespace is set and not empty
 if [ -n "$namespace" ]; then

snap/local/flash_launcher.sh

@@ -10,38 +10,29 @@ if [ "$(id -u)" -ne 0 ]; then
 fi
 
 # Check if the daemon is running and stop it if it is
-if snapctl services "${SNAP_NAME}.daemon" | grep -qw active; then
+if snapctl services "${SNAP_NAME}.daemon" | grep -qw enabled; then
     log_and_echo "Stopping ${SNAP_NAME}.daemon..."
     snapctl stop "${SNAP_NAME}.daemon"
     DAEMON_WAS_RUNNING=true
 else
     DAEMON_WAS_RUNNING=false
 fi
 
-# Source the find_ttyUSB function
-source $SNAP/usr/bin/find_ttyUSB.sh
-
-# Get the serial-port value using snapctl
-SERIAL_PORT=$(snapctl get serial-port)
+# Check if SERIAL_PORT is set to auto or specified
+SERIAL_PORT=$(find_ttyUSB driver.db-serial-port "0403" "6015")
+if [ $? -ne 0 ]; then
+  log_and_echo "Failed to find the serial port."
+  exit 1
+else
+  log_and_echo "Found serial port: $SERIAL_PORT"
+fi
 
-# Check if SERIAL_PORT is set to auto
-if [ "$SERIAL_PORT" == "auto" ]; then
-  # Find the ttyUSB* device
-  SERIAL_PORT=$(find_ttyUSB "0403" "6015")
-  if [ $? -ne 0 ]; then
-    log_and_echo "Failed to find the serial port."
-    exit 1
-  else
-    log_and_echo "Found serial port: $SERIAL_PORT"
-  fi
+# Check if the specified serial port exists
+if [ ! -e "$SERIAL_PORT" ]; then
+  log_and_echo "Specified serial port $SERIAL_PORT does not exist."
+  exit 1
 else
-  # Check if the specified serial port exists
-  if [ ! -e "$SERIAL_PORT" ]; then
-    log_and_echo "Specified serial port $SERIAL_PORT does not exist."
-    exit 1
-  else
-    log_and_echo "Specified serial port exists: $SERIAL_PORT"
-  fi
+  log_and_echo "Specified serial port exists: $SERIAL_PORT"
 fi
 
 ros2 run rosbot_xl_utils flash_firmware --port $SERIAL_PORT

snap/local/foxglove-set-namespace.sh

@@ -1,5 +1,7 @@
 #!/bin/bash -e
 
+# deprecated (using caddy templates instead)
+
 LAYOUT="$(snapctl get webui.layout)"
 NAMESPACE="$(snapctl get driver.namespace)"
 

snap/local/joy_launcher.sh

@@ -0,0 +1,19 @@
+#!/bin/bash -e
+
+ros2 run joy joy_node --ros-args --params-file $SNAP_COMMON/joy_params.yaml
+
+# # Retrieve the namespace using snapctl
+# NAMESPACE="$(snapctl get ros.namespace)"
+
+# # Check if NAMESPACE is not set or is empty
+# if [ -z "$NAMESPACE" ]; then
+#     # No namespace is set, run the launch command without the namespace argument
+#     ros2 launch teleop_twist_joy teleop-launch.py \
+#         config_filepath:=$SNAP_COMMON/joy_teleop.config.yaml
+#     # ros2 launch teleop_twist_joy teleop-launch.py
+# else
+#     # Namespace is set, include it in the launch command
+#     ros2 launch teleop_twist_joy teleop-launch.py \
+#         config_filepath:=$SNAP_COMMON/joy_teleop.config.yaml --ros-args -r __ns:=/$NAMESPACE
+#     # ros2 launch teleop_twist_joy teleop-launch.py --ros-args -r __ns:=/$NAMESPACE
+# fi

snap/local/joy_params.yaml

@@ -0,0 +1,6 @@
+joy_node:
+  ros__parameters:
+    dev: "/dev/input/js0"  # Replace with your joystick device
+    deadzone: 0.1
+    autorepeat_rate: 20.0
+    # dev_ff: "/dev/input/event18" # based on evtest