Update planner.cpp