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