When I set the NAV2GOAL in rviz, the global plan displayed and robot initially starts following it. However just after reaching the goal and when an angular goal is set, sometimes the robot tries to follow the path but mostly becomes clueless and freezes at the position. However,I can see the spin behaviour of front wheels on the same position and backup behaviour.
My github issue for full explanation is Local Planner struggling to follow the global plan #3856. I tried tuning some parameters but still the same situation.
Could someone advise?
Controller and planner server params
ros__parameters:
# controller server parameters (see Controller Server for more info)
use_sim_time: False
controller_frequency: 5.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
# Goal checker parameters
#precise_goal_checker:
# plugin: "nav2_controller::SimpleGoalChecker"
# xy_goal_tolerance: 0.25
# yaw_goal_tolerance: 0.25
# stateful: True
general_goal_checker:
stateful: True
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.1
# DWB parameters
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
min_vel_x: 0.0
min_vel_y: 0.0
max_vel_x: 0.26
max_vel_y: 0.26
max_vel_theta: 1.0
min_speed_xy: 0.0
max_speed_xy: 0.26
min_speed_theta: 0.0
# Add high threshold velocity for turtlebot 3 issue.
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
acc_lim_x: 2.5
acc_lim_y: 2.5
acc_lim_theta: 3.2
decel_lim_x: -2.5
decel_lim_y: -2.5
decel_lim_theta: -3.2
vx_samples: 20
vy_samples: 5
vtheta_samples: 20
sim_time: 1.7
linear_granularity: 0.05
angular_granularity: 0.025
transform_tolerance: 0.2
xy_goal_tolerance: 0.25
trans_stopped_velocity: 0.1
short_circuit_trajectory_evaluation: True
stateful: True
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 0.02
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.1
GoalAlign.scale: 24.0
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
publish_trajectories: true
publish_local_plan: true
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: False
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
use_sim_time: False
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
downsample_costmap: false # whether or not to downsample the map
downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
allow_unknown: true # allow traveling in unknown space
max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution
max_planning_time: 5.0 # max time in s for planner to plan, smooth
motion_model_for_search: "REEDS_SHEPP" # Hybrid-A* Dubin, Redds-Shepp
angle_quantization_bins: 72 # Number of angle bins for search
analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach.
analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
minimum_turning_radius: 1.0 # minimum turning radius in m of path / vehicle
reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1
change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0
non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1
cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
retrospective_penalty: 0.015
lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters.
cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
smooth_path: True # If true, does a simple and quick smoothing post-processing to the path
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1.0e-10
do_refinement: true
refinement_num: 2
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: False
local and global costmap params
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: False
rolling_window: true
width: 5
height: 5
resolution: 0.05
robot_radius: 0.5
plugins: ["obstacle_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 1.0
inflation_radius: 0.55
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 6.0
raytrace_min_range: 0.0
obstacle_max_range: 1.0
obstacle_min_range: 0.0
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
use_sim_time: False
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: False
#global costmap configurations
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: map
robot_base_frame: base_link
use_sim_time: false
robot_radius: 0.5
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 6.0
raytrace_min_range: 0.0
obstacle_max_range: 1.0
obstacle_min_range: 0.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 1.0
inflation_radius: 0.55
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: False
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: False
