DWB local planner and locomotor High cpu usage

edit

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 ...