Nav2 global and local planner conflict

edit

Hello, I'm working with an Ackermann vehicle and using the TEB controller as the local planner (I built it for Foxy) After some tuning, I'm finally getting the vehicle to navigate kind of properly. However, the local planner's path is always different than the global planner's path. This is expected, I assume (but correct me if I'm wrong), but in some complex maneuvers, for example, when involving going in reverse, the path becomes really different and I think the global and local planners are kinda fighting each other. In most cases, the vehicle will make it to the goal but other times it just goes back and forth between the 2 paths and after a while it fails.

Below is an example of a case where the vehicle needs to reverse first to get to the goal. The good thing is that the paths look symmetrical, which means the 2 planners are using similar kinematic constraints. But, regardless, how can I solve this "conflict"?

Here are my Nav2 params:

amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 20.0
    laser_min_range: 0.1
    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
    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
    scan_topic: scan

amcl_map_client:
  ros__parameters:
    use_sim_time: True

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odometry/filtered
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    # default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    default_bt_xml_filename: "navigate_w_replanning_and_round_robin_recovery"
    # default_bt_xml_filename: "follow_point.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_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_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_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 2.0
    min_x_velocity_threshold: 0.005
    min_y_velocity_threshold: 0.005
    min_theta_velocity_threshold: 0.01
    failure_tolerance: 0.3
    odom_topic: /odometry/filtered
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["goal_checker"]
    controller_plugins: ["FollowPath"]

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.05
      movement_time_allowance: 10.0
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.2 # 0.25
      yaw_goal_tolerance: 0.1 #0.25
      stateful: True
    # TEB parameters
    # http://wiki.ros.org/teb_local_planner
    FollowPath:
      plugin: "teb_local_planner::TebLocalPlannerROS"
      odom_topic: odom
      map_frame: map

      # footprint_model.type: circular
      # footprint_model.radius: 2.0
      footprint_model.type: polygon
      footprint_model.vertices: "[ [1.54, 0.405], [1.54, -0.405], [-0.2, -0.405], [-0.2, 0.405] ]"

      min_turning_radius: 2.0
      wheelbase: 1.35
      cmd_angle_instead_rotvel: true # INVESTIGATE MORE!
      min_obstacle_dist: 1.0
      inflation_dist: 1.0
      costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
      costmap_converter_spin_thread: True
      costmap_converter_rate: 15
      enable_homotopy_class_planning: True
      enable_multithreading: True
      optimization_verbose: False
      teb_autoresize: True
      min_samples: 3
      max_samples: 20
      max_global_plan_lookahead_dist: 5.0
      visualize_hc_graph: True
      max_vel_x: 0.75
      max_vel_x_backwards: 0.2
      max_vel_y: 0.0 # should be zero for non-holonomic robots
      acc_lim_y: 0.05
      max_vel_theta: 1.0
      acc_lim_x: 0.5
      acc_lim_theta: 0 ...