Update dt_planner_ros.cpp