diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..e3c8dbe Binary files /dev/null and b/.DS_Store differ diff --git a/.pio/build/f722/src/crsf.o b/.pio/build/f722/src/crsf.o new file mode 100644 index 0000000..e300221 Binary files /dev/null and b/.pio/build/f722/src/crsf.o differ diff --git a/.pio/build/f722/src/i2c1.o b/.pio/build/f722/src/i2c1.o new file mode 100644 index 0000000..19974d3 Binary files /dev/null and b/.pio/build/f722/src/i2c1.o differ diff --git a/.pio/build/f722/src/jetson_cmd.o b/.pio/build/f722/src/jetson_cmd.o new file mode 100644 index 0000000..49974b4 Binary files /dev/null and b/.pio/build/f722/src/jetson_cmd.o differ diff --git a/.pio/build/f722/src/mag.o b/.pio/build/f722/src/mag.o new file mode 100644 index 0000000..654ce88 Binary files /dev/null and b/.pio/build/f722/src/mag.o differ diff --git a/.pio/build/f722/src/mode_manager.o b/.pio/build/f722/src/mode_manager.o new file mode 100644 index 0000000..d609565 Binary files /dev/null and b/.pio/build/f722/src/mode_manager.o differ diff --git a/.pio/build/f722/src/motor_driver.o b/.pio/build/f722/src/motor_driver.o new file mode 100644 index 0000000..14dde9a Binary files /dev/null and b/.pio/build/f722/src/motor_driver.o differ diff --git a/.pio/build/f722/src/mpu6000.o b/.pio/build/f722/src/mpu6000.o new file mode 100644 index 0000000..4257e08 Binary files /dev/null and b/.pio/build/f722/src/mpu6000.o differ diff --git a/.pio/build/f722/src/safety.o b/.pio/build/f722/src/safety.o new file mode 100644 index 0000000..91d6c7d Binary files /dev/null and b/.pio/build/f722/src/safety.o differ diff --git a/jetson/docker-compose.yml b/jetson/docker-compose.yml index eec51e4..8b5c531 100644 --- a/jetson/docker-compose.yml +++ b/jetson/docker-compose.yml @@ -166,6 +166,20 @@ services: # ── Surround vision — 360° bird's-eye view + Nav2 camera obstacle layer ───── saltybot-surround: + + # ── rosbridge WebSocket server ──────────────────────────────────────────── + # Serves ROS2 topics to the web dashboard (roslibjs) on ws://jetson:9090 + # + # Topics exposed (whitelist in ros2_ws/src/saltybot_bringup/config/rosbridge_params.yaml): + # /map /scan /tf /tf_static /saltybot/imu /saltybot/balance_state + # /cmd_vel /person/* /camera/*/image_raw/compressed + # + # Also runs image_transport/republish nodes to compress 4× CSI camera streams. + # Raw 640×480 YUYV → JPEG-compressed sensor_msgs/CompressedImage. + # + # Install (already in image): apt install ros-humble-rosbridge-server + # ros-humble-image-transport-plugins + rosbridge: image: saltybot/ros2-humble:jetson-orin build: context: . @@ -191,6 +205,29 @@ services: start_cameras:=false camera_height:=0.30 publish_rate:=5.0 + + container_name: saltybot-rosbridge + restart: unless-stopped + runtime: nvidia + network_mode: host # port 9090 is directly reachable on host + depends_on: + - saltybot-ros2 # needs /map, /tf published + - stm32-bridge # needs /saltybot/imu, /saltybot/balance_state + - csi-cameras # needs /camera/*/image_raw for compression + environment: + - ROS_DOMAIN_ID=42 + - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + volumes: + - ./ros2_ws/src:/ros2_ws/src:rw + - ./config:/config:ro + # Port 9090 is accessible on all host interfaces via network_mode: host. + # To restrict to a specific interface, set host: "192.168.x.x" in + # rosbridge_params.yaml instead of using Docker port mappings. + command: > + bash -c " + source /opt/ros/humble/setup.bash && + source /ros2_ws/install/local_setup.bash 2>/dev/null || true && + ros2 launch saltybot_bringup rosbridge.launch.py " # ── Nav2 autonomous navigation stack ──────────────────────────────────────── diff --git a/jetson/ros2_ws/src/saltybot_bringup/SENSORS.md b/jetson/ros2_ws/src/saltybot_bringup/SENSORS.md index d62ddf7..2e2638b 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/SENSORS.md +++ b/jetson/ros2_ws/src/saltybot_bringup/SENSORS.md @@ -4,6 +4,60 @@ ROS2 Humble | ROS_DOMAIN_ID=42 | DDS: CycloneDDS --- +## rosbridge WebSocket (web dashboard) + +The web dashboard connects to ROS2 topics via rosbridge WebSocket on **port 9090**. + +```bash +# Start rosbridge (and compressed camera republishers): +ros2 launch saltybot_bringup rosbridge.launch.py + +# Or via docker-compose: +docker compose up -d rosbridge +``` + +**Browser connection (roslibjs):** +```javascript +var ros = new ROSLIB.Ros({ url: 'ws://jetson.local:9090' }); +ros.on('connection', () => console.log('connected')); +ros.on('error', (e) => console.error('error', e)); +ros.on('close', () => console.log('disconnected')); +``` + +**Whitelisted topics** (configured in `config/rosbridge_params.yaml`): + +| Topic | Type | Notes | +|-------|------|-------| +| `/map` | `nav_msgs/OccupancyGrid` | Use `throttle_rate: 5000` (0.2 Hz) | +| `/scan` | `sensor_msgs/LaserScan` | 5.5 Hz, 1440 pts | +| `/tf` | `tf2_msgs/TFMessage` | Use `throttle_rate: 100` (10 Hz) | +| `/tf_static` | `tf2_msgs/TFMessage` | One-shot on connect | +| `/saltybot/imu` | `sensor_msgs/Imu` | 50 Hz, pitch/roll/yaw | +| `/saltybot/balance_state` | `std_msgs/String` | JSON balance controller state | +| `/cmd_vel` | `geometry_msgs/Twist` | Subscribe to monitor Nav2 commands | +| `/person/target` | (custom) | Person tracking target | +| `/person/detections` | (custom) | Person detection results | +| `/camera/front/image_raw/compressed` | `sensor_msgs/CompressedImage` | JPEG 75%, 640×480 | +| `/camera/left/image_raw/compressed` | `sensor_msgs/CompressedImage` | JPEG 75%, 640×480 | +| `/camera/rear/image_raw/compressed` | `sensor_msgs/CompressedImage` | JPEG 75%, 640×480 | +| `/camera/right/image_raw/compressed` | `sensor_msgs/CompressedImage` | JPEG 75%, 640×480 | + +**Bandwidth tips:** +- Raw 640×480 YUYV = ~900 KB/frame. rosbridge republishes JPEG-compressed ≈ 15–25 KB/frame. +- Always use `throttle_rate` for cameras in roslibjs (e.g. `throttle_rate: 200` = max 5 fps). +- `/map` is large (~100 KB); use `throttle_rate: 5000` or subscribe only on demand. + +**Verify connection:** +```bash +# Check WebSocket is listening +ss -tlnp | grep 9090 + +# List topics visible to rosbridge +ros2 topic list | grep -E 'map|scan|imu|camera|person|cmd_vel' +``` + +--- + ## Build & Run ```bash diff --git a/jetson/ros2_ws/src/saltybot_bringup/config/rosbridge_params.yaml b/jetson/ros2_ws/src/saltybot_bringup/config/rosbridge_params.yaml new file mode 100644 index 0000000..47bbf89 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/config/rosbridge_params.yaml @@ -0,0 +1,66 @@ +# rosbridge_params.yaml — rosbridge_websocket server configuration +# +# WebSocket endpoint: ws://:9090 +# Browser client lib: roslibjs (cdn.jsdelivr.net/npm/roslib) +# +# Topic whitelist limits bandwidth and prevents unintended exposure. +# Glob patterns follow Python fnmatch: * matches everything (incl. /). +# +# Bandwidth budget (worst case, all subscriptions active): +# /map ~100 KB/update × 0.2 Hz = ~20 KB/s +# /scan ~5 KB/scan × 5.5 Hz = ~28 KB/s +# /saltybot/imu ~0.2 KB/msg × 50 Hz = ~10 KB/s +# /tf ~1 KB/msg × 10 Hz = ~10 KB/s +# 4× cameras ~20 KB/frame × 5 Hz = ~400 KB/s (JPEG quality 75) +# ───────────────────────────────────────────────────── +# Total ~470 KB/s ≈ 3.8 Mbps +# +# Use throttle_rate in roslibjs subscribe() calls to cap per-topic rates: +# viewer.subscribe({ topic: '/map', throttle_rate: 5000 }) // 0.2 Hz + +rosbridge_websocket: + ros__parameters: + # ── Network ────────────────────────────────────────────────────────────── + port: 9090 + host: "0.0.0.0" # bind all interfaces (Jetson LAN + USB-C) + address: "" + + # ── Authentication ──────────────────────────────────────────────────────── + authenticate: false # no auth on local network; enable if exposed + + # ── Message size ────────────────────────────────────────────────────────── + # OccupancyGrid /map can exceed 1 MB for large environments. + max_message_size: 10000000 # 10 MB + + # ── Topic/service/param whitelists ──────────────────────────────────────── + # JSON-encoded glob list. "*" in a glob matches any chars including "/". + # Only topics matching at least one pattern are accessible to clients. + topics_glob: >- + ["/map", + "/person/target", + "/person/detections", + "/camera/*/image_raw/compressed", + "/scan", + "/cmd_vel", + "/saltybot/imu", + "/saltybot/balance_state", + "/tf", + "/tf_static"] + + services_glob: "[]" # no service calls via WebSocket + params_glob: "[]" # no parameter access via WebSocket + + # ── Connection management ───────────────────────────────────────────────── + # Time (s) before dropping a client that stops responding. + unregister_timeout: 10.0 + + # Fragment timeout: max seconds to wait for all fragments of a large message. + fragment_timeout: 600 + + # Delay between consecutive outgoing messages (ms). 0 = unlimited. + # Set > 0 (e.g. 10) if browser JS event loop is overwhelmed. + delay_between_messages: 0 + + # ── Logging ─────────────────────────────────────────────────────────────── + # Set to true to log every publish/subscribe call (verbose, dev only). + bson_only_mode: false diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/rosbridge.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/rosbridge.launch.py new file mode 100644 index 0000000..31bbd8c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/rosbridge.launch.py @@ -0,0 +1,97 @@ +""" +rosbridge.launch.py — rosbridge WebSocket server + compressed image transport + +Starts two things: + 1. rosbridge_websocket — WebSocket server on port 9090 (roslibjs-compatible) + 2. image_transport/republish × 4 — compresses each CSI camera's raw stream + /camera//image_raw → /camera//image_raw/compressed + +Prerequisites (already installed in saltybot/ros2-humble:jetson-orin): + apt install ros-humble-rosbridge-server + apt install ros-humble-image-transport-plugins # supplies compressed plugin + +Browser connection: + var ros = new ROSLIB.Ros({ url: 'ws://:9090' }); + +Topic whitelist is configured in config/rosbridge_params.yaml. + +Bandwidth tips: + - Subscribe to /camera//image_raw/compressed, not raw images. + - Use throttle_rate in ROSLIB.Topic to cap camera update rate: + new ROSLIB.Topic({ ... messageType: 'sensor_msgs/CompressedImage', + throttle_rate: 200 }) // max 5 fps per camera + - Subscribe to /map with throttle_rate: 5000 (0.2 Hz is enough for display). + +Usage: + ros2 launch saltybot_bringup rosbridge.launch.py + +Verify: + ros2 topic echo /rosbridge_websocket/status + # In browser console: + # var ros = new ROSLIB.Ros({ url: 'ws://jetson.local:9090' }); + # ros.on('connection', () => console.log('connected')); +""" + +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +# Camera names matching saltybot_cameras/launch/csi_cameras.launch.py +# Topics: /camera//image_raw (published by v4l2_camera_node) +_CAMERAS = ['front', 'left', 'rear', 'right'] + +# JPEG quality for compressed output (0–100). +# 75 = good quality/size trade-off at 640×480: ~15–25 KB/frame. +# Lower to 50 for tighter bandwidth budgets; raise to 90 for inspection use. +_JPEG_QUALITY = 75 + + +def generate_launch_description(): + pkg_share = get_package_share_directory('saltybot_bringup') + params_file = os.path.join(pkg_share, 'config', 'rosbridge_params.yaml') + + # ── rosbridge WebSocket server ──────────────────────────────────────────── + rosbridge = Node( + package='rosbridge_server', + executable='rosbridge_websocket', + name='rosbridge_websocket', + parameters=[params_file], + output='screen', + ) + + # ── Compressed image republishers ───────────────────────────────────────── + # image_transport/republish subscribes to raw sensor_msgs/Image and + # re-publishes as sensor_msgs/CompressedImage using the compressed plugin. + # + # Node arguments: ['raw', 'compressed'] + # 'raw' — input transport type + # 'compressed' — output transport type + # + # Topic remappings: + # in → /camera//image_raw + # out/compressed → /camera//image_raw/compressed + # + # Parameter jpeg_quality controls JPEG encoder quality for the + # compressed publisher. The full parameter path in ROS2 is: + # //compressed/jpeg_quality + republishers = [ + Node( + package='image_transport', + executable='republish', + name=f'compress_{name}', + arguments=['raw', 'compressed'], + remappings=[ + ('in', f'/camera/{name}/image_raw'), + ('out/compressed', f'/camera/{name}/image_raw/compressed'), + ], + parameters=[{ + 'compressed.jpeg_quality': _JPEG_QUALITY, + }], + output='screen', + ) + for name in _CAMERAS + ] + + return LaunchDescription([rosbridge] + republishers) diff --git a/jetson/ros2_ws/src/saltybot_bringup/package.xml b/jetson/ros2_ws/src/saltybot_bringup/package.xml index 5083f62..71ac935 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/package.xml +++ b/jetson/ros2_ws/src/saltybot_bringup/package.xml @@ -15,6 +15,9 @@ nav2_bringup robot_state_publisher tf2_ros + rosbridge_server + image_transport + image_transport_plugins ament_python