Include path segment type in pose

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 0b8e363cb3e22e322577cd70e34ef4148a92ab22..a9d8fdc5343be0a1ef20ebc90e226343a0d4ecd9 100644 (file)
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -60,6 +60,8 @@ public:
        void cusp(RRTNode const& p);
 
        bool operator==(RRTNode const& n);
+
+       int segment_type = 0;
 };
 
 class RRTGoal : public virtual RRTNode, public virtual PoseRange {

diff --git a/src/reeds_shepp.cpp b/src/reeds_shepp.cpp
index 2955312c16b8017336675cff52e89372c629ec1e..d49ad57a7ddce56309db11b1262f1d374a20039c 100644 (file)
--- a/src/reeds_shepp.cpp
+++ b/src/reeds_shepp.cpp
@@ -529,14 +529,14 @@ void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size,
     double dist = rho_ * path.length();
 
     for (double seg=0.0; seg<=dist; seg+=step_size){
-        double qnew[4] = {};
+        double qnew[5] = {};
         interpolate(q0, path, seg/rho_, qnew);
         cb( qnew, user_data);
     }
     return;
 }
 
-void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[4])
+void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[5])
 {
 
     if (seg < 0.0) seg = 0.0;
@@ -566,15 +566,18 @@ void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, doubl
                 s[0] += ( sin(phi+v) - sin(phi));
                 s[1] += (-cos(phi+v) + cos(phi));
                 s[2] = phi + v;
+               s[4] = 1;
                 break;
             case RS_RIGHT:
                 s[0] += (-sin(phi-v) + sin(phi));
                 s[1] += ( cos(phi-v) - cos(phi));
                 s[2] = phi - v;
+               s[4] = -1;
                 break;
             case RS_STRAIGHT:
                 s[0] += (v * cos(phi));
                 s[1] += (v * sin(phi));
+               s[4] = 0;
                 break;
             case RS_NOP:
                 break;

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index cbb1e96e229a84607412ca74d1fea600a69ac577..aa6e7c0a588543cee8a229884fbb3d93642d7be4 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -263,6 +263,7 @@ RRTExt13::json() const
                jvo["opath"][i][0] = n->x();
                jvo["opath"][i][1] = n->y();
                jvo["opath"][i][2] = n->h();
+               jvo["opath"][i][3] = n->sp();
                i++;
        }
        jvo["ogoal_cc"] = this->ogoal_cc_;

diff --git a/src/rrtext16.cc b/src/rrtext16.cc
index 8fd2b6d94c042acb9db1ede6ebf1d204e2f6426b..c2235ad16b8ef455b45c1eed44036b3b0de52f98 100644 (file)
--- a/src/rrtext16.cc
+++ b/src/rrtext16.cc
@@ -18,6 +18,7 @@ cb_steer(double q[4], void *w)
        st->back().y(q[1]);
        st->back().h(q[2]);
        st->back().sp(q[3]);
+       st->back().segment_type = q[4];
        return 0;
 }
 

diff --git a/src/rrts.cc b/src/rrts.cc
index 03787b133d2c2d139dc985541debe5c1d8e5481a..485ee6c9d4fbf998cc8f7b020205f64654fd586f 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -347,6 +347,8 @@ RRTS::json() const
                jvo["path"][i][0] = n->x();
                jvo["path"][i][1] = n->y();
                jvo["path"][i][2] = n->h();
+               jvo["path"][i][3] = n->sp();
+               jvo["path"][i][4] = n->segment_type;
                i++;
        }
        jvo["goal_cc"] = this->goal_.cc();