I’m using the NAV2 stack to simulate planning algorithms with gazebo. When using the in-built DWBLocalPlanner the controller immediately fails upon receiving a 2D navigation goal.
[controller_server-3] [WARN] [1626774641.217182182] [controller_server]: Resulting plan has 0 poses in it.
[controller_server-3] [WARN] [1626774641.266783157] [controller_server]: Received plan with zero length
[...]
[controller_server-3] [WARN] [1626774642.216811645] [controller_server]: Received plan with zero length
[controller_server-3] [INFO] [1626774642.266689269] [controller_server]: Passing new path to controller.
[controller_server-3] [WARN] [1626774642.270099499] [controller_server]: Resulting plan has 0 poses in it.
[controller_server-3] [WARN] [1626774642.316759380] [controller_server]: Received plan with zero length
[...]
[controller_server-3] [ERROR] [1626774642.466863971] [controller_server]: Controller patience exceeded
[controller_server-3] [WARN] [1626774642.466942963] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[bt_navigator-6] [ERROR] [1626774642.495711482] [bt_navigator]: Goal failed
The calculate global plan looks good to me. It’s just that the first pose is a little bit off the robot’s current position (how can I change that?).
The relevant code section why the controller fails is probably DWBLocalPlanner::transformGlobalPlan.
I tried to disable pruning and increased the value of prune_distance. However, I couldn’t make it work reliably. Does anybody know what’s the problem? Did I forget to set some parameter or is it a bug?
Some background information:
I started my project by copying the nav2_bringup package and adapted nav2_params.yaml for my use case. I currently use an obstacle free environment and the local and global costmaps are completely empty (i.e. a large static map that is completely marked as ‘free’ is loaded). I use robot_localization for gps/imu based localization and SmacPlannerHybrid for planning. The map and odom frame are identical. In comparison to the turtlebot3 simulation my robot is relatively large (r=3.0m). I don’t know if that is of any relevance here.
This is my current configuration:
planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
tolerance: 0.5
downsample_costmap: false
downsampling_factor: 1
allow_unknown: true
max_iterations: 1000000
max_on_approach_iterations: 1000
max_planning_time: 3.5
motion_model_for_search: "DUBIN"
angle_quantization_bins: 64
analytic_expansion_ratio: 3.5
minimum_turning_radius: 3.0
reverse_penalty: 2.1
change_penalty: 0.4
non_straight_penalty: 2.50
cost_penalty: 1.7
lookup_table_size: 20.
cache_obstacle_heuristic: True
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1e-10
planner_server_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: 1.0
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 100.0
# Goal checker parameters
general_goal_checker:
stateful: True
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 4.0
yaw_goal_tolerance: 0.25
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
min_vel_x: 0.0
min_vel_y: 0.0
max_vel_x: 0.8
max_vel_y: 0.0
max_vel_theta: 1.0
min_speed_xy: 0.0
max_speed_xy: 0.8
min_speed_theta: 0.0
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: 1.7
linear_granularity: 0.05
angular_granularity: 0.025
transform_tolerance: 0.2
xy_goal_tolerance: 0.5
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: 32.0
PathAlign.forward_point_distance: 5.0
GoalAlign.scale: 24.0
GoalAlign.forward_point_distance: 5.0
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
Please tell me if you need any additional information.