From 76067d6d89025dedf87c796bf5d73251bf72be40 Mon Sep 17 00:00:00 2001 From: sl-perception Date: Sat, 28 Feb 2026 17:14:21 -0500 Subject: [PATCH] feat(bd-a2j): RealSense D435i + RPLIDAR A1M8 ROS2 driver integration MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds saltybot_bringup ROS2 package with four launch files: - realsense.launch.py — D435i at 640x480x15fps, IMU unified topic - rplidar.launch.py — RPLIDAR A1M8 via /dev/rplidar udev symlink - sensors.launch.py — both sensors + static TF (base_link→laser/camera) - slam.launch.py — sensors + slam_toolbox online_async (compose entry point) Sensor config YAMLs (mounted at /config/ in container): - realsense_d435i.yaml — Nano power-budget settings (15fps, no pointcloud) - rplidar_a1m8.yaml — Standard scan mode, 115200 baud, laser frame - slam_toolbox_params.yaml — Nano-tuned (2Hz processing, 5cm resolution) Fixes docker-compose volume mount: ./ros2_ws/src:/ros2_ws/src (was ./ros2_ws:/ros2_ws/src — would have double-nested the src directory) Topic reference and verification commands in SENSORS.md. Co-Authored-By: Claude Sonnet 4.6 --- jetson/config/realsense_d435i.yaml | 53 +++++++ jetson/config/rplidar_a1m8.yaml | 30 ++++ jetson/config/slam_toolbox_params.yaml | 79 +++++++++++ jetson/docker-compose.yml | 3 +- .../ros2_ws/src/saltybot_bringup/SENSORS.md | 131 ++++++++++++++++++ .../launch/realsense.launch.py | 85 ++++++++++++ .../saltybot_bringup/launch/rplidar.launch.py | 60 ++++++++ .../saltybot_bringup/launch/sensors.launch.py | 92 ++++++++++++ .../saltybot_bringup/launch/slam.launch.py | 62 +++++++++ .../ros2_ws/src/saltybot_bringup/package.xml | 26 ++++ .../resource/saltybot_bringup | 0 .../saltybot_bringup/__init__.py | 0 jetson/ros2_ws/src/saltybot_bringup/setup.cfg | 4 + jetson/ros2_ws/src/saltybot_bringup/setup.py | 30 ++++ 14 files changed, 654 insertions(+), 1 deletion(-) create mode 100644 jetson/config/realsense_d435i.yaml create mode 100644 jetson/config/rplidar_a1m8.yaml create mode 100644 jetson/config/slam_toolbox_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_bringup/SENSORS.md create mode 100644 jetson/ros2_ws/src/saltybot_bringup/launch/realsense.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/launch/rplidar.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/launch/sensors.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/launch/slam.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_bringup/resource/saltybot_bringup create mode 100644 jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_bringup/setup.py diff --git a/jetson/config/realsense_d435i.yaml b/jetson/config/realsense_d435i.yaml new file mode 100644 index 0000000..5565a38 --- /dev/null +++ b/jetson/config/realsense_d435i.yaml @@ -0,0 +1,53 @@ +# RealSense D435i configuration — Jetson Nano (power-budget tuned) +# +# Profile format: WxHxFPS +# Constraint: ~10W total budget. 640x480x15 saves ~0.4W vs 30fps. +# +# Reference topics at these settings: +# /camera/color/image_raw 640x480 15 Hz RGB8 +# /camera/depth/image_rect_raw 640x480 15 Hz Z16 +# /camera/aligned_depth_to_color/image_raw 640x480 15 Hz Z16 +# /camera/imu 6-axis ~200 Hz (accel@100Hz + gyro@400Hz fused) +# /camera/color/camera_info 640x480 15 Hz +# /camera/depth/camera_info 640x480 15 Hz +# +# Hardware IMU: Bosch BMI055 +# Accelerometer native rate: 100 Hz +# Gyroscope native rate: 400 Hz +# unite_imu_method=2: linearly interpolates accel to match gyro timestamps + +camera: + ros__parameters: + # ── Streams ────────────────────────────────────────────────────────────── + depth_module.profile: "640x480x15" + rgb_camera.profile: "640x480x15" + + enable_depth: true + enable_color: true + enable_infra1: false # not needed for RGB-D SLAM + enable_infra2: false + + # ── IMU ────────────────────────────────────────────────────────────────── + enable_gyro: true + enable_accel: true + # 0=none 1=copy 2=linear_interpolation + # Use 2: aligns accel timestamps to gyro rate, required for sensor fusion + unite_imu_method: 2 + + # ── Alignment ──────────────────────────────────────────────────────────── + # Projects depth pixels into RGB frame — required for rtabmap_ros rgbd input + align_depth.enable: true + + # ── Point cloud ────────────────────────────────────────────────────────── + # Disabled: rtabmap_ros generates its own from aligned depth. + # Maxwell GPU cannot handle both simultaneously at budget. + pointcloud.enable: false + + # ── TF ─────────────────────────────────────────────────────────────────── + publish_tf: true + tf_publish_rate: 0.0 # 0 = publish static transforms only (no redundant timer) + + # ── Device ─────────────────────────────────────────────────────────────── + # Leave serial_no empty to auto-select first found device + # serial_no: '' + # device_type: d435i diff --git a/jetson/config/rplidar_a1m8.yaml b/jetson/config/rplidar_a1m8.yaml new file mode 100644 index 0000000..e44b552 --- /dev/null +++ b/jetson/config/rplidar_a1m8.yaml @@ -0,0 +1,30 @@ +# RPLIDAR A1M8 configuration +# +# Hardware specs: +# Model: RPLIDAR A1M8 (rev 6) +# Interface: USB-UART via CP2102 adapter (VID:10c4 PID:ea60) +# Baud rate: 115200 +# Scan rate: ~5.5 Hz (1440 points/scan @ 8000 samples/s) +# Range: 0.15 m – 12 m (reliable to ~8 m indoors) +# Angular res: ~0.9° per sample +# Scan mode: Standard (A1M8 only supports this mode) +# +# udev symlink (from jetson/docs/pinout.md 99-saltybot.rules): +# /dev/rplidar → ttyUSB* where ATTRS{idVendor}=="10c4" ATTRS{idProduct}=="ea60" +# +# Published topics: +# /scan sensor_msgs/LaserScan ~5.5 Hz +# angle_min: -π rad (-180°) +# angle_max: +π rad (+180°) +# range_min: 0.15 m +# range_max: 12.0 m +# scan_time: ~0.182 s per revolution + +rplidar_node: + ros__parameters: + serial_port: "/dev/rplidar" # udev symlink — stable across reboots + serial_baudrate: 115200 + frame_id: "laser" # must match TF in sensors.launch.py + inverted: false # scan direction: false = counter-clockwise + angle_compensate: true # compensate for motor rotation offset + scan_mode: "Standard" # A1M8 only supports Standard mode diff --git a/jetson/config/slam_toolbox_params.yaml b/jetson/config/slam_toolbox_params.yaml new file mode 100644 index 0000000..612e60b --- /dev/null +++ b/jetson/config/slam_toolbox_params.yaml @@ -0,0 +1,79 @@ +# slam_toolbox — online async SLAM configuration +# Tuned for Jetson Nano 4GB (constrained CPU/RAM, indoor mapping) +# +# Input: /scan (LaserScan from RPLIDAR A1M8, ~5.5 Hz) +# Output: /map (OccupancyGrid, updated every map_update_interval seconds) +# +# Frame assumptions (must match sensors.launch.py static TF): +# map → odom → base_link → laser +# (odom not yet published — slam_toolbox handles this via scan matching) + +slam_toolbox: + ros__parameters: + # ── Frames ─────────────────────────────────────────────────────────────── + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + mode: mapping # 'mapping' or 'localization' + + # ── Map params ─────────────────────────────────────────────────────────── + resolution: 0.05 # 5 cm/cell — good balance for A1M8 angular res + max_laser_range: 8.0 # clip to reliable range of A1M8 (spec: 12m) + map_update_interval: 5.0 # seconds between full map publishes (saves CPU) + minimum_travel_distance: 0.3 # only update after moving 30 cm + minimum_travel_heading: 0.3 # or rotating ~17° + + # ── Performance (Nano-specific) ─────────────────────────────────────────── + # Reduce scan processing rate to stay within ~3.5W CPU budget + minimum_time_interval: 0.5 # max 2 Hz scan processing (A1M8 is ~5.5 Hz) + transform_timeout: 0.2 + tf_buffer_duration: 30.0 + stack_size_to_use: 40000000 # 40 MB stack + enable_interactive_mode: false # disable interactive editing (saves CPU) + + # ── Scan matching ───────────────────────────────────────────────────────── + use_scan_matching: true + use_scan_barycenter: true + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + + # ── Loop closure ────────────────────────────────────────────────────────── + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + loop_search_maximum_distance: 3.0 + + # ── Correlation (coarse scan matching) ─────────────────────────────────── + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # ── Loop search space ───────────────────────────────────────────────────── + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # ── Response expansion ──────────────────────────────────────────────────── + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + use_response_expansion: true + + # ── Penalties (scan matcher quality thresholds) ─────────────────────────── + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + fine_search_angle_offset: 0.00349 # ~0.2° + coarse_search_angle_offset: 0.349 # ~20° + coarse_angle_resolution: 0.0349 # ~2° + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + + # ── Solver ──────────────────────────────────────────────────────────────── + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None diff --git a/jetson/docker-compose.yml b/jetson/docker-compose.yml index 4bd6686..724b322 100644 --- a/jetson/docker-compose.yml +++ b/jetson/docker-compose.yml @@ -29,7 +29,8 @@ services: # X11 socket for RViz2 - /tmp/.X11-unix:/tmp/.X11-unix:rw # ROS2 workspace (host-mounted for live dev) - - ./ros2_ws:/ros2_ws/src:rw + # Mount src/ subdirectory so host structure mirrors container /ros2_ws/src/ + - ./ros2_ws/src:/ros2_ws/src:rw # Persistent SLAM maps - saltybot-maps:/maps # Config files diff --git a/jetson/ros2_ws/src/saltybot_bringup/SENSORS.md b/jetson/ros2_ws/src/saltybot_bringup/SENSORS.md new file mode 100644 index 0000000..d62ddf7 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/SENSORS.md @@ -0,0 +1,131 @@ +# saltybot_bringup — Sensor Topic Reference + +ROS2 Humble | ROS_DOMAIN_ID=42 | DDS: CycloneDDS + +--- + +## Build & Run + +```bash +# From inside the container (or ros2_ws root on host after sourcing ROS2): +cd /ros2_ws +colcon build --packages-select saltybot_bringup --symlink-install +source install/local_setup.bash + +# Sensors only (verify data before SLAM): +ros2 launch saltybot_bringup sensors.launch.py + +# Full SLAM stack (saltybot-ros2 docker-compose service uses this): +ros2 launch saltybot_bringup slam.launch.py + +# Individual drivers: +ros2 launch saltybot_bringup realsense.launch.py +ros2 launch saltybot_bringup rplidar.launch.py +``` + +--- + +## Topic Reference + +### RPLIDAR A1M8 + +| Topic | Type | Rate | Notes | +|-------|------|------|-------| +| `/scan` | `sensor_msgs/LaserScan` | ~5.5 Hz | 360°, 1440 pts/scan, 12m range | + +``` +angle_min: -π (-180°) +angle_max: +π (+180°) +range_min: 0.15 m +range_max: 12.00 m +scan_time: ~0.182 s +frame_id: laser +``` + +### Intel RealSense D435i + +| Topic | Type | Rate | Notes | +|-------|------|------|-------| +| `/camera/color/image_raw` | `sensor_msgs/Image` | 15 Hz | RGB8, 640×480 | +| `/camera/color/camera_info` | `sensor_msgs/CameraInfo` | 15 Hz | Intrinsics | +| `/camera/depth/image_rect_raw` | `sensor_msgs/Image` | 15 Hz | Z16, 640×480 | +| `/camera/depth/camera_info` | `sensor_msgs/CameraInfo` | 15 Hz | Intrinsics | +| `/camera/aligned_depth_to_color/image_raw` | `sensor_msgs/Image` | 15 Hz | Depth in color frame | +| `/camera/imu` | `sensor_msgs/Imu` | ~200 Hz | Accel + gyro fused (linear_interpolation) | + +### TF Tree + +``` +map +└── odom (published by slam_toolbox once moving) + └── base_link (robot body frame) + ├── laser (RPLIDAR A1M8) ← static, 10cm above base + └── camera_link (RealSense D435i) ← static, 5cm fwd, 15cm up + ├── camera_depth_frame + ├── camera_color_frame + └── camera_imu_frame +``` + +**Note:** Static TF values in `sensors.launch.py` are placeholders. +Update after physical mount with real measurements. + +--- + +## Verification Commands + +```bash +# List all active topics +ros2 topic list + +# Check data rates +ros2 topic hz /scan +ros2 topic hz /camera/color/image_raw +ros2 topic hz /camera/imu + +# Spot-check data +ros2 topic echo /scan --once +ros2 topic echo /camera/imu --once + +# TF tree +ros2 run tf2_tools view_frames +ros2 run tf2_ros tf2_echo base_link laser + +# SLAM map topic (after slam.launch.py) +ros2 topic hz /map +ros2 topic echo /map --no-arr # metadata without raw data flood +``` + +--- + +## Troubleshooting + +### RPLIDAR not found +```bash +# Check udev symlink +ls -la /dev/rplidar +# If missing, fall back to: +ros2 launch saltybot_bringup rplidar.launch.py serial_port:=/dev/ttyUSB0 +# Install udev rule (host, not container): +sudo cp /path/to/jetson/docs/99-saltybot.rules /etc/udev/rules.d/ +sudo udevadm control --reload-rules && sudo udevadm trigger +``` + +### RealSense not detected +```bash +# Check USB3 port (blue port on Nano) +lsusb | grep "8086:0b3a" +# Expected: Bus 002 Device 00X: ID 8086:0b3a Intel Corp. Intel RealSense D435i + +# Install udev rules (host, not container): +sudo cp /etc/udev/rules.d/99-realsense-libusb.rules /etc/udev/rules.d/ +sudo udevadm control --reload-rules && sudo udevadm trigger +``` + +### IMU not publishing +```bash +# Verify enable_gyro and enable_accel are true +ros2 param get /camera/camera enable_gyro +ros2 param get /camera/camera enable_accel +# Check unite_imu_method (should be 2) +ros2 param get /camera/camera unite_imu_method +``` diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/realsense.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/realsense.launch.py new file mode 100644 index 0000000..e250b37 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/realsense.launch.py @@ -0,0 +1,85 @@ +""" +realsense.launch.py — Intel RealSense D435i driver (standalone) + +Launches realsense2_camera_node with Jetson Nano power-budget settings: + - 640×480 @ 15fps (depth + RGB) — saves ~0.4W vs 30fps + - IMU enabled with linear interpolation (unified /camera/imu topic) + - Depth aligned to color frame + - Point cloud disabled (expensive on Maxwell GPU) + +Published topics: + /camera/color/image_raw sensor_msgs/Image 15 Hz + /camera/color/camera_info sensor_msgs/CameraInfo 15 Hz + /camera/depth/image_rect_raw sensor_msgs/Image 15 Hz + /camera/depth/camera_info sensor_msgs/CameraInfo 15 Hz + /camera/aligned_depth_to_color/image_raw sensor_msgs/Image 15 Hz + /camera/imu sensor_msgs/Imu ~200 Hz + +Verify: + ros2 topic list | grep camera + ros2 topic hz /camera/color/image_raw + ros2 topic hz /camera/imu +""" + +import os +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + # Allow overriding camera serial number at launch time (useful with multiple cameras) + serial_no_arg = DeclareLaunchArgument( + 'serial_no', + default_value="''", + description='RealSense device serial number (empty = first found)', + ) + + realsense_share = get_package_share_directory('realsense2_camera') + + realsense_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(realsense_share, 'launch', 'rs_launch.py') + ), + launch_arguments={ + # Camera identity + 'serial_no': LaunchConfiguration('serial_no'), + 'camera_name': 'camera', + 'camera_namespace': 'camera', + + # Depth stream — 640×480 @ 15fps saves power on Nano + 'depth_module.profile': '640x480x15', + 'enable_depth': 'true', + + # RGB stream — matched resolution/fps to depth + 'rgb_camera.profile': '640x480x15', + 'enable_color': 'true', + + # IR streams — disabled (not needed for RTAB-Map RGB-D mode) + 'enable_infra1': 'false', + 'enable_infra2': 'false', + + # IMU — enable both sensors, publish unified topic + 'enable_gyro': 'true', + 'enable_accel': 'true', + # 2 = linear_interpolation: aligns accel+gyro timestamps + 'unite_imu_method': '2', + + # Align depth to color frame (required for rtabmap_ros RGB-D input) + 'align_depth.enable': 'true', + + # Point cloud — disabled, too expensive for Maxwell GPU during SLAM + 'pointcloud.enable': 'false', + + # TF — publish camera→IMU extrinsics + 'publish_tf': 'true', + 'tf_publish_rate': '0.0', # static only, no redundant updates + }.items(), + ) + + return LaunchDescription([ + serial_no_arg, + realsense_launch, + ]) diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/rplidar.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/rplidar.launch.py new file mode 100644 index 0000000..aa1ba3a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/rplidar.launch.py @@ -0,0 +1,60 @@ +""" +rplidar.launch.py — RPLIDAR A1M8 driver (standalone) + +Launches rplidar_ros with udev symlink (/dev/rplidar) set in 99-saltybot.rules. +Falls back to /dev/ttyUSB0 if the symlink is not present. + +RPLIDAR A1M8 specs: + - 360° omnidirectional scan + - 8000 samples/s, ~5.5 Hz scan rate at 1440 points/scan + - 12m range (reliable to ~8m indoors) + - 115200 baud via CP2102 USB-UART adapter + +Published topics: + /scan sensor_msgs/LaserScan ~5.5 Hz + +TF frame: laser → matches static_transform_publisher in sensors.launch.py + frame_id = 'laser' + +Verify: + ros2 topic hz /scan + ros2 run tf2_ros tf2_echo base_link laser +""" + +import os +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + serial_port_arg = DeclareLaunchArgument( + 'serial_port', + default_value='/dev/rplidar', + description='RPLIDAR serial port (udev symlink preferred over /dev/ttyUSB0)', + ) + + rplidar_share = get_package_share_directory('rplidar_ros') + + rplidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(rplidar_share, 'launch', 'rplidar_a1_launch.py') + ), + launch_arguments={ + 'serial_port': LaunchConfiguration('serial_port'), + 'serial_baudrate': '115200', + # 'laser' matches the TF frame in sensors.launch.py and slam config + 'frame_id': 'laser', + # Compensate for motor rotation angle offset + 'angle_compensate': 'true', + # A1M8 only supports Standard scan mode + 'scan_mode': 'Standard', + }.items(), + ) + + return LaunchDescription([ + serial_port_arg, + rplidar_launch, + ]) diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/sensors.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/sensors.launch.py new file mode 100644 index 0000000..bb7f1fe --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/sensors.launch.py @@ -0,0 +1,92 @@ +""" +sensors.launch.py — All sensor drivers + static TF + +Brings up both sensors together with placeholder static transforms. +Use this to verify sensor data before running full SLAM stack. + +Static TF (placeholder — update with real measurements after physical mount): + base_link → laser x=0.00 y=0.00 z=0.10 (RPLIDAR: 10cm above base) + base_link → camera_link x=0.05 y=0.00 z=0.15 (D435i: 5cm fwd, 15cm up) + +Published topics summary: + /scan LaserScan ~5.5 Hz (RPLIDAR) + /camera/color/image_raw Image 15 Hz (D435i RGB) + /camera/depth/image_rect_raw Image 15 Hz (D435i depth) + /camera/aligned_depth_to_color/image_raw Image 15 Hz (D435i aligned) + /camera/imu Imu ~200 Hz (D435i IMU) + +Quick verification: + ros2 topic list + ros2 topic hz /scan # expect ~5.5 Hz + ros2 topic hz /camera/color/image_raw # expect 15 Hz + ros2 topic hz /camera/imu # expect ~200 Hz + ros2 run tf2_tools view_frames # check TF tree +""" + +import os +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + bringup_share = get_package_share_directory('saltybot_bringup') + + realsense_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_share, 'launch', 'realsense.launch.py') + ), + ) + + rplidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_share, 'launch', 'rplidar.launch.py') + ), + ) + + # Static TF: base_link → laser (RPLIDAR A1M8 mount position) + # TODO: update x/y/z/yaw with real measurements from robot chassis + laser_tf = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='base_link_to_laser', + arguments=[ + '--x', '0.00', + '--y', '0.00', + '--z', '0.10', # 10cm above base_link + '--roll', '0', + '--pitch', '0', + '--yaw', '0', + '--frame-id', 'base_link', + '--child-frame-id', 'laser', + ], + output='screen', + ) + + # Static TF: base_link → camera_link (RealSense D435i mount position) + # TODO: update x/y/z with real measurements from robot chassis + camera_tf = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='base_link_to_camera', + arguments=[ + '--x', '0.05', # 5cm forward of base_link center + '--y', '0.00', + '--z', '0.15', # 15cm above base_link + '--roll', '0', + '--pitch', '0', + '--yaw', '0', + '--frame-id', 'base_link', + '--child-frame-id', 'camera_link', + ], + output='screen', + ) + + return LaunchDescription([ + realsense_launch, + rplidar_launch, + laser_tf, + camera_tf, + ]) diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/slam.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/slam.launch.py new file mode 100644 index 0000000..1b9f466 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/slam.launch.py @@ -0,0 +1,62 @@ +""" +slam.launch.py — Full SLAM stack (sensors + slam_toolbox) + +Entry point referenced by docker-compose.yml saltybot-ros2 service. + +Stack: + sensors.launch.py (RPLIDAR + RealSense + static TF) + slam_toolbox online_async — 2D LIDAR SLAM from /scan + +SLAM input: + /scan LaserScan from RPLIDAR A1M8 + /tf base_link → laser (from sensors.launch.py) + +SLAM output: + /map OccupancyGrid (2D occupancy map) + /slam_toolbox/... Internal slam_toolbox topics + +Note: slam_toolbox uses LIDAR-only SLAM (no RGB-D from D435i at this stage). +For RGB-D SLAM (RTAB-Map), use a separate launch file once slam_toolbox +mapping is validated — see SLAM-SETUP-PLAN.md (bd-wax) for full plan. + +Config: /config/slam_toolbox_params.yaml (mounted from jetson/config/) + +Verify: + ros2 topic hz /map # expect update every ~5s (map_update_interval) + ros2 run rviz2 rviz2 # visualize map + scan +""" + +import os +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + + +SLAM_PARAMS_FILE = '/config/slam_toolbox_params.yaml' + + +def generate_launch_description(): + bringup_share = get_package_share_directory('saltybot_bringup') + slam_share = get_package_share_directory('slam_toolbox') + + sensors_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_share, 'launch', 'sensors.launch.py') + ), + ) + + slam_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(slam_share, 'launch', 'online_async_launch.py') + ), + launch_arguments={ + 'params_file': SLAM_PARAMS_FILE, + 'use_sim_time': 'false', + }.items(), + ) + + return LaunchDescription([ + sensors_launch, + slam_launch, + ]) diff --git a/jetson/ros2_ws/src/saltybot_bringup/package.xml b/jetson/ros2_ws/src/saltybot_bringup/package.xml new file mode 100644 index 0000000..d22c777 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/package.xml @@ -0,0 +1,26 @@ + + + + saltybot_bringup + 0.1.0 + SaltyBot launch files — RealSense D435i + RPLIDAR A1M8 sensor bringup and SLAM integration + sl-perception + MIT + + rplidar_ros + realsense2_camera + realsense2_description + slam_toolbox + robot_state_publisher + tf2_ros + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_bringup/resource/saltybot_bringup b/jetson/ros2_ws/src/saltybot_bringup/resource/saltybot_bringup new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/__init__.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_bringup/setup.cfg b/jetson/ros2_ws/src/saltybot_bringup/setup.cfg new file mode 100644 index 0000000..c0d46a7 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_bringup +[install] +install_scripts=$base/lib/saltybot_bringup diff --git a/jetson/ros2_ws/src/saltybot_bringup/setup.py b/jetson/ros2_ws/src/saltybot_bringup/setup.py new file mode 100644 index 0000000..1b97221 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/setup.py @@ -0,0 +1,30 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'saltybot_bringup' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), + glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), + glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='sl-perception', + maintainer_email='sl-perception@saltylab.local', + description='SaltyBot sensor bringup and SLAM launch files', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [], + }, +) -- 2.47.2