Integrated Localizer w/ Nav stack

src/simulation/CMakeLists.txt

@@ -17,7 +17,7 @@ find_package(ros_gz_bridge REQUIRED)
 
 # --- Install data directories so package:// URIs work ---
 # Guard each one so CMake doesn't error if a folder is missing.
-foreach(dir urdf meshes config launch worlds textures models)
+foreach(dir urdf meshes config launch worlds textures models rviz)
   if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/${dir})
     install(DIRECTORY ${dir}/
       DESTINATION share/${PROJECT_NAME}/${dir}

src/simulation/launch/ground_truth_tf.launch.py

@@ -22,7 +22,8 @@ def generate_launch_description():
             name='ground_truth_tf_publisher',
             output='screen',
             parameters=[{
-                'child_frame_id': LaunchConfiguration('child_frame_id')
+                'child_frame_id': LaunchConfiguration('child_frame_id'),
+                'use_sim_time': True
             }]
         ),
     ])

src/simulation/launch/spawn.launch.py

@@ -21,9 +21,11 @@
 
 def generate_launch_description():
     pkg_share = get_package_share_directory('description')
-
+    pkg_sim = get_package_share_directory('simulation')
+
     urdf_file = os.path.join(pkg_share, 'urdf', 'athena_drive.urdf.xacro')
     controllers_file = os.path.join(pkg_share, 'config', 'athena_drive_sim_controllers.yaml')
+    rviz_config_file = os.path.join(pkg_sim, 'rviz', 'sim.rviz')
 
 
     namespace = LaunchConfiguration('namespace')
@@ -70,6 +72,8 @@ def generate_launch_description():
             executable='rviz2',
             name='rviz2',
             output='screen',
+            arguments=['-d', rviz_config_file],
+            parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
             condition=conditions.IfCondition(LaunchConfiguration('rviz'))
         ),
     ])

src/subsystems/navigation/athena_planner/config/nav2_params.yaml

@@ -2,137 +2,197 @@ bt_navigator:
   ros__parameters:
     use_sim_time: true
     global_frame: map
-    robot_base_frame: base_link
-    # Note the bt xml file is set up in navigation.launch.py
-
+    robot_base_frame: base_footprint
+    odom_topic: /localization/odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    default_bt_xml_filename: "$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_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_smooth_path_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_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+
 planner_server:
   ros__parameters:
     use_sim_time: true
     expected_planner_frequency: 1.0
     planner_plugins: ["GridBased"]
 
     GridBased:
-      plugin: "nav2_smac_planner/SmacPlannerHybrid"  
-      # Search / map handling
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      tolerance: 0.5
       downsample_costmap: false
       downsampling_factor: 1
       allow_unknown: true
-      tolerance: 0.25
       max_iterations: 1000000
-      max_on_approach_iterations: 1000
       max_planning_time: 5.0
-      motion_model_for_search: "DUBIN"                # "DUBIN" or "REEDS_SHEPP" if reversing allowed
+
+      # Ackermann vehicle parameters
+      motion_model_for_search: "REEDS_SHEPP"
       angle_quantization_bins: 72
-
-      # Analytic expansion (goal approach shortcuts)
       analytic_expansion_ratio: 3.5
       analytic_expansion_max_length: 3.0
-      analytic_expansion_max_cost: 200.0
-      analytic_expansion_max_cost_override: false
-
-      # Vehicle / penalties (tunes path quality)
-      minimum_turning_radius: 0.40
+      minimum_turning_radius: 1.0
       reverse_penalty: 2.0
       change_penalty: 0.0
       non_straight_penalty: 1.2
       cost_penalty: 2.0
       retrospective_penalty: 0.015
-
-      # Heuristics / caching
+
       lookup_table_size: 20.0
       cache_obstacle_heuristic: false
-      downsample_obstacle_heuristic: true
-      use_quadratic_cost_penalty: false
-      allow_primitive_interpolation: false
-      coarse_search_resolution: 4                     # only used with ALL_DIRECTION goal mode
-      goal_heading_mode: "DEFAULT"                    # DEFAULT, BIDIRECTIONAL, ALL_DIRECTION
-
-      # Debug + post-smoothing
-      debug_visualizations: false
       smooth_path: true
-      smoother:
-        max_iterations: 1000
-        w_smooth: 0.3
-        w_data: 0.2
-        tolerance: 1.0e-10
-        do_refinement: true
-        refinement_num: 2
-
-
-global_costmap:
-  ros__parameters:
-    use_sim_time: true
-    global_frame: map
-    robot_base_frame: base_link
-    update_frequency: 2.0
-    publish_frequency: 2.0
-    resolution: 1.0
-    size_x: 799
-    size_y: 799
-    origin_x: 0.0
-    origin_y: 0.0
-    rolling_window: false
-    track_unknown_space: true
-    plugins:
-      - {name: static_layer,    type: "nav2_costmap_2d::StaticLayer"}
-      - {name: inflation_layer, type: "nav2_costmap_2d::InflationLayer"}
 
 controller_server:
   ros__parameters:
-    odom_topic: /ackermann_steering_controller/odometry
     use_sim_time: true
-    enable_stamped_cmd_vel: 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
+    odom_topic: /localization/odom
     progress_checker_plugins: ["progress_checker"]
-    goal_checker_plugins: ["simple_goal_checker"]
-    current_goal_checker: "simple_goal_checker"
+    goal_checker_plugins: ["general_goal_checker"]
+    controller_plugins: ["FollowPath"]
 
     progress_checker:
       plugin: "nav2_controller::SimpleProgressChecker"
       required_movement_radius: 0.5
       movement_time_allowance: 10.0
 
-    simple_goal_checker:
+    general_goal_checker:
+      stateful: true
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 2.0    
-      yaw_goal_tolerance: 6.283185   
-      stateful: true               
-
-    controller_plugins: ["FollowPath"]
+      xy_goal_tolerance: 1.0
+      yaw_goal_tolerance: 0.5
+
     FollowPath:
       plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
       desired_linear_vel: 1.0
+      lookahead_dist: 2.5
+      min_lookahead_dist: 1.5
+      max_lookahead_dist: 4.0
+      lookahead_time: 1.5
+      rotate_to_heading_angular_vel: 0.3
+      transform_tolerance: 0.5
+      use_velocity_scaled_lookahead_dist: true
+      min_approach_linear_velocity: 0.2
+      approach_velocity_scaling_dist: 1.0
+      use_collision_detection: false
+      max_allowed_time_to_collision_up_to_carrot: 1.0
+      use_regulated_linear_velocity_scaling: true
+      use_cost_regulated_linear_velocity_scaling: false
+      regulated_linear_scaling_min_radius: 1.0
+      regulated_linear_scaling_min_speed: 0.25
+      use_rotate_to_heading: true
+      allow_reversing: false
+      rotate_to_heading_min_angle: 0.785
       max_linear_accel: 1.0
       max_linear_decel: 1.0
-      lookahead_dist: 2.0
-      min_lookahead_dist: 1.0
-      max_lookahead_dist: 3.0
-      transform_tolerance: 1.0
-      use_velocity_scaled_lookahead_dist: false
-      use_final_approach_orientation: false
-
-
+      max_angular_accel: 1.5
+      max_robot_pose_search_dist: 10.0
+      use_interpolation: false
 
-local_costmap:
+behavior_server:
   ros__parameters:
     use_sim_time: true
-    global_frame: odom
-    robot_base_frame: base_link
-    rolling_window: true
-    width: 6.0
-    height: 6.0
-    resolution: 1
-    plugins: ["static_layer"]
-    static_layer:
-      plugin: "nav2_costmap_2d::StaticLayer"
-      map_subscribe_transient_local: true
-
+    global_frame: map
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.5
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 0.3
+    min_rotational_vel: 0.05
+    rotational_acc_lim: 0.8
+
+    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_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: true
+      robot_base_frame: base_footprint
+      global_frame: map
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      resolution: 0.5
+      width: 100
+      height: 100
+      rolling_window: true
+      track_unknown_space: false
+      always_send_full_costmap: true
+      footprint: "[[0.45, 0.33], [0.45, -0.33], [-0.35, -0.33], [-0.35, 0.33]]"
+      plugins: ["inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 1.5
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      use_sim_time: true
+      global_frame: map
+      robot_base_frame: base_footprint
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      rolling_window: true
+      width: 10
+      height: 10
+      resolution: 0.25
+      always_send_full_costmap: true
+      footprint: "[[0.45, 0.33], [0.45, -0.33], [-0.35, -0.33], [-0.35, 0.33]]"
+      plugins: ["inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 1.0
 
 lifecycle_manager:
   ros__parameters:
     use_sim_time: true
     autostart: true
     node_names:
-      - planner_server
       - controller_server
-      - bt_navigator
+      - planner_server
+      - behavior_server
+      - bt_navigator