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:
Then the path stops updating as shown in the video below:
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