[Smac] State lattice planner: angle flips with allow_reverse_expansion enabled #5133
Description
Bug report
Required Info:
Operating System:
Ubuntu 22.04
ROS2 Version:
Humble
Version or commit hash:
Humble
DDS implementation:
FastDDS
Nav2 Package:
Smac
Hello! I've noticed an issue when using the SMAC lattice planner with a diff robot. In most cases, everything works fine, but in dificult scenarios that require maneuvering (for example, when the robot is near a wall, and we need to turn around), the final plan's orientation may be flipped 180 degrees from the requested orientation.
After debugging and examining the SMAC source code, I believe the root of the problem may be in these locations:
- NodeLattice::GetIndex does not take the
_backwardsflag into account - NodeLattice::getNeighbors can mark the goal node as backward
As a result, when the desired trajectory is not simple and the heuristic is not perfect for this particular case, the flipped trajectory can be selected during A* path calculation. I've observed that both the isGoal condition and the _best_heuristic_node.first < getToleranceHeuristic() check can result in these incorrectly flipped plans being chosen.
Also, If I disable allow_reverse_expansion flag, the problem does not reproduce.
Some extra logs from planner I've added:
The requested goal:
[planner_server-11] [INFO] [1746567908.368662407] [planner_server]: goal th: 0.016815
[planner_server-11] [INFO] [1746567908.373920331] [planner_server]: goal is back: 0
[planner_server-11] [INFO] [1746567908.369878992] [planner_server]: Index goal: 5730272 //Index of the goal not at the SetGoal method
goal node that A* found and path backtrace:
[planner_server-11] [INFO] [1746567908.405222967] [planner_server]: A* found goal: 1
[planner_server-11] [INFO] [1746567908.405227768] [planner_server]: th goal: 0.000000
[planner_server-11] [INFO] [1746567908.405231161] [planner_server]: th node: 16.000000
[planner_server-11] [INFO] [1746567908.405234835] [planner_server]: BackTracePath
[planner_server-11] [INFO] [1746567908.405237915] [planner_server]: Curr node ind = 5730272 //Index of the node A* take as final, the same as in Goal set, but angle is 180 deg. rotated
[planner_server-11] [INFO] [1746567908.405242513] [planner_server]: curr node is back: 1 //The last(goal) node is backward
...
[planner_server-11] [INFO] [1746567908.405644396] [planner_server]: curr path point.x 294.000000
[planner_server-11] [INFO] [1746567908.405648013] [planner_server]: curr path point.y 300.000000
[planner_server-11] [INFO] [1746567908.405652532] [planner_server]: curr path point.th 3.141593
[planner_server-11] [INFO] [1746567908.405656570] [planner_server]: curr path point.x 295.000000
[planner_server-11] [INFO] [1746567908.405660716] [planner_server]: curr path point.y 300.000000
[planner_server-11] [INFO] [1746567908.405665022] [planner_server]: curr path point.th 3.141593
[planner_server-11] [INFO] [1746567908.405668315] [planner_server]: curr path point.x 296.000000
[planner_server-11] [INFO] [1746567908.405671892] [planner_server]: curr path point.y 300.000000
[planner_server-11] [INFO] [1746567908.405675999] [planner_server]: curr path point.th 3.141593
The resulting plan:
[planner_server-11] [INFO] [1746567908.406408422] [planner_server]: Pose #0: Position(x=-4.674999, y=-5.024999, z=0.000000) Orientation(yaw=-3.141593)
[planner_server-11] [INFO] [1746567908.406423291] [planner_server]: Pose #1: Position(x=-4.724808, y=-5.026018, z=0.000000) Orientation(yaw=-3.112840)
...
[planner_server-11] [INFO] [1746567908.406652179] [planner_server]: Pose #27: Position(x=-5.974999, y=-5.124999, z=0.000000) Orientation(yaw=-2.896614)
[planner_server-11] [INFO] [1746567908.406660567] [planner_server]: Pose #28: Position(x=-5.974999, y=-5.124999, z=0.000000) Orientation(yaw=-3.141593)
The red arrow shows the requested pose, but the plan's orientation is flipped here.
Steps to reproduce issue
- Use State lattice planner, diff model, rectangular footprint, and allow_reverse_expansion: true
- Locate a robot near the wall
- Send a request to a planner to move forward with 180 degree rotated goal pose
- Check the path thetas
Expected behavior
At least the last pose has angle equals to the requested angle
Actual behavior
The last pose has a 180 degree rotated angle.
Reproduction instructions
planner_server:
ros__parameters:
expected_planner_frequency: 5.0
planner_plugins: ["GridBased"]
smoother_plugins: ["SimpleSmoother"]
global_frame: map
robot_base_frame: base_link
GridBased:
robot_base_frame: base_link
plugin: "nav2_smac_planner/SmacPlannerLattice"
tolerance: 0.005
downsample_costmap: false
lattice_filepath: "my_lattice.json"
downsampling_factor: 1
allow_unknown: true
max_iterations: 1000000
max_on_approach_iterations: 10000
max_planning_time: 3.0
non_straight_penalty: 1.5
cost_penalty: 2.0
rotation_penalty: 5.0
reverse_penalty: 2.5
allow_primitive_interpolation: true
change_penalty: 0.3
allow_reverse_expansion: true
retrospective_penalty: 0.05
analytic_expansion_ratio: 1.5
analytic_expansion_max_length: 7.0
cache_obstacle_heuristic: true
smooth_path: true
smoother:
smoother:
max_iterations: 1000
w_smooth: 1.5
w_data: 2.2
tolerance: 0.1
do_refinement: false
refinement_num: 2
Lattice generator config:
"motion_model": "diff",
"turning_radius": 1.0,
"grid_resolution": 0.05,
"stopping_threshold": 7,
"num_of_headings": 32