Nav2 BT not activating collision checker, robot always colide with obstacles even static ones.
I'm trying to run a navigation with nav2 on a simulation on ignition gazebo with an ackermann robot type and been having some problems with collision avoidance. I send a goal msg on a navigation with a static map and if it's too close to the walls the robot collide with it anyway, not activating the collision checker on the BT as i could see from groot. Maybe there is something missing in my bt but i used the "navigate_to_pose_w_replanning_and_recovery.xml" from the nav2_bt. If i use footprint or radius for my robot marking it is still the same. I'm using DWB, but tried with regulated pure pursuit and had the same problem. I already tried to up the rate from 1hz of my bt controller but nothing. I'm using smacplannerhybrid as my planner as you can see from my nav2_params.yaml, any suggestions will be welcomed.
Required Info:
Operating System: Ubuntu 20.04 ROS2 Version: ROS2 Galactic binaries Version or commit hash: ros-galactic-navigation2 1.0.12-1focal.20220526.142755 DDS implementation: Eclipse’s Cyclone DDS Steps to reproduce issue Run a simulation with an ackermann robot, set initial pose on a static pre built map and try to navigate around it. Try to get to close to one of the walls or obstacles in your map and see what happens.
Expected behavior Robot can replan paths to avoid colide with obstacles or stop if collision is eminent.
Actual behavior Robot colide with obstacles or walls in the way of the path.
Robot collides with footprint:
https://freeimage.host/i/hwWdHG
Robot collides with radius:
https://freeimage.host/i/hwWFDl
My Behaviour Tree from Groot:
https://freeimage.host/i/hwWqVS
My nav2_params.yaml:
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: "prius_hybrid/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: 50.0
laser_min_range: -1.0
laser_model_type: "likelihood_field"
max_beams: 60
max_particles: 2000
min_particles: 500
odom_frame_id: "prius_hybrid/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: model/prius_hybrid/laserscan/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: prius_hybrid/base_link
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
enable_groot_monitoring: True
groot_zmq_publisher_port: 1666
groot_zmq_server_port: 1667
#default_nav_to_pose_bt_xml: "/opt/ros/galactic/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
#default_nav_through_poses_bt_xml: "/opt/ros/galactic/share/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml"
#bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
bt_xml_filename: "navigate_to_pose_w_replanning_and_recovery.xml"
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_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
- 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
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 ...
Can you show a video of the starting pose, the plans, and such that get you into such a state? An image you've posted shows a plan which shouldn't be valid with the footprint and map you're in and that makes me a little suspicious. Is that footprint the global costmap's footprint or the local costmap's footprint?
I put the footprint on both. I've managed to get the it not colide with the regulated pure pursuit, it stops right on the verge of hitting a wall or a person (actor), but it still doesnt replan the path. The msg i get in the terminal is that [planner_server-13] [WARN] [1655476453.216305143] [planner_server]: GridBased: failed to create plan, invalid use: Failed to compute path, goal is occupied with no tolerance..
I also tried TEB, but had no success. It doesnt hit walls but it fails to compute a path almost everytime. I tried to tune its parameters for a while but i think RPPursuit it's the way to go. Btw i'm using an ackermann robot model.
EDIT1: Maybe i have to do something with my behaviour tree? but it's strange since it call's navigate_to_pose_w_replanning_and_recovery.xml, but from what i can see with groot it ...(more)