Add pure-pursuit ctrl for test track

diff --git a/README.md b/README.md
index 233e79767cbfff6f077a5c3ba0db3f10b838dc84..d10e6f481b4e459e30c023cde5fa967e118c0371 100644 (file)
--- 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 (file)
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 cc911ce17f8bdd64ee9a0762b7cb6c681ac47b94..e814d818d5d862c32ff86f214f6ae575f3a14b74 100644 (file)
--- 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 d34238bac90872dd981888655be1209f4b737baf..2afb8df87e4932fba696e7f4e9c84725443c7447 100644 (file)
--- 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
+)