Locomotor high cpu usage

Hi,

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

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 10.0000Hz... the loop actually took 1.2971 seconds)
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
`
```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
  obstacle_range: 4.0          # meters
  min_obstacle_height: 0.2     # default 0, meters
  max_obstacle_height: 3.0     # default 3, meters
  expected_update_rate: 0.0    # default 0, if not updating at this rate at least, remove from buffer
  observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest
  inf_is_valid: false          # default false, for laser scans
  filter: "passthrough"        # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on 
  voxel_min_points: 0          # default 0, minimum points per voxel for voxel filter
  clear_after_reading: true    # default false, clear the buffer after the layer gets readings from it
rgbd1_clear:
  data_type: PointCloud2
  topic: /camera/depth/color/points
  marking: false
  clearing: true
  max_z: 7.0                  # default 0, meters
  min_z: 0.1                  # default 10, meters
  vertical_fov_angle: 0.8745  # default 0.7, radians
  horizontal_fov_angle: 1.048 # default 1.04, radians
  decay_acceleration: 5.0     # default 0, 1/s^2. If laser scanner MUST be 0
  model_type: 0                # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar

plugins:

{name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
{name: inflation_layer, type: "costmap_2d::InflationLayer"}
{name: rgbd_obstacle_layer, type: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"}

`

- planners.yaml
`
```yaml
DluxGlobalPlanner:
  neutral_cost: 45
  scale: 3.0
  unknown_interpretation: lethal
  path_caching: false
  improvement_threshold: -1.0
  publish_potential: false
  print_statistics: false
  potential_calculator: dlux_plugins::Dijkstra
  traceback: dlux_plugins::GradientPath

  # AStar
  manhattan_heuristic: false
  use_kernel: true
  minimum_requeue_change: 1.0

  # GradientPath
  step_size: 0.05
  lethal_cost: 255.0
  iteration_factor: 4.0
  grid_step_near_high: false

DWBLocalPlanner:
  update_costmap_before_planning: true
  prune_plan: true
  prune_distance: 1.0
  short_circuit_trajectory_evaluation: true
  debug_trajectory_details: true
  publish_cost_grid_pc: true

  trajectory_generator_name: dwb_plugins::StandardTrajectoryGenerator
  # velocities
  min_vel_x: 0.0
  max_vel_x: 0.42
  min_vel_y: 0.0
  max_vel_y: 0.0
  max_vel_theta: 2.2
  # acceleration
  acc_lim_x: 3.88 
  acc_lim_y: 0.0
  acc_lim_theta: 8.7 
  # deceleration
  decel_lim_x: -4.2 
  decel_lim_y: 0.0
  decel_lim_theta: -10.7 #-3.5
  # absolute speeds (in either direction)
  min_speed_xy: 0.0
  max_speed_xy: 0.42
  min_speed_theta: 0.0

  goal_checker_name: dwb_plugins::SimpleGoalChecker
  xy_goal_tolerance: 0.2
  yaw_goal_tolerance: 0.5

  vx_samples: 10
  vtheta_samples: 20
  sim_time: 2.5
  use_sim_time: False
  debug_trajectory_details: True

  linear_granularity: 0.25
  angular_granularity: 0.25
   critics:
  # - PreferForward
   - RotateToGoal
  - Oscillation
   # - GoalAlign
   # - PathAlign
   - PathDist
   - GoalDist
   - ObstacleFootprint

  footprint: [[0.23,0.23],[-0.23,0.23],[-0.23,-0.23],[0.23,-0.23]]

   ObstacleFootprint:  
     scale: 0.01  
     max_scaling_factor: 0.2  
     scaling_speed: 0.25

  # # PreferForward critic
  # PreferForward:
  #   backward_cost: 1.0
  #   forward_cost: 0.0
  #   scale: 0.0

  # # RotateToGoal critic
  RotateToGoal:
     xy_goal_tolerance: 0.2
     trans_stopped_velocity: 0.1
     slowing_factor: 5.0
     lookahead_time: -1.0
     scale: 5.0

  # # Oscillation critic
   Oscillation:
    x_only_threshold: 0.05
    oscillation_reset_dist: 0.05
     oscillation_reset_angle: 0.2
     oscillation_reset_time: -1.0
     scale: 0.05
   PathDist:  
     scale: 32.0 
   GoalDist:  
     scale: 24.0  
  # PathAlign:  
  #   scale: 0.0  
  #   forward_point_distance: 0.325  
  # GoalAlign:  
  #   scale: 0.0

Please @DLu if you can help out or give A simple guide of how to get it working better tell me. Thanks in advanced.

Also here is the output after running these yaml files on single thread locomotor

` [ INFO] [1614252590.606480311, 2526.479000000]: Loading Global Costmap nav_core_adapter::CostmapAdapter [ INFO] [1614252590.615748742, 2526.482000000]: Initializing Global Costmap [ WARN] [1614252591.076588863, 2526.568000000]: global_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided [ INFO] [1614252591.079119293, 2526.568000000]: global_costmap: Using plugin "static_layer" [ INFO] [1614252591.171387035, 2526.589000000]: Requesting the map... [ INFO] [1614252591.190760756, 2526.595000000]: odom received! [ INFO] [1614252591.551259043, 2526.695000000]: Resizing costmap to 384 X 384 at 0.050000 m/pix [ INFO] [1614252591.914400616, 2526.795000000]: Received a 384 X 384 map at 0.050000 m/pix [ INFO] [1614252591.914453211, 2526.795000000]: Subscribing to updates [ INFO] [1614252591.932839267, 2526.799000000]: global_costmap: Using plugin "obstacle_layer" [ INFO] [1614252591.953351716, 2526.801000000]: Subscribed to Topics: scan_sensor [ INFO] [1614252592.021757818, 2526.814000000]: global_costmap: Using plugin "inflation_layer" [ INFO] [1614252592.202923476, 2526.848000000]: Initializing global planner DluxGlobalPlanner [ INFO] [1614252592.219289554, 2526.851000000]: Using PotentialCalculator "dlux_plugins::Dijkstra" [ INFO] [1614252592.228079217, 2526.852000000]: Using Traceback "dlux_plugins::GradientPath" [ INFO] [1614252592.269000524, 2526.858000000]: Loading Local Costmap nav_core_adapter::CostmapAdapter [ INFO] [1614252592.269132721, 2526.858000000]: Initializing Local Costmap [ INFO] [1614252592.302623445, 2526.874000000]: local_costmap: Using plugin "obstacle_layer" [ INFO] [1614252592.335038595, 2526.886000000]: Subscribed to Topics: scan_sensor [ INFO] [1614252592.465927155, 2526.918000000]: local_costmap: Using plugin "inflation_layer" [ INFO] [1614252592.522880762, 2526.935000000]: local_costmap: Using plugin "rgbd_obstacle_layer" [ INFO] [1614252593.833541136, 2527.314000000]: local_costmap/rgbd_obstacle_layer being initialized as SpatioTemporalVoxelLayer! [ INFO] [1614252593.835184635, 2527.315000000]: local_costmap/rgbd_obstacle_layer's global frame is map. [ INFO] [1614252593.910852827, 2527.339000000]: local_costmap/rgbd_obstacle_layer loaded parameters from parameter server. [ INFO] [1614252593.924620643, 2527.341000000]: local_costmap/rgbd_obstacle_layer created underlying voxel grid. [ INFO] [1614252594.220034589, 2527.430000000]: local_costmap/rgbd_obstacle_layer initialization complete! [ INFO] [1614252594.444322534, 2527.508000000]: Initializing local planner DWBLocalPlanner [ INFO] [1614252594.493997430, 2527.530000000]: Using Trajectory Generator "dwb_plugins::StandardTrajectoryGenerator" [ INFO] [1614252594.693522635, 2527.604000000]: Using Goal Checker "dwb_plugins::SimpleGoalChecker" [ INFO] [1614252594.708579461, 2527.607000000]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic) [ INFO] [1614252594.728087212, 2527.612000000]: Using critic "Oscillation" (dwb_critics::OscillationCritic) [ INFO] [1614252594.739595864, 2527.616000000]: Using critic "ObstacleFootprint" (dwb_critics::ObstacleFootprintCritic) [ INFO] [1614252594.749518133, 2527.620000000]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic) [ INFO] [1614252594.769192692, 2527.624000000]: Using critic "PathAlign" (dwb_critics::PathAlignCritic) [ INFO] [1614252594.783321823, 2527.625000000]: Using critic "PathDist" (dwb_critics::PathDistCritic) [ INFO] [1614252594.789528615, 2527.628000000]: Using critic "GoalDist" (dwb_critics::GoalDistCritic)

locusrobotics / robot_navigation

Locomotor high cpu usage #85