Added tree implementation for RRT planner