Ask Your Question

Jerky motion for navigation from low CPU usage

asked 2021-09-14 19:48:25 -0500

pete1451 gravatar image

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

  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

  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 ...
edit retag flag offensive close merge delete


Built navigation from source? Did you enable compiler optimisations?

There are quite a few Q&As here on ROS Answers about navigation on Jetsons or Xaviers and warnings about control loop rates. Use Google and append to your query to find them.

gvdhoorn gravatar image gvdhoorn  ( 2021-09-15 02:18:51 -0500 )edit

Yes I built it from source, I used -DCMAKE_BUILD_TYPE=Release argument when compiled to ensure maximum speed. From what I have seen with googling similar problem, most of the posted problem comes from insufficient CPU or processing power. But this is not the case for me where I am only using around 50% CPU usage according to htop average cpu in MAXN mode.

pete1451 gravatar image pete1451  ( 2021-09-15 13:41:23 -0500 )edit

You should monitor your cmd_vel topic and make sure messages are being published every 200 milliseconds. If they are not, this could explain a lot of the jerky motion.

There are many things that can cause cmd_vel to not be published, including slow or missing inputs to the local planner. I would turn off planner_retries and recovery_behavior while debugging a problem like this, because they make it difficult to identify the real failure that is happening.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-09-15 19:02:42 -0500 )edit

I forgot to set Jetson Xavier to use its maximum clock speed by runningjetson_clocks command into the terminal. Now that I did DWA is able to maintain 5 Hz majority of the time, however TEB is still unable to maintain 5 Hz consistently. I wonder if it has something to do with Nvidia Xavier AGX because I have heard that people can run TEB on Jetson TX2 at 10 Hz just fine. The robot is still exhibiting clunky movement but I don't think that's a hardware issue anymore. I still need to figure why it perform so much worse than what other people can do on TX2.

pete1451 gravatar image pete1451  ( 2021-09-21 22:39:47 -0500 )edit

i met the same problem,do you figure it out?

smiley gravatar image smiley  ( 2022-04-14 09:24:53 -0500 )edit

2 Answers

Sort by » oldest newest most voted

answered 2021-10-06 22:07:21 -0500

JohnTGZ gravatar image

Hi, you can consider setting the "enable_homotopy_class_planning" parameter to be false too. This is known to cause significant problems with the control loop frequency. According to the ROS documentation for TEB:

Activate homotopy class planning (Requires much more resources that simple planning, since multiple trajectories are optimized at once).

Although this might disable one of the main purposes of using TEB, which allows you to plan multiple trajectories simultaneously, it is sure to increase your control loop frequency to a decent level.

A few other parameters you can consider tuning are the "no_inner_iterations", "no_outer_iterations" which will reduce the number of solver iterations, possibly leading to less optimized paths but will free up CPU resources nevertheless.

edit flag offensive delete link more

answered 2021-09-24 15:30:17 -0500

pete1451 gravatar image

It appears that if you set include_dynamic_obstacles to false (which is a default value for Kinetic), it causes the planner to slow down significantly. Once I set it to true, TEB is able to run at a solid 5 Hz without issue (at least so far). It's strange that TEB would slow down considering setting include_dynamic_obstacles to true should increase its computation requirement.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2021-09-14 19:48:25 -0500

Seen: 180 times

Last updated: Oct 06 '21