Nav2 BT not activating collision checker, robot always colide with obstacles even static ones.

asked 2022-06-15 14:12:49 -0500

marcelomm103 gravatar image

updated 2022-06-17 10:59:47 -0500

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:

Robot collides with radius:

My Behaviour Tree from Groot:

My nav2_params.yaml:

    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

    use_sim_time: True

    use_sim_time: True

    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"
      - 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

    use_sim_time: True

    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 ...
edit retag flag offensive close merge delete


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?

stevemacenski gravatar image stevemacenski  ( 2022-06-15 17:36:14 -0500 )edit

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)

marcelomm103 gravatar image marcelomm103  ( 2022-06-17 09:52:45 -0500 )edit