Merge branch 'use-cusp'

diff --git a/incl/cute_c2.h b/incl/cute_c2.h
index 7faa8fda0cebe0d0f49c72a89da46963811aaba8..79848ccea0c1b7ad5c11f6a94b6335c1e3a2069f 100644 (file)
--- a/incl/cute_c2.h
+++ b/incl/cute_c2.h
@@ -146,7 +146,7 @@
 // resented as a point + radius. usually tools that generate polygons should be
 // constructed so they do not output polygons with too many verts.
 // Note: polygons in cute_c2 are all *convex*.
-#define C2_MAX_POLYGON_VERTS 8
+#define C2_MAX_POLYGON_VERTS 12
 
 // 2d vector
 typedef struct

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 02cb9c6c3046534ea619178a3cafefbd80b3dbec..3cee13bbf0219aeae311a8e07929b20dafefb606 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -161,7 +161,8 @@ class RRTExt11 : public virtual RRTS {
 /*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search).
  *
  * Use Reeds & Shepp path length for building tree data structure and Euclidean
- * distance plus (abs) heading difference for searching it.
+ * distance + (abs) heading difference + 0.1 * backward-forward direction
+ * changes for searching it.
  *
  * \ingroup ext-cost
  * \see https://doi.org/10.1109/TITS.2015.2477355

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 7b7f4fc4c7500cc70fda0254565b7026bdb23be9..603c0a8cc58a7168a8479fc14b04387977226619 100644 (file)
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -36,6 +36,7 @@ private:
        double c_ = 0.0;
        double cc_ = 0.0;
        RRTNode* p_ = nullptr;
+       unsigned int cusp_ = 0;
 public:
        /*! Get cost to parent. */
        double c() const;
@@ -52,6 +53,12 @@ public:
        /*! Set parent node. */
        void p(RRTNode& p);
 
+       /*! Get number of backward-forward direction changes. */
+       unsigned int cusp() const;
+
+       /*! Set number of backward-forward direction changes. */
+       void cusp(RRTNode const& p);
+
        bool operator==(RRTNode const& n);
 };
 

diff --git a/src/rrtext10.cc b/src/rrtext10.cc
index 63e2ade6596f449b21d72c932bc359d9e988e8bf..f5d9b893773c3876f2a3b284ec6ba8261e7b5e39 100644 (file)
--- a/src/rrtext10.cc
+++ b/src/rrtext10.cc
@@ -21,7 +21,12 @@ RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const
 double
 RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const
 {
-       return f.edist(t) + std::abs(t.h() - f.h());
+       double cost = f.edist(t);
+       double heur = std::min(std::abs(t.h() - f.h()),
+               2 * M_PI - std::abs(t.h() - f.h()));
+       heur *= this->bc_.mtr();
+       cost = std::max(cost, heur);
+       return cost + f.cusp() * 0.1;
 }
 
 } // namespace rrts

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 63c2b26aea251f4bd022ad1032748b6662a43b95..cbb1e96e229a84607412ca74d1fea600a69ac577 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -215,8 +215,8 @@ RRTExt13::compute_path()
        if (this->goal_.cc() == 0.0 || this->path_.size() == 0) {
                return;
        }
-#if 0 // TODO 0.59 should work for sc4-1-0 only.
-       if (this->goal_.cc() * 0.59 > this->last_goal_cc_
+#if 1 // TODO 0.59 should work for sc4-1-0 only.
+       if (this->goal_.cc() * 0.8 > this->last_goal_cc_
                        && this->last_goal_cc_ != 0.0) {
                return;
        }

diff --git a/src/rrtext2.cc b/src/rrtext2.cc
index d98d7cb0c817a0b545418d5a01dbeda889aa8471..06caf9fe2f028e6d4b43c0725696633bed5a8be4 100644 (file)
--- a/src/rrtext2.cc
+++ b/src/rrtext2.cc
@@ -63,9 +63,7 @@ void
 RRTExt2::json(Json::Value jvi)
 {
        RRTS::json(jvi);
-       if (jvi["obst"] == Json::nullValue) {
-               return;
-       }
+       assert(jvi["obst"] != Json::nullValue);
        for (auto& o: jvi["obst"]) {
                assert(o.size() < C2_MAX_POLYGON_VERTS);
                c2Poly c2tmp;

diff --git a/src/rrts.cc b/src/rrts.cc
index 23f60a0eebaa7860ce9cea77d5bc8a25324139f7..15f2436e8d3142bb2f61b876428e124bac0b799e 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -59,6 +59,21 @@ RRTNode::p(RRTNode& p)
        }
 }
 
+unsigned int
+RRTNode::cusp() const
+{
+       return this->cusp_;
+}
+
+void
+RRTNode::cusp(RRTNode const& p)
+{
+       this->cusp_ = p.cusp();
+       if (this->sp() != p.sp() || this->sp() == 0.0) {
+               this->cusp_++;
+       }
+}
+
 bool
 RRTNode::operator==(RRTNode const& n)
 {
@@ -108,6 +123,7 @@ RRTS::join_steered(RRTNode* f)
                RRTNode* t = &this->nodes_.back();
                t->p(*f);
                t->c(this->cost_build(*f, *t));
+               t->cusp(*f);
                this->steered_.erase(this->steered_.begin());
                f = t;
        }
@@ -143,6 +159,7 @@ RRTS::connect()
        t = &this->nodes_.back();
        t->p(*f);
        t->c(this->cost_build(*f, *t));
+       t->cusp(*f);
        this->steered_.erase(this->steered_.begin());
        return true;
 }
@@ -282,7 +299,6 @@ RRTS::json(Json::Value jvi)
 {
        assert(jvi["init"] != Json::nullValue);
        assert(jvi["goal"] != Json::nullValue);
-       assert(jvi["obst"] != Json::nullValue);
        this->nodes_.front().x(jvi["init"][0].asDouble());
        this->nodes_.front().y(jvi["init"][1].asDouble());
        this->nodes_.front().h(jvi["init"][2].asDouble());