Make init and goal parts of first and last segments

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index dcf8dedd35ae43c32052eb11a985f14d55e8a1a5..39b351687cb0a3d70b24c308ce867dd70a64ed01 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -457,6 +457,13 @@ RRTS::json(void) const
                        }
                        i++;
                }
+               // Initial point is part of the first segment.
+               jvo["paths"][j][0][3] = jvo["paths"][j][1][3];
+               jvo["paths"][j][0][4] = jvo["paths"][j][1][4];
+               // Goal point is part of the last segment.
+               jvo["paths"][j][i - 1][3] = jvo["paths"][j][i - 2][3];
+               jvo["paths"][j][i - 1][4] = jvo["paths"][j][i - 2][4];
+               // --
                jvo["costs"][j] = path.back().cc();
                j++;
        }
@@ -480,8 +487,14 @@ RRTS::json(void) const
                }
                i++;
        }
+       // Initial point is part of the first segment.
+       jvo["path"][0][3] = jvo["path"][1][3];
+       jvo["path"][0][4] = jvo["path"][1][4];
+       // Goal point is part of the last segment.
+       jvo["path"][i - 1][3] = jvo["path"][i - 2][3];
+       jvo["path"][i - 1][4] = jvo["path"][i - 2][4];
+       // --
        jvo["costs"][j] = this->_path.back()->cc();
-       j++;
        jvo["goal_cc"] = this->_goal.cc(); // TODO remove, use the following
        jvo["cost"] = this->path_cost();
        jvo["time"] = this->scnt();