X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/blobdiff_plain/d091467a1fd22b8b85bca0fad37738aa9d06ccdd..41afde61c4a85d0eb68521a232c43a012330b4aa:/rrts/src/rrts.cc

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 7443bff..1520d61 100644
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -55,25 +55,37 @@ RRTNode::p() const
 }
 
 void
-RRTNode::p(RRTNode& p)
+RRTNode::p(RRTNode& p, bool can_be_too_close)
 {
-	if (this != &p) {
-		this->_p = &p;
+	assert(this != &p);
+	if (!can_be_too_close) {
+		assert(!(std::abs(p.x() - this->x()) < 1e-3
+			&& std::abs(p.y() - this->y()) < 1e-3
+			&& std::abs(p.h() - this->h()) < 1e-3));
 	}
+	this->_p = &p;
+	this->p_is_cusp(this->would_be_cusp_if_parent(p));
+	this->cusp_cnt(p);
+}
+
+void
+RRTNode::p(RRTNode& p)
+{
+	return this->p(p, false);
 }
 
 unsigned int
-RRTNode::cusp() const
+RRTNode::cusp_cnt() const
 {
-	return this->_cusp;
+	return this->_cusp_cnt;
 }
 
 void
-RRTNode::cusp(RRTNode const& p)
+RRTNode::cusp_cnt(RRTNode const& p)
 {
-	this->_cusp = p.cusp();
-	if (this->sp() != p.sp() || this->sp() == 0.0) {
-		this->_cusp++;
+	this->_cusp_cnt = p.cusp_cnt();
+	if (this->_p_is_cusp) {
+		this->_cusp_cnt++;
 	}
 }
 
@@ -89,6 +101,45 @@ RRTNode::st(int st)
 	this->_segment_type = st;
 }
 
+bool
+RRTNode::p_is_cusp(void) const
+{
+	return this->_p_is_cusp;
+}
+
+void
+RRTNode::p_is_cusp(bool isit)
+{
+	this->_p_is_cusp = isit;
+}
+
+bool
+RRTNode::would_be_cusp_if_parent(RRTNode const& p) const
+{
+	if (std::abs(p.sp()) < 1e-3) {
+		// It should not be possible to have two zero speed nodes next
+		// to each other. In practice, this sometimes happens.
+		//assert(std::abs(this->sp()) > 1e-3);
+		if (p.p()) {
+			if (sgn(p.p()->sp()) != sgn(this->sp())) {
+				return true;
+			} else {
+				return false;
+			}
+		} else {
+			return true; // only root has no parent and it is cusp
+		}
+	} else {
+		if (std::abs(this->sp()) < 1e-3) {
+			return false; // this is cusp, not the parent
+		} else if (sgn(p.sp()) != sgn(this->sp())) {
+			return true;
+		} else {
+			return false;
+		}
+	}
+}
+
 bool
 RRTNode::operator==(RRTNode const& n)
 {
@@ -96,32 +147,32 @@ RRTNode::operator==(RRTNode const& n)
 }
 
 void
-RRTS::recompute_cc(RRTNode* g)
+RRTS::recompute_cc_for_predecessors_and(RRTNode* g)
 {
-	this->path_.clear();
+	std::vector<RRTNode*> path;
+	path.reserve(this->_path.size());
 	while (g != nullptr) {
-		this->path_.push_back(g);
+		path.push_back(g);
 		g = g->p();
 	}
-	std::reverse(this->path_.begin(), this->path_.end());
-	for (unsigned int i = 1; i < this->path_.size(); i++) {
-		this->path_[i]->c(this->cost_build(*this->path_[i - 1],
-			*this->path_[i]));
+	std::reverse(path.begin(), path.end());
+	for (unsigned int i = 1; i < path.size(); i++) {
+		path[i]->c(this->cost_build(*path[i - 1], *path[i]));
 	}
 }
 
 void
 RRTS::recompute_path_cc()
 {
-	this->recompute_cc(&this->goal_);
+	this->recompute_cc_for_predecessors_and(&this->_goal);
 }
 
 double
 RRTS::min_gamma_eta() const
 {
-	double ns = this->nodes_.size();
+	double ns = this->_nodes.size();
 	double gamma = pow(log(ns) / ns, 1.0 / 3.0);
-	return std::min(gamma, this->eta_);
+	return std::min(gamma, this->eta());
 }
 
 bool
@@ -133,28 +184,30 @@ RRTS::should_continue() const
 void
 RRTS::join_steered(RRTNode* f)
 {
-	while (this->steered_.size() > 0) {
-		this->store(this->steered_.front());
-		RRTNode* t = &this->nodes_.back();
+	while (this->_steered.size() > 0) {
+		this->store(this->_steered.front());
+		RRTNode* t = &this->_nodes.back();
 		t->p(*f);
 		t->c(this->cost_build(*f, *t));
-		t->cusp(*f);
-		this->steered_.erase(this->steered_.begin());
+		this->_steered.erase(this->_steered.begin());
 		f = t;
 	}
 }
 
-RRTNode&
-RRTS::nn()
-{
-	return *this->nn_;
-}
-
 bool
 RRTS::connect()
 {
-	RRTNode* f = this->nn_;
-	RRTNode* t = &this->steered_.front();
+	RRTNode* f = this->_nn;
+	RRTNode* t = &this->_steered.front();
+	// Require the steer method to return first node equal to nn:
+	assert(std::abs(t->x() - f->x()) < 1e-3
+		&& std::abs(t->y() - f->y()) < 1e-3
+		&& std::abs(t->h() - f->h()) < 1e-3);
+	this->_steered.erase(this->_steered.begin());
+	t = &this->_steered.front();
+	assert(!(std::abs(t->x() - f->x()) < 1e-3
+		&& std::abs(t->y() - f->y()) < 1e-3
+		&& std::abs(t->h() - f->h()) < 1e-3));
 #if USE_RRTS
 	double cost = f->cc() + this->cost_build(*f, *t);
 	for (auto n: this->nv_) {
@@ -165,30 +218,29 @@ RRTS::connect()
 		}
 	}
 	// Check if it's possible to drive from *f to *t. If not, then fallback
-	// to *f = nn_. This could be also solved by additional steer from *f to
+	// to *f = _nn. This could be also solved by additional steer from *f to
 	// *t instead of the following code.
-	this->bc_.set_pose(*f);
-	if (!this->bc_.drivable(*t)) {
-		f = this->nn_;
+	this->set_bc_pose_to(*f);
+	if (!this->_bc.drivable(*t)) {
+		f = this->_nn;
 	}
 #endif
-	this->store(this->steered_.front());
-	t = &this->nodes_.back();
+	this->store(this->_steered.front());
+	t = &this->_nodes.back();
 	t->p(*f);
 	t->c(this->cost_build(*f, *t));
-	t->cusp(*f);
-	this->steered_.erase(this->steered_.begin());
+	this->_steered.erase(this->_steered.begin());
 	return true;
 }
 
 void
 RRTS::rewire()
 {
-	RRTNode *f = &this->nodes_.back();
-	for (auto n: this->nv_) {
+	RRTNode *f = &this->_nodes.back();
+	for (auto n: this->_nv) {
 		double fc = f->cc() + this->cost_build(*f, *n);
-		this->bc_.set_pose(*f);
-		bool drivable = this->bc_.drivable(*n);
+		this->set_bc_pose_to(*f);
+		bool drivable = this->_bc.drivable(*n);
 		if (drivable && fc < n->cc()) {
 			n->p(*f);
 			n->c(this->cost_build(*f, *n));
@@ -199,14 +251,14 @@ RRTS::rewire()
 bool
 RRTS::goal_drivable_from(RRTNode const& f)
 {
-	this->bc_.set_pose(f);
-	return this->bc_.drivable(this->goal_);
+	this->set_bc_pose_to(f);
+	return this->_bc.drivable(this->_goal);
 }
 
 void
 RRTS::store(RRTNode n)
 {
-	this->nodes_.push_back(n);
+	this->_nodes.push_back(n);
 }
 
 double
@@ -224,12 +276,12 @@ RRTS::cost_search(RRTNode const& f, RRTNode const& t) const
 void
 RRTS::find_nn(RRTNode const& t)
 {
-	this->nn_ = &this->nodes_.front();
-	this->cost_ = this->cost_search(*this->nn_, t);
-	for (auto& f: this->nodes_) {
-		if (this->cost_search(f, t) < this->cost_) {
-			this->nn_ = &f;
-			this->cost_ = this->cost_search(f, t);
+	this->_nn = &this->_nodes.front();
+	this->_cost = this->cost_search(*this->_nn, t);
+	for (auto& f: this->_nodes) {
+		if (this->cost_search(f, t) < this->_cost) {
+			this->_nn = &f;
+			this->_cost = this->cost_search(f, t);
 		}
 	}
 }
@@ -237,11 +289,11 @@ RRTS::find_nn(RRTNode const& t)
 void
 RRTS::find_nv(RRTNode const& t)
 {
-	this->nv_.clear();
-	this->cost_ = this->min_gamma_eta();
-	for (auto& f: this->nodes_) {
-		if (this->cost_search(f, t) < this->cost_) {
-			this->nv_.push_back(&f);
+	this->_nv.clear();
+	this->_cost = this->min_gamma_eta();
+	for (auto& f: this->_nodes) {
+		if (this->cost_search(f, t) < this->_cost) {
+			this->_nv.push_back(&f);
 		}
 	}
 }
@@ -250,16 +302,16 @@ void
 RRTS::compute_path()
 {
 	this->_path.clear();
-	RRTNode *g = &this->goal();
+	RRTNode *g = &this->_goal;
 	if (g->p() == nullptr) {
 		return;
 	}
 	while (g != nullptr && this->_path.size() < 10000) {
 		/* FIXME in ext13
 		 *
-		 * There shouldn't be this->path_.size() < 10000 condition.
+		 * There shouldn't be this->_path.size() < 10000 condition.
 		 * However, the RRTS::compute_path() called from
-		 * RRTExt13::compute_path tends to re-allocate this->path_
+		 * RRTExt13::compute_path tends to re-allocate this->_path
 		 * infinitely. There's probably node->p() = &node somewhere...
 		 */
 		this->_path.push_back(g);
@@ -268,7 +320,7 @@ RRTS::compute_path()
 	std::reverse(this->_path.begin(), this->_path.end());
 }
 
-RRTS::RRTS() : goal_(0.0, 0.0, 0.0, 0.0), gen_(std::random_device{}())
+RRTS::RRTS() : _goal(0.0, 0.0, 0.0, 0.0), _gen(std::random_device{}())
 {
 	this->_nodes.reserve(4000000);
 	this->_steered.reserve(1000);
@@ -277,13 +329,19 @@ RRTS::RRTS() : goal_(0.0, 0.0, 0.0, 0.0), gen_(std::random_device{}())
 	this->store(RRTNode()); // root
 }
 
-BicycleCar&
-RRTS::bc(void) const
+void
+RRTS::set_bc_pose_to(Pose const& p)
+{
+	this->_bc.set_pose_to(p);
+}
+
+void
+RRTS::set_bc_to_become(std::string what)
 {
-	return this->_bc;
+	this->_bc.become(what);
 }
 
-RRTGoal&
+RRTGoal const&
 RRTS::goal(void) const
 {
 	return this->_goal;
@@ -319,6 +377,12 @@ RRTS::icnt_max(unsigned int i)
 	this->_icnt_max = i;
 }
 
+void
+RRTS::tstart(void)
+{
+	this->_ter.start();
+}
+
 double
 RRTS::scnt() const
 {
@@ -326,11 +390,11 @@ RRTS::scnt() const
 }
 
 void
-RRTS::start(double x, double y, double h)
+RRTS::set_init_pose_to(Pose const& p)
 {
-	this->_nodes.front().x(x);
-	this->_nodes.front().y(y);
-	this->_nodes.front().h(h);
+	this->_nodes.front().x(p.x());
+	this->_nodes.front().y(p.y());
+	this->_nodes.front().h(p.h());
 }
 
 std::vector<Pose>
@@ -346,19 +410,17 @@ RRTS::path() const
 double
 RRTS::path_cost() const
 {
-	return this->goal().cc();
+	return this->_goal.cc();
 }
 
 double
 RRTS::last_path_cost(void) const
 {
-	return this->_last_path_cost;
-}
-
-void
-RRTS::last_path_cost(double c)
-{
-	this->_last_path_cost = c;
+	if (this->_logged_paths.size() == 0) {
+		return 0.0;
+	}
+	assert(this->_logged_paths.back().size() > 0);
+	return this->_logged_paths.back().back().cc();
 }
 
 double
@@ -374,19 +436,67 @@ RRTS::eta(double e)
 }
 
 Json::Value
-RRTS::json() const
+RRTS::json(void) const
 {
 	Json::Value jvo;
-	unsigned int i = 0;
-	for (auto n: this->_path) { // TODO because path() has just x, y, h
+	unsigned int i = 0, j = 0;
+	for (auto path: this->_logged_paths) {
+		i = 0;
+		for (auto n: path) {
+			jvo["paths"][j][i][0] = n.x();
+			jvo["paths"][j][i][1] = n.y();
+			jvo["paths"][j][i][2] = n.h();
+			jvo["paths"][j][i][3] = n.sp();
+			jvo["paths"][j][i][4] = n.st();
+			jvo["paths"][j][i][5] = false;
+			if (n.p_is_cusp()) {
+				assert(i > 0);
+				jvo["paths"][j][i - 1][5] = true;
+			}
+			i++;
+		}
+		// Initial point is part of the first segment.
+		jvo["paths"][j][0][3] = jvo["paths"][j][1][3];
+		jvo["paths"][j][0][4] = jvo["paths"][j][1][4];
+		// Goal point is part of the last segment.
+		jvo["paths"][j][i - 1][3] = jvo["paths"][j][i - 2][3];
+		jvo["paths"][j][i - 1][4] = jvo["paths"][j][i - 2][4];
+		// --
+		jvo["costs"][j] = path.back().cc();
+		j++;
+	}
+	i = 0;
+	for (auto n: this->_path) {
+		jvo["paths"][j][i][0] = n->x();
+		jvo["paths"][j][i][1] = n->y();
+		jvo["paths"][j][i][2] = n->h();
+		jvo["paths"][j][i][3] = n->sp();
+		jvo["paths"][j][i][4] = n->st();
+		jvo["paths"][j][i][5] = n->p_is_cusp();
 		jvo["path"][i][0] = n->x();
 		jvo["path"][i][1] = n->y();
 		jvo["path"][i][2] = n->h();
 		jvo["path"][i][3] = n->sp();
 		jvo["path"][i][4] = n->st();
+		jvo["path"][i][5] = false;
+		if (n->p_is_cusp()) {
+			assert(i > 0);
+			jvo["path"][i - 1][5] = true;
+		}
 		i++;
 	}
-	jvo["goal_cc"] = this->goal().cc();
+	// Initial point is part of the first segment.
+	jvo["path"][0][3] = jvo["path"][1][3];
+	jvo["path"][0][4] = jvo["path"][1][4];
+	// Goal point is part of the last segment.
+	jvo["path"][i - 1][3] = jvo["path"][i - 2][3];
+	jvo["path"][i - 1][4] = jvo["path"][i - 2][4];
+	// --
+	if (this->_path.size() > 0) {
+		jvo["costs"][j] = this->_path.back()->cc();
+	}
+	jvo["goal_cc"] = this->_goal.cc(); // TODO remove, use the following
+	jvo["cost"] = this->path_cost();
 	jvo["time"] = this->scnt();
 	return jvo;
 }
@@ -396,10 +506,10 @@ RRTS::json(Json::Value jvi)
 {
 	assert(jvi["init"] != Json::nullValue);
 	assert(jvi["goal"] != Json::nullValue);
-	this->start(
+	this->set_init_pose_to(Pose(
 		jvi["init"][0].asDouble(),
 		jvi["init"][1].asDouble(),
-		jvi["init"][2].asDouble());
+		jvi["init"][2].asDouble()));
 	if (jvi["goal"].size() == 4) {
 		this->goal(
 			jvi["goal"][0].asDouble(),
@@ -418,27 +528,27 @@ RRTS::json(Json::Value jvi)
 bool
 RRTS::next()
 {
-	if (this->icnt_ == 0) {
-		this->ter_.start();
+	if (this->icnt() == 0) {
+		this->tstart();
 	}
-	this->icnt_ += 1;
 	auto rs = this->sample();
 #if 1 // anytime RRTs
 {
-	double d1 = this->cost_search(this->nodes_.front(), rs);
-	double d2 = this->cost_search(rs, this->goal_);
-	if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) {
-		rs = this->last_path_[rand() % this->last_path_.size()];
+	double d1 = this->cost_search(this->_nodes.front(), rs);
+	double d2 = this->cost_search(rs, this->_goal);
+	if (this->last_path_cost() != 0.0 && d1 + d2 > this->last_path_cost()) {
+		auto& last_path = this->_logged_paths.back();
+		rs = last_path[rand() % last_path.size()];
 	}
 }
 #endif
 	this->find_nn(rs);
-	this->steer(this->nn(), rs);
+	this->steer(*this->_nn, rs);
 	if (this->collide_steered()) {
 		return this->should_continue();
 	}
 #if USE_RRTS
-	this->find_nv(this->steered_.front());
+	this->find_nv(this->_steered.front());
 #endif
 	if (!this->connect()) {
 		return this->should_continue();
@@ -446,28 +556,31 @@ RRTS::next()
 #if USE_RRTS
 	this->rewire();
 #endif
-	unsigned int ss = this->steered_.size();
-	this->join_steered(&this->nodes_.back());
-	RRTNode* just_added = &this->nodes_.back();
+	unsigned int ss = this->_steered.size();
+	this->join_steered(&this->_nodes.back());
+	RRTNode* just_added = &this->_nodes.back();
 	bool gf = false;
-	while (ss > 0 && just_added->p() != nullptr) {
-		this->steer(*just_added, this->goal_);
+	while (ss > 0 && just_added != nullptr) {
+		this->steer(*just_added, this->_goal);
 		if (this->collide_steered()) {
 			ss--;
 			just_added = just_added->p();
 			continue;
 		}
+		// The first of steered is the same as just_added.
+		this->_steered.erase(this->_steered.begin());
 		this->join_steered(just_added);
-		bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
-		bool gd = this->goal_drivable_from(this->nodes_.back());
+		bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
+		bool gd = this->goal_drivable_from(this->_nodes.back());
 		if (gn && gd) {
-			double nc = this->cost_build(this->nodes_.back(),
-				this->goal_);
-			double ncc = this->nodes_.back().cc() + nc;
-			if (this->goal_.p() == nullptr
-					|| ncc < this->goal_.cc()) {
-				this->goal_.p(this->nodes_.back());
-				this->goal_.c(nc);
+			double nc = this->cost_build(
+				this->_nodes.back(),
+				this->_goal);
+			double ncc = this->_nodes.back().cc() + nc;
+			if (this->_goal.p() == nullptr
+					|| ncc < this->_goal.cc()) {
+				this->_goal.p(this->_nodes.back(), true);
+				this->_goal.c(nc);
 				gf = true;
 			}
 		}
@@ -477,7 +590,8 @@ RRTS::next()
 	if (gf) {
 		this->compute_path();
 	}
-	this->time_ = this->ter_.scnt();
+	this->_time = this->scnt();
+	this->icnt(this->icnt() + 1);
 	return this->should_continue();
 }
 
@@ -486,17 +600,31 @@ RRTS::reset()
 {
 	if (this->path_cost() != 0.0
 			&& this->path_cost() < this->last_path_cost()) {
-		this->last_path_cost(this->path_cost);
-		this->_last_path.clear();
+		this->_logged_paths.push_back(std::vector<RRTNode>());
+		auto& last_path = this->_logged_paths.back();
+		last_path.reserve(this->_path.size());
+		RRTNode* p = nullptr;
 		for (auto n: this->_path) {
-			this->_last_path.push_back(*n);
+			last_path.push_back(*n);
+			if (p != nullptr) {
+				last_path.back().p(*p);
+			}
+			p = &last_path.back();
+		}
+		// Test that last path cost matches.
+		auto last_path_cost = last_path.back().cc();
+		for (unsigned int i = 1; i < last_path.size(); i++) {
+			last_path[i].c(this->cost_build(
+				last_path[i - 1],
+				last_path[i]));
 		}
+		assert(last_path_cost == last_path.back().cc());
 	}
-	this->goal(
-		this->goal().x(),
-		this->goal().y(),
-		this->goal().b(),
-		this->goal().e());
+	this->_goal = RRTGoal(
+		this->_goal.x(),
+		this->_goal.y(),
+		this->_goal.b(),
+		this->_goal.e());
 	this->_path.clear();
 	this->_steered.clear();
 	this->_nodes.erase(this->_nodes.begin() + 1, this->_nodes.end());