diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 2a2b7a2..ff6a03b 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,11 +1,7 @@ { - "dockerComposeFile": [ - "../docker/docker-compose.yml", - "../docker/docker-compose-dev.yml" - ], + "dockerComposeFile": ["../docker/docker-compose.yml", "../docker/docker-compose-dev.yml"], "service": "sim", "runServices": ["sim"], - "forwardPorts": [6080, 5900], "workspaceFolder": "/home/trickfire/simulations", "remoteEnv": { diff --git a/.devcontainer/nvidia/devcontainer.json b/.devcontainer/nvidia/devcontainer.json index ad12d52..937dbc3 100644 --- a/.devcontainer/nvidia/devcontainer.json +++ b/.devcontainer/nvidia/devcontainer.json @@ -6,7 +6,8 @@ ], "service": "sim", "runServices": ["sim"], - "forwardPorts": [6080, 5900], + // Ports are published in docker/docker-compose.yml (sourced from docker/.env), + // so VS Code auto-forwards them without needing forwardPorts here. "workspaceFolder": "/home/trickfire/simulations", "remoteEnv": { diff --git a/.devcontainer/x_server.sh b/.devcontainer/x_server.sh index 95cdd9d..844e616 100755 --- a/.devcontainer/x_server.sh +++ b/.devcontainer/x_server.sh @@ -1,5 +1,4 @@ #!/usr/bin/env bash -set -eo pipefail # -------------------------------------------------------------------------------------------- # Starts a headless X11 desktop (Xorg/Xvfb + Openbox + x11vnc + noVNC) inside the container. @@ -7,36 +6,41 @@ set -eo pipefail # Supports --verbose flag for debugging (prints all output to console instead of log file). # -------------------------------------------------------------------------------------------- +set -eo pipefail +trap '' HUP + VERBOSE=false +FORCE_VNC=${FORCE_VNC:-} LOG_FILE="/tmp/start_x_server.log" PIDS=() : >"$LOG_FILE" log() { printf "\033[1;36m%s\033[0m\n" "$1"; } -# Foreground commands: exit with log dump on failure run() { if $VERBOSE; then "$@" - else "$@" >>"$LOG_FILE" 2>&1 || { - echo - echo "[ERROR] Command failed: $*" - echo "────────── OUTPUT START ──────────" - cat "$LOG_FILE" - echo "─────────── OUTPUT END ───────────" - exit 1 - }; fi + else + "$@" >>"$LOG_FILE" 2>&1 || { + echo + echo "[ERROR] Command failed: $*" + echo "────────── OUTPUT START ──────────" + cat "$LOG_FILE" + echo "─────────── OUTPUT END ───────────" + exit 1 + } + fi } -# Background services: just redirect output start() { if $VERBOSE; then "$@" & - else "$@" >>"$LOG_FILE" 2>&1 & fi + else + "$@" >>"$LOG_FILE" 2>&1 & + fi PIDS+=($!) } -# Cleanup function to kill background processes on exit cleanup() { trap - SIGINT SIGTERM EXIT log "[CLEANUP] Shutting down X11 / VNC / noVNC…" @@ -44,130 +48,148 @@ cleanup() { wait 2>/dev/null || true } -# -------------------------------------------------------------------------------------------- +parse_args() { + for arg in "$@"; do + case "$arg" in + -v | --verbose) VERBOSE=true ;; + --force-vnc) FORCE_VNC=1 ;; + esac + done +} -# Parse --verbose flag -for arg in "$@"; do - case "$arg" in -v | --verbose) VERBOSE=true ;; esac -done - -# wayland is the best for this, just use that if the host has it -WAYLAND_SOCK="/run/host-runtime/${WAYLAND_DISPLAY:-wayland-0}" -if [ -S "$WAYLAND_SOCK" ]; then - log "[X11] Using Wayland socket at $WAYLAND_SOCK" - exit 0 -fi - -# Jetson/Tegra has no /dev/dri but Xorg drivers (nvidia, modesetting, dummy) all need it. -# Use Xvfb on Jetson; GPU rendering still happens via EGL through the injected Tegra libs. -# Desktop NVIDIA has /proc/driver/nvidia; everything else gets the dummy Xorg driver. -if [ -e /dev/nvmap ] && [ ! -d /dev/dri ]; then - BACKEND="xvfb" - XORG_CONF="/etc/X11/xorg.nvidia.conf" - log "[X11] Jetson/Tegra detected (no DRI), using Xvfb + EGL" -elif [ -e /proc/driver/nvidia ] || nvidia-smi &>/dev/null; then - BACKEND="xorg" - XORG_CONF="/etc/X11/xorg.nvidia.conf" - log "[X11] Desktop NVIDIA GPU detected, using Xorg nvidia driver" -else +# Exits the script early (code 0) if a host Wayland compositor socket is +# available, since Vulkan/GL clients can talk to it directly without any +# in-container X server. FORCE_VNC always skips this and forces the VNC path. +try_wayland_passthrough() { + if [ -n "$FORCE_VNC" ]; then + DISPLAY="${FORCE_VNC_DISPLAY:-:77}" + unset WAYLAND_DISPLAY + log "[X11] FORCE_VNC set. Forcing virtual display $DISPLAY + VNC/noVNC" + return + fi + + local wayland_sock="/run/host-runtime/${WAYLAND_DISPLAY:-wayland-0}" + if [ -S "$wayland_sock" ]; then + log "[X11] Using Wayland socket at $wayland_sock" + exit 0 + fi +} + +# Sets BACKEND ("xorg"/"xvfb") and XORG_CONF based on available hardware. +detect_backend() { + if [ -n "$FORCE_VNC" ]; then + # FORCE_VNC always runs alongside a real host session, so real Xorg here + # (dummy, vkms-targeted, or real GPU driver - it doesn't matter which) has + # repeatedly ended up touching real display hardware and hijacking the + # host's screen. Xvfb is the only backend that's structurally incapable of + # touching /dev/dri at all, so FORCE_VNC always uses it, no exceptions. + BACKEND="xvfb" + XORG_CONF="/etc/X11/xorg.dummy.conf" # only used below to read screen resolution + log "[X11] FORCE_VNC: using Xvfb unconditionally (no direct GPU access)" + elif [ -e /dev/nvmap ] && [ ! -d /dev/dri ]; then + BACKEND="xvfb" + XORG_CONF="/etc/X11/xorg.nvidia.conf" + log "[X11] Jetson/Tegra detected (no DRI), using Xvfb + EGL" + elif [ -e /proc/driver/nvidia ] || nvidia-smi &>/dev/null; then + BACKEND="xorg" + XORG_CONF="/etc/X11/xorg.nvidia.conf" + log "[X11] Desktop NVIDIA GPU detected, using Xorg nvidia driver" + else + detect_vkms_backend + fi +} + +# No-GPU case: sets BACKEND and XORG_CONF, preferring a vkms virtual display +# over plain Xvfb when the kernel module is available. +detect_vkms_backend() { BACKEND="xorg" log "[X11] No GPU detected - trying vkms for DRI3-capable headless display" sudo modprobe vkms 2>/dev/null || true + + local card drv dev_path VKMS_CARD="" for card in /dev/dri/card*; do - drv=$(readlink -f "/sys/class/drm/$(basename "$card")/device/driver" 2>/dev/null) || continue - [[ $drv == *vkms* ]] && { + drv=$(readlink -f "/sys/class/drm/$(basename "$card")/device/driver" 2>/dev/null) + dev_path=$(readlink -f "/sys/class/drm/$(basename "$card")/device" 2>/dev/null) + [[ $drv == *vkms* || $dev_path == *vkms* ]] && { VKMS_CARD="$card" break } done - if [ -n "$VKMS_CARD" ]; then - log "[X11] Using vkms virtual display ($VKMS_CARD)" - XORG_CONF=$(mktemp /tmp/xorg-vkms.XXXXXX.conf) - cat >"$XORG_CONF" <"$XORG_CONF" +} + +# Reads SCREEN_WIDTH/SCREEN_HEIGHT/SCREEN_DEPTH out of $XORG_CONF. +parse_screen_resolution() { + SCREEN_MODE=$(grep -oP '(?<=Modes ")[^"]+' "$XORG_CONF" | head -1) + SCREEN_WIDTH=$(echo "$SCREEN_MODE" | cut -dx -f1) + SCREEN_HEIGHT=$(echo "$SCREEN_MODE" | cut -dx -f2) + SCREEN_DEPTH=$(grep -oP '(?<=DefaultDepth )\d+' "$XORG_CONF" | head -1) + if [ -z "$SCREEN_WIDTH" ] || [ -z "$SCREEN_HEIGHT" ] || [ -z "$SCREEN_DEPTH" ]; then + echo "[ERROR] Could not parse resolution from $XORG_CONF" + exit 1 + fi +} + +start_services() { + trap cleanup SIGINT SIGTERM + + # Start X server (Xorg or Xvfb determined above) + if [ "$BACKEND" = "xvfb" ]; then + log "[X11] Starting Xvfb on display ${DISPLAY} (${SCREEN_WIDTH}x${SCREEN_HEIGHT}x${SCREEN_DEPTH})" + start Xvfb "$DISPLAY" -screen 0 "${SCREEN_WIDTH}x${SCREEN_HEIGHT}x${SCREEN_DEPTH}" else - log "[X11] vkms unavailable, falling back to dummy driver (no DRI3)" - XORG_CONF="/etc/X11/xorg.dummy.conf" + log "[X11] Starting Xorg on display ${DISPLAY} (${SCREEN_WIDTH}x${SCREEN_HEIGHT}x${SCREEN_DEPTH})" + start sudo Xorg "$DISPLAY" -noreset -config "$XORG_CONF" fi -fi - -# Parse screen resolution from Xorg config (in docker/xorg.dummy|nvidia.conf) -SCREEN_MODE=$(grep -oP '(?<=Modes ")[^"]+' "$XORG_CONF" | head -1) -SCREEN_WIDTH=$(echo "$SCREEN_MODE" | cut -dx -f1) -SCREEN_HEIGHT=$(echo "$SCREEN_MODE" | cut -dx -f2) -SCREEN_DEPTH=$(grep -oP '(?<=DefaultDepth )\d+' "$XORG_CONF" | head -1) -if [ -z "$SCREEN_WIDTH" ] || [ -z "$SCREEN_HEIGHT" ] || [ -z "$SCREEN_DEPTH" ]; then - echo "[ERROR] Could not parse resolution from $XORG_CONF" - exit 1 -fi - -# Set in the Dockerfile and docker compose. -: "${DISPLAY:?DISPLAY is not set}" -: "${VNC_PORT:?VNC_PORT is not set}" -: "${NOVNC_PORT:?NOVNC_PORT is not set}" - -# Check if DISPLAY is already in use -if xdpyinfo -display "$DISPLAY" &>/dev/null; then - log "[ERROR] Display $DISPLAY is already in use!" - exit 1 -fi - -# Kill all child processes on exit -trap cleanup SIGINT SIGTERM - -# Start X server (Xorg or Xvfb determined above) -if [ "$BACKEND" = "xvfb" ]; then - log "[X11] Starting Xvfb on display ${DISPLAY} (${SCREEN_WIDTH}x${SCREEN_HEIGHT}x${SCREEN_DEPTH})" - start Xvfb "$DISPLAY" -screen 0 "${SCREEN_WIDTH}x${SCREEN_HEIGHT}x${SCREEN_DEPTH}" -else - log "[X11] Starting Xorg on display ${DISPLAY} (${SCREEN_WIDTH}x${SCREEN_HEIGHT}x${SCREEN_DEPTH})" - start sudo Xorg "$DISPLAY" -noreset -config "$XORG_CONF" -fi -sleep 1 -stty sane 2>/dev/null || true # Xorg alters terminal CR/LF settings - -# Start window manager -log "[WM] Starting Openbox window manager" -start openbox-session - -# Start x11vnc server for VNC connection -log "[VNC] Starting x11vnc on port ${VNC_PORT}" -start x11vnc -display "$DISPLAY" -forever -shared -rfbport "$VNC_PORT" -nopw -xkb - -# Start noVNC web client to serve the VNC desktop in a browser -log "[noVNC] Starting browser-based desktop on port ${NOVNC_PORT}" -start /usr/share/novnc/utils/novnc_proxy --vnc "localhost:${VNC_PORT}" --listen "${NOVNC_PORT}" -log "[noVNC] Desktop available at: http://localhost:${NOVNC_PORT}/vnc.html" - -# Detach all background services so they survive this script exiting -disown "${PIDS[@]}" || true -log "[MAIN] All services started" + sleep 1 + stty sane 2>/dev/null || true # Xorg alters terminal CR/LF settings + + # Start window manager + log "[WM] Starting Openbox window manager" + start openbox-session + + # Start x11vnc server for VNC connection + log "[VNC] Starting x11vnc on port ${VNC_PORT}" + start x11vnc -display "$DISPLAY" -forever -shared -rfbport "$VNC_PORT" -nopw -xkb + + # Start noVNC web client to serve the VNC desktop in a browser + log "[noVNC] Starting browser-based desktop on port ${NOVNC_PORT}" + start /usr/share/novnc/utils/novnc_proxy --vnc "localhost:${VNC_PORT}" --listen "${NOVNC_PORT}" + log "[noVNC] Desktop available at: http://localhost:${NOVNC_PORT}/vnc.html" + + # Detach all background services so they survive this script exiting + disown "${PIDS[@]}" || true + log "[MAIN] All services started" +} + +main() { + parse_args "$@" + try_wayland_passthrough + + detect_backend + parse_screen_resolution + + : "${DISPLAY:?DISPLAY is not set}" + : "${VNC_PORT:?VNC_PORT is not set}" + : "${NOVNC_PORT:?NOVNC_PORT is not set}" + + if xdpyinfo -display "$DISPLAY" &>/dev/null; then + log "[ERROR] Display $DISPLAY is already in use!" + exit 1 + fi + + start_services +} + +main "$@" diff --git a/.gitignore b/.gitignore index 7d9db75..aa2688e 100644 --- a/.gitignore +++ b/.gitignore @@ -31,6 +31,7 @@ chrono/results/* # env **/*.env +!docker/.env # trickfire-docs .trickfire-docs/ diff --git a/.vscode/settings.json b/.vscode/settings.json index 78603a2..482d539 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -74,5 +74,6 @@ "${workspaceFolder}/gazebo/sim_common" ], "ROS2.distro": "jazzy", - "cmake.ignoreCMakeListsMissing": true + "cmake.ignoreCMakeListsMissing": true, + "autoDevcontainer.enabled": true } diff --git a/docs/assets/trickfire-wallpaper.png b/assets/trickfire-wallpaper.png similarity index 100% rename from docs/assets/trickfire-wallpaper.png rename to assets/trickfire-wallpaper.png diff --git a/cli/chrono/chrono.py b/cli/chrono/chrono.py index 8346629..5e8bc37 100644 --- a/cli/chrono/chrono.py +++ b/cli/chrono/chrono.py @@ -7,12 +7,12 @@ def run(): - result = subprocess.run(["make", "run"], cwd=CHRONO_TERRAIN_DIR) + result = subprocess.run(["make", "run"], cwd=CHRONO_TERRAIN_DIR, check=False) if result.returncode != 0: sys.exit(result.returncode) def clean(): - result = subprocess.run(["make", "clean"], cwd=CHRONO_TERRAIN_DIR) + result = subprocess.run(["make", "clean"], cwd=CHRONO_TERRAIN_DIR, check=False) if result.returncode != 0: sys.exit(result.returncode) diff --git a/cli/cli.py b/cli/cli.py index bdfe061..e9bb82c 100644 --- a/cli/cli.py +++ b/cli/cli.py @@ -10,7 +10,6 @@ from .output import die, info from .paths import GAZEBO_WORKSPACE_DIR - _GAZEBO_SUBCOMMANDS = frozenset({"clean", "create", "update", "auth"}) @@ -85,7 +84,7 @@ def main() -> None: _launch_gazebo(sys.argv[2], sys.argv[3:]) except KeyboardInterrupt: sys.exit(130) - except Exception as e: + except Exception as e: # noqa: BLE001 - top-level handler, report and exit die(str(e)) return @@ -95,8 +94,8 @@ def main() -> None: ) subparsers = parser.add_subparsers(dest="sim") - gazebo_parser, gazebo_subs, create_p = _build_gazebo_parser(subparsers) - chrono_parser, chrono_subs = _build_chrono_parser(subparsers) + gazebo_parser, _gazebo_subs, create_p = _build_gazebo_parser(subparsers) + chrono_parser, _chrono_subs = _build_chrono_parser(subparsers) args = parser.parse_args() @@ -116,7 +115,7 @@ def main() -> None: parser.print_help() except KeyboardInterrupt: sys.exit(130) - except Exception as e: + except Exception as e: # noqa: BLE001 - top-level handler, report and exit die(str(e)) diff --git a/cli/drpc.py b/cli/drpc.py index f2264b4..885410b 100644 --- a/cli/drpc.py +++ b/cli/drpc.py @@ -45,5 +45,5 @@ def rpc_start(is_docker: bool = False, robot_name: str = "Unknown Robot") -> Non ) while True: time.sleep(15) - except Exception as e: # pylint: disable=broad-except + except Exception as e: # noqa: BLE001 - RPC is best-effort, never fatal print(f"Discord RPC unavailable: {e}") diff --git a/cli/gazebo/create/__init__.py b/cli/gazebo/create/__init__.py index 55c4e74..3e94ec4 100644 --- a/cli/gazebo/create/__init__.py +++ b/cli/gazebo/create/__init__.py @@ -6,7 +6,7 @@ from pathlib import Path from ...output import info -from ...paths import REPO_DIR, GAZEBO_WORKSPACE_DIR +from ...paths import GAZEBO_WORKSPACE_DIR, REPO_DIR PACKAGE_DIR = Path(__file__).parent TEMPLATES = PACKAGE_DIR / "templates" diff --git a/cli/gazebo/create/commands.py b/cli/gazebo/create/commands.py index 0b555fb..4f0a93b 100644 --- a/cli/gazebo/create/commands.py +++ b/cli/gazebo/create/commands.py @@ -4,8 +4,9 @@ import tempfile from pathlib import Path +from ...output import die as err +from ...output import info, warn from . import PACKAGE_DIR -from ...output import die as err, info, warn from .onshape import download, parse_onshape_url from .reduce_stl import batch_process_directory from .registry import load_robots_json, save_robots_json diff --git a/cli/gazebo/create/onshape.py b/cli/gazebo/create/onshape.py index f440f82..b2d479a 100644 --- a/cli/gazebo/create/onshape.py +++ b/cli/gazebo/create/onshape.py @@ -11,7 +11,8 @@ from ... import config as cfg from ...auth import DASHBOARD_URL -from ...output import die as err, info +from ...output import die as err +from ...output import info _ONSHAPE_URL_RE = re.compile( r"documents/([0-9a-f]+)/[wv]/([0-9a-f]+)/e/([0-9a-f]+)", @@ -102,7 +103,7 @@ def download( if not resp.ok: try: msg = resp.json().get("error", resp.text) - except Exception: + except Exception: # noqa: BLE001 - fall back to raw response text msg = resp.text err(f"Export failed ({resp.status_code}): {msg}") @@ -113,8 +114,7 @@ def download( archive_path = tmpdir / "export.tar.gz" info("Downloading export archive...") with open(archive_path, "wb") as f: - for chunk in resp.iter_content(chunk_size=65536): - f.write(chunk) + f.writelines(resp.iter_content(chunk_size=65536)) info("Extracting...") with tarfile.open(archive_path, "r:gz") as tf: diff --git a/cli/gazebo/create/reduce_stl.py b/cli/gazebo/create/reduce_stl.py index 16d98d5..7db830a 100644 --- a/cli/gazebo/create/reduce_stl.py +++ b/cli/gazebo/create/reduce_stl.py @@ -62,7 +62,7 @@ def batch_process_directory(input_dir: str, output_dir: str, reduction_ratio: fl old_file_size = 0 new_file_size = 0 - trimesh, pyfqmr = _imports() + trimesh, _pyfqmr = _imports() for file_path in stl_files: filename = os.path.basename(file_path) out_path = os.path.join(output_dir, filename) diff --git a/cli/gazebo/create/ros_packages.py b/cli/gazebo/create/ros_packages.py index 44a5659..0f07414 100644 --- a/cli/gazebo/create/ros_packages.py +++ b/cli/gazebo/create/ros_packages.py @@ -3,8 +3,9 @@ import shutil from pathlib import Path +from ...output import die as err +from ...output import info from . import TEMPLATES -from ...output import die as err, info from .template import write_template diff --git a/cli/gazebo/create/templates/bringup/launch/__ROBOT__.launch.py b/cli/gazebo/create/templates/bringup/launch/__ROBOT__.launch.py index e67b93c..b847dbd 100644 --- a/cli/gazebo/create/templates/bringup/launch/__ROBOT__.launch.py +++ b/cli/gazebo/create/templates/bringup/launch/__ROBOT__.launch.py @@ -2,46 +2,25 @@ Launch script for the Gazebo __ROBOT__ simulation """ -import os -import shutil -import subprocess -import sys - -import xacro -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - RegisterEventHandler, - TimerAction, -) +from launch.actions import DeclareLaunchArgument, TimerAction from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from sim_common.launch_utils import get_asset - - -def _gz_supports_sim_command(): - """Return True when `gz` exists and exposes the `sim` subcommand.""" - if shutil.which("gz") is None: - return False - - try: - result = subprocess.run( - ["gz", "--commands"], - check=False, - capture_output=True, - text=True, - ) - except OSError: - return False +from sim_common.launch_utils import ( + chain_controller_spawners, + clock_bridge_node, + controller_spawner_node, + gazebo_launch_actions, + get_asset, + joint_state_broadcaster_spawner, + process_robot_description, + robot_state_publisher_node, + rviz_node, + spawn_robot_node, +) - commands = result.stdout.splitlines() - return any(line.strip() == "sim" for line in commands) +ROBOT_NAME = "__ROBOT__" def generate_launch_description(): @@ -53,121 +32,12 @@ def generate_launch_description(): urdf_file = get_asset("__ROBOT___description", "urdf", "__ROBOT__.urdf") gz_gui_config = get_asset("sim_worlds", "gui", "gui.config") - # ---------------------------------------------- - # xacro generate URDF - # ---------------------------------------------- - - robot_desc = xacro.process_file( - urdf_file, - mappings={ - "controller_config": controller_config, - "plugin_extension": ".dylib" if sys.platform == "darwin" else ".so", - }, - ).toxml() - - # ---------------------------------------------- - # Gazebo launch arguments - # ---------------------------------------------- - - use_split_gui = _gz_supports_sim_command() - gz_server_args = ( - ["-r", "-s", world_file] - if use_split_gui - else ["-r", world_file, "--gui-config", gz_gui_config] - ) - - gz_server = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py") - ), - launch_arguments={"gz_args": " ".join(gz_server_args)}.items(), - ) - - gz_gui = ExecuteProcess( - cmd=["gz", "sim", "--force-version", "8", "-g", "--gui-config", gz_gui_config], - output="screen", - condition=IfCondition(LaunchConfiguration("gui")), - ) - - # ---------------------------------------------- - # Gazebo spawn model - # ---------------------------------------------- - - spawn_robot = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-name", - "__ROBOT__", - "-string", - robot_desc, - "-x", - "0", - "-y", - "0", - "-z", - "0.1", - ], - output="screen", - ) - - # ---------------------------------------------- - # Robot state publisher - # ---------------------------------------------- - - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="both", - parameters=[{"robot_description": robot_desc}], - ) - - # ---------------------------------------------- - # Joint controllers - # ---------------------------------------------- - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "joint_state_broadcaster", - ], - ) - joint_trajectory_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "joint_trajectory_controller", - "--param-file", - controller_config, - ], - ) - - # ---------------------------------------------- - # RVIZ - # ---------------------------------------------- - - rviz = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - arguments=[ - "-d", - rviz_config, - ], - condition=IfCondition(LaunchConfiguration("rviz")), - ) + robot_desc = process_robot_description(urdf_file, controller_config) - # ---------------------------------------------- - # ROS GZ BRIDGE - # ---------------------------------------------- - - bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], - output="screen", + spawn_robot = spawn_robot_node(ROBOT_NAME, robot_desc) + joint_state_broadcaster = joint_state_broadcaster_spawner() + joint_trajectory_controller = controller_spawner_node( + "joint_trajectory_controller", controller_config ) # ---------------------------------------------- @@ -182,32 +52,18 @@ def generate_launch_description(): output="screen", ) - spawn_robot_delayed = TimerAction(period=5.0, actions=[spawn_robot]) - gz_gui_delayed = TimerAction(period=2.0, actions=[gz_gui]) - maybe_gui_action = gz_gui_delayed if use_split_gui else None - return LaunchDescription( [ - gz_server, - *([maybe_gui_action] if maybe_gui_action is not None else []), - spawn_robot_delayed, - robot_state_publisher, + *gazebo_launch_actions(world_file, gz_gui_config), + TimerAction(period=5.0, actions=[spawn_robot]), + robot_state_publisher_node(robot_desc), DeclareLaunchArgument("rviz", default_value="true", description="Open RViz."), DeclareLaunchArgument("gui", default_value="true", description="Open Joint GUI."), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=spawn_robot, - on_exit=[joint_state_broadcaster_spawner], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[joint_trajectory_controller_spawner], - ) + *chain_controller_spawners( + spawn_robot, joint_state_broadcaster, joint_trajectory_controller ), - bridge, - rviz, + clock_bridge_node(), + rviz_node(rviz_config), joint_gui, ] ) diff --git a/cli/gazebo/launch.py b/cli/gazebo/launch.py index 6920b4d..f676c0f 100644 --- a/cli/gazebo/launch.py +++ b/cli/gazebo/launch.py @@ -121,6 +121,23 @@ def _check_display() -> None: die("Cannot connect to display " + display) +def _configure_force_vnc_rendering(env: dict[str, str]) -> list[str]: + """Force software GL rendering for Gazebo/OGRE2 when running under FORCE_VNC. + + FORCE_VNC intentionally never touches real GPU/render-node devices - every + attempt to give it direct GPU access (Xorg with a real or virtual KMS + device, VirtualGL through the render node) has ended up interfering with + the host's real display. Mesa's software rasterizer is unaccelerated but + it's the only backend that's actually safe here. + """ + if not os.environ.get("FORCE_VNC"): + return [] + + info("FORCE_VNC: forcing software rendering (llvmpipe) - no direct GPU access") + env["LIBGL_ALWAYS_SOFTWARE"] = "1" + return [] + + def _setup_pixi_env() -> None: """Configure Gazebo plugin/resource paths and Qt platform for a pixi environment.""" pixi_dir = REPO_DIR / ".pixi" @@ -171,6 +188,7 @@ def build_and_launch(robot_name: str, *, build_only: bool = False, no_build: boo build = not no_build launch = not build_only env = os.environ.copy() + render_prefix = _configure_force_vnc_rendering(env) bringup_pkg, description_pkg, launch_file_name = _validate_robot_layout(robot_name) if build: @@ -237,7 +255,7 @@ def build_and_launch(robot_name: str, *, build_only: bool = False, no_build: boo echo \" Expected: $SIM_WORLDS_SHARE/worlds\" fi -exec ros2 launch \"{bringup_pkg}\" \"{launch_file_name}\" gui:=true +exec {" ".join(render_prefix)} ros2 launch \"{bringup_pkg}\" \"{launch_file_name}\" gui:=true """.strip() try: diff --git a/cli/output.py b/cli/output.py index 0e84348..be09172 100644 --- a/cli/output.py +++ b/cli/output.py @@ -1,10 +1,11 @@ -import sys import os -from typing import Callable, NoReturn +import sys +from collections.abc import Callable +from typing import NoReturn try: _is_tty = os.isatty(sys.stdout.fileno()) -except Exception: # pylint: disable=broad-except +except Exception: # noqa: BLE001 - fall back to non-tty output _is_tty = False if _is_tty: _B = "\033[1m" # bold diff --git a/docker/.env b/docker/.env new file mode 100644 index 0000000..e32b75a --- /dev/null +++ b/docker/.env @@ -0,0 +1,11 @@ +# main source of truth +# for docker exposed ports + +VNC_PORT=5900 +NOVNC_PORT=6080 +ROSBRIDGE_PORT=9090 + +# set to one if you want +# to force vnc startup +# (ignore wayland, existing xserver etc) +# FORCE_VNC=1 diff --git a/docker/Dockerfile b/docker/Dockerfile index 2b55d11..ff52414 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -4,6 +4,10 @@ ARG DEBIAN_FRONTEND=noninteractive ARG TZ="America/Los_Angeles" ARG BUILD_JOBS +ARG VNC_PORT +ARG NOVNC_PORT +ARG ROSBRIDGE_PORT + # ---------------------------------------------------------------------------- # # BASE # # ---------------------------------------------------------------------------- # @@ -218,7 +222,8 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ ros-jazzy-rviz2 \ ros-jazzy-xacro \ ros-jazzy-joint-state-publisher-gui \ - ros-jazzy-gz-ros2-control + ros-jazzy-gz-ros2-control \ + ros-jazzy-rosbridge-suite RUN --mount=type=cache,target=/root/.cache/pip \ pip3 install --break-system-packages pypresence trimesh pyfqmr @@ -236,9 +241,6 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ mesa-utils libglx-mesa0 libgl1-mesa-dri x11-apps \ xserver-xorg-core xserver-xorg-video-dummy kmod -EXPOSE 6080 5900 -ENV VNC_PORT=5900 -ENV NOVNC_PORT=6080 # ---------------------------------------------------------------------------- # # DEV TOOLING # @@ -264,6 +266,16 @@ RUN rosdep fix-permissions && \ COPY docker/xorg.nvidia.conf /etc/X11/xorg.nvidia.conf COPY docker/xorg.dummy.conf /etc/X11/xorg.dummy.conf +COPY docker/xorg.vkms.conf /etc/X11/xorg.vkms.conf COPY docker/bashrc.sh /home/trickfire/.bashrc +EXPOSE ${NOVNC_PORT} +EXPOSE ${VNC_PORT} +EXPOSE ${ROSBRIDGE_PORT} + +ENV VNC_PORT=${VNC_PORT} +ENV NOVNC_PORT=${NOVNC_PORT} +ENV ROSBRIDGE_PORT=${ROSBRIDGE_PORT} + + USER trickfire diff --git a/docker/bashrc.sh b/docker/bashrc.sh index 2822068..7db664d 100644 --- a/docker/bashrc.sh +++ b/docker/bashrc.sh @@ -72,7 +72,12 @@ if [ -z "${TF_ENV_SOURCED:-}" ]; then fi # ---------- display ---------- -if [[ -n ${DISPLAY:-} && ${DISPLAY} != :* ]]; then +if [ -n "${FORCE_VNC:-}" ]; then + # Point new shells at the virtual display x_server.sh set up for FORCE_VNC, + # not the host's real DISPLAY (which is still reachable via the bind-mounted + # X11 socket and would otherwise render GUI apps on the host instead of VNC). + export DISPLAY="${FORCE_VNC_DISPLAY:-:77}" +elif [[ -n ${DISPLAY:-} && ${DISPLAY} != :* ]]; then export DISPLAY=":${DISPLAY}" fi @@ -107,8 +112,6 @@ alias c='clear' alias ll='ls -alF --color=auto' alias la='ls -A --color=auto' alias l='ls -CF --color=auto' -alias ..='cd ..' -alias ...='cd ../..' alias rs='tf_source_env' alias rsource='tf_source_env' @@ -117,3 +120,6 @@ alias rtl='ros2 topic list' alias rte='ros2 topic echo' alias gtl='gz topic -l' alias gte='gz topic -e -t' + +alias sg='sim gazebo' +alias sc='sim chrono' diff --git a/docker/docker-compose-dev.yml b/docker/docker-compose-dev.yml index 8a1b74d..1af67cf 100644 --- a/docker/docker-compose-dev.yml +++ b/docker/docker-compose-dev.yml @@ -1,13 +1,13 @@ services: sim: environment: - - DISPLAY - - WAYLAND_DISPLAY - - XDG_RUNTIME_DIR=/run/host-runtime - - QT_SCALE_FACTOR - - QT_ENABLE_HIGHDPI_SCALING - - GDK_SCALE - - GDK_DPI_SCALE + WAYLAND_DISPLAY: "${WAYLAND_DISPLAY:-}" + XDG_RUNTIME_DIR: /run/host-runtime + QT_SCALE_FACTOR: "${QT_SCALE_FACTOR:-}" + QT_ENABLE_HIGHDPI_SCALING: "${QT_ENABLE_HIGHDPI_SCALING:-}" + GDK_SCALE: "${GDK_SCALE:-}" + GDK_DPI_SCALE: "${GDK_DPI_SCALE:-}" + FORCE_VNC: "${FORCE_VNC:-}" build: args: USER_UID: "${UID:-1000}" diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 7f96770..36004ac 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -5,14 +5,19 @@ services: dockerfile: docker/Dockerfile target: sim network: host + args: + VNC_PORT: "${VNC_PORT:?VNC_PORT must be set, see docker/.env}" + NOVNC_PORT: "${NOVNC_PORT:?NOVNC_PORT must be set, see docker/.env}" + ROSBRIDGE_PORT: "${ROSBRIDGE_PORT:?ROSBRIDGE_PORT must be set, see docker/.env}" image: simulations:latest container_name: simulations user: trickfire environment: DISPLAY: "${DISPLAY:-:0}" - VNC_PORT: "5900" - NOVNC_PORT: "6080" + VNC_PORT: "${VNC_PORT:?VNC_PORT must be set, see docker/.env}" + NOVNC_PORT: "${NOVNC_PORT:?NOVNC_PORT must be set, see docker/.env}" + ROSBRIDGE_PORT: "${ROSBRIDGE_PORT:?ROSBRIDGE_PORT must be set, see docker/.env}" QT_X11_NO_MITSHM: "1" TZ: ${TZ:-UTC} @@ -28,8 +33,9 @@ services: working_dir: /home/trickfire/simulations ports: - - "5900:5900" - - "6080:6080" + - "${VNC_PORT}:${VNC_PORT}" + - "${NOVNC_PORT}:${NOVNC_PORT}" + - "${ROSBRIDGE_PORT}:${ROSBRIDGE_PORT}" privileged: true diff --git a/docker/xorg.dummy.conf b/docker/xorg.dummy.conf index 1cb1088..5e14bb2 100644 --- a/docker/xorg.dummy.conf +++ b/docker/xorg.dummy.conf @@ -1,3 +1,13 @@ +Section "ServerFlags" + # Prevent Xorg from auto-probing and attaching real /dev/dri devices (this + # container has host device access via `privileged: true`). Without this, + # Xorg grabs the host's real GPU/display-subsystem DRM nodes even with the + # dummy driver configured, which can steal DRM master from a live compositor + # (e.g. Hyprland) running on the same physical display and blank the screen. + Option "AutoAddGPU" "off" + Option "AutoEnableDevices" "off" +EndSection + Section "Module" Load "glx" EndSection diff --git a/docker/xorg.vkms.conf b/docker/xorg.vkms.conf new file mode 100644 index 0000000..e231485 --- /dev/null +++ b/docker/xorg.vkms.conf @@ -0,0 +1,32 @@ +Section "Module" + Load "glx" +EndSection + +Section "Device" + Identifier "VKMSDevice" + Driver "modesetting" + Option "kmsdev" "__VKMS_CARD__" + Option "DRI" "3" +EndSection + +Section "Monitor" + Identifier "DummyMonitor" + HorizSync 28-80 + VertRefresh 48-75 +EndSection + +Section "Screen" + Identifier "DummyScreen" + Device "VKMSDevice" + Monitor "DummyMonitor" + DefaultDepth 24 + SubSection "Display" + Depth 24 + Modes "1920x1080" + EndSubSection +EndSection + +Section "ServerLayout" + Identifier "DummyLayout" + Screen "DummyScreen" +EndSection diff --git a/gazebo/arm_bringup/launch/arm.launch.py b/gazebo/arm_bringup/launch/arm.launch.py index fcc62c8..c658d15 100644 --- a/gazebo/arm_bringup/launch/arm.launch.py +++ b/gazebo/arm_bringup/launch/arm.launch.py @@ -2,46 +2,25 @@ Launch script for the Gazebo arm simulation """ -import os -import shutil -import subprocess -import sys - -import xacro -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - RegisterEventHandler, - TimerAction, -) +from launch.actions import DeclareLaunchArgument, TimerAction from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from sim_common.launch_utils import get_asset - - -def _gz_supports_sim_command(): - """Return True when `gz` exists and exposes the `sim` subcommand.""" - if shutil.which("gz") is None: - return False - - try: - result = subprocess.run( - ["gz", "--commands"], - check=False, - capture_output=True, - text=True, - ) - except OSError: - return False +from sim_common.launch_utils import ( + chain_controller_spawners, + clock_bridge_node, + controller_spawner_node, + gazebo_launch_actions, + get_asset, + joint_state_broadcaster_spawner, + process_robot_description, + robot_state_publisher_node, + rviz_node, + spawn_robot_node, +) - commands = result.stdout.splitlines() - return any(line.strip() == "sim" for line in commands) +ROBOT_NAME = "arm" def generate_launch_description(): @@ -53,121 +32,12 @@ def generate_launch_description(): urdf_file = get_asset("arm_description", "urdf", "arm.urdf") gz_gui_config = get_asset("sim_worlds", "gui", "gui.config") - # ---------------------------------------------- - # xacro generate URDF - # ---------------------------------------------- - - robot_desc = xacro.process_file( - urdf_file, - mappings={ - "controller_config": controller_config, - "plugin_extension": ".dylib" if sys.platform == "darwin" else ".so", - }, - ).toxml() - - # ---------------------------------------------- - # Gazebo launch arguments - # ---------------------------------------------- - - use_split_gui = _gz_supports_sim_command() - gz_server_args = ( - ["-r", "-s", world_file] - if use_split_gui - else ["-r", world_file, "--gui-config", gz_gui_config] - ) - - gz_server = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py") - ), - launch_arguments={"gz_args": " ".join(gz_server_args)}.items(), - ) - - gz_gui = ExecuteProcess( - cmd=["gz", "sim", "--force-version", "8", "-g", "--gui-config", gz_gui_config], - output="screen", - condition=IfCondition(LaunchConfiguration("gui")), - ) - - # ---------------------------------------------- - # Gazebo spawn model - # ---------------------------------------------- - - spawn_robot = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-name", - "arm", - "-string", - robot_desc, - "-x", - "0", - "-y", - "0", - "-z", - "0.1", - ], - output="screen", - ) - - # ---------------------------------------------- - # Robot state publisher - # ---------------------------------------------- - - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="both", - parameters=[{"robot_description": robot_desc}], - ) - - # ---------------------------------------------- - # Joint controllers - # ---------------------------------------------- - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "joint_state_broadcaster", - ], - ) - joint_trajectory_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "joint_trajectory_controller", - "--param-file", - controller_config, - ], - ) - - # ---------------------------------------------- - # RVIZ - # ---------------------------------------------- - - rviz = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - arguments=[ - "-d", - rviz_config, - ], - condition=IfCondition(LaunchConfiguration("rviz")), - ) + robot_desc = process_robot_description(urdf_file, controller_config) - # ---------------------------------------------- - # ROS GZ BRIDGE - # ---------------------------------------------- - - bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], - output="screen", + spawn_robot = spawn_robot_node(ROBOT_NAME, robot_desc) + joint_state_broadcaster = joint_state_broadcaster_spawner() + joint_trajectory_controller = controller_spawner_node( + "joint_trajectory_controller", controller_config ) # ---------------------------------------------- @@ -182,32 +52,18 @@ def generate_launch_description(): output="screen", ) - spawn_robot_delayed = TimerAction(period=5.0, actions=[spawn_robot]) - gz_gui_delayed = TimerAction(period=2.0, actions=[gz_gui]) - maybe_gui_action = gz_gui_delayed if use_split_gui else None - return LaunchDescription( [ - gz_server, - *([maybe_gui_action] if maybe_gui_action is not None else []), - spawn_robot_delayed, - robot_state_publisher, + *gazebo_launch_actions(world_file, gz_gui_config), + TimerAction(period=5.0, actions=[spawn_robot]), + robot_state_publisher_node(robot_desc), DeclareLaunchArgument("rviz", default_value="true", description="Open RViz."), DeclareLaunchArgument("gui", default_value="true", description="Open Joint GUI."), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=spawn_robot, - on_exit=[joint_state_broadcaster_spawner], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[joint_trajectory_controller_spawner], - ) + *chain_controller_spawners( + spawn_robot, joint_state_broadcaster, joint_trajectory_controller ), - bridge, - rviz, + clock_bridge_node(), + rviz_node(rviz_config), joint_gui, ] ) diff --git a/gazebo/chassis_bringup/CMakeLists.txt b/gazebo/chassis_bringup/CMakeLists.txt new file mode 100644 index 0000000..56e639e --- /dev/null +++ b/gazebo/chassis_bringup/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.14) +project(chassis_bringup) + +find_package(ament_cmake REQUIRED) +find_package(chassis_description REQUIRED) + + +install( + DIRECTORY + launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + +install( + DIRECTORY + config/ + DESTINATION share/${PROJECT_NAME}/config +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/gazebo/chassis_bringup/config/chassis.controller.yaml b/gazebo/chassis_bringup/config/chassis.controller.yaml new file mode 100644 index 0000000..51669ed --- /dev/null +++ b/gazebo/chassis_bringup/config/chassis.controller.yaml @@ -0,0 +1,21 @@ +controller_manager: + ros__parameters: + use_sim_time: true + update_rate: 60 # Hz + + forward_velocity_controller: + type: forward_command_controller/ForwardCommandController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +forward_velocity_controller: + ros__parameters: + joints: + - fl_wheel + - ml_wheel + - bl_wheel + - fr_wheel + - mr_wheel + - br_wheel + interface_name: velocity diff --git a/gazebo/chassis_bringup/config/chassis.rviz b/gazebo/chassis_bringup/config/chassis.rviz new file mode 100644 index 0000000..6a187ec --- /dev/null +++ b/gazebo/chassis_bringup/config/chassis.rviz @@ -0,0 +1,219 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /RobotModel1/Description Topic1 + Splitter Ratio: 0.5 + Tree Height: 770 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + front_arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + part_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + part_1_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + part_1_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + part_1_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + part_1_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + part_1_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + part_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + part_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top__mid__arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ttb_0094_ttb_0095_2x1_grid_pattern_aluminum_extrusion: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.715324878692627 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7453979253768921 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.923583507537842 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1061 + Hide Left Dock: false + Hide Right Dock: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 0 diff --git a/gazebo/chassis_bringup/launch/chassis.launch.py b/gazebo/chassis_bringup/launch/chassis.launch.py new file mode 100644 index 0000000..afdf229 --- /dev/null +++ b/gazebo/chassis_bringup/launch/chassis.launch.py @@ -0,0 +1,88 @@ +""" +Launch script for the Gazebo chassis simulation +""" + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, TimerAction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from sim_common.launch_utils import ( + chain_controller_spawners, + clock_bridge_node, + controller_spawner_node, + gazebo_launch_actions, + get_asset, + joint_state_broadcaster_spawner, + process_robot_description, + robot_state_publisher_node, + rviz_node, + spawn_robot_node, +) + +ROBOT_NAME = "chassis" + + +def generate_launch_description(): + """ROS launch method, must have this name""" + + controller_config = get_asset("chassis_bringup", "config", "chassis.controller.yaml") + world_file = get_asset("sim_worlds", "worlds", "empty.world.sdf") + rviz_config = get_asset("chassis_bringup", "config", "chassis.rviz") + urdf_file = get_asset("chassis_description", "urdf", "chassis.urdf") + gz_gui_config = get_asset("sim_worlds", "gui", "gui.config") + + robot_desc = process_robot_description(urdf_file, controller_config) + + spawn_robot = spawn_robot_node(ROBOT_NAME, robot_desc) + joint_state_broadcaster = joint_state_broadcaster_spawner() + forward_velocity_controller = controller_spawner_node( + "forward_velocity_controller", controller_config + ) + + # ---------------------------------------------- + # Drivebase bridge + # ---------------------------------------------- + + drivebase_bridge = Node( + package="sim_common", + executable="drivebase", + output="screen", + ) + + # ---------------------------------------------- + # Rosbridge + # ---------------------------------------------- + + rosbridge = Node( + package="rosbridge_server", + executable="rosbridge_websocket", + output="screen", + parameters=[ + {"port": ParameterValue(LaunchConfiguration("rosbridge_port"), value_type=int)} + ], + ) + + return LaunchDescription( + [ + *gazebo_launch_actions(world_file, gz_gui_config), + TimerAction(period=5.0, actions=[spawn_robot]), + robot_state_publisher_node(robot_desc), + DeclareLaunchArgument("rviz", default_value="true", description="Open RViz."), + DeclareLaunchArgument("gui", default_value="true", description="Open Gazebo GUI."), + DeclareLaunchArgument( + "rosbridge_port", + default_value=os.environ.get("ROSBRIDGE_PORT", "9090"), + description="Port for the rosbridge WebSocket server.", + ), + *chain_controller_spawners( + spawn_robot, joint_state_broadcaster, forward_velocity_controller + ), + clock_bridge_node(), + rviz_node(rviz_config), + drivebase_bridge, + rosbridge, + ] + ) diff --git a/gazebo/chassis_bringup/package.xml b/gazebo/chassis_bringup/package.xml new file mode 100644 index 0000000..636d04c --- /dev/null +++ b/gazebo/chassis_bringup/package.xml @@ -0,0 +1,21 @@ + + + + chassis_bringup + 0.0.0 + Launch files for the chassis simulation + Trickfire Robotics + Apache-2.0 + + chassis_description + sim_common + rosbridge_server + + ament_cmake + + ament_lint_auto + + + ament_cmake + + diff --git a/gazebo/chassis_description/CMakeLists.txt b/gazebo/chassis_description/CMakeLists.txt new file mode 100644 index 0000000..3579a4d --- /dev/null +++ b/gazebo/chassis_description/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(chassis_description) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY + urdf/ + DESTINATION share/${PROJECT_NAME}/urdf +) + +install( + DIRECTORY + meshes/ + DESTINATION share/${PROJECT_NAME}/meshes +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/gazebo/chassis_description/meshes/front_arm.part b/gazebo/chassis_description/meshes/front_arm.part new file mode 100644 index 0000000..6598cad --- /dev/null +++ b/gazebo/chassis_description/meshes/front_arm.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "fc2c9f06a678801d77ce900d", + "documentMicroversion": "d54a915c618e87519dc31c64", + "elementId": "641fd7b7212dae8b4351b688", + "fullConfiguration": "default", + "id": "MfARvIl1g86yGAuF1", + "isStandardContent": false, + "name": "front arm <1>", + "partId": "JNL", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/gazebo/chassis_description/meshes/front_arm.stl b/gazebo/chassis_description/meshes/front_arm.stl new file mode 100644 index 0000000..c6d9564 Binary files /dev/null and b/gazebo/chassis_description/meshes/front_arm.stl differ diff --git a/gazebo/chassis_description/meshes/part_1.part b/gazebo/chassis_description/meshes/part_1.part new file mode 100644 index 0000000..fbb836a --- /dev/null +++ b/gazebo/chassis_description/meshes/part_1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "fc2c9f06a678801d77ce900d", + "documentMicroversion": "d54a915c618e87519dc31c64", + "elementId": "e61ce8158eefa60f7efab06b", + "fullConfiguration": "default", + "id": "Mv53Sr3RMAOH3+V2e", + "isStandardContent": false, + "name": "Part 1 <6>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/gazebo/chassis_description/meshes/part_1.stl b/gazebo/chassis_description/meshes/part_1.stl new file mode 100644 index 0000000..8fef24b Binary files /dev/null and b/gazebo/chassis_description/meshes/part_1.stl differ diff --git a/gazebo/chassis_description/meshes/part_3.part b/gazebo/chassis_description/meshes/part_3.part new file mode 100644 index 0000000..83a9fea --- /dev/null +++ b/gazebo/chassis_description/meshes/part_3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "fc2c9f06a678801d77ce900d", + "documentMicroversion": "d54a915c618e87519dc31c64", + "elementId": "641fd7b7212dae8b4351b688", + "fullConfiguration": "default", + "id": "MX1puQgww0AuNOlC+", + "isStandardContent": false, + "name": "Part 3 <1>", + "partId": "JiD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/gazebo/chassis_description/meshes/part_3.stl b/gazebo/chassis_description/meshes/part_3.stl new file mode 100644 index 0000000..d009abe Binary files /dev/null and b/gazebo/chassis_description/meshes/part_3.stl differ diff --git a/gazebo/chassis_description/meshes/part_4.part b/gazebo/chassis_description/meshes/part_4.part new file mode 100644 index 0000000..794a617 --- /dev/null +++ b/gazebo/chassis_description/meshes/part_4.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "fc2c9f06a678801d77ce900d", + "documentMicroversion": "d54a915c618e87519dc31c64", + "elementId": "641fd7b7212dae8b4351b688", + "fullConfiguration": "default", + "id": "MPU5wrH/1+xhP5Rn9", + "isStandardContent": false, + "name": "Part 4 <1>", + "partId": "JiH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/gazebo/chassis_description/meshes/part_4.stl b/gazebo/chassis_description/meshes/part_4.stl new file mode 100644 index 0000000..d02b442 Binary files /dev/null and b/gazebo/chassis_description/meshes/part_4.stl differ diff --git a/gazebo/chassis_description/meshes/top__mid__arm.part b/gazebo/chassis_description/meshes/top__mid__arm.part new file mode 100644 index 0000000..19da33e --- /dev/null +++ b/gazebo/chassis_description/meshes/top__mid__arm.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "fc2c9f06a678801d77ce900d", + "documentMicroversion": "d54a915c618e87519dc31c64", + "elementId": "641fd7b7212dae8b4351b688", + "fullConfiguration": "default", + "id": "MLocTA1BlOcUlFIBS", + "isStandardContent": false, + "name": "top. mid. arm <1>", + "partId": "JNH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/gazebo/chassis_description/meshes/top__mid__arm.stl b/gazebo/chassis_description/meshes/top__mid__arm.stl new file mode 100644 index 0000000..1c3be2d Binary files /dev/null and b/gazebo/chassis_description/meshes/top__mid__arm.stl differ diff --git a/gazebo/chassis_description/meshes/ttb_0094_ttb_0095_2x1_grid_pattern_aluminum_extrusion.part b/gazebo/chassis_description/meshes/ttb_0094_ttb_0095_2x1_grid_pattern_aluminum_extrusion.part new file mode 100644 index 0000000..2c42073 --- /dev/null +++ b/gazebo/chassis_description/meshes/ttb_0094_ttb_0095_2x1_grid_pattern_aluminum_extrusion.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "fc2c9f06a678801d77ce900d", + "documentMicroversion": "d54a915c618e87519dc31c64", + "elementId": "702f527b5a96081aeeb8b56c", + "fullConfiguration": "default", + "id": "MVQ5PaPXoBPgZ1TH3", + "isStandardContent": false, + "name": "TTB-0094/TTB-0095 2X1 Grid Pattern Aluminum Extrusion <1>", + "partId": "JaD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/gazebo/chassis_description/meshes/ttb_0094_ttb_0095_2x1_grid_pattern_aluminum_extrusion.stl b/gazebo/chassis_description/meshes/ttb_0094_ttb_0095_2x1_grid_pattern_aluminum_extrusion.stl new file mode 100644 index 0000000..6d26347 Binary files /dev/null and b/gazebo/chassis_description/meshes/ttb_0094_ttb_0095_2x1_grid_pattern_aluminum_extrusion.stl differ diff --git a/gazebo/chassis_description/package.xml b/gazebo/chassis_description/package.xml new file mode 100644 index 0000000..b003488 --- /dev/null +++ b/gazebo/chassis_description/package.xml @@ -0,0 +1,17 @@ + + + + chassis_description + 0.0.0 + chassis simulation models and descriptions + Trickfire Robotics + TODO + + ament_cmake + + ament_lint_auto + + + ament_cmake + + diff --git a/gazebo/chassis_description/urdf/chassis.urdf b/gazebo/chassis_description/urdf/chassis.urdf new file mode 100644 index 0000000..1237f63 --- /dev/null +++ b/gazebo/chassis_description/urdf/chassis.urdf @@ -0,0 +1,396 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1.0 + 1.0 + + + 1.0 + 1.0 + + + 1.0 + 1.0 + + + 1.0 + 1.0 + + + 1.0 + 1.0 + + + 1.0 + 1.0 + + + + diff --git a/gazebo/chassis_description/urdf/chassis_control.urdf.xacro b/gazebo/chassis_description/urdf/chassis_control.urdf.xacro new file mode 100644 index 0000000..c3f4a96 --- /dev/null +++ b/gazebo/chassis_description/urdf/chassis_control.urdf.xacro @@ -0,0 +1,66 @@ + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + robot_description + robot_state_publisher + $(arg controller_config) + 60 + + + diff --git a/gazebo/sim_common/package.xml b/gazebo/sim_common/package.xml index 278429b..690a8aa 100644 --- a/gazebo/sim_common/package.xml +++ b/gazebo/sim_common/package.xml @@ -12,6 +12,7 @@ rclpy trajectory_msgs builtin_interfaces + std_msgs ament_lint_auto diff --git a/gazebo/sim_common/setup.py b/gazebo/sim_common/setup.py index 9e4ef96..26211ff 100644 --- a/gazebo/sim_common/setup.py +++ b/gazebo/sim_common/setup.py @@ -16,6 +16,7 @@ "console_scripts": [ "move_joints = sim_common.move_joints:main", "joint_gui = sim_common.joint_gui:main", + "drivebase = sim_common.drivebase:main", ], }, install_requires=["setuptools"], diff --git a/gazebo/sim_common/sim_common/drivebase.py b/gazebo/sim_common/sim_common/drivebase.py new file mode 100644 index 0000000..13c3bae --- /dev/null +++ b/gazebo/sim_common/sim_common/drivebase.py @@ -0,0 +1,64 @@ +""" +Sim equivalent of the real rover's drivebase node +""" + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32, Float64MultiArray + +# matches urc-2023/src/drivebase/drivebase/drivebase.py so a given joystick +# deflection produces the same wheel speed on the sim as on the real rover +SPEED = 6.28 * 1.5 # rad/s + +# order must match the joints list in chassis_bringup/config/chassis.controller.yaml +WHEEL_ORDER = ["fl_wheel", "ml_wheel", "bl_wheel", "fr_wheel", "mr_wheel", "br_wheel"] +LEFT_WHEELS = ("fl_wheel", "ml_wheel", "bl_wheel") +RIGHT_WHEELS = ("fr_wheel", "mr_wheel", "br_wheel") + + +class Drivebase(Node): + """Main class""" + + def __init__(self): + super().__init__("drivebase") + + self.wheel_velocity = dict.fromkeys(WHEEL_ORDER, 0.0) + + self.command_pub = self.create_publisher( + Float64MultiArray, "/forward_velocity_controller/commands", 10 + ) + + self.create_subscription(Float32, "move_left_drivebase_side_message", self._on_left, 10) + self.create_subscription(Float32, "move_right_drivebase_side_message", self._on_right, 10) + + def _on_left(self, msg: Float32) -> None: + vel = msg.data * SPEED + for wheel in LEFT_WHEELS: + self.wheel_velocity[wheel] = vel + self._publish() + + def _on_right(self, msg: Float32) -> None: + vel = msg.data * SPEED + for wheel in RIGHT_WHEELS: + self.wheel_velocity[wheel] = -vel + self._publish() + + def _publish(self) -> None: + msg = Float64MultiArray() + msg.data = [self.wheel_velocity[wheel] for wheel in WHEEL_ORDER] + self.command_pub.publish(msg) + + +def main(args=None): + """Main method""" + rclpy.init(args=args) + node = Drivebase() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/gazebo/sim_common/sim_common/joint_gui.py b/gazebo/sim_common/sim_common/joint_gui.py old mode 100644 new mode 100755 diff --git a/gazebo/sim_common/sim_common/launch_utils.py b/gazebo/sim_common/sim_common/launch_utils.py index ca06096..0fd963b 100644 --- a/gazebo/sim_common/sim_common/launch_utils.py +++ b/gazebo/sim_common/sim_common/launch_utils.py @@ -1,10 +1,24 @@ """Shared launch utilities for simulation packages.""" import os +import shutil +import subprocess import sys from pathlib import Path +import xacro from ament_index_python.packages import get_package_share_directory +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + RegisterEventHandler, + TimerAction, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node def log(msg): @@ -28,3 +42,142 @@ def get_asset(package, *parts): if not Path(path).exists(): err(f"File {path} does not exist!") return path + + +def gz_supports_sim_command(): + """Return True when `gz` exists and exposes the `sim` subcommand.""" + if shutil.which("gz") is None: + return False + + try: + result = subprocess.run( + ["gz", "--commands"], + check=False, + capture_output=True, + text=True, + ) + except OSError: + return False + + commands = result.stdout.splitlines() + return any(line.strip() == "sim" for line in commands) + + +def process_robot_description(urdf_file, controller_config): + """Run xacro over `urdf_file`, wiring in the controller config and platform plugin extension.""" + return xacro.process_file( + urdf_file, + mappings={ + "controller_config": controller_config, + "plugin_extension": ".dylib" if sys.platform == "darwin" else ".so", + }, + ).toxml() + + +def gazebo_launch_actions(world_file, gz_gui_config, gui_launch_arg="gui"): + """ + Bring up the gz sim server, plus its GUI if the installed `gz` supports launching + them as separate processes (falls back to the server's built-in GUI otherwise). + Returns a list of launch actions to splice into a LaunchDescription. + """ + use_split_gui = gz_supports_sim_command() + gz_server_args = ( + ["-r", "-s", world_file] + if use_split_gui + else ["-r", world_file, "--gui-config", gz_gui_config] + ) + + gz_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py") + ), + launch_arguments={"gz_args": " ".join(gz_server_args)}.items(), + ) + + if not use_split_gui: + return [gz_server] + + gz_gui = ExecuteProcess( + cmd=["gz", "sim", "--force-version", "8", "-g", "--gui-config", gz_gui_config], + output="screen", + condition=IfCondition(LaunchConfiguration(gui_launch_arg)), + ) + return [gz_server, TimerAction(period=2.0, actions=[gz_gui])] + + +def spawn_robot_node(robot_name, robot_desc): + """Node that spawns `robot_desc` into the running gz sim world as `robot_name`.""" + return Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", robot_name, "-string", robot_desc, "-x", "0", "-y", "0", "-z", "0.1"], + output="screen", + ) + + +def robot_state_publisher_node(robot_desc): + return Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[{"robot_description": robot_desc}], + ) + + +def clock_bridge_node(): + """ros_gz_bridge node that bridges the gz sim clock onto /clock.""" + return Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + output="screen", + ) + + +def rviz_node(rviz_config, rviz_launch_arg="rviz"): + return Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config], + condition=IfCondition(LaunchConfiguration(rviz_launch_arg)), + ) + + +def joint_state_broadcaster_spawner(): + return Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ) + + +def controller_spawner_node(controller_name, controller_config=None): + arguments = [controller_name] + if controller_config is not None: + arguments += ["--param-file", controller_config] + return Node( + package="controller_manager", + executable="spawner", + arguments=arguments, + ) + + +def chain_controller_spawners(spawn_robot, *spawners): + """ + Start `spawners` one after another, each once the previous one exits, starting + once `spawn_robot` exits. Mirrors how controller_manager spawners need the + robot (and each other) to already be loaded before they can activate. + Returns the list of RegisterEventHandler actions to splice into a LaunchDescription. + """ + handlers = [] + previous_action = spawn_robot + for spawner in spawners: + handlers.append( + RegisterEventHandler( + event_handler=OnProcessExit(target_action=previous_action, on_exit=[spawner]) + ) + ) + previous_action = spawner + return handlers diff --git a/robots.json b/robots.json index 533bfb4..e8eec2f 100644 --- a/robots.json +++ b/robots.json @@ -3,5 +3,10 @@ "name": "arm", "url": "https://trickfire.onshape.com/documents/9b05a4dfb79241a5b295b397/w/d00f3086da43b93d9c689413/e/f5729953e1df72ab5053d822", "world_base_link": true + }, + { + "name": "chassis", + "url": "https://trickfire.onshape.com/documents/fc2c9f06a678801d77ce900d/w/3174861c9ae36101feac1882/e/51aaca767a20d2161d5ff539", + "world_base_link": false } ] diff --git a/ruff.toml b/ruff.toml index bb44f48..598ec00 100644 --- a/ruff.toml +++ b/ruff.toml @@ -1,4 +1,4 @@ -exclude = ["gazebo/packages/build", "gazebo/packages/log", "gazebo/packages/install"] +exclude = ["gazebo/build", "gazebo/log", "gazebo/install"] line-length = 100 indent-width = 4 target-version = "py310"