Nav2 global and local planner conflict
Hello, I'm working with an Ackermann vehicle and using the TEB controller as the local planner (I built it for Foxy) After some tuning, I'm finally getting the vehicle to navigate kind of properly. However, the local planner's path is always different than the global planner's path. This is expected, I assume (but correct me if I'm wrong), but in some complex maneuvers, for example, when involving going in reverse, the path becomes really different and I think the global and local planners are kinda fighting each other. In most cases, the vehicle will make it to the goal but other times it just goes back and forth between the 2 paths and after a while it fails.
Below is an example of a case where the vehicle needs to reverse first to get to the goal. The good thing is that the paths look symmetrical, which means the 2 planners are using similar kinematic constraints. But, regardless, how can I solve this "conflict"?
Here are my Nav2 params:
amcl:
ros__parameters:
use_sim_time: True
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 0.2
base_frame_id: "base_link"
beam_skip_distance: 0.5
beam_skip_error_threshold: 0.9
beam_skip_threshold: 0.3
do_beamskip: false
global_frame_id: "map"
lambda_short: 0.1
laser_likelihood_max_dist: 2.0
laser_max_range: 20.0
laser_min_range: 0.1
laser_model_type: "likelihood_field"
max_beams: 60
max_particles: 2000
min_particles: 500
odom_frame_id: "odom"
pf_err: 0.05
pf_z: 0.99
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
resample_interval: 1
robot_model_type: "differential"
save_pose_rate: 0.5
sigma_hit: 0.2
tf_broadcast: true
transform_tolerance: 1.0
update_min_a: 0.2
update_min_d: 0.25
z_hit: 0.5
z_max: 0.05
z_rand: 0.5
z_short: 0.05
scan_topic: scan
amcl_map_client:
ros__parameters:
use_sim_time: True
amcl_rclcpp_node:
ros__parameters:
use_sim_time: True
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odometry/filtered
enable_groot_monitoring: True
groot_zmq_publisher_port: 1666
groot_zmq_server_port: 1667
# default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
default_bt_xml_filename: "navigate_w_replanning_and_round_robin_recovery"
# default_bt_xml_filename: "follow_point.xml"
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_back_up_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_distance_traveled_condition_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 2.0
min_x_velocity_threshold: 0.005
min_y_velocity_threshold: 0.005
min_theta_velocity_threshold: 0.01
failure_tolerance: 0.3
odom_topic: /odometry/filtered
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.05
movement_time_allowance: 10.0
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.2 # 0.25
yaw_goal_tolerance: 0.1 #0.25
stateful: True
# TEB parameters
# http://wiki.ros.org/teb_local_planner
FollowPath:
plugin: "teb_local_planner::TebLocalPlannerROS"
odom_topic: odom
map_frame: map
# footprint_model.type: circular
# footprint_model.radius: 2.0
footprint_model.type: polygon
footprint_model.vertices: "[ [1.54, 0.405], [1.54, -0.405], [-0.2, -0.405], [-0.2, 0.405] ]"
min_turning_radius: 2.0
wheelbase: 1.35
cmd_angle_instead_rotvel: true # INVESTIGATE MORE!
min_obstacle_dist: 1.0
inflation_dist: 1.0
costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
costmap_converter_spin_thread: True
costmap_converter_rate: 15
enable_homotopy_class_planning: True
enable_multithreading: True
optimization_verbose: False
teb_autoresize: True
min_samples: 3
max_samples: 20
max_global_plan_lookahead_dist: 5.0
visualize_hc_graph: True
max_vel_x: 0.75
max_vel_x_backwards: 0.2
max_vel_y: 0.0 # should be zero for non-holonomic robots
acc_lim_y: 0.05
max_vel_theta: 1.0
acc_lim_x: 0.5
acc_lim_theta: 0.1
yaw_goal_tolerance: 0.2
xy_goal_tolerance: 0.1
include_costmap_obstacles: True
publish_feedback: True # Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging)
costmap_converter:
ros__parameters:
use_sim_time: True
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 10
height: 10
origin_x: -2.0 # Relative to width
# origin_y: 0.0 # Relative to height
origin_z: 0.96
resolution: 0.05
# Length 1.74 m
# Width 0.81 m
footprint: "[ [1.54, 0.405], [1.54, -0.405], [-0.2, -0.405], [-0.2, 0.405] ]"
# robot_radius: 0.22
plugins: ["voxel_layer", "obstacle_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 10.0
inflation_radius: 1.0 #0.55
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: zed_front_pointcloud #??
zed_front_pointcloud: #??
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
obstacle_max_range: 10.0
obstacle_min_range: 0.2
raytrace_max_range: 3.0
raytrace_min_range: 1.0
inf_is_valid: true # CHECK!
# data_type: "PointCloud2"
data_type: "LaserScan"
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 2.0
mark_threshold: 0
observation_sources: front_pointcloud
front_pointcloud:
topic: /scan
max_obstacle_height: 2.0
min_obstacle_height: 0.2
obstacle_max_range: 10.0
obstacle_min_range: 0.2
raytrace_max_range: 3.0
raytrace_min_range: 1.0
clearing: True
marking: True
inf_is_valid: true # CHECK!
# data_type: "PointCloud2"
data_type: "LaserScan"
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
use_sim_time: True
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
# robot_radius: 1.0
# Length 1.74 m
# Width 0.81 m
footprint: "[ [1.54, 0.405], [1.54, -0.405], [-0.2, -0.405], [-0.2, 0.405] ]"
footprint_padding: 0.05
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: zed_front_pointcloud #??
zed_front_pointcloud: #??
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
obstacle_max_range: 10.0
obstacle_min_range: 0.2
raytrace_max_range: 3.0
raytrace_min_range: 1.0
# data_type: "PointCloud2"
data_type: "LaserScan"
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
# map_topic: map
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 10.0
inflation_radius: 4.0
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: True
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
map_saver:
ros__parameters:
use_sim_time: True
save_map_timeout: 5000
free_thresh_default: 0.25
occupied_thresh_default: 0.65
map_subscribe_transient_local: False
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
use_sim_time: True
GridBased:
plugin: "smac_planner/SmacPlanner"
tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node
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)
allow_unknown: true # false # allow traveling in unknown space
max_iterations: -1 # maximum total iterations to search for before failing
max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only
max_planning_time_ms: 2000.0 # max time in ms for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
smooth_path: false # Whether to smooth searched path
motion_model_for_search: "REEDS_SHEPP" # 2D Moore, Von Neumann; SE2 Dubin, Redds-Shepp - Options for SE2: DUBIN, REEDS_SHEPP. 2D: MOORE, VON_NEUMANN
angle_quantization_bins: 72 # For SE2 node: Number of angle bins for search, must be 1 for 2D node (no angle search)
minimum_turning_radius: 2.0 # For SE2 node & smoother: minimum turning radius in m of path / vehicle
reverse_penalty: 1.2 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
change_penalty: 0.20 # For SE2 node: penalty to apply if motion is changing directions, must be >= 0
non_straight_penalty: 1.05 # For SE2 node: penalty to apply if motion is non-straight, must be => 1
cost_penalty: 1.3 # For SE2 node: penalty to apply to higher cost zones
smoother:
smoother:
w_curve: 30.0 # weight to minimize curvature of path
w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight
w_smooth: 30000.0 # weight to maximize smoothness of path
w_cost: 0.025 # weight to steer robot away from collision and cost
cost_scaling_factor: 10.0 # this should match the inflation layer's parameter
# I do not recommend users mess with this unless they're doing production tuning
optimizer:
max_time: 0.10 # maximum compute time for smoother
max_iterations: 500 # max iterations of smoother
debug_optimizer: false # print debug info
gradient_tol: 1.0e-10
fn_tol: 1.0e-20
param_tol: 1.0e-15
advanced:
min_line_search_step_size: 1.0e-20
max_num_line_search_step_size_iterations: 50
line_search_sufficient_function_decrease: 1.0e-20
max_num_line_search_direction_restarts: 10
max_line_search_step_expansion: 50
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: True
recoveries_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["spin", "back_up", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
back_up:
plugin: "nav2_recoveries/BackUp"
wait:
plugin: "nav2_recoveries/Wait"
global_frame: odom
robot_base_frame: base_link
transform_timeout: 0.1
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 0.5
min_rotational_vel: 0.4
rotational_acc_lim: 0.5
robot_state_publisher:
ros__parameters:
use_sim_time: True
Asked by guidout on 2023-06-18 14:44:55 UTC
Answers
You're not seeing "fighting" because there is no feedback between them. The global planner sends things to the local planner and it makes the judgement call about what to do.
Keep in mind TEB predates the Smac Planners by ... a decade or so, so it doesn't really know anything about the nature of the feasible path. Its used to working with outputs from NavFn which are infeasible and TEB's job is to make it feasible by those constraints itself -- so that's what its doing but ignoring Smac's requested maneuvers.
This isn't necessarily a problem unless you want it to exactly follow the Smac Planner's requested direction or TEB is oscillating so often that its not making progress. If you want the robot to always just follow the Smac Plan, you may want to instead just use the RPP controller which will just following the Smac plan exactly and there's no problem because its not a predictive controller that will try to deviate from the path in order to optimize for other criteria.
If you want that predictive behavior in general but not when talking about directional changes, I'd recommend using the MPPI controller that's been released which is a successor to TEB. This will do that for you for the most part currently and I'm working on some branches currently which will provide better enforce doing those path inversions at the same place as the Smac Planner requested very shortly (approx. by the end of the week).
Asked by stevemacenski on 2023-06-19 14:15:30 UTC
Comments