if (rn->ccost() + gn->ccost() < mc &&
IS_NEAR(rn, gn)) {
gf = true;
+ p.goal_found(true);
ron = rn;
gon = gn;
mc = rn->ccost() + gn->ccost();
}
pthread_join(rt, NULL);
pthread_join(gt, NULL);
- p.goal_found(gf);
// connect trees
while (gon != p.goal()) {
p.p_root_.nodes().push_back(new RRTNode(