controller_server:
  ros__parameters:
    odom_topic: /odom
    use_sim_time: True

    controller_frequency: 10.0
    min_x_velocity_threshold: 0.05
    min_y_velocity_threshold: 0.05
    min_theta_velocity_threshold: 0.001
    # Seems like this is the controller_patience equivalent in ROS2
    failure_tolerance: 20.0
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPathTeb"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0

    # Goal checker parameters
    general_goal_checker:
      plugin: "nav2_controller::StoppedGoalChecker"
      stateful: True
      xy_goal_tolerance: 0.3
      yaw_goal_tolerance: 0.4
      trans_stopped_velocity: 0.1
      rot_stopped_velocity: 0.1

    FollowPathTeb:
      plugin: teb_local_planner::TebLocalPlannerROS

      # Trajectory
      teb_autosize: 1.0
      dt_ref: 0.3
      dt_hysteresis: 0.1
      global_plan_overwrite_orientation: True
      allow_init_with_backwards_motion: False
      max_global_plan_lookahead_dist: 3.0
      feasibility_check_no_poses: 3

      # Robot
      max_vel_x: 0.5        # 0.7 #0.5
      # TODO: Look at how to change this. Teb planner warns undefined behaviour, recommends
      # increasing weight for penalizing backwards driving
      max_vel_x_backwards: 0.5        # 0.7 #0 #0.1
      max_vel_theta: 0.7        # 0.5 #0.7
      acc_lim_x: 0.5        # 1.0 #0.5
      acc_lim_theta: 0.7        # 0.5 #1.0 #0.5
      min_turning_radius: 0.0        # 0.0 means can perform in-place rotation
      footprint_model:        # types: "point", "circular", "two_circles", "line", "polygon"
        type: "circular"        # "circular"
        radius: 0.22        # for type "circular"
        line_start: [-0.4, 0.0]        # for type "line"
        line_end: [0.4, 0.0]        # for type "line"
        front_offset: 0.2        # for type "two_circles"
        front_radius: 0.2        # for type "two_circles"
        rear_offset: 0.2        # for type "two_circles"
        rear_radius: 0.2        # for type "two_circles"
        vertices: "[[0.25, -0.05], [0.18, -0.05], [0.18, -0.18],
             [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18],
             [0.18, 0.18], [0.18, 0.05], [0.25, 0.05]]"        # for type "polygon"

      # GoalTolerance
      # Tolerance parameters removed in ROS2, use the params from goal checker instead
      free_goal_vel: False

      # Obstacles
      min_obstacle_dist: 0.1
      # 0.45 #0.4 # robot outer radius is included since footprint model is chosen to be point
      inflation_dist: 0.6
      include_costmap_obstacles: True
      dynamic_obstacle_inflation_dist: 0.1        # 0.6
      costmap_obstacles_behind_robot_dist: 0.0        # 1.0
      obstacle_poses_affected: 30        # 30
      costmap_converter_plugin: ""
      costmap_converter_spin_thread: True
      costmap_converter_rate: 5

      # Optimization
      no_inner_iterations: 5
      no_outer_iterations: 4
      optimization_activate: True
      optimization_verbose: False
      penalty_epsilon: 0.1
      weight_max_vel_x: 10.0        # 2
      weight_max_vel_theta: 10.0        # 1
      weight_acc_lim_x: 0.5        # 1
      weight_acc_lim_theta: 1.0        # 1
      weight_kinematics_nh: 1000.0
      weight_kinematics_forward_drive: 10000.0        # 1 #100 #1
      weight_kinematics_turning_radius: 100.0        # 100 #1
      weight_optimaltime: 20.0        # 10 #5 #1
      weight_obstacle: 60.0        # 50.0
      weight_inflation: 0.3
      weight_dynamic_obstacle: 10.0        # not in use yet
      weight_dynamic_obstacle_inflation: 0.3
      alternative_time_cost: False        # not in use yet
      weight_viapoint: 1.0
      weight_adapt_factor: 2.0

      # Viapoints
      global_plan_viapoint_sep: 1.0        # 0.5 #-0.1

      # Homotopy Class Planner
      enable_homotopy_class_planning: False
      enable_multithreading: True
      simple_exploration: False    # Not in ROS2?
      max_number_classes: 4
      selection_cost_hysteresis: 1.0
      selection_obst_cost_scale: 1.0
      selection_alternative_time_cost: True
      roadmap_graph_no_samples: 15
      roadmap_graph_area_width: 5.0
      h_signature_prescaler: 0.5
      h_signature_threshold: 0.1
      obstacle_keypoint_offset: 0.1
      obstacle_heading_threshold: 0.45
      visualize_hc_graph: False
      viapoints_all_candidates: True
      visualize_with_time_as_z_axis_scale: 0.2

      # Recovery
      shrink_horizon_backup: True

      # Additional params added in ROS2
      divergence_detection_enable: False
      divergence_detection_max_chi_squared: 10
