MpcLocalPlannerROS:

  odom_topic: tricycle_controller/odom
  map_frame: map

  costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
    # costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
    #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
    #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
    #costmap_converter_plugin: "" # deactivate plugin
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5
    # costmap_converter/CostmapToLinesDBSRANSAC:
    #    cluster_max_distance: 0.4
      

    
   ## Robot settings
  robot:
    type: "simple_car"
    
    simple_car:
      wheelbase: 0.885
      front_wheel_driving: True
      max_vel_x: 0.4
      max_vel_x_backwards: 0.4
      max_steering_angle: 0.47
      acc_lim_x: 0.5 # deactive bounds with zero
      dec_lim_x: 0.5 # deactive bounds with zero
      max_steering_rate: 0.0 # deactive bounds with zero
    
    kinematic_bicycle_vel_input:
      length_rear: 0.01
      length_front: 0.884
      max_vel_x: 0.4
      max_vel_x_backwards: 0.2
      max_steering_angle: 0.79
      acc_lim_x: 0.0 # deactive bounds with zero
      dec_lim_x: 0.0 # deactive bounds with zero
      max_steering_rate: 0.5 #0.5 deactive bounds with zero

  ## Footprint model for collision avoidance
  footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
    type: "line"
    radius: 0.2 # for type "circular"
    line_start: [0.0, 0.0] # for type "line"
    line_end: [0.4, 0.0] # for type "line"

  ## Collision avoidance
  collision_avoidance:
    min_obstacle_dist: 0.27 # Note, this parameter must be chosen w.r.t. the footprint_model
    enable_dynamic_obstacles: true
    force_inclusion_dist: 0.5 #0.5
    cutoff_dist: 2.5  #2.5
    include_costmap_obstacles: True
    costmap_obstacles_behind_robot_dist: 1.0
    collision_check_no_poses: 5


  ## Planning grid 
  grid:
    type: "fd_grid"
    grid_size_ref: 20
    dt_ref: 0.3 #0.3
    xf_fixed: [True, True, True]
    warm_start: True
    collocation_method: "forward_differences" #forward_differences midpoint_differences crank_nicolson_differences
    cost_integration_method: "left_sum" #left_sum trapezoidal_rule
    variable_grid:
      enable: True  
      min_dt: 0.0;
      max_dt: 10.0;
      grid_adaptation:
        enable: True
        dt_hyst_ratio: 0.1
        min_grid_size: 2
        max_grid_size: 50

  ## Planning options
  planning:
    objective:
      type: "minimum_time" #minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly
      minimum_time:
        state_weights: [2.0, 2.0, 2.0]
        control_weights: [1, 1]
        integral_form: True
      quadratic_form:
        state_weights: [2.0, 2.0, 0.25]
        control_weights: [0.1, 0.05]
        integral_form: False
      minimum_time_via_points:
        position_weight: 10.0 #10.5
        orientation_weight: 1.0
        via_points_ordered: True
    terminal_cost:
      type: "none" # can be "none"
      quadratic:
        final_state_weights: [2.0, 2.0, 2.0]
    terminal_constraint:
      type: "none" # can be "none"
      l2_ball:
        weight_matrix: [1.0, 1.0, 1.0]
        radius: 5

  ## Controller options
  controller:
    outer_ocp_iterations: 1
    xy_goal_tolerance: 0.2
    yaw_goal_tolerance: 0.1
    global_plan_overwrite_orientation: True
    global_plan_prune_distance: 1.0
    allow_init_with_backward_motion: True
    max_global_plan_lookahead_dist: 8.0 
    force_reinit_new_goal_dist: 1.0
    force_reinit_new_goal_angular: 1.57
    prefer_x_feedback: True
    publish_ocp_results: True
    
  ## Solver settings
  solver:
    type: "ipopt"
    ipopt:
      iterations: 100 #100
      max_cpu_time: -1.0
      ipopt_numeric_options:
        tol: 1e-4
      ipopt_string_options:
        linear_solver: "mumps" 
        hessian_approximation: "limited-memory" # exact/limited-memory, WARNING 'exact' does currently not work well with the carlike model
    lsq_lm:
      iterations: 100
      weight_init_eq: 2
      weight_init_ineq: 2
      weight_init_bounds: 2
      weight_adapt_factor_eq: 1.5
      weight_adapt_factor_ineq: 1.5
      weight_adapt_factor_bounds: 1.5
      weight_adapt_max_eq: 500
      weight_adapt_max_ineq: 500
      weight_adapt_max_bounds: 500