jetson/ros2_ws/src/saltybot_nav2_slam/config/costmap_common_params.yaml

@@ -0,0 +1,47 @@
+costmap_common_params:
+  update_frequency: 10.0
+  publish_frequency: 5.0
+  global_frame: map
+  robot_base_frame: base_link
+  use_sim_time: false
+  rolling_window: false
+  robot_radius: 0.35
+  plugins:
+    - name: static_layer
+      type: nav2_costmap_2d::StaticLayer
+      map_subscribe_transient_local: true
+      enabled: true
+    - name: obstacle_layer
+      type: nav2_costmap_2d::ObstacleLayer
+      enabled: true
+      observation_sources: scan camera_depth
+      scan:
+        topic: /scan
+        max_obstacle_height: 2.5
+        min_obstacle_height: 0.0
+        clearing: true
+        marking: true
+        data_type: LaserScan
+      camera_depth:
+        topic: /camera/depth/color/points
+        max_obstacle_height: 2.5
+        min_obstacle_height: 0.0
+        clearing: false
+        marking: true
+        data_type: PointCloud2
+        sensor_frame: camera_link
+        expected_update_rate: 15.0
+        observation_persistence: 0.0
+        inf_is_valid: false
+        filter: passthrough
+        filter_value: 5.0
+    - name: inflation_layer
+      type: nav2_costmap_2d::InflationLayer
+      enabled: true
+      inflation_radius: 0.55
+      cost_scaling_factor: 10.0
+      inflate_unknown: false
+      inflate_around_unknown: true
+  lethal_cost_threshold: 100
+  unknown_cost_value: -1
+  resolution: 0.05

jetson/ros2_ws/src/saltybot_nav2_slam/config/dwb_local_planner_params.yaml

@@ -0,0 +1,59 @@
+dwb_local_planner:
+  ros__parameters:
+    max_vel_x: 5.5
+    min_vel_x: 0.0
+    max_vel_y: 0.0
+    min_vel_y: 0.0
+    max_vel_theta: 3.0
+    min_vel_theta: -3.0
+    acc_lim_x: 2.5
+    acc_lim_y: 0.0
+    acc_lim_theta: 2.5
+    decel_lim_x: -2.5
+    decel_lim_y: 0.0
+    decel_lim_theta: -2.5
+    vx_samples: 20
+    vy_samples: 1
+    vtheta_samples: 40
+    sim_time: 1.7
+    sim_granularity: 0.05
+    angular_sim_granularity: 0.1
+    path_distance_bias: 32.0
+    goal_distance_bias: 24.0
+    occdist_scale: 0.02
+    forward_point_distance: 0.325
+    use_dwa: true
+    dwa_padding_inaccuracies: 0.1
+    max_allowed_time_female: 0.0
+    oscillation_max_iterations: 1800
+    oscillation_max_distance: 0.02
+    use_velocity_scaled_lookahead_dist: false
+    min_approach_linear_velocity: 0.1
+    approach_velocity_scaling_dist: 1.0
+    critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+    rotate_to_goal:
+      slowing_factor: 5.0
+      lookahead_time: -1.0
+    base_obstacle:
+      scale: 0.02
+      max_scaling_factor: 0.02
+      forward_point_distance: 0.325
+    goal_align:
+      scale: 24.0
+      forward_point_distance: 0.325
+    path_align:
+      scale: 32.0
+      forward_point_distance: 0.325
+      max_allowed_time_female: 0.0
+    path_dist:
+      scale: 32.0
+      max_allowed_time_female: 0.0
+    goal_dist:
+      scale: 24.0
+      max_allowed_time_female: 0.0
+    prune_plan: true
+    prune_distance: 1.5
+    debug_trajectory_details: false
+    publish_evaluation: true
+    publish_scored_sampling_policies: false
+    publish_trajectories: false

jetson/ros2_ws/src/saltybot_nav2_slam/config/global_costmap_params.yaml

@@ -0,0 +1,36 @@
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: false
+      rolling_window: false
+      robot_radius: 0.35
+      plugins:
+        - name: static_layer
+          type: nav2_costmap_2d::StaticLayer
+          map_subscribe_transient_local: true
+          enabled: true
+        - name: obstacle_layer
+          type: nav2_costmap_2d::ObstacleLayer
+          enabled: true
+          observation_sources: scan
+          scan:
+            topic: /scan
+            max_obstacle_height: 2.5
+            min_obstacle_height: 0.0
+            clearing: true
+            marking: true
+            data_type: LaserScan
+        - name: inflation_layer
+          type: nav2_costmap_2d::InflationLayer
+          enabled: true
+          inflation_radius: 0.55
+          cost_scaling_factor: 10.0
+          inflate_unknown: false
+          inflate_around_unknown: true
+      resolution: 0.05
+      track_unknown_space: true
+      unknown_cost_value: -1

jetson/ros2_ws/src/saltybot_nav2_slam/config/local_costmap_params.yaml

@@ -0,0 +1,51 @@
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 10.0
+      publish_frequency: 5.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: false
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.35
+      plugins:
+        - name: obstacle_layer
+          type: nav2_costmap_2d::ObstacleLayer
+          enabled: true
+          observation_sources: scan camera_depth
+          scan:
+            topic: /scan
+            max_obstacle_height: 2.5
+            min_obstacle_height: 0.0
+            clearing: true
+            marking: true
+            data_type: LaserScan
+            raytrace_max_range: 10.0
+            raytrace_min_range: 0.0
+            expected_update_rate: 5.5
+            observation_persistence: 0.0
+          camera_depth:
+            topic: /camera/depth/color/points
+            max_obstacle_height: 2.5
+            min_obstacle_height: 0.0
+            clearing: false
+            marking: true
+            data_type: PointCloud2
+            sensor_frame: camera_link
+            expected_update_rate: 15.0
+            observation_persistence: 0.5
+            inf_is_valid: false
+            filter: passthrough
+            filter_value: 5.0
+        - name: inflation_layer
+          type: nav2_costmap_2d::InflationLayer
+          enabled: true
+          inflation_radius: 0.55
+          cost_scaling_factor: 10.0
+          inflate_unknown: true
+          inflate_around_unknown: false
+      track_unknown_space: true
+      unknown_cost_value: 0

jetson/ros2_ws/src/saltybot_nav2_slam/config/nav2_params.yaml

@@ -0,0 +1,219 @@
+amcl:
+  ros__parameters:
+    use_sim_time: false
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_link"
+    beam_search_increment: 0.1
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "differential"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    sigma_short: 0.05
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: false
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: false
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    enable_groot_monitoring: true
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
+    default_nav_through_poses_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
+    default_nav_to_pose_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_assisted_teleop_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+controller_server:
+  ros__parameters:
+    use_sim_time: false
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["general_goal_checker"]
+    controller_plugins: ["FollowPath"]
+
+    progress_checker:
+      plugin: "nav2_core::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_core::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: true
+      min_vel_x: 0.0
+      max_vel_x: 5.5
+      max_vel_theta: 3.0
+      min_speed_xy: 0.0
+      max_speed_xy: 5.5
+      acc_lim_x: 2.5
+      acc_lim_theta: 2.5
+      decel_lim_x: -2.5
+      decel_lim_theta: -2.5
+      vx_samples: 20
+      vtheta_samples: 40
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.1
+      transform_tolerance: 0.2
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: true
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      GoalAlign.scale: 24.0
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: false
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: true
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors/DriveOnHeading"
+    assisted_teleop:
+      plugin: "nav2_behaviors/AssistedTeleop"
+    wait:
+      plugin: "nav2_behaviors/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_timeout: 0.1
+    use_sim_time: false
+    simulate_ahead_time: 2.0
+    max_rotations: 1.0
+    max_backup_dist: 0.15
+    backup_speed: 0.5
+
+behavior_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: false
+
+lifecycle_manager_navigation:
+  ros__parameters:
+    use_sim_time: false
+    autostart: true
+    node_names: ['controller_server', 'planner_server', 'behavior_server', 'bt_navigator']
+
+lifecycle_manager_localization:
+  ros__parameters:
+    use_sim_time: false
+    autostart: true
+    node_names: ['amcl']
+
+map_server:
+  ros__parameters:
+    use_sim_time: false
+    yaml_filename: "map.yaml"
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: false
+    smoothing_frequency: 20.0
+    scale_velocities: false
+    feedback: "odometry"
+    max_velocity: [5.5, 0.0, 3.0]
+    min_velocity: [-5.5, 0.0, -3.0]
+    max_accel: [2.5, 0.0, 2.5]
+    max_decel: [-2.5, 0.0, -2.5]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0

jetson/ros2_ws/src/saltybot_nav2_slam/config/slam_toolbox_params.yaml

@@ -0,0 +1,58 @@
+slam_toolbox:
+  ros__parameters:
+    mode: mapping
+    async_mode: true
+    use_scan_matching: true
+    use_scan_bamatching: true
+    minimum_time_interval: 0.5
+    minimum_travel_distance: 0.2
+    minimum_rotation_deg: 5.0
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.05
+    correlation_search_space_smear_deviation: 0.1
+    correlation_search_space_std_dev_ratio: 0.5
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    scan_buffer_size: 10
+    scan_buffer_max_scan_distance: 12.0
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
+    fine_search_angle_offset: 0.00349
+    coarse_search_angle_offset: 0.349
+    coarse_angle_search_step_size: 0.0349
+    fine_angle_search_step_size: 0.00349
+    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
+    do_loop_closing: true
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_queue_size: 10
+    loop_match_minimum_response_fine: 0.1
+    loop_match_minimum_response_coarse: 0.3
+    minimum_score_to_accept_loop_closure: 0.07
+    do_final_optimization: true
+    final_optimization_particle_filter_size: 800
+    final_optimization_min_particles: 400
+    final_optimization_max_particles: 2000
+    final_optimization_threshold_metric_improvement: 0.0002
+    add_loops_as_edges: false
+    num_for_scan_matching: 0
+    icp_iterations: 10
+    interactive_mode: false
+    print_timing_information: false
+    map_frame: map
+    odom_frame: odom
+    base_frame: base_link
+    scan_topic: /scan
+    publish_tf: true
+    tf_buffer_duration: 30.0
+    transform_timeout: 0.2
+    queue_size: 10
+    throttle_scans: 1
+    use_map_saver: true
+    map_start_at_dock: true
+    map_start_pose: [0.0, 0.0, 0.0]
+    rollout_velocity_smoothing_factor: 0.07
+    force_turtlebot_rep: false

jetson/ros2_ws/src/saltybot_nav2_slam/launch/depth_to_costmap.launch.py

@@ -0,0 +1,28 @@
+"""RealSense Depth to Costmap Integration"""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_sim_time', default_value='false'),
+
+        Node(
+            package='depth_image_proc',
+            executable='convert_node',
+            name='depth_image_converter',
+            namespace='camera',
+            remappings=[
+                ('image_rect', '/camera/aligned_depth_to_color/image_raw'),
+                ('camera_info', '/camera/color/camera_info'),
+                ('points', '/camera/depth/color/points'),
+            ],
+            parameters=[{'use_sim_time': use_sim_time}],
+            output='screen',
+        ),
+    ])

jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_slam_bringup.launch.py

@@ -0,0 +1,66 @@
+"""Nav2 SLAM Bringup for SaltyBot (Issue #422)"""
+
+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():
+    nav2_slam_dir = get_package_share_directory('saltybot_nav2_slam')
+    bringup_dir = get_package_share_directory('saltybot_bringup')
+
+    nav2_params = os.path.join(nav2_slam_dir, 'config', 'nav2_params.yaml')
+    slam_params = os.path.join(nav2_slam_dir, 'config', 'slam_toolbox_params.yaml')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_sim_time', default_value='false'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'sensors.launch.py')
+            ),
+            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
+        ),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(nav2_slam_dir, 'launch', 'odometry_bridge.launch.py')
+            ),
+            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
+        ),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(nav2_slam_dir, 'launch', 'nav2_slam_integrated.launch.py')
+            ),
+            launch_arguments={
+                'slam_params_file': slam_params,
+                'use_sim_time': LaunchConfiguration('use_sim_time'),
+            }.items(),
+        ),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(nav2_slam_dir, 'launch', 'depth_to_costmap.launch.py')
+            ),
+            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
+        ),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(
+                    get_package_share_directory('nav2_bringup'),
+                    'launch', 'navigation_launch.py'
+                )
+            ),
+            launch_arguments={
+                'use_sim_time': LaunchConfiguration('use_sim_time'),
+                'params_file': nav2_params,
+                'autostart': 'true',
+                'map_subscribe_transient_local': 'true',
+            }.items(),
+        ),
+    ])

jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_slam_integrated.launch.py

@@ -0,0 +1,35 @@
+"""SLAM Toolbox integrated launch for SaltyBot"""
+
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+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():
+    slam_share = get_package_share_directory('slam_toolbox')
+    nav2_slam_dir = get_package_share_directory('saltybot_nav2_slam')
+
+    slam_params = LaunchConfiguration('slam_params_file')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'slam_params_file',
+            default_value=os.path.join(nav2_slam_dir, 'config', 'slam_toolbox_params.yaml'),
+        ),
+
+        DeclareLaunchArgument('use_sim_time', default_value='false'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(slam_share, 'launch', 'online_async_launch.py')
+            ),
+            launch_arguments={
+                'params_file': slam_params,
+                'use_sim_time': use_sim_time,
+            }.items(),
+        ),
+    ])

jetson/ros2_ws/src/saltybot_nav2_slam/launch/odometry_bridge.launch.py

@@ -0,0 +1,36 @@
+"""VESC Telemetry to Odometry Bridge"""
+
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_sim_time', default_value='false'),
+
+        Node(
+            package='saltybot_nav2_slam',
+            executable='vesc_odometry_bridge',
+            name='vesc_odometry_bridge',
+            parameters=[{
+                'use_sim_time': use_sim_time,
+                'vesc_state_topic': '/vesc/state',
+                'odometry_topic': '/odom',
+                'odom_frame_id': 'odom',
+                'base_frame_id': 'base_link',
+                'update_frequency': 50,
+                'wheel_base': 0.35,
+                'wheel_radius': 0.10,
+                'max_rpm': 60000,
+                'publish_tf': True,
+                'tf_queue_size': 10,
+            }],
+            output='screen',
+        ),
+    ])

jetson/ros2_ws/src/saltybot_nav2_slam/package.xml

@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_nav2_slam</name>
+  <version>0.1.0</version>
+  <description>
+    Nav2 SLAM integration for SaltyBot autonomous navigation.
+    Combines SLAM Toolbox (RPLIDAR 2D SLAM), RealSense depth for obstacle avoidance,
+    VESC odometry, and DWB planner for autonomous navigation up to 20km/h.
+  </description>
+  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <exec_depend>nav2_bringup</exec_depend>
+  <exec_depend>slam_toolbox</exec_depend>
+  <exec_depend>rplidar_ros</exec_depend>
+  <exec_depend>realsense2_camera</exec_depend>
+  <exec_depend>depth_image_proc</exec_depend>
+  <exec_depend>pointcloud_to_laserscan</exec_depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/__init__.py

@@ -0,0 +1 @@
+# Nav2 SLAM integration for SaltyBot

jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/vesc_odometry_bridge.py

@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+"""VESC telemetry to Nav2 odometry bridge."""
+
+import json
+import math
+import time
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import TransformStamped, Quaternion
+from tf2_ros import TransformBroadcaster
+
+
+class VESCOdometryBridge(Node):
+    """ROS2 node bridging VESC telemetry to Nav2 odometry."""
+
+    def __init__(self):
+        super().__init__("vesc_odometry_bridge")
+
+        self.declare_parameter("vesc_state_topic", "/vesc/state")
+        self.declare_parameter("odometry_topic", "/odom")
+        self.declare_parameter("odom_frame_id", "odom")
+        self.declare_parameter("base_frame_id", "base_link")
+        self.declare_parameter("update_frequency", 50)
+        self.declare_parameter("wheel_base", 0.35)
+        self.declare_parameter("wheel_radius", 0.10)
+        self.declare_parameter("max_rpm", 60000)
+        self.declare_parameter("publish_tf", True)
+
+        self.vesc_state_topic = self.get_parameter("vesc_state_topic").value
+        self.odometry_topic = self.get_parameter("odometry_topic").value
+        self.odom_frame_id = self.get_parameter("odom_frame_id").value
+        self.base_frame_id = self.get_parameter("base_frame_id").value
+        frequency = self.get_parameter("update_frequency").value
+        self.wheel_base = self.get_parameter("wheel_base").value
+        self.wheel_radius = self.get_parameter("wheel_radius").value
+        self.publish_tf = self.get_parameter("publish_tf").value
+
+        self.last_rpm = 0
+        self.last_update_time = time.time()
+
+        self.x = 0.0
+        self.y = 0.0
+        self.theta = 0.0
+        self.vx = 0.0
+        self.vy = 0.0
+        self.vtheta = 0.0
+
+        self.pub_odom = self.create_publisher(Odometry, self.odometry_topic, 10)
+
+        if self.publish_tf:
+            self.tf_broadcaster = TransformBroadcaster(self)
+
+        self.create_subscription(String, self.vesc_state_topic, self._on_vesc_state, 10)
+
+        period = 1.0 / frequency
+        self.create_timer(period, self._publish_odometry)
+
+        self.get_logger().info(
+            f"VESC odometry bridge initialized: "
+            f"wheel_base={self.wheel_base}m, wheel_radius={self.wheel_radius}m"
+        )
+
+    def _on_vesc_state(self, msg: String) -> None:
+        """Update VESC telemetry from JSON."""
+        try:
+            data = json.loads(msg.data)
+            current_rpm = data.get("rpm", 0)
+            motor_rad_s = (current_rpm / 60.0) * (2.0 * math.pi)
+            wheel_velocity = motor_rad_s * self.wheel_radius
+
+            self.vx = wheel_velocity
+            self.vy = 0.0
+            self.vtheta = 0.0
+            self.last_rpm = current_rpm
+
+        except json.JSONDecodeError:
+            pass
+
+    def _publish_odometry(self) -> None:
+        """Publish odometry message and TF."""
+        current_time = time.time()
+        dt = current_time - self.last_update_time
+        self.last_update_time = current_time
+
+        if abs(self.vtheta) > 0.001:
+            self.theta += self.vtheta * dt
+            self.x += (self.vx / self.vtheta) * (math.sin(self.theta) - math.sin(self.theta - self.vtheta * dt))
+            self.y += (self.vx / self.vtheta) * (math.cos(self.theta - self.vtheta * dt) - math.cos(self.theta))
+        else:
+            self.x += self.vx * math.cos(self.theta) * dt
+            self.y += self.vx * math.sin(self.theta) * dt
+
+        odom_msg = Odometry()
+        odom_msg.header.stamp = self.get_clock().now().to_msg()
+        odom_msg.header.frame_id = self.odom_frame_id
+        odom_msg.child_frame_id = self.base_frame_id
+
+        odom_msg.pose.pose.position.x = self.x
+        odom_msg.pose.pose.position.y = self.y
+        odom_msg.pose.pose.position.z = 0.0
+        odom_msg.pose.pose.orientation = self._euler_to_quaternion(0, 0, self.theta)
+
+        odom_msg.pose.covariance = [
+            0.1, 0, 0, 0, 0, 0,
+            0, 0.1, 0, 0, 0, 0,
+            0, 0, 0.1, 0, 0, 0,
+            0, 0, 0, 0.1, 0, 0,
+            0, 0, 0, 0, 0.1, 0,
+            0, 0, 0, 0, 0, 0.1,
+        ]
+
+        odom_msg.twist.twist.linear.x = self.vx
+        odom_msg.twist.twist.linear.y = self.vy
+        odom_msg.twist.twist.linear.z = 0.0
+        odom_msg.twist.twist.angular.z = self.vtheta
+
+        odom_msg.twist.covariance = [
+            0.05, 0, 0, 0, 0, 0,
+            0, 0.05, 0, 0, 0, 0,
+            0, 0, 0.05, 0, 0, 0,
+            0, 0, 0, 0.05, 0, 0,
+            0, 0, 0, 0, 0.05, 0,
+            0, 0, 0, 0, 0, 0.05,
+        ]
+
+        self.pub_odom.publish(odom_msg)
+
+        if self.publish_tf:
+            self._publish_tf(odom_msg.header.stamp)
+
+    def _publish_tf(self, timestamp) -> None:
+        """Publish odom → base_link TF."""
+        tf_msg = TransformStamped()
+        tf_msg.header.stamp = timestamp
+        tf_msg.header.frame_id = self.odom_frame_id
+        tf_msg.child_frame_id = self.base_frame_id
+
+        tf_msg.transform.translation.x = self.x
+        tf_msg.transform.translation.y = self.y
+        tf_msg.transform.translation.z = 0.0
+        tf_msg.transform.rotation = self._euler_to_quaternion(0, 0, self.theta)
+
+        self.tf_broadcaster.sendTransform(tf_msg)
+
+    @staticmethod
+    def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> Quaternion:
+        """Convert Euler angles to quaternion."""
+        cy = math.cos(yaw * 0.5)
+        sy = math.sin(yaw * 0.5)
+        cp = math.cos(pitch * 0.5)
+        sp = math.sin(pitch * 0.5)
+        cr = math.cos(roll * 0.5)
+        sr = math.sin(roll * 0.5)
+
+        q = Quaternion()
+        q.w = cr * cp * cy + sr * sp * sy
+        q.x = sr * cp * cy - cr * sp * sy
+        q.y = cr * sp * cy + sr * cp * sy
+        q.z = cr * cp * sy - sr * sp * cy
+
+        return q
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VESCOdometryBridge()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_nav2_slam/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/saltybot_nav2_slam
+[install]
+install_scripts=$base/lib/saltybot_nav2_slam

jetson/ros2_ws/src/saltybot_nav2_slam/setup.py

@@ -0,0 +1,39 @@
+from setuptools import setup
+
+package_name = "saltybot_nav2_slam"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (f"share/{package_name}/launch", [
+            "launch/nav2_slam_bringup.launch.py",
+            "launch/nav2_slam_integrated.launch.py",
+            "launch/depth_to_costmap.launch.py",
+            "launch/odometry_bridge.launch.py",
+        ]),
+        (f"share/{package_name}/config", [
+            "config/nav2_params.yaml",
+            "config/slam_toolbox_params.yaml",
+            "config/costmap_common_params.yaml",
+            "config/global_costmap_params.yaml",
+            "config/local_costmap_params.yaml",
+            "config/dwb_local_planner_params.yaml",
+        ]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-controls",
+    maintainer_email="sl-controls@saltylab.local",
+    description="Nav2 SLAM integration with RPLIDAR + RealSense for autonomous navigation",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "vesc_odometry_bridge = saltybot_nav2_slam.vesc_odometry_bridge:main",
+        ],
+    },
+)