From 55d8844933139ce78b755e5d4a255d80e1d616aa Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Thu, 17 Jun 2021 17:27:14 +0200
Subject: [PATCH] Add pure-pursuit ctrl for test track

---
 README.md               |  3 ++
 ctrl/ttpp.cc            | 95 +++++++++++++++++++++++++++++++++++++++++
 meson.build             |  5 +++
 worlds/test_track.world |  8 ++++
 4 files changed, 111 insertions(+)
 create mode 100644 ctrl/ttpp.cc

diff --git a/README.md b/README.md
index 233e797..d10e6f4 100644
--- a/README.md
+++ b/README.md
@@ -60,4 +60,7 @@ group. The `libgoforward` controls the robot to just go straight.
 `libgobackward` is the same as `libgoforward` but with the negative speed. It's
 written in more OOP way.
 
+`libttpp` implements Pure-Pursuit controller with static target points of test
+track. It's based on `libgobackward`.
+
 [IID]: https://iid.ciirc.cvut.cz/
diff --git a/ctrl/ttpp.cc b/ctrl/ttpp.cc
new file mode 100644
index 0000000..4bde8a9
--- /dev/null
+++ b/ctrl/ttpp.cc
@@ -0,0 +1,95 @@
+#include "stage.hh"
+using namespace Stg;
+
+#define PATH_LEN 17
+#define MIN_LADIST 0.5
+#define WHEELBASE 0.32
+
+struct point {
+	double x;
+	double y;
+};
+
+struct pose {
+	double x;
+	double y;
+	double h;
+};
+
+struct point PATH[] = {
+	{3.5, -8.5},
+	{5, -6},
+	{3, -4},
+	{-4, -3.5},
+	{-5.5, -0.5},
+	{-4, 1.5},
+	{-1, 1},
+	{6, -2},
+	{8.5, 3},
+	{6, 4.5},
+	{3, 5.5},
+	{0, 8},
+	{-6, 8},
+	{-8, 0},
+	{-8, -6},
+	{-7, -8},
+	{-5, -8.5},
+};
+
+class ControlledRobot {
+private:
+	ModelPosition *pos = nullptr;
+	unsigned int tpi = 0; // target pose index
+public:
+	ControlledRobot(Model *m) : pos(dynamic_cast<ModelPosition *>(m))
+	{
+		assert(this->pos != nullptr);
+		this->pos->Subscribe();
+	}
+	ModelPosition *get_pos() const { return this->pos; }
+	unsigned int get_tpi() const { return this->tpi; }
+	void incr_tpi() { this->tpi = (this->tpi + 1) % PATH_LEN; }
+};
+
+struct pose point_to_pose(struct point const p)
+{
+	return {p.x, p.y, 0.0};
+}
+
+double edist(struct pose const p1, struct pose const p2)
+{
+	double dx = p2.x - p1.x;
+	double dy = p2.y - p1.y;
+	return sqrt(dx * dx + dy * dy);
+}
+
+double pp(struct pose const ep, struct pose const tp)
+// see https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PurePursuit.html
+{
+	double alpha = atan2(tp.y - ep.y, tp.x - ep.x);
+	alpha -= ep.h;
+	return atan(2 * WHEELBASE * sin(alpha) / edist(ep, tp));
+}
+
+int steer(Model *m, ControlledRobot *r)
+{
+	auto p = r->get_pos()->GetPose();
+	struct pose ep = {p.x, p.y, p.a};
+	assert(r->get_tpi() < PATH_LEN);
+	struct pose tp = point_to_pose(PATH[r->get_tpi()]);
+	if (edist(ep, tp) < MIN_LADIST) {
+		r->incr_tpi();
+		tp = point_to_pose(PATH[r->get_tpi()]);
+		m->GetWorld()->GetModel("tp")->SetPose({tp.x, tp.y, 0.0, 0.0});
+	}
+	r->get_pos()->SetTurnSpeed(pp(ep, tp));
+	return 0;
+}
+
+extern "C" int Init(Model *model, CtrlArgs *)
+{
+	auto r = new ControlledRobot(model);
+	r->get_pos()->AddCallback(Model::CB_UPDATE, model_callback_t(steer), r);
+	r->get_pos()->SetXSpeed(2.0);
+	return 0;
+}
diff --git a/meson.build b/meson.build
index cc911ce..e814d81 100644
--- a/meson.build
+++ b/meson.build
@@ -14,3 +14,8 @@ library('gobackward',
 	dependencies: stgdep,
 	include_directories: stginc,
 )
+library('ttpp',
+	'ctrl/ttpp.cc',
+	dependencies: stgdep,
+	include_directories: stginc,
+)
diff --git a/worlds/test_track.world b/worlds/test_track.world
index d34238b..2afb8df 100644
--- a/worlds/test_track.world
+++ b/worlds/test_track.world
@@ -20,3 +20,11 @@ car (
 	color "blue"
 	ctrl "libgoforward"
 )
+
+position (
+	pose [ 3.5 -8.5 0 0]
+	name "tp"
+	color "green"
+	obstacle_return 0
+	ranger_return 0
+)
-- 
2.39.2