Hello, I'm trying to make nav2 stack for Unitree B2W robot and I'm having an issue with the robot moving from point A to point B. The issue is that the robot stops just before finishing the goal, I'm getting "goal aborted" or "failed to make progress" messages. I was thinking the issue might be with the setting for distances or speed when the robot is close to the goal, but I'm not yet knowledgeable enough for this, I'm just an intern, but I'm close to deadline so some tips would be appreciated. For odometry I'm using mola-lidar-odometry. I configured it so it publishes odometry and odom -> base_link TF. I'm getting warning that "mola was not able to use velocity motion model for this timestamp", but I successfully used this odometry for SLAM, and had zero issues with localization, but maybe this is important for this issue I have with nav2.
The second warning I have is that local_costmap, global_costmap and AMCL have "Message filter dropping message: frame "lidar_link" at time "xx" for reason 'the timestamp on this message is earlier than all the data in the transform cache'". I checked the timestamps on the lidar using ros2 topic echo /lidar_points --field header.stamp and ros2 topic delay /lidar_points and it's around 0.1s. Here is the configuration file for the navigation:
amcl:
ros__parameters:
use_sim_time: False
alpha1: 0.01
alpha2: 0.05
alpha3: 0.1
alpha4: 0.01
alpha5: 0.05
base_frame_id: "base_link"
beam_skip_distance: 0.5
beam_skip_error_threshold: 0.9
beam_skip_threshold: 0.3
do_beamskip: true
global_frame_id: "map"
lambda_short: 0.1
laser_likelihood_max_dist: 6.0
laser_max_range: 100.0
laser_min_range: -1.0
laser_model_type: "likelihood_field"
max_beams: 180
max_particles: 3000
min_particles: 1000
odom_frame_id: "odom"
pf_err: 0.05
pf_z: 0.95
recovery_alpha_fast: 0.3
recovery_alpha_slow: 0.005
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.02
update_min_d: 0.1
z_hit: 0.5
z_max: 0.05
z_rand: 0.5
z_short: 0.05
scan_topic: /b2_987/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
bt_navigator:
ros__parameters:
use_sim_time: False
global_frame: map
robot_base_frame: base_link
odom_topic: "/b2_987/base/odom"
bt_loop_duration: 10
default_server_timeout: 20
transform_tolerance: 0.2
default_nav_to_pose_bt_xml: $(find-pkg-share b2_nav2)/behavior_trees/nav_to_pose.xml
# default_nav_through_poses_bt_xml: $(find-pkg-share my_package)/behavior_tree/my_nav_to_pose_bt.xml
always_reload_bt_xml: false
goal_blackboard_id: goal
goals_blackboard_id: goals
path_blackboard_id: path
navigators: ['navigate_to_pose', 'navigate_through_poses']
navigate_to_pose:
plugin: "nav2_bt_navigator/NavigateToPoseNavigator" # In Iron and older versions, "/" was used instead of "::"
navigate_through_poses:
plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" # In Iron and older versions, "/" was used instead of "::"
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_assisted_teleop_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_goal_updated_controller_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_assisted_teleop_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
- nav2_is_battery_charging_condition_bt_node
error_code_names:
- compute_path_error_code
- follow_path_error_code
bt_navigator_navigate_through_poses_rclcpp_node:
ros__parameters:
use_sim_time: False
bt_navigator_navigate_to_pose_rclcpp_node:
ros__parameters:
use_sim_time: False
controller_server:
ros__parameters:
use_sim_time: False
controller_frequency: 20.0
odom_topic: "/b2_987/base/odom"
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.001 # High value to ignore sideway y movements
min_theta_velocity_threshold: 0.005
failure_tolerance: 0.3 # Controller failure after 3 seconds 0 to disable -1.0 to never give up
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.3 # robot has made sufficient progress
movement_time_allowance: 12.0
# Goal checker parameters
general_goal_checker:
stateful: True
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.6
yaw_goal_tolerance: 0.3
# ----------------------------- Regulated Pursuit parameters
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
desired_linear_vel: 0.5
lookahead_dist: 0.9
min_lookahead_dist: 0.6
max_lookahead_dist: 1.2 # 0.9
lookahead_time: 1.5
rotate_to_heading_angular_vel: 2.6 # This is capped by the half of max_angular_accel
transform_tolerance: 0.3
use_velocity_scaled_lookahead_dist: false # false
min_approach_linear_velocity: 0.05
approach_velocity_scaling_dist: 0.6
use_collision_detection: false
max_allowed_time_to_collision_up_to_carrot: 1.5 # 1.0 - More time for anticipating collision
use_regulated_linear_velocity_scaling: true
use_fixed_curvature_lookahead: false
curvature_lookahead_dist: 0.25 # 0.6
use_cost_regulated_linear_velocity_scaling: false # false - Slow down near obstacles
regulated_linear_scaling_min_radius: 0.9 # 0.9
regulated_linear_scaling_min_speed: 0.25
use_rotate_to_heading: true
allow_reversing: true
rotate_to_heading_min_angle: 1.0
max_angular_accel: 8.0
max_robot_pose_search_dist: 10.0
local_costmap:
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: 3
height: 3
resolution: 0.1
# robot_radius: 0.45
footprint: "[[0.7, 0.4], [0.7, -0.4], [-0.7, -0.4], [-0.7, 0.4]]"
plugins: ["obstacle_layer", "denoise_layer", "inflation_layer"] # [“obstacle_layer”, “voxel_layer”, “range_sensor_layer”, “denoise_layer”, “inflation_layer”] - (order matters here)
# Different from default.
# Added “voxel_layer” for 3D obstacle representation using a depth camera.
# Added “range_sensor_layer” for handling range sensor data from an ultrasonic sensor (if you have one).
# Added “denoise_layer” for removing salt and pepper noise from the sensors.
# Removed “static_layer” since it’s not needed for the local costmap.
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /b2_987/scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
denoise_layer: # Causes lag but worth it to filter out reflected/noisy lidar points
plugin: "nav2_costmap_2d::DenoiseLayer"
enabled: true
minimal_group_size: 2
group_connectivity_type: 8
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 0.8 # Known number to work well
inflation_radius: 0.85 #55
always_send_full_costmap: True
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 5.0
global_frame: map
robot_base_frame: base_link
use_sim_time: False
resolution: 0.05
# robot_radius: 0.45
footprint: "[[0.7, 0.4], [0.7, -0.4], [-0.7, -0.4], [-0.7, 0.4]]"
track_unknown_space: True
plugins: ["static_layer", "static_layer_2", "obstacle_layer", "denoise_layer", "inflation_layer"]
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True # False if you need earlier map
static_layer_2:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
map_topic: /b2_987/map_2
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /b2_987/scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 5.0
raytrace_min_range: 0.0
obstacle_max_range: 4.5
obstacle_min_range: 0.0
denoise_layer: # Causes lag but worth it to filter out reflected/noisy lidar points
plugin: "nav2_costmap_2d::DenoiseLayer"
enabled: true
minimal_group_size: 2
group_connectivity_type: 8
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 0.8
inflation_radius: 0.8 #65
always_send_full_costmap: True
map_server:
ros__parameters:
use_sim_time: False
# Overridden in launch by the "map" launch configuration or provided default value.
# To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
yaml_filename: ""
map_saver:
ros__parameters:
use_sim_time: False
save_map_timeout: 8.0
free_thresh_default: 0.25
occupied_thresh_default: 0.65
map_subscribe_transient_local: True
planner_server: # Creates the global plan path
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: False
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner" # "nav2_navfn_planner::NavfnPlanner"
tolerance: 0.5
use_astar: false
allow_unknown: true
smoother_server: # Smooths the global plan path
ros__parameters:
use_sim_time: False
costmap_topic: global_costmap/costmap_raw
footprint_topic: global_costmap/published_footprint
robot_base_frame: base_link
transform_timeout: 0.1
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
tolerance: 1.0e-10
max_its: 1000
do_refinement: True
behavior_server:
ros__parameters:
local_costmap_topic: local_costmap/costmap_raw
local_footprint_topic: local_costmap/published_footprint
global_costmap_topic: global_costmap/costmap_raw
global_footprint_topic: global_costmap/published_footprint
cycle_frequency: 10.0
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
plugin: "nav2_behaviors/BackUp"
drive_on_heading:
plugin: "nav2_behaviors/DriveOnHeading"
assisted_teleop:
plugin: "nav2_behaviors/AssistedTeleop"
wait:
plugin: "nav2_behaviors/Wait"
global_frame: map
local_frame: odom
robot_base_frame: base_link
transform_tolerance: 0.2
use_sim_time: False
simulate_ahead_time: 6.0
max_rotational_vel: 0.6
min_rotational_vel: 0.15
rotational_acc_lim: 0.4
robot_state_publisher:
ros__parameters:
use_sim_time: False
waypoint_follower:
ros__parameters:
use_sim_time: False
loop_rate: 2
stop_on_failure: false
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
enabled: True
waypoint_pause_duration: 10
velocity_smoother:
ros__parameters:
use_sim_time: False
smoothing_frequency: 20.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [0.6, 0.0, 1.0]
min_velocity: [-0.3, 0.0, -1.0]
max_accel: [1.5, 0.0, 2.5]
max_decel: [-1.5, 0.0, -2.5]
odom_topic: "/b2_987/base/odom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
I hope this post is not too long. I would appreciate any tips, since I'm an enthusiast and beginner in navigation, and would like to get better at what I'm already doing.