Nav2 controller server high CPU load

edit

I'm struggling to understand why the controller server is taking so much CPU (12th get i9-12900H) even when not navigating at all.

Here is my nav_params.yaml

I can share more info if needed, but, hopefully, I can get some pointers so I can start looking more into it since as of now I have no idea where to start from.

Thank you in advance

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
    odom_topic: /wheel/odometry
    transform_tolerance: 0.3
    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: "simple_navigation_gfr.xml"
    # default_bt_xml_filename: "follow_point.xml"
    # default_bt_xml_filename: "followpath_only.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

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    use_sim_time: True

    GridBased:
      plugin: "smac_planner/SmacPlanner" 
      tolerance: 0.5                    
      downsample_costmap: false
      downsampling_factor: 1       
      allow_unknown: true # false
      max_iterations: -1                
      max_on_approach_iterations: 1000
      max_planning_time_ms: 2000.0    
      smooth_path: true                
      motion_model_for_search: "REEDS_SHEPP" 
      angle_quantization_bins: 72       
      minimum_turning_radius: 1.6     
      reverse_penalty: 5.0              
      change_penalty: 0.              
      non_straight_penalty: 1.05  
      cost_penalty: 1.3                 

      smoother:
        smoother:
          w_curve: 30.0                 # weight to minimize curvature of path
          w_dist: 0.0                   # weight to bind path to original as optional replacement for cost weight
          w_smooth: 30000.0             # weight to maximize smoothness of path
          w_cost: 0.025                 # weight to steer robot away from collision and cost
          cost_scaling_factor: 10.0     # this should match the inflation layer's parameter

        # I do not recommend users mess with this unless they're doing production tuning
        optimizer:
          max_time: 0.10                # maximum compute time for smoother
          max_iterations: 500           # max iterations of smoother
          debug_optimizer: false        # print debug info
          gradient_tol: 1.0e-10
          fn_tol: 1.0e-20
          param_tol: 1.0e-15
          advanced:
            min_line_search_step_size: 1.0e-20
            max_num_line_search_step_size_iterations: 50
            line_search_sufficient_function_decrease: 1.0e-20
            max_num_line_search_direction_restarts: 10
            max_line_search_step_expansion: 50

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 5.0
    min_x_velocity_threshold: 0.01 # Odometry values below this threshold (in m/s) will be set to 0.0.
    min_y_velocity_threshold: 0.0
    min_theta_velocity_threshold: 0.01
    failure_tolerance: 0.5
    # odom_topic: /odometry/filtered
    odom_topic: /wheel/odometry
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["goal_checker"]
    controller_plugins: ["FollowPath"]

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.001
      movement_time_allowance: 20.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
      dt_ref: 0.3
      dt_hysteresis: 0.05
      exact_arc_length: True ...