Mobile Robot doesn't reach nav goal within goal tolerances in nav2
I have found no resources on this online (or I'm just that bad at searching) so I thought I could get this answered here.
I have a differential drive robot that I am simulating in nvidia's Isaac sim. I'm using the navigation2 stack in order for the mobile robot to navigate its surroundings and pick up objects within the environment. Everything works fine except for one minor detail.
I noticed in testing that the mobile robot does not completely reach the navigation goals that are set for it (i.e. the controller claims to have reached its goal when the base_frame of the robot fails to reach the set goal point).
Obviously this is generally fine and understandable since the controller must prevent the robot from oscillating around the goal point, but after checking positions several times I realized that the robot doesn't even reach the goal within the specified tolerance of the goal checker (currently set at 1mm).
Here is the navigation parameters file:
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: 100.0
laser_min_range: 0.4
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: "nav2_amcl::DifferentialMotionModel"
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
map_topic: map
set_initial_pose: true
always_reset_initial_pose: false
first_map_only: false
initial_pose:
x: 0.0
y: 0.0
z: 0.0
yaw: 0.0
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: /odom
bt_loop_duration: 20
default_server_timeout: 40
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_smooth_path_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node
- nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
- nav2_controller_cancel_bt_node
- nav2_path_longer_on_approach_bt_node
- nav2_wait_cancel_bt_node
- nav2_spin_cancel_bt_node
- nav2_back_up_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: "progress_checker"
goal_checker_plugin: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
# Goal checker parameters
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.001
yaw_goal_tolerance: 0.001
stateful: true
FollowPath:
# Rotation Shim Controller parameters
plugin: "nav2_rotation_shim_controller::RotationShimController"
primary_controller: "nav2_mppi_controller::MPPIController"
angular_dist_threshold: 0.785
forward_sampling_distance: 2.0
rotate_to_heading_angular_vel: 1.8
max_angular_accel: 3.2
simulate_ahead_time: 1.0
# DWB parameters
time_steps: 56
model_dt: 0.05
batch_size: 2000
vx_std: 0.2
vy_std: 0.2
wz_std: 0.4
vx_max: 0.5
vx_min: -0.35
vy_max: 0.5
wz_max: 1.9
iteration_count: 1
prune_distance: 1.7
transform_tolerance: 0.1
temperature: 0.3
gamma: 0.015
motion_model: "DiffDrive"
visualize: false
reset_period: 1.0 # (only in Humble)
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstrains:
min_turning_r: 0.2
critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
ConstraintCritic:
enabled: true
cost_power: 1
cost_weight: 4.0
GoalCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 1.0
GoalAngleCritic:
enabled: true
cost_power: 1
cost_weight: 3.0
threshold_to_consider: 0.4
PreferForwardCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 0.4
ObstaclesCritic:
enabled: true
cost_power: 1
repulsion_weight: 1.5
critical_weight: 20.0
consider_footprint: false
collision_cost: 10000.0
collision_margin_distance: 0.1
near_goal_distance: 0.5
inflation_radius: 0.55 # (only in Humble)
cost_scaling_factor: 10.0 # (only in Humble)
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 3
threshold_to_consider: 0.40
offset_from_furthest: 20
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
offset_from_furthest: 5
threshold_to_consider: 0.6
PathAngleCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
offset_from_furthest: 4
threshold_to_consider: 0.40
max_angle_to_furthest: 1.0
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 10.0
publish_frequency: 10.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: True
width: 10
height: 10
resolution: 0.05
robot_radius: 0.5
plugins: ["obstacle_layer", "inflation_layer", "js_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 0.4
inflation_radius: 0.25
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"
js_layer:
plugin: "jackstand_objective_costmap_plugin::JSLayer"
enabled: true
costmap_resolution: 0.05
target_clear_radius: 0.5
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: 10.0
publish_frequency: 10.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
rolling_window: True
width: 200
height: 200
robot_radius: 0.5
resolution: 0.05
# origin_x: -100.0
# origin_y: -100.0
plugins: ["static_layer", "obstacle_layer", "inflation_layer", "js_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: 10.0
raytrace_min_range: 0.0
obstacle_max_range: 10.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: 3.0
inflation_radius: 0.55
js_layer:
plugin: "jackstand_objective_costmap_plugin::JSLayer"
enabled: true
costmap_resolution: 0.05
target_clear_radius: 0.5
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_server:
ros__parameters:
use_sim_time: True
yaml_filename: "test_map_occupancy_params.yaml"
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: True
planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_theta_star_planner/ThetaStarPlanner"
how_many_corners: 8
w_euc_cost: 1.0
w_traversal_cost: 2.0
w_heuristic_cost: 1.0
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: True
smoother_server:
ros__parameters:
use_sim_time: True
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
tolerance: 1.0e-10
max_its: 1000
do_refinement: True
behavior_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 5.0
behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
plugin: "nav2_behaviors/BackUp"
drive_on_heading:
plugin: "nav2_behaviors/DriveOnHeading"
wait:
plugin: "nav2_behaviors/Wait"
global_frame: odom
robot_base_frame: base_link
transform_tolerance: 0.2
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
robot_state_publisher:
ros__parameters:
use_sim_time: True
waypoint_follower:
ros__parameters:
loop_rate: 20
stop_on_failure: false
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
enabled: True
waypoint_pause_duration: 200
I have messed with the settings for hours now and have seen no change in the behavior of the mobile robot. Setting the goal tolerances lower than the default doesn't change anything with respect to how far the robot stops away from its goal.
Any help with dealing with this is appreciated.
Asked by JSLbotbuilder on 2023-06-14 15:32:09 UTC
Answers
So update,
The problem is being caused by the fact that, for whatever reason, the nav2 stack is basically ignoring the goal tolerances you set within the parameters file, which causes it to use the default 0.25 m for positional and angular tolerances. You can actually verify this by calling the get_parameters service for the controller_server (you can list all services with "ros2 service list" if your having trouble with the service name).
There is a hack around this problem, which is to just call the set_parameters service for the controller_server from the terminal or from a client within a node and set the values of the goal tolerance fields to whatever you want. This is obviously not ideal, but it is the only way I've been able to "solve" the issue.
Also, if you're looking for better answers to solve this problem, I wouldn't look here. Unfortunately, not many people answer questions on this site.
Asked by JSLbotbuilder on 2023-08-07 15:49:32 UTC
Comments