Jerky motion for navigation from low CPU usage
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 ...
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
site:answers.ros.org
to your query to find them.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.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.
I forgot to set Jetson Xavier to use its maximum clock speed by running
jetson_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.i met the same problem,do you figure it out?