From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Tue, 31 May 2022 15:27:27 +0000 (+0200)
Subject: Add path and path cost getters
X-Git-Tag: v0.10.0^0
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/13440e209019d5948e3a96ff7c4854a50907aeff?ds=sidebyside

Add path and path cost getters
---

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 7b730c1..e0930d5 100644
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -120,6 +120,12 @@ public:
 	/*! Set start. */
 	void set_start(double x, double y, double h);
 
+	/*! Get path. */
+	std::vector<Pose> get_path() const;
+
+	/*! Get path cost. */
+	double get_path_cost() const;
+
 	/*! Get iterations counter. */
 	unsigned int icnt() const;
 
diff --git a/src/rrts.cc b/src/rrts.cc
index 46f289e..1d66b3c 100644
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -292,6 +292,22 @@ RRTS::set_start(double x, double y, double h)
 	this->nodes_.front().h(h);
 }
 
+std::vector<Pose>
+RRTS::get_path() const
+{
+	std::vector<Pose> path;
+	for (auto n: this->path_) {
+		path.push_back(Pose(n->x(), n->y(), n->h()));
+	}
+	return path;
+}
+
+double
+RRTS::get_path_cost() const
+{
+	return this->goal_.cc();
+}
+
 unsigned int
 RRTS::icnt() const
 {