From 2ca3c29402cbeeaa7085a5b58366850fb4a54175 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Mon, 28 Nov 2022 22:46:45 +0100
Subject: [PATCH] move internal function to its own header

---
 libselfdriving/src/algos/TPS_Astar.cpp        | 67 +------------------
 .../src/algos/refine_trajectory.cpp           |  9 +--
 2 files changed, 2 insertions(+), 74 deletions(-)

diff --git a/libselfdriving/src/algos/TPS_Astar.cpp b/libselfdriving/src/algos/TPS_Astar.cpp
index af42467..4d89ec7 100644
--- a/libselfdriving/src/algos/TPS_Astar.cpp
+++ b/libselfdriving/src/algos/TPS_Astar.cpp
@@ -7,6 +7,7 @@
 #include <mrpt/math/wrap2pi.h>
 #include <mrpt/opengl/COpenGLScene.h>
 #include <selfdriving/algos/TPS_Astar.h>
+#include <selfdriving/algos/edge_interpolated_path.h>
 #include <selfdriving/algos/render_tree.h>
 #include <selfdriving/algos/tp_obstacles_single_path.h>
 #include <selfdriving/algos/transform_pc_square_clipping.h>
@@ -81,72 +82,6 @@ TPS_Astar::TPS_Astar() : mrpt::system::COutputLogger("TPS_Astar")
     profiler_().setName("TPS_Astar");
 }
 
-void edge_interpolated_path(
-    MoveEdgeSE2_TPS& edge, const TrajectoriesAndRobotShape& trs,
-    const std::optional<mrpt::math::TPose2D>& reconstrRelPoseOpt = std::nullopt,
-    const std::optional<size_t>&              ptg_stepOpt        = std::nullopt,
-    const std::optional<size_t>&              numSegments        = std::nullopt)
-{
-    size_t nSeg = 0;
-
-    if (numSegments.has_value()) { nSeg = *numSegments; }
-    else
-    {
-        // Use same number than existing edge interpolated path:
-        ASSERT_(edge.interpolatedPath.size() > 1);
-        nSeg = edge.interpolatedPath.size();
-    }
-
-    auto& ptg = trs.ptgs.at(edge.ptgIndex);
-
-    const duration_seconds_t dt = ptg->getPathStepDuration();
-
-    mrpt::math::TPose2D reconstrRelPose;
-    if (reconstrRelPoseOpt.has_value())
-        reconstrRelPose = reconstrRelPoseOpt.value();
-    else
-    {
-        MRPT_TODO("continue...");
-        THROW_EXCEPTION("To do");
-
-        // ptg->inverseMap_WS2TP(edgePoseIncr.x, edgePoseIncr.y)
-    }
-
-    size_t ptg_step = 0;
-    if (ptg_stepOpt)
-    {  //
-        ptg_step = *ptg_stepOpt;
-    }
-    else
-    {
-        MRPT_TODO("continue...");
-        THROW_EXCEPTION("To do");
-    }
-
-    auto& ip = edge.interpolatedPath;
-    ip.clear();
-
-    // t=0: fixed start relative pose
-    // ---------------------------------
-    ip[0 * dt] = {0, 0, 0};
-
-    // interpolated path in between start and goal
-    // ----------------------------------------------
-    for (size_t i = 0; i < nSeg; i++)
-    {
-        const auto               iStep = ((i + 1) * ptg_step) / (nSeg + 2);
-        const duration_seconds_t t     = iStep * dt;
-        ip[t] = ptg->getPathPose(edge.ptgPathIndex, iStep);
-    }
-
-    // Final, known pose and time
-    // ----------------------------------------------
-    ip[ptg_step * dt] = reconstrRelPose;
-
-    // Motion execution time:
-    edge.estimatedExecTime = ptg_step * dt;
-}
-
 PlannerOutput TPS_Astar::plan(const PlannerInput& in)
 {
     MRPT_START
diff --git a/libselfdriving/src/algos/refine_trajectory.cpp b/libselfdriving/src/algos/refine_trajectory.cpp
index 9e29cc2..1e82c65 100644
--- a/libselfdriving/src/algos/refine_trajectory.cpp
+++ b/libselfdriving/src/algos/refine_trajectory.cpp
@@ -4,19 +4,12 @@
  * See LICENSE for license information.
  * ------------------------------------------------------------------------- */
 
+#include <selfdriving/algos/edge_interpolated_path.h>
 #include <selfdriving/algos/refine_trajectory.h>
 #include <selfdriving/ptgs/SpeedTrimmablePTG.h>
 
 #include <iostream>
 
-using namespace selfdriving;
-
-void edge_interpolated_path(
-    MoveEdgeSE2_TPS& edge, const TrajectoriesAndRobotShape& trs,
-    const std::optional<mrpt::math::TPose2D>& reconstrRelPoseOpt = std::nullopt,
-    const std::optional<size_t>&              ptg_stepOpt        = std::nullopt,
-    const std::optional<size_t>&              numSegments = std::nullopt);
-
 void selfdriving::refine_trajectory(
     MotionPrimitivesTreeSE2::path_t&          inPath,
     MotionPrimitivesTreeSE2::edge_sequence_t& inEdges,