RRT visitor calls in wrong location

[adamlm]

The RRT visitor's on_add_node() function gets called in the wrong location. The call for the source node in the tree should happen only once. Instead, it gets called on each loop iteration.