CForest first version

Sorry about the (void) convention in functions, this is what happens when you copy and paste ignoring the other changes :)  Other RRTstar tweaks have been also overwritten.
I would do a scoped lock here, to avoid the situation of multiple planners calling this function at the same time.
the easiest way is to declare a mutex in CForest class:
boost::mutex newSolutionFoundMutex_;
and then at the top of this function,
boost::mutex::scoped_lock slock(newSolutionFoundMutex_);