DWB local planner and locomotor High cpu usage

asked 2021-02-28 06:23:26 -0600

omarwalid96 gravatar image

I've been using Locomotor for a while now tuning every parameter i can tune. Tested in simulation and in real life robot.

Tested same robot with the standard move base all running well , but i need the flexibility of this nav_core2.


Using melodic , both i3 - i7 8th gen - 8GB ram

Tested with: - Cartographer (mapping mode - localization mode) - Gmapping - amcl

But always end up with high cpu usage to get the desired output and all the costmaps and controller get out of sync. ex.(Control loop missed its desired rate of 5.0000Hz... the loop actually took 0.2479 seconds (>0.2000).)


Velocity output is never smooth. ( I guess that can be described in another issue ,but hope it's smth easy) Initial output velocity always low , not suitable for the given motors


Here are the parameters that i'm using with tuned parameters to get low cpu usage ( behavior is very bad )

global_costmap.yaml `

controller_frequency: 5.0
global_plan_topic: global_plan
global_plan_type: Path3D
global_plan_epsilon: -1.0
twist_topic: cmd_vel
twist_type: Twist3D

global_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  rolling_window: false
  track_unknown_space: true
  always_send_full_costmap: false
  footprint_topic: "footprint"
  width: 15
  height: 15
  origin_x: -7.5
  origin_y: -7.5
  resolution: 0.05
  update_frequency: 2.0
  publish_frequency: 2.0

  footprint: [[0.22,0.22],[-0.22,0.22],[-0.22,-0.22],[0.22,-0.22]]
  robot_radius: 0.22
  footprint_padding: 0.0

  static_layer:
    map_topic: map
    first_map_only: false
    subscribe_to_updates: true
    use_maximum: false
    lethal_cost_threshold: 70
    unknown_cost_value: -1
    trinary_costmap: true

  obstacle_layer:
    transform_tolerance: 0.2
    observation_sources: scan_sensor
    scan_sensor: {
      topic: scan,
      sensor_frame: base_scan,
      observation_persistence: 0.0,
      expected_update_rate: 0.0,
      data_type: LaserScan,
      min_obstacle_height: -3.0,
      max_obstacle_height: 3.0,
      inf_is_valid: true,
      clearing: true,
      marking: true
    }
    obstacle_range: 6.0
    raytrace_range: 6.5

  inflation_layer:
    cost_scaling_factor: 10.0
    inflation_radius: 0.88
    inflate_unknown: false

  plugins:
   - {name: static_layer, type: "costmap_2d::StaticLayer"}
   - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
   - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

`

local_costmap.yaml `

local_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  track_unknown_space: false
  always_send_full_costmap: true
  footprint_topic: "footprint"
  width: 3
  height: 3
  origin_x: -1.5
  origin_y: -1.5
  resolution: 0.05
  update_frequency: 5.0
  publish_frequency: 5.0
  rolling_window: true

  footprint: [[0.23,0.23],[-0.23,0.23],[-0.23,-0.23],[0.23,-0.23]]
  robot_radius: 0.23
  footprint_padding: -0.0

  obstacle_layer:
    transform_tolerance: 0.2
    observation_sources: scan_sensor
    scan_sensor: {
      topic: scan,
      sensor_frame: base_scan,
      observation_persistence: 0.0,
      expected_update_rate: 0.0,
      data_type: LaserScan,
      min_obstacle_height: -3.0,
      max_obstacle_height: 3.0,
      inf_is_valid: false,
      clearing: true,
      marking: true
    }
    obstacle_range: 4.0
    raytrace_range: 4.50

  inflation_layer:
    cost_scaling_factor: 2.58
    inflation_radius: 1.75
    inflate_unknown: false

  rgbd_obstacle_layer:
    enabled:                  true
    voxel_decay:              5.0  # seconds if linear, e^n if exponential
    decay_model:              0     # 0=linear, 1=exponential, -1=persistent
    voxel_size:               0.2  # meters
    track_unknown_space:      true  # default space is known
    max_obstacle_height:      3.0   # meters
    unknown_threshold:        15    # voxel height
    mark_threshold:           0     # voxel height
    update_footprint_enabled: true
    combination_method:       1     # 1=max, 0=override
    origin_z:                 0.0   # meters
    publish_voxel_map:        false # default off
    transform_tolerance:      0.2   # seconds
    mapping_mode:             false # default off, saves map not for navigation
    map_save_duration:        60.0  # default 60s, how often to autosave
    observation_sources:      rgbd1_mark rgbd1_clear
    rgbd1_mark:
      data_type: PointCloud2
      topic: /camera/depth/color/points
      marking: true
      clearing: false ...
(more)
edit retag flag offensive close merge delete

Comments

What does memory usage look like when you are running everything? Which processes are using the most CPU?

David Lu gravatar image David Lu  ( 2021-03-01 20:31:24 -0600 )edit

When i'm running everything without locomotor usage is about 60% and 2.2 ram, after running locomotor and sending a goal it jumps to 100% the keeps bouncing from 80-100 and ram is 2.6 (increases by time).

  • Decreasing : linear_granularity & angular_granularity increases processing
  • Increasing : vx_samples & vtheta_samples increases processing

Sometimes the processing power is stable at 70% and the control loop also outputs the warning message tha it missed it's desired rate.

Also i've noticed before that when i'm running cartograpger in mapping mode based on the num of submaps that are stored the processing increases and also the consumed ram, so i close cartographer and restart to debug the issue with Locomotor.

omarwalid96 gravatar image omarwalid96  ( 2021-03-02 06:30:06 -0600 )edit

Ok so i've mad some investigation and found that Xorg process was taking alot of processing power especially when locomotor data like local costmap and path are represented on rviz.

So i used wayland instead of xorg. There was a huge improvement in the processing power.

But after that i tested an noticed that always one core maxes out to 100% while the others are in between 40% ( not a specific core it varies).

So now i get better results but still issues with the same warning message that the control loop is missed

omarwalid96 gravatar image omarwalid96  ( 2021-03-02 16:26:15 -0600 )edit