#include "obstacle_marker.H"
#include <cmath>
#include <algorithm>
#include <cassert>
#include "uc_planner_param.H"