struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
-/**
- * Safe distance for target recognition
- */
-const double approach_radius = TARGET_RADIUS_M + 2.0*MAP_CELL_SIZE_M + ROBOT_DIAGONAL_RADIUS_M;
-
void set_initial_position()
{
robot_set_est_pos_trans(ROBOT_START_X_M, ROBOT_START_Y_M, DEG2RAD(ROBOT_START_ANGLE_DEG));