ctrl/ttpp.cc

   1 #include "stage.hh"
   2 using namespace Stg;
   3
   4 #define PATH_LEN 17
   5 #define MIN_LADIST 0.5
   6 #define WHEELBASE 0.32
   7
   8 struct point {
   9         double x;
  10         double y;
  11 };
  12
  13 struct pose {
  14         double x;
  15         double y;
  16         double h;
  17 };
  18
  19 struct point PATH[] = {
  20         {3.5, -8.5},
  21         {5, -6},
  22         {3, -4},
  23         {-4, -3.5},
  24         {-5.5, -0.5},
  25         {-4, 1.5},
  26         {-1, 1},
  27         {6, -2},
  28         {8.5, 3},
  29         {6, 4.5},
  30         {3, 5.5},
  31         {0, 8},
  32         {-6, 8},
  33         {-8, 0},
  34         {-8, -6},
  35         {-7, -8},
  36         {-5, -8.5},
  37 };
  38
  39 class ControlledRobot {
  40 private:
  41         ModelPosition *pos = nullptr;
  42         unsigned int tpi = 0; // target pose index
  43 public:
  44         ControlledRobot(Model *m) : pos(dynamic_cast<ModelPosition *>(m))
  45         {
  46                 assert(this->pos != nullptr);
  47                 this->pos->Subscribe();
  48         }
  49         ModelPosition *get_pos() const { return this->pos; }
  50         unsigned int get_tpi() const { return this->tpi; }
  51         void incr_tpi() { this->tpi = (this->tpi + 1) % PATH_LEN; }
  52 };
  53
  54 struct pose point_to_pose(struct point const p)
  55 {
  56         return {p.x, p.y, 0.0};
  57 }
  58
  59 double edist(struct pose const p1, struct pose const p2)
  60 {
  61         double dx = p2.x - p1.x;
  62         double dy = p2.y - p1.y;
  63         return sqrt(dx * dx + dy * dy);
  64 }
  65
  66 double pp(struct pose const ep, struct pose const tp)
  67 // see https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PurePursuit.html
  68 {
  69         double alpha = atan2(tp.y - ep.y, tp.x - ep.x);
  70         alpha -= ep.h;
  71         return atan(2 * WHEELBASE * sin(alpha) / edist(ep, tp));
  72 }
  73
  74 int steer(Model *m, ControlledRobot *r)
  75 {
  76         auto p = r->get_pos()->GetPose();
  77         struct pose ep = {p.x, p.y, p.a};
  78         assert(r->get_tpi() < PATH_LEN);
  79         struct pose tp = point_to_pose(PATH[r->get_tpi()]);
  80         if (edist(ep, tp) < MIN_LADIST) {
  81                 r->incr_tpi();
  82                 tp = point_to_pose(PATH[r->get_tpi()]);
  83                 m->GetWorld()->GetModel("tp")->SetPose({tp.x, tp.y, 0.0, 0.0});
  84         }
  85         r->get_pos()->SetTurnSpeed(pp(ep, tp));
  86         return 0;
  87 }
  88
  89 extern "C" int Init(Model *model, CtrlArgs *)
  90 {
  91         auto r = new ControlledRobot(model);
  92         r->get_pos()->AddCallback(Model::CB_UPDATE, model_callback_t(steer), r);
  93         r->get_pos()->SetXSpeed(2.0);
  94         return 0;
  95 }