- Alternative steering procedure.
- Reeds and Shepp cost, steering procedures.
- Bicycle car simulation.
+- Closed loop controller with PI for speed and PurePursuit based on
+ [Coulter1992] paper for steering.
### Changed
- Adding JSON ouput for edges, samples.
std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal);
std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal);
std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal);
+std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal);
#endif
*/
#include <cmath>
+#include "bcar.h"
#include "reeds_shepp.h"
#include "steer.h"
#define ST2WHEELBASE 2.450
#define ST2TURNINGRADI 10.82
#define ST2MAXSTEERING atan(ST2WHEELBASE / ST2TURNINGRADI)
+#define ST3COUNT 10
+#define ST3KP 1
+#define ST3KI 0
+#define ST3KD 0
std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
{
rsss.sample(q0, q1, 1, cb_st3, &nodes); // TODO const
return nodes;
}
+
+std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
+{
+ std::vector<RRTNode *> nodes;
+ BicycleCar bc(init->x(), init->y(), init->h());
+
+ float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
+ float co = cos(angl - init->h());
+ //float si = sin(angl - init->h());
+
+ float speed = 1; // desired
+ float gx = goal->x();
+ float gy = goal->y();
+ if (co < 0)
+ speed = -speed;
+ float cerr = 0;
+ float lerr = 0;
+ float rx = 0; // recomputed goal x
+ float ry = 0; // recomputed goal y
+ float r = 0;
+ int i = 0;
+ for (i = 0; i < ST3COUNT; i++) {
+ // speed controller
+ cerr = speed - bc.speed();
+ bc.speed(ST3KP * cerr + ST3KI * lerr);
+ lerr += cerr;
+ // steer controller
+ rx = (gx - bc.x()) * cos(-bc.h()) -
+ (gy - bc.y()) * sin(-bc.h());
+ ry = (gx - bc.x()) * sin(-bc.h()) -
+ (gy - bc.y()) * cos(-bc.h());
+ if (ry != 0) {
+ r = pow(rx, 2) + pow(ry, 2) / (2 * ry);
+ bc.steer(1 / r);
+ }
+ // next iteration
+ bc.next();
+ nodes.push_back(new RRTNode(bc.x(), bc.y(), bc.h()));
+ }
+ return nodes;
+}