From 13440e209019d5948e3a96ff7c4854a50907aeff Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Tue, 31 May 2022 17:27:27 +0200
Subject: [PATCH] Add path and path cost getters

---
 incl/rrts.hh |  6 ++++++
 src/rrts.cc  | 16 ++++++++++++++++
 2 files changed, 22 insertions(+)

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
 {
-- 
2.39.2