Nav2 controller server high CPU load
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
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: 0.8 # 1.6=~40 degrees steering angle
wheelbase: 1.35
cmd_angle_instead_rotvel: true
min_obstacle_dist: 1.
inflation_dist: 1.2
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 #3
max_samples: 100 #20
max_global_plan_lookahead_dist: 6.0
visualize_hc_graph: True
max_vel_x: 0.5
max_vel_x_backwards: 0.5
max_vel_y: 0. # should be zero for non-holonomic robots
acc_lim_y: 0.
max_vel_theta: 0.1
acc_lim_x: 0.2
acc_lim_theta: 0.05
yaw_goal_tolerance: 0.2
xy_goal_tolerance: 0.1
include_costmap_obstacles: True
publish_feedback: True
# weight_kinematics_forward_drive: 10.0
# weight_kinematics_turning_radius: 10.0
allow_init_with_backwards_motion: True
global_plan_viapoint_sep: 5.0
# Optimization
no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: true
optimization_verbose: false
penalty_epsilon: 0.05
weight_max_vel_x: 10.0
weight_max_vel_y: 2.0
weight_max_vel_theta: 1.
weight_acc_lim_x: 1.
weight_acc_lim_y: 1.
weight_acc_lim_theta: 1.
weight_kinematics_nh: 100.
weight_kinematics_forward_drive: 1.
weight_kinematics_turning_radius: 1.
weight_optimaltime: 1.
weight_shortest_path: 0.
weight_obstacle: 50.
weight_inflation: 0.1
weight_dynamic_obstacle: 50.
weight_dynamic_obstacle_inflation: 0.1
weight_viapoint: 1.
weight_prefer_rotdir: 50.
weight_adapt_factor: 2.0
obstacle_cost_exponent: 10.0
enable_homotopy_class_planning: True
enable_multithreading: True
costmap_converter:
ros__parameters:
use_sim_time: True
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 5.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
transform_tolerance: 0.3
width: 10 # Edited after suggestion in comments
height: 10 # Edited after suggestion in comments
resolution: 0.05
footprint: "[ [1.54, 0.405], [1.54, -0.405], [-0.2, -0.405], [-0.2, 0.405] ]"
plugins: ["obstacle_layer", "inflation_layer", "stvl_layer"] # "voxel_layer"
clearable_layers: ["obstacle_layer", "inflation_layer", "stvl_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 3.0 #0.55
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: uss_sensors front_sensor rear_sensor
uss_sensors:
topic: /uss_scan
max_obstacle_height: 100.0
min_obstacle_height: -100.0
obstacle_max_range: 10.0
obstacle_min_range: 0.2
raytrace_max_range: 10.0
raytrace_min_range: 0.0
clearing: True
marking: True
data_type: "LaserScan"
# ##################################
# VLP16 front only
front_sensor:
topic: /vlp16_front_pointcloud
observation_persistence: 0.0
max_obstacle_height: 2.0
min_obstacle_height: 0.2
obstacle_range: 10.0
# obstacle_min_range: 0.0
raytrace_range: 11.0
# raytrace_min_range: 0.0
clearing: true
marking: true
data_type: "PointCloud2"
stvl_layer:
plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
enabled: true
voxel_decay: 15.
decay_model: 0
voxel_size: 0.05
track_unknown_space: true
unknown_threshold: 15
mark_threshold: 0
update_footprint_enabled: true
combination_method: 1
origin_z: 0.0
publish_voxel_map: true
transform_tolerance: 0.2
mapping_mode: false
map_save_duration: 60.0
observation_sources: front_sensor
front_sensor:
data_type: PointCloud2
topic: /vlp16_front_pointcloud
marking: true
clearing: true
obstacle_range: 10.0
min_obstacle_height: 0.2
max_obstacle_height: 3.0
expected_update_rate: 0.0
observation_persistence: 0.0
inf_is_valid: false
filter: "voxel"
voxel_min_points: 0
clear_after_reading: true
max_z: 7.0
min_z: 0.1
vertical_fov_angle: 0.5233
horizontal_fov_angle: 1.57
decay_acceleration: 15.0
model_type: 0
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.1
z_voxels: 30
max_obstacle_height: 4.0
mark_threshold: 0
observation_sources: front_sensor
front_sensor:
topic: /scan #/vlp16_front_pointcloud
max_obstacle_height: 5.0
min_obstacle_height: 0.2
obstacle_range: 5.5
# obstacle_min_range: 0.0
raytrace_range: 6.0
# raytrace_min_range: 0.0
clearing: True
marking: True
data_type: "LaserScan" #"PointCloud2"
static_layer:
map_subscribe_transient_local: True
transform_tolerance: 0.3 # ADDED
always_send_full_costmap: False
local_costmap_client:
ros__parameters:
use_sim_time: True
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 5.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
# Length 1.74 m
# Width 0.81 m
footprint: "[ [1.54, 0.405], [1.54, -0.405], [-0.2, -0.405], [-0.2, 0.405] ]"
footprint_padding: 0.2
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "inflation_layer", "stvl_layer"] #"obstacle_layer", "voxel_layer"
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.1
z_voxels: 30
max_obstacle_height: 4.0
mark_threshold: 0
observation_sources: front_sensor rear_sensor
# ##################################
# VLP16 front only
front_sensor:
topic: /scan #/vlp16_front_pointcloud
max_obstacle_height: 5.0
min_obstacle_height: 0.2
obstacle_range: 5.5
# obstacle_min_range: 0.0
raytrace_range: 6.0
# raytrace_min_range: 0.0
clearing: True
marking: True
data_type: "LaserScan" #"PointCloud2"
stvl_layer:
plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
enabled: true
voxel_decay: 15.
decay_model: 0
voxel_size: 0.05
track_unknown_space: true
unknown_threshold: 15
mark_threshold: 0
update_footprint_enabled: true
combination_method: 1
origin_z: 0.0
publish_voxel_map: true
transform_tolerance: 0.2
mapping_mode: false
map_save_duration: 60.0
observation_sources: front_sensor
front_sensor:
data_type: PointCloud2
topic: /vlp16_front_pointcloud
marking: true
clearing: true
obstacle_range: 10.0
min_obstacle_height: 0.2
max_obstacle_height: 3.0
expected_update_rate: 0.0
observation_persistence: 0.0
inf_is_valid: false
filter: "voxel"
voxel_min_points: 0
clear_after_reading: true
max_z: 7.0
min_z: 0.1
vertical_fov_angle: 0.5233
horizontal_fov_angle: 1.57
decay_acceleration: 15.0
model_type: 0
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
# VLP16 front only
observation_sources: front_sensor
front_sensor:
topic: /vlp16_front_pointcloud
max_obstacle_height: 2.0
min_obstacle_height: 0.1
clearing: True
marking: True
data_type: "PointCloud2"
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
transform_tolerance: 0.3 # ADDED
# map_topic: map
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 5.0
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: True
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
map_saver:
ros__parameters:
use_sim_time: True
save_map_timeout: 5000
free_thresh_default: 0.25
occupied_thresh_default: 0.65
map_subscribe_transient_local: False
recoveries_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["spin", "back_up", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
back_up:
plugin: "nav2_recoveries/BackUp"
wait:
plugin: "nav2_recoveries/Wait"
global_frame: odom
robot_base_frame: base_link
transform_timeout: 0.1
transform_tolerance: 0.3 # ADDED
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 0.5
min_rotational_vel: 0.4
rotational_acc_lim: 0.5
robot_state_publisher:
ros__parameters:
use_sim_time: True
Asked by guidout on 2023-07-16 20:36:42 UTC
Answers
The controller / planner servers contain the local and global costmaps, respectively. If you’re not actively navigating, that compute time is relative to your perception plugins in your costmap configs. Given a VLP 16 at full rate + a 50 m local costmap, I wouldn’t expect that to be particularly low. That’s alot of raytracing.
Asked by stevemacenski on 2023-07-17 01:19:59 UTC
Comments
@stevemacenski thanks for the input. Good point. However, I tried to reduce the local costmap from 50 to 10m but the CPU usage doesn't seem to have changed at all. Does any other perception configuration pops up to you as a possible culprit?
Asked by guidout on 2023-07-17 07:47:15 UTC
Without digging into your application / specific configs to try to find a solution, I don't have cookie cutter advice. Perception requirements are highly dependent on application needs, dynamics, and environment types -- not to mention obviously sensors being used and their configurations/mounting visibility. If you wanted to chat more about this for a private engagement with your company, I'm happy to :-) My email is steve@opennav.org.
Asked by stevemacenski on 2023-07-22 10:09:55 UTC
Comments
You might want also to take a look to the memory. RAM is full and swap memory almost too.
Asked by pgarcia-dev on 2023-07-17 13:26:13 UTC
not following, they seem both <50%...
Asked by guidout on 2023-07-17 19:39:28 UTC