search

ros-navigation / navigation2

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

NavfnPlanner randomly fails for a clearly reachable goal saying "Failed to create a plan from potential when a legal .... found" #4655

Open saad-waqar-robotics opened 2 weeks ago

saad-waqar-robotics commented 2 weeks ago

Bug report

Required Info:

Steps to reproduce issue

I am using a diff_drive robot with ROS2 Humble, NAV2 & slam_toolbox. 
I am currently simulating the robot in Gazebo classic, House map. 
I have started using RegulatedPurePursuit with NavfnPlanner.
My params file is:

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_footprint"
    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: -1.0
    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

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    default_nav_to_pose_bt_xml: $(find-pkg-share carrier_bot_sim)/config/navigate_w_replanning_and_recovery.xml
    navigators: ['navigate_to_pose']
    navigate_to_pose:
      plugin: "nav2_bt_navigator::NavigateToPoseNavigator" # In Iron and older versions, "/" was used instead of "::"
    # '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_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

bt_navigator_navigate_through_poses_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator_navigate_to_pose_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 8.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

    # Controller server parameters
    FollowPath:

      # RPP controller
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.5
      lookahead_dist: 0.6
      min_lookahead_dist: 0.3
      max_lookahead_dist: 0.9
      lookahead_time: 1.5
      rotate_to_heading_angular_vel: 0.8
      transform_tolerance: 0.5
      use_velocity_scaled_lookahead_dist: true
      min_approach_linear_velocity: 0.05
      approach_velocity_scaling_dist: 1.0
      use_collision_detection: true
      max_allowed_time_to_collision_up_to_carrot: 1.0
      use_regulated_linear_velocity_scaling: true
      use_cost_regulated_linear_velocity_scaling: false
      regulated_linear_scaling_min_radius: 0.9
      regulated_linear_scaling_min_speed: 0.25
      use_rotate_to_heading: true
      rotate_to_heading_min_angle: 0.6
      max_angular_accel: 1.0
      max_robot_pose_search_dist: 10.0
      use_interpolation: true
      cost_scaling_dist: 0.3
      cost_scaling_gain: 1.0
      inflation_cost_scaling_factor: 3.0

      # plugin: "nav2_rotation_shim_controller::RotationShimController"
      # primary_controller: "dwb_core::DWBLocalPlanner"
      # angular_dist_threshold: 0.6
      # forward_sampling_distance: 0.5
      # rotate_to_heading_angular_vel: 0.8
      # max_angular_accel: 1.0
      # simulate_ahead_time: 1.0
      # rotate_to_goal_heading: true
      # # DWA PARAMETERS
      # debug_trajectory_details: True
      # min_vel_x: -0.0
      # min_vel_y: 0.0
      # max_vel_x: 0.4
      # max_vel_y: 0.0
      # max_vel_theta: 1.0
      # min_speed_xy: 0.0
      # max_speed_xy: 0.4
      # 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: 0.10
      # acc_lim_y: 0.0
      # acc_lim_theta: 1.0
      # decel_lim_x: -0.10
      # decel_lim_y: 0.0
      # decel_lim_theta: -1.0
      # vx_samples: 20
      # vy_samples: 0
      # vtheta_samples: 25
      # sim_time: 2.3
      # linear_granularity: 0.05
      # angular_granularity: 0.025
      # transform_tolerance: 0.8
      # xy_goal_tolerance: 0.25
      # trans_stopped_velocity: 0.25
      # short_circuit_trajectory_evaluation: True
      # stateful: True
      # critics: ["RotateToGoal", "BaseObstacle", "Oscillation", "ObstacleFootprint", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      # BaseObstacle.scale: 0.08
      # PathAlign.scale: 32.0
      # PathAlign.forward_point_distance: 0.1
      # GoalAlign.scale: 24.0
      # GoalAlign.forward_point_distance: 0.1
      # PathDist.scale: 32.0
      # GoalDist.scale: 24.0
      # RotateToGoal.scale: 32.0
      # RotateToGoal.slowing_factor: 5.0
      # RotateToGoal.lookahead_time: -1.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: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      # robot_radius: 0.23
      footprint: '[ [0.35307, 0.16], [-0.04693, 0.16], [-0.04693, -0.16], [0.35307, -0.16] ]'

      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.40
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: pointcloud scan
        scan:
          topic: /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

        pointcloud:  # no frame set, uses frame from message
          topic: /oak/points
          max_obstacle_height: 2.0
          min_obstacle_height: 0.2
          obstacle_max_range: 4.5
          obstacle_min_range: 0.0
          raytrace_max_range: 6.0
          raytrace_min_range: 0.0
          clearing: True
          marking: True
          data_type: "PointCloud2"

      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      #robot_radius: 0.23
      footprint: '[ [0.35307, 0.16], [-0.04693, 0.16], [-0.04693, -0.16], [0.35307, -0.16] ]' #W.R.T new baselink position at the center of rotation.

      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer", "voxel_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: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 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: 0.40
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: pointcloud scan
        scan:
          topic: /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

        pointcloud:  # no frame set, uses frame from message
          topic: /oak/points
          max_obstacle_height: 2.0
          min_obstacle_height: 0.2
          obstacle_max_range: 4.5
          obstacle_min_range: 0.0
          raytrace_max_range: 6.0
          raytrace_min_range: 0.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
      always_send_full_costmap: True

planner_server:
  ros__parameters:    
    expected_planner_frequency: 2.0
    use_sim_time: True
    planner_plugins: ["GridBased"]

    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: true
      allow_unknown: 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: 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"
    wait:
      plugin: "nav2_behaviors/Wait"
    assisted_teleop:
      plugin: "nav2_behaviors/AssistedTeleop"
    global_frame: odom
    robot_base_frame: base_link
    transform_tolerance: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: -1.0
    rotational_acc_lim: 0.9

robot_state_publisher:
  ros__parameters:
    use_sim_time: True

waypoint_follower:
  ros__parameters:
    use_sim_time: True
    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

velocity_smoother:
  ros__parameters:
    use_sim_time: True
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.26, 0.0, 1.0]
    min_velocity: [-0.26, 0.0, -1.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

Expected behavior

It should successfully complete the navigation.

Actual behavior

Global planning fails randomly on the way while re-planning. At the beginning, it perfectly finds a plan for a clearly reachable goal, starts driving on it, but randomly fails during re-planning saying: [planner_server]: Failed to create a plan from potential when a legal potential was found. This shouldn't happen. [planner_server]: GridBased: failed to create plan with tolerance 0.50. [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-6.30, -2.68)

Additional information

I am using a diff_drive robot with ROS2 Humble, NAV2 & slam_toolbox. Everything seems to be in place.

I am currently simulating the robot in Gazebo classic, House map.

I have started using RegulatedPurePursuit with NavfnPlanner. Usually it works nicely, but randomly I am getting this error from planner:

[planner_server]: Failed to create a plan from potential when a legal potential was found. This shouldn't happen. [planner_server]: GridBased: failed to create plan with tolerance 0.50. [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-6.30, -2.68)

This issue comes purely RANDOMLY. It usually does not occur when I send a new goal request. Usually, it takes a goal request, plans the path and starts moving. While moving on its way to a clearly reachable goal, it randomly starts giving this error while re-planning the global path. Due to this error, it falls into recovery mode, sometimes it recovers, sometimes it gives a failure after multiple tries. I have tried to look on google, I could not find anything helplful.

Please let me know if I am doing something wrong. :) :)

image

In this screenshot, I was travelling from position 3 to position 1, that is clearly reachable. It started on the path perfectly fine, but in the middle it starting giving this error and fell down to recovery mode.

saad-waqar-robotics commented 2 weeks ago

Additional info: I enabled debug mode, and then reproduced the issue on a long path:

[planner_server-8] [DEBUG] [1725088039.676608922] [planner_server.rclcpp_action]: Accepted goal 7f8e7e847727a3bebe82e1ea16f1576 [planner_server-8] [DEBUG] [1725088039.676846811] [planner_server]: [compute_path_to_pose] [ActionServer] Receiving a new goal [planner_server-8] [DEBUG] [1725088039.676868748] [planner_server]: [compute_path_to_pose] [ActionServer] Executing goal asynchronously. [planner_server-8] [DEBUG] [1725088039.676953543] [planner_server]: [compute_path_to_pose] [ActionServer] Executing the goal... [planner_server-8] [DEBUG] [1725088039.676959452] [rcl]: Service server taking service request [bt_navigator-10] [DEBUG] [1725088039.676618873] [rcl_action]: Taking action goal response [bt_navigator-10] [DEBUG] [1725088039.676627021] [rcl]: Client taking service response [bt_navigator-10] [DEBUG] [1725088039.676634695] [rcl]: Client take response succeeded: true [bt_navigator-10] [DEBUG] [1725088039.676638454] [rcl_action]: Action goal response taken [bt_navigator-10] [DEBUG] [1725088039.676648860] [rcl_action]: Sending action result request [bt_navigator-10] [DEBUG] [1725088039.676652736] [rcl]: Client sending service request [bt_navigator-10] [DEBUG] [1725088039.676673816] [rcl_action]: Action result request sent [bt_navigator-10] [DEBUG] [1725088039.676942051] [rcl_action]: Taking action feedback [bt_navigator-10] [DEBUG] [1725088039.676970917] [rcl]: Subscription taking message [bt_navigator-10] [DEBUG] [1725088039.676992406] [rcl]: Subscription take succeeded: true [bt_navigator-10] [DEBUG] [1725088039.677002877] [rcl_action]: Action feedback taken [bt_navigator-10] [DEBUG] [1725088039.677015764] [bt_navigator.rclcpp_action]: Received feedback for unknown goal. Ignoring... [planner_server-8] [DEBUG] [1725088039.677073986] [rcl]: Service take request succeeded: true [planner_server-8] [DEBUG] [1725088039.677099029] [planner_server]: Attempting to a find path from (2.20, 0.38) to (-6.45, -2.10). [planner_server-8] [DEBUG] [1725088039.677158458] [planner_server]: Making plan from (2.20,0.38) to (-6.45,-2.10) [planner_server-8] [DEBUG] [1725088039.677204235] [rclcpp]: [NavFn] Array is 299 x 212 [planner_server-8] [planner_server-8] [DEBUG] [1725088039.677447142] [rclcpp]: [NavFn] Setting start to 21,65 [planner_server-8] [planner_server-8] [DEBUG] [1725088039.677504076] [rclcpp]: [NavFn] Setting goal to 194,115 [planner_server-8] [lifecycle_manager-13] [DEBUG] [1725088039.677942484] [rcl]: Calling timer [lifecycle_manager-13] [DEBUG] [1725088039.678152287] [rcl]: Subscription taking message [lifecycle_manager-13] [DEBUG] [1725088039.678171900] [rcl]: Subscription take succeeded: true [lifecycle_manager-13] [DEBUG] [1725088039.678186451] [rcl]: Subscription taking message [lifecycle_manager-13] [DEBUG] [1725088039.678192139] [rcl]: Subscription take succeeded: true [lifecycle_manager-13] [DEBUG] [1725088039.678199954] [rcl]: Subscription taking message [lifecycle_manager-13] [DEBUG] [1725088039.678205543] [rcl]: Subscription take succeeded: true [lifecycle_manager-13] [DEBUG] [1725088039.678213775] [rcl]: Subscription taking message [lifecycle_manager-13] [DEBUG] [1725088039.678219796] [rcl]: Subscription take succeeded: true [lifecycle_manager-13] [DEBUG] [1725088039.678227202] [rcl]: Subscription taking message [lifecycle_manager-13] [DEBUG] [1725088039.678232494] [rcl]: Subscription take succeeded: true [lifecycle_manager-13] [DEBUG] [1725088039.678240141] [rcl]: Subscription taking message [lifecycle_manager-13] [DEBUG] [1725088039.678245729] [rcl]: Subscription take succeeded: true [lifecycle_manager-13] [DEBUG] [1725088039.678252797] [rcl]: Subscription taking message [lifecycle_manager-13] [DEBUG] [1725088039.678258418] [rcl]: Subscription take succeeded: true [velocity_smoother-12] [DEBUG] [1725088039.678229050] [rcl]: Subscription taking message [velocity_smoother-12] [DEBUG] [1725088039.678282073] [rcl]: Subscription take succeeded: true [bt_navigator-10] [DEBUG] [1725088039.678222613] [rcl]: Subscription taking message [bt_navigator-10] [DEBUG] [1725088039.678283295] [rcl]: Subscription take succeeded: true [behavior_server-9] [DEBUG] [1725088039.678230539] [rcl]: Subscription taking message [behavior_server-9] [DEBUG] [1725088039.678286470] [rcl]: Subscription take succeeded: true [smoother_server-7] [DEBUG] [1725088039.678239359] [rcl]: Subscription taking message [smoother_server-7] [DEBUG] [1725088039.678281619] [rcl]: Subscription take succeeded: true [smoother_server-7] [DEBUG] [1725088039.678322052] [rcl]: Initializing timer with period: 4000000000ns [smoother_server-7] [DEBUG] [1725088039.678373467] [rcl]: Timer canceled [smoother_server-7] [DEBUG] [1725088039.678382177] [rcl]: Timer canceled [waypoint_follower-11] [DEBUG] [1725088039.678383800] [rcl]: Subscription taking message [waypoint_follower-11] [DEBUG] [1725088039.678456380] [rcl]: Subscription take succeeded: true [planner_server-8] [DEBUG] [1725088039.678409545] [rcl]: Subscription taking message [planner_server-8] [DEBUG] [1725088039.678451321] [rcl]: Subscription take succeeded: true [controller_server-6] [DEBUG] [1725088039.678410547] [rcl]: Subscription taking message [controller_server-6] [DEBUG] [1725088039.678464369] [rcl]: Subscription take succeeded: true [planner_server-8] [DEBUG] [1725088039.679474291] [rclcpp]: [NavFn] Used 467 cycles, 20807 cells visited (35%), priority buf max 800 [planner_server-8] [planner_server-8] [DEBUG] [1725088039.679501429] [rclcpp]: [NavFn] Setting start to 21,65 [planner_server-8] [planner_server-8] [DEBUG] [1725088039.679512286] [rclcpp]: [Path] Pot fn boundary, following grid (14374.8/1) [planner_server-8] [DEBUG] [1725088039.679523009] [rclcpp]: [Path] Pot: 14308.1 pos: 21.0,65.0 [planner_server-8] [DEBUG] [1725088039.679546467] [rclcpp]: [Path] Pot fn boundary, following grid (11682.7/109) [planner_server-8] [DEBUG] [1725088039.679553436] [rclcpp]: [Path] Pot: 11582.7 pos: 24.0,119.2 [planner_server-8] [DEBUG] [1725088039.679628345] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679634748] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/533) [planner_server-8] [DEBUG] [1725088039.679639081] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679646582] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679650429] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/551) [planner_server-8] [DEBUG] [1725088039.679654737] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679660843] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679664158] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/569) [planner_server-8] [DEBUG] [1725088039.679668100] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679675634] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679679614] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/587) [planner_server-8] [DEBUG] [1725088039.679683590] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679689517] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679693246] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/605) [planner_server-8] [DEBUG] [1725088039.679697096] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679703066] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679706795] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/623) [planner_server-8] [DEBUG] [1725088039.679711774] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679718197] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679721683] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/641) [planner_server-8] [DEBUG] [1725088039.679727298] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679734062] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679737409] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/659) [planner_server-8] [DEBUG] [1725088039.679741305] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679747437] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679750855] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/677) [planner_server-8] [DEBUG] [1725088039.679754690] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679760847] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679764126] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/695) [planner_server-8] [DEBUG] [1725088039.679768037] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679774473] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679777746] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/713) [planner_server-8] [DEBUG] [1725088039.679781609] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679787624] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679790986] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/731) [planner_server-8] [DEBUG] [1725088039.679795019] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679801216] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679804523] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/749) [planner_server-8] [DEBUG] [1725088039.679808392] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679839054] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679843498] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/767) [planner_server-8] [DEBUG] [1725088039.679848187] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679854215] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679857482] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/785) [planner_server-8] [DEBUG] [1725088039.679861304] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679867556] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679871070] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/803) [planner_server-8] [DEBUG] [1725088039.679875203] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679881248] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679884935] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/821) [planner_server-8] [DEBUG] [1725088039.679889077] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679895898] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679899425] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/839) [planner_server-8] [DEBUG] [1725088039.679903483] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679909823] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679913179] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/857) [planner_server-8] [DEBUG] [1725088039.679918001] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679923991] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679928135] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/875) [planner_server-8] [DEBUG] [1725088039.679932254] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679938376] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679941993] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/893) [planner_server-8] [DEBUG] [1725088039.679945890] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679951933] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679955714] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/911) [planner_server-8] [DEBUG] [1725088039.679960157] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679966608] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679969889] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/929) [planner_server-8] [DEBUG] [1725088039.679974416] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679980683] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679984551] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/947) [planner_server-8] [DEBUG] [1725088039.679989166] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.679995404] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.679999599] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/965) [planner_server-8] [DEBUG] [1725088039.680003512] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680009385] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680012725] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/983) [planner_server-8] [DEBUG] [1725088039.680016700] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680022633] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680026037] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/1001) [planner_server-8] [DEBUG] [1725088039.680030607] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680036923] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680040431] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/1019) [planner_server-8] [DEBUG] [1725088039.680046535] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680053426] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680057516] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/1037) [planner_server-8] [DEBUG] [1725088039.680067346] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680074066] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680077430] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/1055) [planner_server-8] [DEBUG] [1725088039.680081877] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680088002] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680091421] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/1073) [planner_server-8] [DEBUG] [1725088039.680095441] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680101603] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680104889] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/1091) [planner_server-8] [DEBUG] [1725088039.680108984] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680114817] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680118277] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/1109) [planner_server-8] [DEBUG] [1725088039.680122157] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680128009] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680131454] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/1127) [planner_server-8] [DEBUG] [1725088039.680135620] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680141603] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680144889] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/1145) [planner_server-8] [DEBUG] [1725088039.680148782] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680154695] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680157926] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/1163) [planner_server-8] [DEBUG] [1725088039.680161923] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680167770] [rclcpp]: [PathCalc] oscillation detected, attempting fix. [planner_server-8] [DEBUG] [1725088039.680171016] [rclcpp]: [Path] Pot fn boundary, following grid (906.3/1181) [planner_server-8] [DEBUG] [1725088039.680174948] [rclcpp]: [Path] Pot: 856.3 pos: 179.2,114.2 [planner_server-8] [DEBUG] [1725088039.680180790] [rclcpp]: [PathCalc] No path found, path too long [planner_server-8] [ERROR] [1725088039.680184300] [planner_server]: Failed to create a plan from potential when a legal potential was found. This shouldn't happen. [planner_server-8] [WARN] [1725088039.680211369] [planner_server]: GridBased: failed to create plan with tolerance 0.50. [planner_server-8] [WARN] [1725088039.680226135] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-6.45, -2.10) [planner_server-8] [WARN] [1725088039.680242217] [planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.

SteveMacenski commented 2 weeks ago

Please provide a reproducable example. You might find https://github.com/botsandus/planner_playground useful to create that example if you fork + add your map and start/goal locations

Please also show your costmap + validate that your footprint is valid

saad-waqar-robotics commented 1 week ago

Thank you for your kind response @SteveMacenski

Here is the robot footprint, seems valid. image

The cost-map looks like this: image

Map png file: bug_wala_map

map yaml file: image: bug_wala_map.png mode: trinary resolution: 0.05 origin: [-7.47, -5.27, 0] negate: 0 occupied_thresh: 0.65 free_thresh: 0.25

Issue was reproduced multiple-times for the goal: image

I have created my fork of planner_playground repo (https://github.com/saad-waqar-robotics/planner_playground_fork) it has my custom map in it. But I could not reproduce the issue on it, because, the planner plans the path successfully, it usually gets the error while moving on the path (during replanning). So I could not reproduce this replanning scenario here on the playground. I dont think I ever got this error on the first time planning. The path is usually very clear and it starts following the path, but eventually on the way.

SteveMacenski commented 1 week ago

I will need a way to reproduce in order to help. There should be no difference in replanning with NavFn, but you could adjust the planner playground to plans 2x in a row if the replan is what causes the issue to be reproducable (should I be wrong about that!).

saad-waqar-robotics commented 6 days ago

I am creating a package so you will be able to reproduce the issue. I have tried changing the inflation radius etc, but still the problem persists. I have noticed that this issue usually occurs when the robot is passing through a narrow area (still enough distance to go). It creates plan correctly, but when enters the narrow areas it throws the error and retries. Sometimes it moves through after some tries, sometimes it declares failure. I will try to get a package and steps to reproduce in a while. Here again you can see that it planned the path, but when it started moving, it gave the error as soon as it reached the narrow entrance. Then started recovery, then failed.

image

Thanks for the help.

SteveMacenski commented 5 days ago

Still need something preproducible.

I wonder if this relates to the fact that you have a very low robot radius since you have the pivot point so close to the front of the robot. As you can see, the blue inscribed band around obstacles is incredibly small - and very unusual. It could be that the neighbor expansion in some situations isn't hitting that cell so it by passes it and then causes issues when doing the gradient descent its creating a problem.

What happens if you add footprint_padding on the radius so that the inscribed 253 cost has a non-single-pixel wide band? i.e. 5-10cm?

saad-waqar-robotics commented 1 hour ago

Thanks @SteveMacenski. I'll definitely check that and report the results. The pivot point is actually on the rear side of the robot (here in the image we are looking at the rear side, its a rear side differential drive, forward 2 wheels are castor wheels. Here in the image, it was going straight on the path, but when the failure occurred, then it went into recovery behavior and spun towards right side.) Recently, I increased the inflation radius and tinkered with cost-scaling factor, now I have not seen this issue in a while. I will keep on testing, and I will also check out footprint_padding as well. Note: My robot radius is not small, I am using robot_footprint through a polygon (as you can see in the image in green). But yes, I will try adding padding and will report the results.

THanks!

saad-waqar-robotics commented 32 minutes ago

I checked with footprint padding of 7.5cm. The blue inscribed band definitely increased, but I still got the error around the corners. image

I tried with robot radius (instead of robot_footprint polygon), then it inflated the inscribed blue band appropriately). image

(I have my baselink at the center of rotation, so the circle is also on offset but the obstacles are inflated correctly. Although the error is very less frequent which this, but still I saw it once. May be due to this invalid circle position, I will confirm it by fixing the center of the circle.)

My question is, isnt costmap supposed to inflate the inscribed region according to robot footprint as well (just like it does with robot_radius)? If not, is addition of padding the only way to increase the inscribed region? May be its a silly question, but I could not get it from the docs.

Thanks a lot.

saad-waqar-robotics commented 20 minutes ago

After setting the circle to the center (by changing robot_base_frame for costmaps to chassis_link {center link in my robot}), the error is very rare. But I still got it 3 times, even in open spaces. image

Here is my latest .yaml 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_footprint" 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: -1.0 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

bt_navigator: ros__parameters: use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 default_nav_to_pose_bt_xml: $(find-pkg-share carrier_bot_sim)/config/navigate_w_replanning_and_recovery.xml navigators: ['navigate_to_pose'] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" # In Iron and older versions, "/" was used instead of "::"

'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_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

bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: use_sim_time: True

bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: use_sim_time: True

controller_server: ros__parameters: use_sim_time: True controller_frequency: 5.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

# Controller server parameters
FollowPath:

  # RPP controller
  plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
  desired_linear_vel: 0.5
  lookahead_dist: 1.0
  min_lookahead_dist: 0.3
  max_lookahead_dist: 0.9
  lookahead_time: 1.5
  rotate_to_heading_angular_vel: 0.8
  transform_tolerance: 0.5
  use_velocity_scaled_lookahead_dist: true
  min_approach_linear_velocity: 0.05
  approach_velocity_scaling_dist: 1.0
  use_collision_detection: true
  max_allowed_time_to_collision_up_to_carrot: 1.0
  use_regulated_linear_velocity_scaling: true
  use_cost_regulated_linear_velocity_scaling: false
  regulated_linear_scaling_min_radius: 0.9
  regulated_linear_scaling_min_speed: 0.15
  use_rotate_to_heading: true
  rotate_to_heading_min_angle: 0.6
  max_angular_accel: 1.0
  max_robot_pose_search_dist: 10.0
  use_interpolation: true
  cost_scaling_dist: 0.6
  cost_scaling_gain: 1.0
  inflation_cost_scaling_factor: 2.5

  # plugin: "nav2_rotation_shim_controller::RotationShimController"
  # primary_controller: "dwb_core::DWBLocalPlanner"
  # angular_dist_threshold: 0.6
  # forward_sampling_distance: 0.5
  # rotate_to_heading_angular_vel: 0.8
  # max_angular_accel: 1.0
  # simulate_ahead_time: 1.0
  # rotate_to_goal_heading: true
  # # DWA PARAMETERS
  # debug_trajectory_details: True
  # min_vel_x: -0.0
  # min_vel_y: 0.0
  # max_vel_x: 0.4
  # max_vel_y: 0.0
  # max_vel_theta: 1.0
  # min_speed_xy: 0.0
  # max_speed_xy: 0.4
  # 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: 0.10
  # acc_lim_y: 0.0
  # acc_lim_theta: 1.0
  # decel_lim_x: -0.10
  # decel_lim_y: 0.0
  # decel_lim_theta: -1.0
  # vx_samples: 20
  # vy_samples: 0
  # vtheta_samples: 25
  # sim_time: 2.3
  # linear_granularity: 0.05
  # angular_granularity: 0.025
  # transform_tolerance: 0.8
  # xy_goal_tolerance: 0.25
  # trans_stopped_velocity: 0.25
  # short_circuit_trajectory_evaluation: True
  # stateful: True
  # critics: ["RotateToGoal", "BaseObstacle", "Oscillation", "ObstacleFootprint", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
  # BaseObstacle.scale: 0.08
  # PathAlign.scale: 32.0
  # PathAlign.forward_point_distance: 0.1
  # GoalAlign.scale: 24.0
  # GoalAlign.forward_point_distance: 0.1
  # PathDist.scale: 32.0
  # GoalDist.scale: 24.0
  # RotateToGoal.scale: 32.0
  # RotateToGoal.slowing_factor: 5.0
  # RotateToGoal.lookahead_time: -1.0

local_costmap: local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom robot_base_frame: chassis_link use_sim_time: True rolling_window: true width: 3 height: 3 resolution: 0.05 robot_radius: 0.25

footprint: '[ [0.35307, 0.16], [-0.04693, 0.16], [-0.04693, -0.16], [0.35307, -0.16] ]'

  # footprint_padding: 0.075
  plugins: ["voxel_layer", "inflation_layer"]
  inflation_layer:
    plugin: "nav2_costmap_2d::InflationLayer"
    cost_scaling_factor: 2.5
    inflation_radius: 0.60
  voxel_layer:
    plugin: "nav2_costmap_2d::VoxelLayer"
    enabled: True
    publish_voxel_map: True
    origin_z: 0.0
    z_resolution: 0.05
    z_voxels: 16
    max_obstacle_height: 2.0
    mark_threshold: 0
    observation_sources: pointcloud scan
    scan:
      topic: /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

    pointcloud:  # no frame set, uses frame from message
      topic: /oak/points
      max_obstacle_height: 2.0
      min_obstacle_height: 0.2
      obstacle_max_range: 4.5
      obstacle_min_range: 0.0
      raytrace_max_range: 6.0
      raytrace_min_range: 0.0
      clearing: True
      marking: True
      data_type: "PointCloud2"

  static_layer:
    plugin: "nav2_costmap_2d::StaticLayer"
    map_subscribe_transient_local: True
  always_send_full_costmap: True

global_costmap: global_costmap: ros__parameters: update_frequency: 1.0 publish_frequency: 1.0 global_frame: map robot_base_frame: chassis_link use_sim_time: True robot_radius: 0.25

footprint: '[ [0.35307, 0.16], [-0.04693, 0.16], [-0.04693, -0.16], [0.35307, -0.16] ]' #W.R.T new baselink position at the center of rotation.

  #footprint_padding: 0.075
  resolution: 0.05
  track_unknown_space: true
  plugins: ["static_layer", "obstacle_layer", "inflation_layer", "voxel_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: 3.0
      raytrace_min_range: 0.0
      obstacle_max_range: 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: 2.5
    inflation_radius: 0.60
  voxel_layer:
    plugin: "nav2_costmap_2d::VoxelLayer"
    enabled: True
    publish_voxel_map: True
    origin_z: 0.0
    z_resolution: 0.05
    z_voxels: 16
    max_obstacle_height: 2.0
    mark_threshold: 0
    observation_sources: pointcloud scan
    scan:
      topic: /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

    pointcloud:  # no frame set, uses frame from message
      topic: /oak/points
      max_obstacle_height: 2.0
      min_obstacle_height: 0.2
      obstacle_max_range: 4.5
      obstacle_min_range: 0.0
      raytrace_max_range: 6.0
      raytrace_min_range: 0.0
      clearing: True
      marking: True
      data_type: "PointCloud2"
  always_send_full_costmap: True

planner_server: ros__parameters:
expected_planner_frequency: 2.0 use_sim_time: True planner_plugins: ["GridBased"]

GridBased:
  plugin: "nav2_navfn_planner/NavfnPlanner"
  tolerance: 0.5
  use_astar: true
  allow_unknown: true

smoother_server: ros__parameters: 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 do_refinement: True

behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_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" wait: plugin: "nav2_behaviors/Wait" assisted_teleop: plugin: "nav2_behaviors/AssistedTeleop" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: -1.0 rotational_acc_lim: 0.9

robot_state_publisher: ros__parameters: use_sim_time: True

waypoint_follower: ros__parameters: use_sim_time: True 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

velocity_smoother: ros__parameters: use_sim_time: True smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" max_velocity: [0.5, 0.0, 1.0] min_velocity: [-0.5, 0.0, -1.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0