Jerky motion for navigation from low CPU usage

edit

I am running ros-melodic on Nvidia Xavier AGX on a custom skid/steer (tracked) robot. I am having a difficult time getting the navigation to work on it.

I am able to produce map and global path, but my local planner produces jerky velocity commands (moving and stopping in open area). I have tried both DWA and TEB with the same jerky motion result. The custom robot is working as expected (receiving and sending messaging correctly) and when everything is running, Nvidia is only at ~50% CPU usage, therefore I don't believe it is the hardware.

One thing that concern me is the map/control missing its target rate warning I would get sometimes when giving a goal for the robot on rviz. I get these warning often when using TEB, and very rarely on DWA.

Control loop missed its desired rate of 5.0000Hz... the loop actually took 5.7060 seconds
Map update loop missed its desired rate of 1.0000Hz... the loop actually took 12.7856 seconds

I am not unfamiliar with these warnings, but I am confused by why it is missing its target rate exclusively when planning trajectory even though Nvidia is only using ~50% CPU. How come local planner or ROS is not drawing more resources, considering there are plenty left to use?

Both DWA and Teb is using odometry from robot_localization package publishing at 10 Hz

Move_base parameter

base_global_planner: global_planner/GlobalPlanner #navfn/NavfnROS
base_local_planner: teb_local_planner/TebLocalPlannerROS #dwa_local_planner/DWAPlannerROS
controller_frequency: 5  # hz
max_planning_retries: 50
planner_frequency: 1.0
planner_patience: 5.0
clearing_rotation_allowed: false
recovery_behavior_enabled: true
oscillation_timeout: 10
oscillation_distance: 0.5 # m

This is my Teb local planner parameters

TebLocalPlannerROS:
  odom_topic: /odometry/filtered
  map_frame: /odom

 # Trajectory

 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 5

 # Robot

 max_vel_x: 0.4
 max_vel_x_backwards: 0.2
 max_vel_theta: 0.3
 acc_lim_x: 0.5
 acc_lim_theta: 0.5
 min_turning_radius: 0.0
 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   type: "circular"
   radius: 0.35 # for type "circular"



# GoalTolerance

 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False

 # Obstacles

 min_obstacle_dist: 0.25
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 30
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization

 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 2

 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 alternative_time_cost: False # not in use yet

 # Homotopy Class Planner

 enable_homotopy_class_planning: True
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 4
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_keypoint_offset: 0.1
 obstacle_heading_threshold: 0.45
 visualize_hc_graph: False

This is my DWA parameters

DWAPlannerROS:
  acc_lim_x: 0.5
  acc_lim_y: 0
  acc_lim_th: 0.5

  max_vel_x: 1.0
  min_vel_x: 0.0
  max_vel_y: 0
  min_vel_y: 0

  max_vel_trans: 1.0
  min_vel_trans: 0.1
  max_vel_rot: 5.0
  min_vel_rot: 1.0

  # Goal Tolerance
  yaw_goal_tolerance: 0.1
  xy_goal_tolerance: 0.2

  latch_xy_goal_tolerance: false

  # Simulation
  sim_time: 1.7 # sec
  sim_granularity: 0.025 # m
  vx_samples: 20
  vy_samples: 1
  vth_samples: 40

  # Trajectory scoring
  path_distance_bias: 50.0 # 30.0
  goal_distance_bias: 0.0 # 1.0 10.0 ...