After successfully reaching goal planner spams "trying to change gears"
Description
When the vehicle reaches the goal in the planner, it will continue to try and change gears.
How to Reproduce
Run the AVP demo
Current Behavior
Once the vehicle reaches the goal, it will print "Trying to change gears"
[behavior_planner_node_exe-18] [INFO] [1618312758.822454789] [planning.behavior_planner_node]: Received trajectory from planner
[behavior_planner_node_exe-18] [INFO] [1618312758.828685259] [planning.behavior_planner_node]: Trying to change gear
[behavior_planner_node_exe-18] [INFO] [1618312764.473660771] [planning.behavior_planner_node]: Trying to change gear
[behavior_planner_node_exe-18] [INFO] [1618312764.473702026] [planning.behavior_planner_node]: Reached goal. Wait for another route
[behavior_planner_node_exe-18] [INFO] [1618312767.209196569] [planning.behavior_planner_node]: Trying to change gear
[behavior_planner_node_exe-18] [INFO] [1618312767.475849276] [planning.behavior_planner_node]: Trying to change gear
[behavior_planner_node_exe-18] [INFO] [1618312770.476386464] [planning.behavior_planner_node]: Trying to change gear
[behavior_planner_node_exe-18] [INFO] [1618312773.476403926] [planning.behavior_planner_node]: Trying to change gear
[behavior_planner_node_exe-18] [INFO] [1618312776.476439190] [planning.behavior_planner_node]: Trying to change gear
[behavior_planner_node_exe-18] [INFO] [1618312779.479450830] [planning.behavior_planner_node]: Trying to change gear
No gear is actually changing and it will stay like this until a new command is given.
Expected behavior
Vehicle shouldn't be trying to change gears or stating that it is trying to change gears if none of the gears are changing.
Edited by Jilada Eccleston