Robotics StackExchange | Archived questions

Smac planner does not update while turning

I am using ROS2 Humble on Ubuntu 22.04 and I'm trying to simulate an ackermann steering robot. I have the nav2 stack running with Smac Hybrid A* Planner and RPP controller.

Everything seems to be working fine on the straights, however when the robot has to turn, the planner stops updating and throws a warning "Gridbased: failed to create plan, no valid path found" (shown in the log below). The controller seems to be working fine because the robot follows the path correctly.

The correct working (on straight) is shown in the video below: image description

Then the path stops updating as shown in the video below: image description

I receive the warning when the plan stops updating. The warning is shown in the terminal log below.

[component_container_isolated-1] [INFO] [1689922467.903757969] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1689922471.003748807] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1689922474.103766268] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1689922477.203744205] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1689922480.304344730] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1689922483.303745057] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1689922486.403744809] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1689922489.503859143] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [WARN] [1689922497.491517782] [planner_server]: GridBased: failed to create plan, no valid path found.
[component_container_isolated-1] [WARN] [1689922497.494185114] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.74, -17.96)
[component_container_isolated-1] [WARN] [1689922497.494258228] [planner_server]: [compute_path_through_poses] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1689922497.510602409] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container_isolated-1] [INFO] [1689922497.603753645] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1689922497.604235013] [controller_server]: Invalid path, Path is empty.
[component_container_isolated-1] [WARN] [1689922497.604398792] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1689922497.630670663] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container_isolated-1] [INFO] [1689922497.643845893] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-1] [ERROR] [1689922497.644227983] [controller_server]: Invalid path, Path is empty.
[component_container_isolated-1] [WARN] [1689922497.646348372] [controller_server]: [follow_path] [ActionServer] Aborting handle.

Here is my nav2_params.yaml config file:

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odometry/filtered #/wheel/odometry
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    # '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_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: 10.0
    min_x_velocity_threshold: 0.01
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older
    goal_checker_plugins: ["goal_checker"]
    controller_plugins: ["FollowPath"]

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.5
      yaw_goal_tolerance: 0.5
      stateful: True
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.8
      lookahead_dist: 0.5
      min_lookahead_dist: 0.4
      max_lookahead_dist: 1.0
      lookahead_time: 3.0
      rotate_to_heading_angular_vel: 0.6
      transform_tolerance: 0.5
      use_velocity_scaled_lookahead_dist: false
      min_approach_linear_velocity: 0.05
      approach_velocity_scaling_dist: 0.6
      use_collision_detection: true
      max_allowed_time_to_collision_up_to_carrot: 1.0
      use_regulated_linear_velocity_scaling: true
      use_fixed_curvature_lookahead: false
      curvature_lookahead_dist: 0.25
      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: false
      allow_reversing: false
      rotate_to_heading_min_angle: 0.785
      max_angular_accel: 0.6
      max_robot_pose_search_dist: 10.0
      use_interpolation: false

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

local_costmap:
  local_costmap:
    ros__parameters:
      footprint: "[ [1.45, 0.95], [1.45, -0.95], [-1.55, -0.95], [-1.55, 0.95] ]"
      update_frequency: 5.0 
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 25
      height: 25
      resolution: 0.05
      # robot_radius: 0.5
      plugins: ["inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 1.0
        inflation_radius: 0.5
      # 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: scan
      #   scan:
      #     topic: /scan
      #     max_obstacle_height: 2.0
      #     clearing: True
      #     marking: True
      #     data_type: "LaserScan"
      #     raytrace_max_range: 10.0
      #     raytrace_min_range: 0.0
      #     obstacle_max_range: 9.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:
      height: 50
      width: 50
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      footprint: "[ [1.45, 0.95], [1.45, -0.95], [-1.55, -0.95], [-1.55, 0.95] ]"
      # robot_radius: 2.0
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "inflation_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: 10.0
        #   raytrace_min_range: 0.0
        #   obstacle_max_range: 9.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: 1.0
        inflation_radius: 0.5
      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: "barnstorm_field.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:
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      downsample_costmap: false
      downsampling_factor: 1
      tolerance: 1.0
      allow_unknown: true
      max_iterations: -1
      max_on_approach_iterations: 1000
      max_planning_time: 5.0
      motion_model_for_search: "DUBIN"
      angle_quantization_bins: 72
      analytic_expansion_ratio: 3.5
      analytic_expansion_max_length: 3.0
      minimum_turning_radius: 2.5
      reverse_penalty: 2.0
      change_penalty: 4.0
      non_straight_penalty: 3.2
      cost_penalty: 1.0
      retrospective_penalty: 0.025
      lookup_table_size: 20.0
      cache_obstacle_heuristic: false
      viz_expansions: false
      cost_travel_multiplier: 1.0
      smooth_path: true         

      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1e-10
        do_refSmacPlannerHybridinement: true
        refinement_num: 2

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    scale_velocities: false
    feedback: "OPEN_LOOP"
    max_velocity: [0.8, 0.0, 0.7]
    min_velocity: [-0.1, 0.0, -0.7]
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0
    max_accel: [0.8, 0.0, 0.6]
    max_decel: [-0.8, 0.0, -0.6]
    odom_topic: "/odometry/filtered"
    odom_duration: 0.2

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: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 0.5
    min_rotational_vel: 0.2
    rotational_acc_lim: 3.2

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", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    wait:
      plugin: "nav2_behaviors/Wait"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_link
    transform_timeout: 1.0
    simulate_ahead_time: 2.0
    max_rotational_vel: 0.6
    min_rotational_vel: -0.6
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: True

waypoint_follower:
  ros__parameters:
    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: 0

Any solutions to this would be appreciated. Thanks

Asked by akarsh2906 on 2023-07-22 04:10:51 UTC

Comments

Answers