From f85a83aeda45d43008def1e4825c8f7a64fd1b5f Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 20 Mar 2023 13:13:05 +0100 Subject: [PATCH] Fix ext13 (path opt) because of current changes --- rrts/src/rrtext13.cc | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/rrts/src/rrtext13.cc b/rrts/src/rrtext13.cc index 1619d7b..7277bdb 100644 --- a/rrts/src/rrtext13.cc +++ b/rrts/src/rrtext13.cc @@ -37,7 +37,7 @@ RRTExt13::interesting_forward() i = j; break; } - if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) { + if (nj.p_is_cusp()) { this->dn_.push_back(DijkstraNode(&njl)); this->dn_.back().i = this->dn_.size() - 1; } @@ -79,7 +79,7 @@ RRTExt13::interesting_backward() i = j; break; } - if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) { + if (nj.p_is_cusp()) { this->dn_.push_back(DijkstraNode(&njl)); this->dn_.back().i = this->dn_.size() - 1; this->dn_.back().d = goal_cc - njl.cc(); @@ -119,12 +119,14 @@ RRTExt13::dijkstra_forward() || ss != this->_steered.size()) { continue; } + // The first steered is the same as f. + this->_steered.erase(this->_steered.begin()); this->set_bc_pose_to(this->_steered.back()); bool td = this->_bc.drivable(t); bool tn = this->_bc.edist(t) < 2.0 * this->_eta; if (cost < t.cc() && td && tn) { this->join_steered(&f); - t.p(this->_nodes.back()); + t.p(this->_nodes.back(), true); t.c(this->cost_build(this->_nodes.back(), t)); if (!this->dn_[i].vi()) { pq.push(this->dn_[i]); @@ -162,13 +164,15 @@ RRTExt13::dijkstra_backward() || ss != this->_steered.size()) { continue; } + // The first steered is the same as f. + this->_steered.erase(this->_steered.begin()); this->set_bc_pose_to(this->_steered.back()); bool td = this->_bc.drivable(t); bool tn = this->_bc.edist(t) < 2.0 * this->_eta; if (cost < fd.d && td && tn) { fd.d = cost; this->join_steered(&f); - t.p(this->_nodes.back()); + t.p(this->_nodes.back(), true); t.c(this->cost_build(this->_nodes.back(), t)); this->recompute_cc_for_predecessors_and(&t); if (!this->dn_[i].vi()) { -- 2.39.2