From 205c4668ab9463ca69b6b011706aec43bdcaed4b Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 17 Jun 2019 10:45:51 +0200 Subject: [PATCH] Decide to steer forward/backward in st5 --- incl/rrtnode.h | 4 ++++ vehicle_platform/steer.cc | 4 +++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/incl/rrtnode.h b/incl/rrtnode.h index 24fb4b4..a515386 100644 --- a/incl/rrtnode.h +++ b/incl/rrtnode.h @@ -109,6 +109,10 @@ class RRTNode { static bool comp_ccost(RRTNode *n1, RRTNode *n2); float update_ccost(); bool visit(); + /** Return ``true`` if ``n`` is in front of ``this``. + * + * @param n The node that is beeing checked. + */ bool inFront(RRTNode *n); }; diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc index 4207971..0954e13 100644 --- a/vehicle_platform/steer.cc +++ b/vehicle_platform/steer.cc @@ -165,7 +165,9 @@ int cb_st5(double q[3], double t, void *user_data) std::vector st5(RRTNode *init, RRTNode *goal) { - return st5(init, goal, ST5STEP, false); + if (init->inFront(goal)) + return st5(init, goal, ST5STEP, false); + return st5(init, goal, ST5STEP, true); } std::vector st5(RRTNode *init, RRTNode *goal, bool bw) -- 2.39.2