public RRTPlannerImpl(RobotTwoWheelStateProvider stateProvider)
 {
     rrt = new RRTPlannerControl();
     obstacles = new List<Polygon>();
     currentPose = new RobotPose();
     waypoints = new PointPath();
     this.stateProvider = stateProvider;
 }
 public RRTPlannerImpl(RobotTwoWheelStateProvider stateProvider)
 {
     rrt                = new RRTPlannerControl();
     obstacles          = new List <Polygon>();
     currentPose        = new RobotPose();
     waypoints          = new PointPath();
     this.stateProvider = stateProvider;
 }