Auto serial (#1)

* default serial-port=auto - detect the serial from device and vendor id

.github/workflows/snap.yaml

@@ -5,9 +5,9 @@ on:
       - '*'
     branches:
       - main
-  pull_request:
-    branches:
-      - main
+  # pull_request:
+  #   branches:
+  #     - main
   workflow_dispatch:
 
 jobs:

README.md

@@ -1,2 +1,118 @@
-# husarion-rplidar-snap
-Snap package for RPLIDAR lidars with configurations for Husarion robots
+## husarion-rplidar-snap
+
+Snap for SLAMTEC LIDARs customized for Husarion robots.
+
+## Apps
+
+| app | description |
+| - | - |
+| `husarion-rplidar.start` | Start the `husarion-rplidar.daemon` service |
+| `husarion-rplidar.stop` | Stop the `husarion-rplidar.daemon` service |
+| `husarion-rplidar` | Start the application in the foreground (run in the current terminal). Remember to stop the daemon first |
+
+## Quick start
+
+1. Connect the RPLIDAR to the USB port.
+2. Install the snap:
+
+   ```bash
+   sudo snap install husarion-rplidar
+   ```
+
+3. Verify the `/scan` topic is available:
+
+   ```bash
+   ros2 topic list
+   ```
+
+   You should see `/scan` listed.
+
+## Unplugging and Plugging the LIDAR
+
+### Before Unplugging
+
+Run the following command to stop the service:
+
+```bash
+husarion-rplidar.stop
+```
+
+### After Plugging Back In
+
+Run the following command to start the service:
+
+```bash
+husarion-rplidar.start
+```
+
+### Restarting the Driver
+
+If you need to restart the driver, run:
+
+```bash
+husarion-rplidar.stop
+husarion-rplidar.start
+```
+
+## Setup RPLIDAR Params
+
+### ROS 2 Parameters
+
+All `husarion-rplidar` ROS 2 parameters are available under the `driver` key.
+
+| Key | Default Value |
+| driver.device-namespace | (unset) |
+| driver.namespace | (unset) |
+| driver.scan-mode | (unset) |
+| driver.channel-type | serial |
+| driver.frame-id | laser |
+| driver.inverted | false |
+| driver.angle-compensate | true |
+
+to set the parameters, use the `snap set` command. For example:
+
+```bash
+snap set husarion-rplidar driver.namespace=myrobot
+```
+
+### Additional Parameters
+
+| Key | Default Value |
+| driver | {...} |
+| ros-domain-id | 0 |
+| ros-localhost-only | 0 |
+| transport | udp |
+| serial-port | auto |
+| serial-baudrate | 256000 |
+
+Available DDS configs for the `transport` parameter are `builtin`, `udp` and `shm`.
+
+You can modify configurations for `udp` and `shm` under this path:
+
+```bash
+ls /var/snap/husarion-rplidar/common/
+```
+
+You should see:
+
+```bash
+shm.xml  udp.xml
+```
+
+You can also create your own DDS config files, eg.:
+
+```bash
+vim /var/snap/husarion-rplidar/common/my-custom-transport.xml
+```
+
+and use them with:
+
+```bash
+sudo snap set husarion-rplidar transport=my-custom-transport
+```
+
+By default, `serial-port` is set to `auto`, which tries to automatically determine the serial port where the LIDAR is connected. If you have multiple LIDARs, it's recommended to set the full path to the serial interface (or a symlink):
+
+```bash
+sudo snap set husarion-rplidar serial=/dev/ttyUSB0
+```

justfile

@@ -35,6 +35,8 @@ iterate:
     unsquashfs husarion-rplidar*.snap
     sudo snap try squashfs-root/
     sudo snap connect husarion-rplidar:raw-usb
+    sudo snap connect husarion-rplidar:shm-plug husarion-rplidar:shm-slot
+    sudo husarion-rplidar.stop
 
     end_time=$(date +%s)
     duration=$(( end_time - start_time ))

snap/hooks/configure

@@ -61,14 +61,23 @@ for OPT in ${OPTS}; do
   fi
 done
 
-# Make sure ROS 2 nodes parameters value is a boolean
+# Make sure the serial-baudrate value is valid
 OPT="serial-baudrate"
-VALUE="$(snapctl get driver.${OPT})"
+VALUE="$(snapctl get ${OPT})"
 if ! is_integer "${VALUE}" ; then
   log_and_echo "'${VALUE}' is not a supported value for '${OPT}'. Possible values are integers."
   exit 1
 fi
 
+# Make sure the serial-port value is valid
+OPT="serial-port"
+VALUE="$(snapctl get ${OPT})"
+
+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
+
 # restart services with new ROS 2 config
 for service in daemon; do
   if snapctl services ${SNAP_NAME}.${service} | grep -qw active; then

snap/hooks/install

@@ -12,15 +12,17 @@ $SNAP/usr/bin/install_hook_ros.sh
 # Parameters are 'unset'.
 # Default configuration values is left to the launch file.
 snapctl set driver.channel-type="serial"
-snapctl set driver.serial-port="/dev/ttyUSB0"
-snapctl set driver.serial-baudrate="256000"
 snapctl set driver.frame-id="laser"
 snapctl set driver.inverted="false"
 snapctl set driver.angle-compensate="true"
 snapctl set driver.scan-mode!
 snapctl set driver.namespace!
 snapctl set driver.device-namespace!
 
+# snapctl set serial-port="/dev/ttyRPLIDAR"
+snapctl set serial-port="auto"
+snapctl set serial-baudrate="256000"
+
 if ! snapctl is-connected raw-usb; then
   log "Plug 'raw-usb' isn't connected, please run:"
   log "sudo snap connect ${SNAP_NAME}:raw-usb"

snap/local/find_ttyUSB.sh

@@ -0,0 +1,40 @@
+#!/bin/bash
+
+# Function to find the ttyUSB* device for the specified USB Vendor and Product ID
+find_ttyUSB() {
+  # Extract vendor and product ID
+  VENDOR_ID="$1"
+  PRODUCT_ID="$2"
+
+  # Get the serial-port value using snapctl
+  SERIAL_PORT=$(snapctl get serial-port)
+
+  if [ "$SERIAL_PORT" == "auto" ]; then
+    for device in /sys/bus/usb/devices/*; do
+      if [ -f "$device/idVendor" ]; then
+        current_vendor_id=$(cat "$device/idVendor")
+        if [ "$current_vendor_id" == "$VENDOR_ID" ]; then
+          if [ -z "$PRODUCT_ID" ] || ([ -f "$device/idProduct" ] && [ "$(cat "$device/idProduct")" == "$PRODUCT_ID" ]); then
+            # Look for ttyUSB device in the subdirectories
+            for subdir in "$device/"*; do
+              if [ -d "$subdir" ]; then
+                for tty in $(find "$subdir" -name "ttyUSB*" -print); do
+                  if [ -e "$tty" ]; then
+                    ttydev=$(basename "$tty")
+                    echo "/dev/$ttydev"
+                    return 0
+                  fi
+                done
+              fi
+            done
+          fi
+        fi
+      fi
+    done
+    echo "Error: Device with ID $VENDOR_ID:${PRODUCT_ID:-*} not found."
+    return 1
+  else
+    echo "$SERIAL_PORT"
+    return 0
+  fi
+}

snap/local/launcher.sh

@@ -1,17 +1,51 @@
 #!/usr/bin/bash
 
+log_and_echo() {
+    local message="$1"
+    # Log the message with logger
+    logger -t "${SNAP_NAME}" "configure hook: $message"
+    # Echo the message to standard error
+    echo >&2 "$message"
+}
+
 log() {
     local message="$1"
     # Log the message with logger
-    logger -t "${SNAP_NAME}" "launcher: $message"
+    logger -t "${SNAP_NAME}" "configure hook: $message"
 }
 
+# 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
+if [ "$SERIAL_PORT" == "auto" ]; then
+  # Find the ttyUSB* device
+  SERIAL_PORT=$(find_ttyUSB "10c4" "ea60")
+  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
+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
+fi
+
 # Iterate over the snap parameters and retrieve their value.
 # If a value is set, it is forwarded to the launch file.
+LAUNCH_OPTIONS=""
+
 OPTIONS="\
  channel-type \
- serial-port \
- serial-baudrate \
  frame-id \
  inverted \
  angle-compensate \
@@ -20,21 +54,34 @@ OPTIONS="\
  device-namespace \
 "
 
-LAUNCH_OPTIONS=""
-
 for OPTION in ${OPTIONS}; do
   VALUE="$(snapctl get driver.${OPTION})"
   if [ -n "${VALUE}" ]; then
     LAUNCH_OPTIONS+="${OPTION}:=${VALUE} "
   fi
 done
 
+OPTIONS="\
+ serial-baudrate \
+"
+
+for OPTION in ${OPTIONS}; do
+  VALUE="$(snapctl get ${OPTION})"
+  if [ -n "${VALUE}" ]; then
+    LAUNCH_OPTIONS+="${OPTION}:=${VALUE} "
+  fi
+done
+
+# Add the serial port to LAUNCH_OPTIONS
+LAUNCH_OPTIONS+="serial-port:=${SERIAL_PORT} "
+
 # Replace '-' with '_'
 LAUNCH_OPTIONS=$(echo ${LAUNCH_OPTIONS} | tr - _)
 
 if [ "${LAUNCH_OPTIONS}" ]; then
   # watch the log with: "journalctl -t rosbot-xl"
-  log "Running with options: ${LAUNCH_OPTIONS}"
+  log_and_echo "Running with options: ${LAUNCH_OPTIONS}"
 fi
 
+# Launch the ros2 launch file with the determined options
 ros2 launch $SNAP/usr/bin/rplidar.launch.py ${LAUNCH_OPTIONS}

snap/snapcraft.yaml

@@ -3,18 +3,29 @@ adopt-info: husarion-rplidar
 license: Apache-2.0
 summary: The husarion-rplidar
 description: |
-  The rhusarion-rplidar snap contains all the necessary software to bring the robot up
-  such as controller, robot state publisher etc.
-
-  It offers the following parameters,
-
-  - ros-domain-id
-  - ros-localhost-only
-  - transport
-  - serial-port
-  - driver.namespace
-
-  which can be set as follows, e.g.,
+  The husarion-rplidar snap contains all the necessary software to bring the RPLIDAR laser sensors up.
+  It offers the following parameters:
+
+    Key                 Default Value
+    driver              {...}
+    ros-domain-id       0
+    ros-localhost-only  0
+    transport           udp
+    serial-port         auto
+    serial-baudrate     256000
+    
+  The driver parameter is a dictionary that contains the following keys:
+
+    Key                            Default Value
+    driver.device-namespace        (unset)
+    driver.namespace               (unset)
+    driver.scan-mode               (unset)
+    driver.channel-type            serial
+    driver.frame-id                laser
+    driver.inverted                false
+    driver.angle-compensate        true
+
+  To set the parameters, use the snap set command, e.g.,
 
     snap set husarion-rplidar driver.namespace=robot
 
@@ -74,6 +85,7 @@ parts:
     plugin: nil
     stage-packages:
       - ros-humble-rplidar-ros
+      - usbutils # lsusb
     override-stage: |
       craftctl default
       version="$(apt-cache policy ros-humble-rplidar-ros | grep Candidate | awk '{print $2}')"