search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.5k stars 1.27k forks source link

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

Closed Marcelomm103 closed 2 years ago

Marcelomm103 commented 2 years ago

Bug report

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:

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:

Screenshot from 2022-06-15 14-31-13

Robot collides with radius:

Screenshot from 2022-06-15 14-36-16

My Behaviour Tree from Groot:

Screenshot from 2022-06-15 14-31-56

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: 0.3 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.5
  movement_time_allowance: 10.0
# Goal checker parameters
#precise_goal_checker:
#  plugin: "nav2_controller::SimpleGoalChecker"
#  xy_goal_tolerance: 0.25
#  yaw_goal_tolerance: 0.25
#  stateful: True
general_goal_checker:
  stateful: True
  plugin: "nav2_controller::SimpleGoalChecker"
  xy_goal_tolerance: 0.25
  yaw_goal_tolerance: 0.25
FollowPath:
  plugin: "dwb_core::DWBLocalPlanner"
  debug_trajectory_details: True
  min_vel_x: -1.26
  min_vel_y: 0.0
  max_vel_x: 10.0 # 1.26
  max_vel_y: 0.0
  max_vel_theta: 3.0 # 1.0
  min_speed_xy: -1.26
  max_speed_xy: 10.0 # 1.26
  min_speed_theta: 0.0
  # Add high threshold velocity for turtlebot 3 issue.
  # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
  acc_lim_x: 2.5
  acc_lim_y: 0.0
  acc_lim_theta: 3.2
  decel_lim_x: -2.5
  decel_lim_y: 0.0
  decel_lim_theta: -3.2
  vx_samples: 20
  vy_samples: 5
  vtheta_samples: 20
  sim_time: 0.5 # 1.7
  linear_granularity: 0.05
  angular_granularity: 0.025
  transform_tolerance: 0.2
  xy_goal_tolerance: 0.25
  trans_stopped_velocity: 0.25
  short_circuit_trajectory_evaluation: True
  stateful: True
  critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
  BaseObstacle.scale: 0.02
  PathAlign.scale: 16.0 # 32.0
  PathAlign.forward_point_distance: 0.1
  GoalAlign.scale: 12.0 # 24.0
  GoalAlign.forward_point_distance: 0.1
  PathDist.scale: 16.0 # 32.0
  GoalDist.scale: 12.0 # 24.0
  RotateToGoal.scale: 32.0
  RotateToGoal.slowing_factor: 5.0
  RotateToGoal.lookahead_time: -1.0

controller_server_rclcpp_node: ros__parameters: use_sim_time: True

local_costmap: local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: prius_hybrid/odom robot_base_frame: prius_hybrid/base_link use_sim_time: True rolling_window: true width: 6 # 3 height: 6 # 3 resolution: 0.10 robot_radius: 2.0

footprint: "[ [2.43, 1.3], [2.43, -1.3], [-2.43, -1.3], [-2.43, 1.3] ]"

  plugins: ["voxel_layer", "obstacle_layer", "inflation_layer"]
  obstacle_layer:
    plugin: "nav2_costmap_2d::ObstacleLayer"
    enabled: True
    observation_sources: scan
    scan:
      topic: /model/prius_hybrid/laserscan/points
      max_obstacle_height: 2.0
      clearing: True
      marking: True
      data_type: "PointCloud2"
      raytrace_max_range: 5.0 # 3.0
      raytrace_min_range: 0.0
      obstacle_max_range: 3.0 # 2.5
      obstacle_min_range: 0.0
  inflation_layer:
    plugin: "nav2_costmap_2d::InflationLayer"
    cost_scaling_factor: 3.0
    inflation_radius: 1.0 # 0.55
  voxel_layer:
    plugin: "nav2_costmap_2d::VoxelLayer"
    enabled: True
    publish_voxel_map: True
    origin_z: 0.0
    z_resolution: 0.20 # 0.10
    z_voxels: 16
    max_obstacle_height: 2.0
    mark_threshold: 0
    observation_sources: scan
    scan:
      topic: /model/prius_hybrid/laserscan/points
      max_obstacle_height: 2.0
      clearing: True
      marking: True
      data_type: "PointCloud2"
      raytrace_max_range: 6.0 # 3.0
      raytrace_min_range: 0.0
      obstacle_max_range: 5.0 # 2.5
      obstacle_min_range: 0.0
  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: prius_hybrid/base_link use_sim_time: True robot_radius: 2.0

footprint: "[ [2.43, 1.3], [2.43, -1.3], [-2.43, -1.3], [-2.43, 1.3] ]"

  resolution: 0.10
  track_unknown_space: true
  plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
  obstacle_layer:
    plugin: "nav2_costmap_2d::ObstacleLayer"
    enabled: True
    observation_sources: scan
    scan:
      topic: /model/prius_hybrid/laserscan/points
      max_obstacle_height: 2.0
      clearing: True
      marking: True
      data_type: "PointCloud2"
      raytrace_max_range: 3.0 # 3.0
      raytrace_min_range: 0.0
      obstacle_max_range: 2.5 # 2.5
      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: 1.0 # 0.55
  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: "sonoma2.yaml"

map_saver: ros__parameters: use_sim_time: True save_map_timeout: 5.0 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_smac_planner/SmacPlannerHybrid" 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 # allow traveling in unknown space max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable max_planning_time: 5.0 # max time in s for planner to plan, smooth motion_model_for_search: "REEDS_SHEPP" # "DUBIN" # Hybrid-A* Dubin, Redds-Shepp angle_quantization_bins: 72 # Number of angle bins for search analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle reverse_penalty: 1.0 # Penalty to apply if motion is reversing, must be => 1 change_penalty: 0.0 # 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. retrospective_penalty: 0.015 lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. smooth_path: True # If true, does a simple and quick smoothing post-processing to the path

  smoother:
    max_iterations: 1000
    w_smooth: 0.3
    w_data: 0.2
    tolerance: 1e-10
    do_refinement: true

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", "backup", "wait"] spin: plugin: "nav2_recoveries/Spin" backup: plugin: "nav2_recoveries/BackUp" wait: plugin: "nav2_recoveries/Wait" global_frame: prius_hybrid/odom robot_base_frame: prius_hybrid/base_link transform_timeout: 0.1 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`

SteveMacenski commented 2 years ago

Please ask on ROS Answers, this isn't the right place for configuration advice