From 56964fda699ec2dce4209edca9d7244fdd5b41ce Mon Sep 17 00:00:00 2001 From: Dominik Nowak Date: Wed, 10 Jul 2024 16:28:02 +0200 Subject: [PATCH] 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 | 38 ++ demo/config/joy2twist.yaml | 22 ++ demo/net.env | 18 + justfile | 1 + snap/hooks/configure | 78 ++-- snap/hooks/install | 10 +- snap/local/arm_activate_launcher.sh | 7 + snap/local/bridge_launcher.sh | 13 +- snap/local/caddy_launcher.sh | 4 +- snap/local/flash_launcher.sh | 37 +- snap/local/foxglove-set-namespace.sh | 2 + snap/local/joy_launcher.sh | 19 + snap/local/joy_params.yaml | 6 + snap/local/joy_servo.yaml | 69 ++++ snap/local/joy_teleop.config.yaml | 26 ++ snap/local/launcher.sh | 43 +- .../check_daemon_running.sh | 0 .../configure_hook_ros.sh | 45 +-- .../install_hook_ros.sh | 7 +- .../{ros_common => local-ros}/ros_setup.sh | 0 snap/local/{ros_common => local-ros}/shm.xml | 0 .../udp-lo-cyclone.xml | 0 .../{ros_common => local-ros}/udp-lo.xml | 0 snap/local/{ros_common => local-ros}/udp.xml | 0 snap/local/local-ros/utils.sh | 224 +++++++++++ snap/local/ros_common/find_ttyUSB.sh | 40 -- snap/local/ros_common/utils.sh | 23 -- snap/local/rosbot-manipulator.launch.py | 368 ++++++++++++++++++ snap/local/rosbot.launch.py | 254 ++++++++++++ snap/local/teleop_launcher.sh | 2 +- snap/snapcraft.yaml | 132 ++++--- 31 files changed, 1268 insertions(+), 220 deletions(-) create mode 100644 demo/compose.yaml create mode 100644 demo/config/joy2twist.yaml create mode 100644 demo/net.env create mode 100755 snap/local/arm_activate_launcher.sh create mode 100755 snap/local/joy_launcher.sh create mode 100644 snap/local/joy_params.yaml create mode 100644 snap/local/joy_servo.yaml create mode 100644 snap/local/joy_teleop.config.yaml rename snap/local/{ros_common => local-ros}/check_daemon_running.sh (100%) rename snap/local/{ros_common => local-ros}/configure_hook_ros.sh (83%) rename snap/local/{ros_common => local-ros}/install_hook_ros.sh (81%) rename snap/local/{ros_common => local-ros}/ros_setup.sh (100%) rename snap/local/{ros_common => local-ros}/shm.xml (100%) rename snap/local/{ros_common => local-ros}/udp-lo-cyclone.xml (100%) rename snap/local/{ros_common => local-ros}/udp-lo.xml (100%) rename snap/local/{ros_common => local-ros}/udp.xml (100%) create mode 100755 snap/local/local-ros/utils.sh delete mode 100755 snap/local/ros_common/find_ttyUSB.sh delete mode 100755 snap/local/ros_common/utils.sh create mode 100644 snap/local/rosbot-manipulator.launch.py create mode 100644 snap/local/rosbot.launch.py diff --git a/demo/compose.yaml b/demo/compose.yaml new file mode 100644 index 0000000..6cd9d00 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/demo/config/joy2twist.yaml b/demo/config/joy2twist.yaml new file mode 100644 index 0000000..4806940 --- /dev/null +++ b/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 diff --git a/demo/net.env b/demo/net.env new file mode 100644 index 0000000..a498c9c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/justfile b/justfile index db011e7..953c7d8 100644 --- a/justfile +++ b/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 )) diff --git a/snap/hooks/configure b/snap/hooks/configure index 245c04e..5425f40 100644 --- a/snap/hooks/configure +++ b/snap/hooks/configure @@ -7,31 +7,13 @@ 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") @@ -39,19 +21,14 @@ VALID_LIDAR_OPTIONS=("None" "slamtec_rplidar_a2" "slamtec_rplidar_a3" "slamtec_r 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 diff --git a/snap/hooks/install b/snap/hooks/install index cf1ed1c..8b4bea7 100644 --- a/snap/hooks/install +++ b/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}/ \ No newline at end of file diff --git a/snap/local/arm_activate_launcher.sh b/snap/local/arm_activate_launcher.sh new file mode 100755 index 0000000..43eae20 --- /dev/null +++ b/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}'}}" \ No newline at end of file diff --git a/snap/local/bridge_launcher.sh b/snap/local/bridge_launcher.sh index 4ad5267..f34213b 100755 --- a/snap/local/bridge_launcher.sh +++ b/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} diff --git a/snap/local/caddy_launcher.sh b/snap/local/caddy_launcher.sh index dcda6c0..75b2bc8 100755 --- a/snap/local/caddy_launcher.sh +++ b/snap/local/caddy_launcher.sh @@ -29,8 +29,8 @@ echo "${index_html_content/"$replace_pattern"/$replace_value}" > $index_html_pat sed -i "s|
|\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 diff --git a/snap/local/flash_launcher.sh b/snap/local/flash_launcher.sh index c9a5c65..ef6ef46 100755 --- a/snap/local/flash_launcher.sh +++ b/snap/local/flash_launcher.sh @@ -10,7 +10,7 @@ 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 @@ -18,30 +18,21 @@ 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 diff --git a/snap/local/foxglove-set-namespace.sh b/snap/local/foxglove-set-namespace.sh index 6f4d036..e1fe76c 100755 --- a/snap/local/foxglove-set-namespace.sh +++ b/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)" diff --git a/snap/local/joy_launcher.sh b/snap/local/joy_launcher.sh new file mode 100755 index 0000000..b41f19a --- /dev/null +++ b/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 diff --git a/snap/local/joy_params.yaml b/snap/local/joy_params.yaml new file mode 100644 index 0000000..99dd41d --- /dev/null +++ b/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 \ No newline at end of file diff --git a/snap/local/joy_servo.yaml b/snap/local/joy_servo.yaml new file mode 100644 index 0000000..be39277 --- /dev/null +++ b/snap/local/joy_servo.yaml @@ -0,0 +1,69 @@ +/**: + ros__parameters: + joint_names: ["joint1", "joint2", "joint3", "joint4"] + end_effector_name: "end_effector_link" + + axis_deadzone: 0.05 + + dead_man_switch: + control_type: "single_button" + button_id: 5 + + home_manipulator: + control_type: "single_button" + button_id: 9 + manipulator_move_group_velocity_scaling_factor: 1.0 + manipulator_move_group_acceleration_scaling_factor: 1.0 + + # max_velocity set to quite low values (for both cartesian and joints control) + # so that collistion checker will have enough time to stop manipulator + + # Cartesian control + cartesian_control_reference_frame: "link2" + cartesian_control_names: ["linear_x", "linear_y", "linear_z", "angular_y"] + cartesian_drift_dimensions: [false, false, false, false, false, true] # translation x, y, z; rotation x, y, z + cartesian_control: + linear_x: + control_type: "axis" + axis_id: 1 + max_velocity: 0.1 # m/s + linear_y: + control_type: "axis" + axis_id: 0 + max_velocity: 0.2 # m/s + linear_z: + control_type: "axis" + axis_id: 3 + max_velocity: 0.1 # m/s + angular_y: + control_type: "axis" + axis_id: 2 + max_velocity: 0.8 # rad/s + + # Joints control + joints_control: + joint1: + control_type: "axis" + axis_id: 4 + max_velocity: 0.4 # rad/s + joint2: + control_type: "axis" + axis_id: 5 + inverted: true + max_velocity: 0.4 # rad/s + joint3: + control_type: "double_button" + positive_button_id: 0 + negative_button_id: 2 + max_velocity: 0.4 # rad/s + joint4: + control_type: "double_button" + positive_button_id: 1 + negative_button_id: 3 + max_velocity: 0.4 # rad/s + + gripper_control: + # Toggles between opened and closed positions + toggle: + control_type: "single_button" + button_id: 7 diff --git a/snap/local/joy_teleop.config.yaml b/snap/local/joy_teleop.config.yaml new file mode 100644 index 0000000..a8783a6 --- /dev/null +++ b/snap/local/joy_teleop.config.yaml @@ -0,0 +1,26 @@ +# joy_node: +# ros__parameters: +# device_id: 4 +# device_name: "''" +# deadzone: 0.5 +# autorepeat_rate: 20.0 +# sticky_buttons: false +# coalesce_interval_ms: 1 +--- +/**: + ros__parameters: + axis_linear: + x: 1 + scale_linear: + x: 0.7 + scale_linear_turbo: + x: 1.5 + + axis_angular: + yaw: 2 + scale_angular: + yaw: 0.4 + + require_enable_button: true + enable_button: 4 # TL shoulder button + enable_turbo_button: -1 # disabled \ No newline at end of file diff --git a/snap/local/launcher.sh b/snap/local/launcher.sh index 9ce905c..7c8fe1e 100755 --- a/snap/local/launcher.sh +++ b/snap/local/launcher.sh @@ -2,10 +2,19 @@ source $SNAP/usr/bin/utils.sh +LAUNCH_OPTIONS="" + +# Retrieve the namespace using snapctl +NAMESPACE="$(snapctl get ros.namespace)" + +# Check if NAMESPACE is not set or is empty +if [ -n "$NAMESPACE" ]; then + LAUNCH_OPTIONS+="namespace:=${NAMESPACE} " +fi + # Iterate over the snap parameters and retrieve their value. # If a value is set, it is forwarded to the launch file. -OPTIONS="namespace mecanum include-camera-mount camera-model lidar-model" -LAUNCH_OPTIONS="" +OPTIONS="mecanum include-camera-mount camera-model lidar-model" for OPTION in ${OPTIONS}; do VALUE="$(snapctl get driver.${OPTION})" @@ -20,4 +29,32 @@ if [ "${LAUNCH_OPTIONS}" ]; then log "Running with options: ${LAUNCH_OPTIONS}" fi -ros2 launch rosbot_xl_bringup combined.launch.py ${LAUNCH_OPTIONS} +OPT="configuration" +VALUE="$(snapctl get ${OPT})" +if [ "${VALUE}" == "basic" ]; then + ros2 launch rosbot_xl_bringup bringup.launch.py ${LAUNCH_OPTIONS} +elif [ "${VALUE}" == "manipulation" ]; then + # Check if SERIAL_PORT is set to auto or specified + SERIAL_PORT=$(find_ttyUSB driver.manipulator-serial-port "0403" "6014") + 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 + log_and_echo "Specified serial port exists: $SERIAL_PORT" + fi + + ros2 launch rosbot_xl_manipulation_bringup bringup.launch.py \ + ${LAUNCH_OPTIONS} \ + manipulator_usb_port:=${SERIAL_PORT} \ + manipulator_baud_rate:=1000000 \ + joy_servo_params_file:=${SNAP_COMMON}/joy_servo.yaml \ + antenna_rotation_angle:=-1.57 +fi diff --git a/snap/local/ros_common/check_daemon_running.sh b/snap/local/local-ros/check_daemon_running.sh similarity index 100% rename from snap/local/ros_common/check_daemon_running.sh rename to snap/local/local-ros/check_daemon_running.sh diff --git a/snap/local/ros_common/configure_hook_ros.sh b/snap/local/local-ros/configure_hook_ros.sh similarity index 83% rename from snap/local/ros_common/configure_hook_ros.sh rename to snap/local/local-ros/configure_hook_ros.sh index 158e0d7..7483d15 100755 --- a/snap/local/ros_common/configure_hook_ros.sh +++ b/snap/local/local-ros/configure_hook_ros.sh @@ -45,31 +45,26 @@ check_xml_profile_type() { fi } +VALID_ROS_KEYS=("localhost-only" "domain-id" "transport" "namespace") + +# Call the validation function +validate_keys "ros" VALID_ROS_KEYS[@] + +ROS_LOCALHOST_ONLY="$(snapctl get ros.localhost-only)" +ROS_DOMAIN_ID="$(snapctl get ros.domain-id)" + # Make sure ROS_LOCALHOST_ONLY is valid -OPT="ros-localhost-only" -ROS_LOCALHOST_ONLY="$(snapctl get ${OPT})" -if [ -n "${ROS_LOCALHOST_ONLY}" ]; then - case "${ROS_LOCALHOST_ONLY}" in - 1) ;; - 0) ;; - *) - log_and_echo "'${ROS_LOCALHOST_ONLY}' is not a supported value for '${OPT}'. Possible values are 0 or 1." - exit 1 - ;; - esac -fi +VALID_ROS_LOCALHOST_ONLY_OPTIONS=(1 0) +validate_option "ros.localhost-only" VALID_ROS_LOCALHOST_ONLY_OPTIONS[@] # Make sure ROS_DOMAIN_ID is valid -OPT="ros-domain-id" -ROS_DOMAIN_ID="$(snapctl get ${OPT})" - -if ! is_integer "${ROS_DOMAIN_ID}" || [ "${ROS_DOMAIN_ID}" -lt 0 ] || [ "${ROS_DOMAIN_ID}" -gt 232 ]; then - log_and_echo "'${ROS_DOMAIN_ID}' is not a supported value for '${OPT}'. Possible values are integers between 0 and 232." - exit 1 -fi +# Make sure WEBUI_PORT is valid +SUPPORTED_RANGE=(0 232) +# Validate a specific port, for example, webui.port +validate_number "ros.domain-id" SUPPORTED_RANGE[@] -# Get the transport setting using snapctl -OPT="transport" +# Get the ros.transport setting using snapctl +OPT="ros.transport" TRANSPORT_SETTING="$(snapctl get ${OPT})" # Check if TRANSPORT_SETTING is "builtin" @@ -110,18 +105,18 @@ ROS_SNAP_ARGS="${SNAP_COMMON}/ros_snap_args" echo "export ROS_DOMAIN_ID=${ROS_DOMAIN_ID}" > "${ROS_ENV_FILE}" echo "export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}" >> "${ROS_ENV_FILE}" -NAMESPACE=$(snapctl get driver.namespace) +NAMESPACE=$(snapctl get ros.namespace) # Check if the namespace is set and not empty if [ -n "$NAMESPACE" ]; then - echo "ros-domain-id=${ROS_DOMAIN_ID} ros-localhost-only=${ROS_LOCALHOST_ONLY} transport=${TRANSPORT_SETTING} driver.namespace=${NAMESPACE}" > "${ROS_SNAP_ARGS}" + echo "ros.domain-id=${ROS_DOMAIN_ID} ros.localhost-only=${ROS_LOCALHOST_ONLY} ros.transport=${TRANSPORT_SETTING} ros.namespace=${NAMESPACE}" > "${ROS_SNAP_ARGS}" echo "export ROS_NAMESPACE=${NAMESPACE}" >> "${ROS_ENV_FILE}" else - echo "ros-domain-id=${ROS_DOMAIN_ID} ros-localhost-only=${ROS_LOCALHOST_ONLY} transport=${TRANSPORT_SETTING} driver.namespace!" > "${ROS_SNAP_ARGS}" + echo "ros.domain-id=${ROS_DOMAIN_ID} ros.localhost-only=${ROS_LOCALHOST_ONLY} ros.transport=${TRANSPORT_SETTING} ros.namespace!" > "${ROS_SNAP_ARGS}" echo "unset ROS_NAMESPACE" >> "${ROS_ENV_FILE}" fi -# Check the transport setting and export the appropriate environment variable +# Check the ros.transport setting and export the appropriate environment variable if [ "$TRANSPORT_SETTING" != "rmw_fastrtps_cpp" ] && [ "$TRANSPORT_SETTING" != "rmw_cyclonedds_cpp" ]; then profile_type=$(check_xml_profile_type "${SNAP_COMMON}/${TRANSPORT_SETTING}.xml") if [[ "$profile_type" == "rmw_fastrtps_cpp" ]]; then diff --git a/snap/local/ros_common/install_hook_ros.sh b/snap/local/local-ros/install_hook_ros.sh similarity index 81% rename from snap/local/ros_common/install_hook_ros.sh rename to snap/local/local-ros/install_hook_ros.sh index 8ffc18e..86848fb 100755 --- a/snap/local/ros_common/install_hook_ros.sh +++ b/snap/local/local-ros/install_hook_ros.sh @@ -3,9 +3,10 @@ # Define a function to log messages source $SNAP/usr/bin/utils.sh -snapctl set transport="udp" -snapctl set ros-localhost-only=0 -snapctl set ros-domain-id=0 +snapctl set ros.transport="udp" +snapctl set ros.localhost-only=0 +snapctl set ros.domain-id=0 +snapctl set ros.namespace! # unset if ! snapctl is-connected ros-humble-ros-base; then log "Plug 'ros-humble-ros-base' isn't connected, please run:" diff --git a/snap/local/ros_common/ros_setup.sh b/snap/local/local-ros/ros_setup.sh similarity index 100% rename from snap/local/ros_common/ros_setup.sh rename to snap/local/local-ros/ros_setup.sh diff --git a/snap/local/ros_common/shm.xml b/snap/local/local-ros/shm.xml similarity index 100% rename from snap/local/ros_common/shm.xml rename to snap/local/local-ros/shm.xml diff --git a/snap/local/ros_common/udp-lo-cyclone.xml b/snap/local/local-ros/udp-lo-cyclone.xml similarity index 100% rename from snap/local/ros_common/udp-lo-cyclone.xml rename to snap/local/local-ros/udp-lo-cyclone.xml diff --git a/snap/local/ros_common/udp-lo.xml b/snap/local/local-ros/udp-lo.xml similarity index 100% rename from snap/local/ros_common/udp-lo.xml rename to snap/local/local-ros/udp-lo.xml diff --git a/snap/local/ros_common/udp.xml b/snap/local/local-ros/udp.xml similarity index 100% rename from snap/local/ros_common/udp.xml rename to snap/local/local-ros/udp.xml diff --git a/snap/local/local-ros/utils.sh b/snap/local/local-ros/utils.sh new file mode 100755 index 0000000..7f05342 --- /dev/null +++ b/snap/local/local-ros/utils.sh @@ -0,0 +1,224 @@ +#!/bin/bash -e + +# Define a function to log and echo messages +log_and_echo() { + local message="$1" + local script_name=$(basename "$0") + # Log the message with logger + logger -t "${SNAP_NAME}" "${script_name}: $message" + # Echo the message to standard error + echo -e >&2 "$message" +} + +log() { + local message="$1" + local script_name=$(basename "$0") + # Log the message with logger + logger -t "${SNAP_NAME}" "${script_name}: $message" +} + +is_integer() { + expr "$1" : '-\?[0-9][0-9]*$' >/dev/null 2>&1 +} + +# Function to validate the option values +validate_option() { + local OPT=$1 + local VALID_OPTIONS=("${!2}") + + VALUE="$(snapctl get ${OPT})" + + # 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}' parameter. Possible values are:\n${JOINED_OPTIONS}" + exit 1 + fi + fi +} + +# Function to validate configuration keys +validate_keys() { + local top_level_key=$1 + local valid_keys=("${!2}") + + # Get the current configuration keys + local config_keys=$(snapctl get "$top_level_key" | yq '. | keys' | sed 's/- //g' | tr -d '"') + + # Create an associative array to check valid keys + declare -A valid_keys_map + for key in "${valid_keys[@]}"; do + valid_keys_map["$key"]=1 + done + + # Join the valid options with newlines + JOINED_OPTIONS=$(printf "%s\n" "${valid_keys[@]}") + + # Iterate over the keys in the configuration + for key in $config_keys; do + # Check if the key is in the list of valid keys + if [[ -z "${valid_keys_map[$key]}" ]]; then + log_and_echo "'${key}' is not a supported value for '${top_level_key}' key. Possible values are:\n${JOINED_OPTIONS}" + exit 1 + fi + done +} + +validate_number() { + local value_key=$1 + local range=("${!2}") + local excluded_values=("${!3:-}") + + # Get the value using snapctl + local value=$(snapctl get "$value_key") + + # Extract the min and max range values + local min_value=${range[0]} + local max_value=${range[1]} + + # Join the excluded values with newlines if they exist + local joined_excluded_values + local exclude_message + if [ -n "$excluded_values" ]; then + joined_excluded_values=$(printf "%s\n" "${excluded_values[@]}") + exclude_message=" excluding:\n${joined_excluded_values[*]}" + else + exclude_message="" + fi + + # Check if the value is an integer + if ! [[ "$value" =~ ^[0-9]+$ ]]; then + log_and_echo "'${value}' is not a supported value for '${value_key}'. Possible values are integers between ${min_value} and ${max_value}.${exclude_message}" + exit 1 + fi + + # Check if the value is in the valid range + if [ "$value" -lt "$min_value" ] || [ "$value" -gt "$max_value" ]; then + log_and_echo "'${value}' is not a supported value for '${value_key}'. Possible values are integers between ${min_value} and ${max_value}.${exclude_message}" + exit 1 + fi + + # Check if the value is in the excluded list + if [ -n "$excluded_values" ]; then + for excluded_value in "${excluded_values[@]}"; do + if [ "$value" -eq "$excluded_value" ]; then + log_and_echo "'${value}' is not a supported value for '${value_key}'. Possible values are integers between ${min_value} and ${max_value}.${exclude_message}" + exit 1 + fi + done + fi +} + +validate_regex() { + local value_key=$1 + local regex=$2 + local error_message=$3 + + # Get the value using snapctl + local value=$(snapctl get "$value_key") + + # Check if the value matches the regex + if ! [[ "$value" =~ $regex ]]; then + log_and_echo "'${value}' is not a supported value for '${value_key}'. ${error_message}" + exit 1 + fi +} + +validate_path() { + local value_key=$1 + + # Get the value using snapctl + local config_path=$(snapctl get "$value_key") + + # Check if the path is a valid file + if [ ! -f "$config_path" ]; then + log_and_echo "The path specified in '$value_key' does not exist: '$config_path'." + exit 1 + fi +} + +validate_ipv4_addr() { + local value_key=$1 + + # Get the value using snapctl + local ip_address=$(snapctl get "$value_key") + local ip_address_regex='^(([0-9]{1,3}\.){3}[0-9]{1,3})$' + + if [[ "$ip_address" =~ $ip_address_regex ]]; then + # Split the IP address into its parts + IFS='.' read -r -a octets <<< "$ip_address" + + # Check each octet + for octet in "${octets[@]}"; do + if ((octet < 0 || octet > 255)); then + log_and_echo "Invalid format for '$value_key'. Each part of the IPv4 address must be between 0 and 255. Received: '$ip_address'." + exit 1 + fi + done + else + log_and_echo "Invalid format for '$value_key'. Expected format: a valid IPv4 address. Received: '$ip_address'." + exit 1 + fi +} + +# Universal function to validate serial ports +validate_serial_port() { + local port_key=$1 + + # Get the port value using snapctl + local port_value=$(snapctl get "$port_key") + + # Check if the value is "auto" or a valid serial port + if [ "$port_value" != "auto" ] && [ ! -e "$port_value" ]; then + log_and_echo "'${port_value}' is not a valid value for '${port_key}'. It must be 'auto' or a valid serial port in the /dev/ directory." + exit 1 + fi +} + +# Function to find the ttyUSB* device for the specified USB Vendor and Product ID +find_ttyUSB() { + # Extract port parameter, vendor, and product ID + PORT_PARAM="$1" + VENDOR_ID="$2" + PRODUCT_ID="$3" + + # Get the serial-port value using snapctl + SERIAL_PORT=$(snapctl get "$PORT_PARAM") + + 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 2>/dev/null); 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 +} \ No newline at end of file diff --git a/snap/local/ros_common/find_ttyUSB.sh b/snap/local/ros_common/find_ttyUSB.sh deleted file mode 100755 index 76bb0cf..0000000 --- a/snap/local/ros_common/find_ttyUSB.sh +++ /dev/null @@ -1,40 +0,0 @@ -#!/bin/bash -e - -# 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 2>/dev/null); 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 -} diff --git a/snap/local/ros_common/utils.sh b/snap/local/ros_common/utils.sh deleted file mode 100755 index bdbc41f..0000000 --- a/snap/local/ros_common/utils.sh +++ /dev/null @@ -1,23 +0,0 @@ -#!/bin/bash -e - -# Define a function to log and echo messages -log_and_echo() { - local message="$1" - local script_name=$(basename "$0") - # Log the message with logger - logger -t "${SNAP_NAME}" "${script_name}: $message" - # Echo the message to standard error - echo -e >&2 "$message" -} - -log() { - local message="$1" - local script_name=$(basename "$0") - # Log the message with logger - logger -t "${SNAP_NAME}" "${script_name}: $message" -} - -is_integer() { - expr "$1" : '-\?[0-9][0-9]*$' >/dev/null 2>&1 -} - diff --git a/snap/local/rosbot-manipulator.launch.py b/snap/local/rosbot-manipulator.launch.py new file mode 100644 index 0000000..205c3f1 --- /dev/null +++ b/snap/local/rosbot-manipulator.launch.py @@ -0,0 +1,368 @@ +# Copyright 2024 Husarion sp. z o.o. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +import os + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + LogInfo, + OpaqueFunction, + SetEnvironmentVariable, + IncludeLaunchDescription +) +from launch.conditions import UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, + ThisLaunchFileDir, +) +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory + +def generate_microros_agent_node(context, *args, **kwargs): + env_setup_actions = [] + + ros_domain_id = os.environ.get("ROS_DOMAIN_ID") + if ros_domain_id: + env_setup_actions.append( + SetEnvironmentVariable(name="XRCE_DOMAIN_ID_OVERRIDE", value=ros_domain_id) + ) + + port = LaunchConfiguration("port").perform(context) + + localhost_only_fastrtps_profiles_file = LaunchConfiguration( + "localhost_only_fastrtps_profiles_file" + ).perform(context) + + if os.environ.get("ROS_LOCALHOST_ONLY") == "1": + env_setup_actions.extend( + [ + LogInfo( + msg=[ + "ROS_LOCALHOST_ONLY set to 1. Using FASTRTPS_DEFAULT_PROFILES_FILE=", + localhost_only_fastrtps_profiles_file, + ".", + ] + ), + SetEnvironmentVariable(name="RMW_IMPLEMENTATION", value="rmw_fastrtps_cpp"), + SetEnvironmentVariable( + name="FASTRTPS_DEFAULT_PROFILES_FILE", + value=localhost_only_fastrtps_profiles_file, + ), + ] + ) + + microros_agent_node = Node( + package="micro_ros_agent", + executable="micro_ros_agent", + arguments=["udp4", "--port", port], + output="screen", + ) + + return env_setup_actions + [microros_agent_node] + +def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + + declare_port_arg = DeclareLaunchArgument( + "port", + default_value="8888", + description="UDP4 port for micro-ROS agent", + ) + + # Locate the rosbot_bringup package + package_dir = FindPackageShare("rosbot_xl_bringup").find("rosbot_xl_bringup") + + # Construct the path to the XML file within the package + fastrtps_profiles_file = PathJoinSubstitution( + [package_dir, "config", "microros_localhost_only.xml"] + ) + + declare_localhost_only_fastrtps_profiles_file_arg = DeclareLaunchArgument( + "localhost_only_fastrtps_profiles_file", + default_value=fastrtps_profiles_file, + description=( + "Path to the Fast RTPS default profiles file for Micro-ROS agent for localhost only" + " setup" + ), + ) + + mecanum = LaunchConfiguration("mecanum") + declare_mecanum_arg = DeclareLaunchArgument( + "mecanum", + default_value="False", + description="Whether to use mecanum drive controller, otherwise use diff drive", + ) + + camera_model = LaunchConfiguration("camera_model") + declare_camera_model_arg = DeclareLaunchArgument( + "camera_model", + default_value="None", + description="Add camera model to the robot URDF", + choices=[ + "None", + "intel_realsense_d435", + "orbbec_astra", + "stereolabs_zed", + "stereolabs_zedm", + "stereolabs_zed2", + "stereolabs_zed2i", + "stereolabs_zedx", + "stereolabs_zedxm", + ], + ) + + lidar_model = LaunchConfiguration("lidar_model") + declare_lidar_model_arg = DeclareLaunchArgument( + "lidar_model", + default_value="slamtec_rplidar_s1", + description="Add LiDAR model to the robot URDF", + choices=[ + "None", + "slamtec_rplidar_a2", + "slamtec_rplidar_a3", + "slamtec_rplidar_s1", + "slamtec_rplidar_s2", + "slamtec_rplidar_s3", + "velodyne_puck", + ], + ) + + include_camera_mount = LaunchConfiguration("include_camera_mount") + declare_include_camera_mount_arg = DeclareLaunchArgument( + "include_camera_mount", + default_value="False", + description="Whether to include camera mount to the robot URDF", + ) + + use_sim = LaunchConfiguration("use_sim") + declare_use_sim_arg = DeclareLaunchArgument( + "use_sim", + default_value="False", + description="Whether simulation is used", + ) + + simulation_engine = LaunchConfiguration("simulation_engine") + declare_simulation_engine_arg = DeclareLaunchArgument( + "simulation_engine", + default_value="ignition-gazebo", + description="Which simulation engine will be used", + ) + + combined_launch_deprecated = LaunchConfiguration("combined_launch_deprecated") + declare_combined_launch_deprecated_arg = DeclareLaunchArgument( + "combined_launch_deprecated", + default_value="False", + ) + + rosbot_xl_controller = FindPackageShare("rosbot_xl_controller") + rosbot_xl_bringup = FindPackageShare("rosbot_xl_bringup") + rosbot_xl_manipulation_controller = FindPackageShare("rosbot_xl_manipulation_controller") + rosbot_xl_manipulation_moveit = FindPackageShare("rosbot_xl_manipulation_moveit") + + controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "lidar_model": lidar_model, + "camera_model": camera_model, + "include_camera_mount": include_camera_mount, + "use_sim": use_sim, + "simulation_engine": simulation_engine, + "namespace": namespace, + }.items(), + ) + + ekf_config = PathJoinSubstitution([rosbot_xl_bringup, "config", "ekf.yaml"]) + + robot_localization_node = Node( + package="robot_localization", + executable="ekf_node", + name="ekf_filter_node", + output="screen", + parameters=[ekf_config], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + namespace=namespace, + ) + + laser_filter_config = PathJoinSubstitution( + [ + rosbot_xl_bringup, + "config", + "laser_filter.yaml", + ] + ) + + laser_filter_node = Node( + package="laser_filters", + executable="scan_to_scan_filter_chain", + parameters=[ + laser_filter_config, + ], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + namespace=namespace, + ) + + launch_joy_node = LaunchConfiguration("launch_joy_node") + declare_launch_joy_node_arg = DeclareLaunchArgument( + "launch_joy_node", + default_value="False", + ) + + manipulator_usb_port = LaunchConfiguration("manipulator_usb_port") + declare_manipulator_usb_port_arg = DeclareLaunchArgument( + "manipulator_usb_port", + default_value="/dev/ttyMANIPULATOR", + ) + + manipulator_baud_rate = LaunchConfiguration("manipulator_baud_rate") + declare_manipulator_baud_rate_arg = DeclareLaunchArgument( + "manipulator_baud_rate", + default_value="1000000", + ) + + joy_servo_config = LaunchConfiguration("joy_servo_params_file") + declare_joy_servo_config_arg = DeclareLaunchArgument( + "joy_servo_params_file", + default_value=PathJoinSubstitution( + [ + rosbot_xl_manipulation_moveit, + "config", + "joy_servo.yaml", + ] + ), + description="ROS2 parameters file to use with joy_servo node", + ) + + joint1_limit_min = LaunchConfiguration("joint1_limit_min") + declare_joint1_limit_min_arg = DeclareLaunchArgument( + "joint1_limit_min", + default_value="-2.356", + description="Min angle (in radians) that can be achieved by rotating joint1 of the manipulator", + ) + joint1_limit_max = LaunchConfiguration("joint1_limit_max") + declare_joint1_limit_max_arg = DeclareLaunchArgument( + "joint1_limit_max", + default_value="5.934", + description="Max angle (in radians) that can be achieved by rotating joint1 of the manipulator", + ) + + manipulator_controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_manipulation_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "manipulator_usb_port": manipulator_usb_port, + "manipulator_baud_rate": manipulator_baud_rate, + "joint1_limit_min": joint1_limit_min, + "joint1_limit_max": joint1_limit_max, + "mecanum": mecanum, + "use_sim": use_sim, + }.items(), + ) + + moveit_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_manipulation_moveit, + "launch", + "move_group.launch.py", + ] + ) + ), + launch_arguments={ + "joint1_limit_min": joint1_limit_min, + "joint1_limit_max": joint1_limit_max, + "mecanum": mecanum, + "use_sim": use_sim, + }.items(), + ) + + servo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_manipulation_moveit, + "launch", + "servo.launch.py", + ] + ) + ), + launch_arguments={ + "launch_joy_node": launch_joy_node, + "joy_servo_params_file": joy_servo_config, + "joint1_limit_min": joint1_limit_min, + "joint1_limit_max": joint1_limit_max, + "mecanum": mecanum, + "use_sim": use_sim, + }.items(), + ) + + return LaunchDescription( + [ + declare_port_arg, + declare_localhost_only_fastrtps_profiles_file_arg, + declare_namespace_arg, + declare_mecanum_arg, + declare_lidar_model_arg, + declare_camera_model_arg, + declare_include_camera_mount_arg, + declare_use_sim_arg, + declare_simulation_engine_arg, + declare_combined_launch_deprecated_arg, + declare_launch_joy_node_arg, + declare_manipulator_usb_port_arg, + declare_manipulator_baud_rate_arg, + declare_joy_servo_config_arg, + declare_joint1_limit_min_arg, + declare_joint1_limit_max_arg, + SetParameter(name="use_sim_time", value=use_sim), + controller_launch, + robot_localization_node, + laser_filter_node, + OpaqueFunction(function=generate_microros_agent_node), + manipulator_controller_launch, + moveit_launch, + servo_launch, + ] + ) diff --git a/snap/local/rosbot.launch.py b/snap/local/rosbot.launch.py new file mode 100644 index 0000000..2029b5f --- /dev/null +++ b/snap/local/rosbot.launch.py @@ -0,0 +1,254 @@ +# Copyright 2024 Husarion sp. z o.o. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +import os + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + LogInfo, + OpaqueFunction, + SetEnvironmentVariable, + IncludeLaunchDescription +) +from launch.conditions import UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, + ThisLaunchFileDir, +) +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare + +def generate_microros_agent_node(context, *args, **kwargs): + env_setup_actions = [] + + ros_domain_id = os.environ.get("ROS_DOMAIN_ID") + if ros_domain_id: + env_setup_actions.append( + SetEnvironmentVariable(name="XRCE_DOMAIN_ID_OVERRIDE", value=ros_domain_id) + ) + + port = LaunchConfiguration("port").perform(context) + + localhost_only_fastrtps_profiles_file = LaunchConfiguration( + "localhost_only_fastrtps_profiles_file" + ).perform(context) + + if os.environ.get("ROS_LOCALHOST_ONLY") == "1": + env_setup_actions.extend( + [ + LogInfo( + msg=[ + "ROS_LOCALHOST_ONLY set to 1. Using FASTRTPS_DEFAULT_PROFILES_FILE=", + localhost_only_fastrtps_profiles_file, + ".", + ] + ), + SetEnvironmentVariable(name="RMW_IMPLEMENTATION", value="rmw_fastrtps_cpp"), + SetEnvironmentVariable( + name="FASTRTPS_DEFAULT_PROFILES_FILE", + value=localhost_only_fastrtps_profiles_file, + ), + ] + ) + + microros_agent_node = Node( + package="micro_ros_agent", + executable="micro_ros_agent", + arguments=["udp4", "--port", port], + output="screen", + ) + + return env_setup_actions + [microros_agent_node] + +def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + + declare_port_arg = DeclareLaunchArgument( + "port", + default_value="8888", + description="UDP4 port for micro-ROS agent", + ) + + # Locate the rosbot_bringup package + package_dir = FindPackageShare("rosbot_xl_bringup").find("rosbot_xl_bringup") + + # Construct the path to the XML file within the package + fastrtps_profiles_file = PathJoinSubstitution( + [package_dir, "config", "microros_localhost_only.xml"] + ) + + declare_localhost_only_fastrtps_profiles_file_arg = DeclareLaunchArgument( + "localhost_only_fastrtps_profiles_file", + default_value=fastrtps_profiles_file, + description=( + "Path to the Fast RTPS default profiles file for Micro-ROS agent for localhost only" + " setup" + ), + ) + + mecanum = LaunchConfiguration("mecanum") + declare_mecanum_arg = DeclareLaunchArgument( + "mecanum", + default_value="False", + description="Whether to use mecanum drive controller, otherwise use diff drive", + ) + + camera_model = LaunchConfiguration("camera_model") + declare_camera_model_arg = DeclareLaunchArgument( + "camera_model", + default_value="None", + description="Add camera model to the robot URDF", + choices=[ + "None", + "intel_realsense_d435", + "orbbec_astra", + "stereolabs_zed", + "stereolabs_zedm", + "stereolabs_zed2", + "stereolabs_zed2i", + "stereolabs_zedx", + "stereolabs_zedxm", + ], + ) + + lidar_model = LaunchConfiguration("lidar_model") + declare_lidar_model_arg = DeclareLaunchArgument( + "lidar_model", + default_value="slamtec_rplidar_s1", + description="Add LiDAR model to the robot URDF", + choices=[ + "None", + "slamtec_rplidar_a2", + "slamtec_rplidar_a3", + "slamtec_rplidar_s1", + "slamtec_rplidar_s2", + "slamtec_rplidar_s3", + "velodyne_puck", + ], + ) + + include_camera_mount = LaunchConfiguration("include_camera_mount") + declare_include_camera_mount_arg = DeclareLaunchArgument( + "include_camera_mount", + default_value="False", + description="Whether to include camera mount to the robot URDF", + ) + + use_sim = LaunchConfiguration("use_sim") + declare_use_sim_arg = DeclareLaunchArgument( + "use_sim", + default_value="False", + description="Whether simulation is used", + ) + + simulation_engine = LaunchConfiguration("simulation_engine") + declare_simulation_engine_arg = DeclareLaunchArgument( + "simulation_engine", + default_value="ignition-gazebo", + description="Which simulation engine will be used", + ) + + combined_launch_deprecated = LaunchConfiguration("combined_launch_deprecated") + declare_combined_launch_deprecated_arg = DeclareLaunchArgument( + "combined_launch_deprecated", + default_value="False", + ) + + rosbot_xl_controller = FindPackageShare("rosbot_xl_controller") + rosbot_xl_bringup = FindPackageShare("rosbot_xl_bringup") + + controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "lidar_model": lidar_model, + "camera_model": camera_model, + "include_camera_mount": include_camera_mount, + "use_sim": use_sim, + "simulation_engine": simulation_engine, + "namespace": namespace, + }.items(), + ) + + ekf_config = PathJoinSubstitution([rosbot_xl_bringup, "config", "ekf.yaml"]) + + robot_localization_node = Node( + package="robot_localization", + executable="ekf_node", + name="ekf_filter_node", + output="screen", + parameters=[ekf_config], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + namespace=namespace, + ) + + laser_filter_config = PathJoinSubstitution( + [ + rosbot_xl_bringup, + "config", + "laser_filter.yaml", + ] + ) + + laser_filter_node = Node( + package="laser_filters", + executable="scan_to_scan_filter_chain", + parameters=[ + laser_filter_config, + ], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + namespace=namespace, + ) + + return LaunchDescription( + [ + declare_port_arg, + declare_localhost_only_fastrtps_profiles_file_arg, + declare_namespace_arg, + declare_mecanum_arg, + declare_lidar_model_arg, + declare_camera_model_arg, + declare_include_camera_mount_arg, + declare_use_sim_arg, + declare_simulation_engine_arg, + declare_combined_launch_deprecated_arg, + SetParameter(name="use_sim_time", value=use_sim), + controller_launch, + robot_localization_node, + laser_filter_node, + OpaqueFunction(function=generate_microros_agent_node), + ] + ) diff --git a/snap/local/teleop_launcher.sh b/snap/local/teleop_launcher.sh index 8cd7da1..d1780ba 100755 --- a/snap/local/teleop_launcher.sh +++ b/snap/local/teleop_launcher.sh @@ -1,7 +1,7 @@ #!/bin/bash -e # Retrieve the namespace using snapctl -NAMESPACE="$(snapctl get driver.namespace)" +NAMESPACE="$(snapctl get ros.namespace)" # Check if NAMESPACE is not set or is empty if [ -z "$NAMESPACE" ]; then diff --git a/snap/snapcraft.yaml b/snap/snapcraft.yaml index 5105967..90af801 100644 --- a/snap/snapcraft.yaml +++ b/snap/snapcraft.yaml @@ -11,7 +11,7 @@ description: | To install this snap on the Single Board Computer (SBC) within the ROSbot XL chassis, follow these steps: 1. Connect the SBC to the Digital Board inside ROSbot XL using an Ethernet cable. - 2. Configure the Ethernet Interface with the static IP address `192.168.77.2` (port `8888` on this IP is used by the STM32F4 microcontroller). + 2. Configure the Ethernet Interface with the static IP address `192.168.77.2` (port `8888` on this IP is used by the firmware). 3. Run the following command to install the snap: snap install rosbot-xl @@ -20,22 +20,28 @@ description: | The snap provides the following configurable parameters (`param name`: `default value`): + * `configuration`: `basic` - presets for ROSbot XL * `driver`: `{...}` - * `ros-domain-id`: `0` - Sets the `ROS_DOMAIN_ID` environment variable for the ROS driver. - * `ros-localhost-only`: `0` - Sets the `ROS_LOCALHOST_ONLY` environment variable for the ROS driver. - * `serial-port`: `auto` - Configures the serial port for firmware updates (e.g., `/dev/ttyUSB0`), or set it to `auto`. - * `transport`: `udp` - Configures DDS transport. Options are `udp`, `shm`, `builtin` (or `rmw_fastrtps_cpp`), `rmw_cyclonedds_cpp`. Corresponding DDS XML files can be found in the `/var/snap/rosbot-xl/common` directory (custom FastDDS setups can also be created here). + * `ros`: `{...}` * `webui`: `{...}` - The `driver` parameter is a dictionary containing the following keys for a ROS 2 driver: + The `ros` contains the following keys: + + * `ros.domain-id`: `0` - Sets the `ROS_DOMAIN_ID` environment variable for the ROS driver. + * `ros.localhost-only`: `0` - Sets the `ROS_LOCALHOST_ONLY` environment variable for the ROS driver. + * `ros.transport`: `udp` - Configures DDS transport. Options are `udp`, `shm`, `builtin` (or `rmw_fastrtps_cpp`), `rmw_cyclonedds_cpp`. Corresponding DDS XML files can be found in the `/var/snap/rosbot-xl/common` directory (custom FastDDS setups can also be created here). + * `ros.namespace`: `(unset)` - Namespace for all topics and transforms. + + The `driver` contains the following keys: - * `driver.namespace`: `(unset)` - Namespace for all topics and transforms. * `driver.mecanum`: `True` - Enables the mecanum drive controller; otherwise, uses the differential drive controller. * `driver.include-camera-mount`: `True` - Includes the camera mount in the robot URDF. * `driver.camera-model`: `None` - Adds the camera model to the robot URDF. * `driver.lidar-model`: `None` - Adds the LIDAR model to the robot URDF. + * `driver.db-serial-port`: `auto` - Serial port for firmware (e.g., `/dev/ttyUSB0`), or set it to `auto`. + * `driver.manipulator-serial-port`: `auto` - Serial port for OpenManipulator-X (e.g., `/dev/ttyUSB0`), or set it to `auto`. - The `webui` parameter is a dictionary containing the following keys for the Web UI: + The `webui` contains the following keys: * `webui.layout`: `default` - Specifies the layout for the Web UI. Available `*.json` layout files can be found in the `/var/snap/rosbot-xl/common` directory (custom layouts can also be created here). * `webui.port`: `8080` - Specifies the port for the built-in web server hosting the Web UI. @@ -66,7 +72,7 @@ description: | Example usage: # Set parameters in the rosbot-xl snap - sudo snap set rosbot-xl transport=udp ros-domain-id=123 driver.namespace=abc + sudo snap set rosbot-xl transport=udp ros-domain-id=123 ros.namespace=abc # Mirror the setup for other ROS 2 snaps sudo snap set husarion-depthai $(cat /var/snap/rosbot-xl/common/ros_snap_args) @@ -134,7 +140,7 @@ apps: command-chain: [usr/bin/ros_setup.sh] daemon: simple install-mode: enable - plugs: [network, network-bind, shm-plug] + plugs: [network, network-bind, shm-plug, raw-usb] slots: [shm-slot] extensions: [ros2-humble-ros-base] @@ -147,7 +153,7 @@ apps: rosbot-xl: command: usr/bin/launcher.sh command-chain: [usr/bin/check_daemon_running.sh, usr/bin/ros_setup.sh] - plugs: [network, network-bind, shm-plug] + plugs: [network, network-bind, shm-plug, raw-usb] slots: [shm-slot] extensions: [ros2-humble-ros-base] @@ -163,6 +169,31 @@ apps: slots: [shm-slot] extensions: [ros2-humble-ros-base] + joy: + command: usr/bin/joy_launcher.sh + command-chain: [usr/bin/ros_setup.sh] + daemon: simple + install-mode: disable + environment: + LD_LIBRARY_PATH: "$SNAP/usr/lib/$SNAPCRAFT_ARCH_TRIPLET/pulseaudio:$LD_LIBRARY_PATH" + plugs: [hardware-observe, joystick, network, network-bind, shm-plug] + slots: [shm-slot] + extensions: [ros2-humble-ros-base] + + arm-activate: + command: usr/bin/arm_activate_launcher.sh active + command-chain: [usr/bin/ros_setup.sh] + plugs: [network, network-bind, shm-plug] + slots: [shm-slot] + extensions: [ros2-humble-ros-base] + + arm-disactivate: + command: usr/bin/arm_activate_launcher.sh inactive + command-chain: [usr/bin/ros_setup.sh] + plugs: [network, network-bind, shm-plug] + slots: [shm-slot] + extensions: [ros2-humble-ros-base] + print-serial-number: command: usr/bin/serial_number_launcher.sh command-chain: [usr/bin/ros_setup.sh] @@ -206,10 +237,41 @@ apps: command: usr/bin/stop_web_ui_launcher.sh parts: + # rosbot-xl: + # plugin: colcon + # source: https://github.com/husarion/rosbot_xl_ros.git + # source-branch: "0.11.5" + # build-packages: + # - python3-vcstool + # stage-packages: + # - stm32flash + # - libusb-1.0-0 + # - usbutils + # - ros-humble-rmw-cyclonedds-cpp + # override-pull: | + # craftctl default + + # vcs import $CRAFT_PART_SRC < $CRAFT_PART_SRC/rosbot_xl/rosbot_xl_hardware.repos + # cp -r $CRAFT_PART_SRC/ros2_controllers/diff_drive_controller $CRAFT_PART_SRC/ + # cp -r $CRAFT_PART_SRC/ros2_controllers/imu_sensor_broadcaster $CRAFT_PART_SRC/ + # rm -rf $CRAFT_PART_SRC/ros2_controllers + # rm -r $CRAFT_PART_SRC/rosbot_xl_gazebo + # # Ignore so that rosdep doesn't pull deps + # # and colcon doesn't build + # # touch $CRAFT_PART_SRC/rosbot_xl_gazebo/COLCON_IGNORE + + # # Set the snap version from the git tag + # # The grade is set to 'stable' if the latest entry in the git history + # # is the tag itself, otherwise set to devel + # version="$(git describe --always --tags| sed -e 's/^v//;s/-/+git/;y/-/./')" + # [ -n "$(echo $version | grep "+git")" ] && grade=devel || grade=stable + # craftctl set version="$version" + # craftctl set grade="$grade" + rosbot-xl: plugin: colcon - source: https://github.com/husarion/rosbot_xl_ros.git - source-branch: "0.11.5" + source: https://github.com/husarion/rosbot_xl_manipulation_ros.git + source-branch: "1.1.2" build-packages: - python3-vcstool stage-packages: @@ -220,11 +282,15 @@ parts: override-pull: | craftctl default - vcs import $CRAFT_PART_SRC < $CRAFT_PART_SRC/rosbot_xl/rosbot_xl_hardware.repos + vcs import $CRAFT_PART_SRC < $CRAFT_PART_SRC/rosbot_xl_manipulation/rosbot_xl_manipulation.repos + vcs import $CRAFT_PART_SRC < $CRAFT_PART_SRC/rosbot_xl_ros/rosbot_xl/rosbot_xl_hardware.repos + vcs import $CRAFT_PART_SRC < $CRAFT_PART_SRC/open_manipulator_x/open_manipulator_x.repos + cp -r $CRAFT_PART_SRC/ros2_controllers/diff_drive_controller $CRAFT_PART_SRC/ cp -r $CRAFT_PART_SRC/ros2_controllers/imu_sensor_broadcaster $CRAFT_PART_SRC/ rm -rf $CRAFT_PART_SRC/ros2_controllers - rm -r $CRAFT_PART_SRC/rosbot_xl_gazebo + rm -r $CRAFT_PART_SRC/rosbot_xl_ros/rosbot_xl_gazebo + rm -r $CRAFT_PART_SRC/rosbot_xl_manipulation_gazebo # Ignore so that rosdep doesn't pull deps # and colcon doesn't build # touch $CRAFT_PART_SRC/rosbot_xl_gazebo/COLCON_IGNORE @@ -237,40 +303,6 @@ parts: craftctl set version="$version" craftctl set grade="$grade" - # rosbot-xl: - # plugin: colcon - # source: https://github.com/husarion/rosbot_xl_manipulation_ros.git - # # source-branch: "0.11.0" - # build-packages: - # - python3-vcstool - # stage-packages: - # - stm32flash - # - libusb-1.0-0 - # - usbutils - # override-pull: | - # craftctl default - - # vcs import $CRAFT_PART_SRC < $CRAFT_PART_SRC/rosbot_xl_manipulation/rosbot_xl_manipulation.repos - # vcs import $CRAFT_PART_SRC < $CRAFT_PART_SRC/rosbot_xl_ros/rosbot_xl/rosbot_xl_hardware.repos - # vcs import $CRAFT_PART_SRC < $CRAFT_PART_SRC/open_manipulator_x/open_manipulator_x.repos - - # cp -r $CRAFT_PART_SRC/ros2_controllers/diff_drive_controller $CRAFT_PART_SRC/ - # cp -r $CRAFT_PART_SRC/ros2_controllers/imu_sensor_broadcaster $CRAFT_PART_SRC/ - # rm -rf $CRAFT_PART_SRC/ros2_controllers - # rm -r $CRAFT_PART_SRC/rosbot_xl_ros/rosbot_xl_gazebo - # rm -r $CRAFT_PART_SRC/rosbot_xl_manipulation_gazebo - # # Ignore so that rosdep doesn't pull deps - # # and colcon doesn't build - # # touch $CRAFT_PART_SRC/rosbot_xl_gazebo/COLCON_IGNORE - - # # Set the snap version from the git tag - # # The grade is set to 'stable' if the latest entry in the git history - # # is the tag itself, otherwise set to devel - # version="$(git describe --always --tags| sed -e 's/^v//;s/-/+git/;y/-/./')" - # [ -n "$(echo $version | grep "+git")" ] && grade=devel || grade=stable - # craftctl set version="$version" - # craftctl set grade="$grade" - teleop: plugin: nil stage-packages: @@ -358,7 +390,7 @@ parts: # copy local scripts to the snap usr/bin local-files-ros: plugin: dump - source: snap/local/ros_common/ + source: snap/local/local-ros/ organize: '*.sh': usr/bin/ '*.xml': usr/share/rosbot-xl/config/