cli and viz: interpolate trajectory

MRPT · Sep 30, 2024 · 5c1a065 · 5c1a065
1 parent 6d7d174
commit 5c1a065
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 10 deletions.
diff --git a/apps/path-planner-cli/path-planner-cli.cpp b/apps/path-planner-cli/path-planner-cli.cpp
@@ -50,6 +50,11 @@ TCLAP::ValueArg<float> argObstaclesGridResolution(
     "pixel in meters.",
     false, 0.05, "0.05", cmd);
 
+TCLAP::ValueArg<float> arg_interpolation_period(
+    "", "interpolarion-period",
+    "Interpolation (and animation) time step between keyframes [s].", false,
+    0.25, "0.25", cmd);
+
 TCLAP::ValueArg<std::string> arg_ptgs_file(
     "c", "ptg-config", "Input .ini file with PTG definitions.", true, "",
     "ptgs.ini", cmd);
@@ -384,7 +389,17 @@ static void do_plan_path()
         {
             const auto t0 = mrpt::Clock::nowDouble();
 
-            traj = mpp::plan_to_trajectory(pathEdges, pi.ptgs);
+            const double interpPeriod = arg_interpolation_period.getValue();
+
+            traj = mpp::plan_to_trajectory(pathEdges, pi.ptgs, interpPeriod);
+
+            // Note: trajectory is in local frame of reference
+            // of plan.originalInput.stateStart.pose
+            // so, correct that relative pose so we keep everything in global
+            // frame:
+            const auto& startPose = plan.originalInput.stateStart.pose;
+            for (auto& kv : *traj)
+                kv.second.state.pose = startPose + kv.second.state.pose;
 
             const auto dt = mrpt::Clock::nowDouble() - t0;
 

diff --git a/mrpt_path_planning/include/mpp/algos/refine_trajectory.h b/mrpt_path_planning/include/mpp/algos/refine_trajectory.h
@@ -17,12 +17,14 @@ namespace mpp
 void refine_trajectory(
     const MotionPrimitivesTreeSE2::path_t&    inPath,
     MotionPrimitivesTreeSE2::edge_sequence_t& edgesToRefine,
-    const TrajectoriesAndRobotShape&          ptgInfo);
+    const TrajectoriesAndRobotShape&          ptgInfo,
+    const double                              ptg_tolerance_dist = 0.10);
 
 /// \overload taking `std::vector` of nodes and edges, instead of `std::list`
 void refine_trajectory(
     const std::vector<MotionPrimitivesTreeSE2::node_t>& inPath,
     std::vector<MotionPrimitivesTreeSE2::edge_t>&       edgesToRefine,
-    const TrajectoriesAndRobotShape&                    ptgInfo);
+    const TrajectoriesAndRobotShape&                    ptgInfo,
+    const double ptg_tolerance_dist = 0.10);
 
 }  // namespace mpp
diff --git a/mrpt_path_planning/src/algos/refine_trajectory.cpp b/mrpt_path_planning/src/algos/refine_trajectory.cpp
@@ -14,7 +14,7 @@
 void mpp::refine_trajectory(
     const mpp::MotionPrimitivesTreeSE2::path_t& inPath,
     MotionPrimitivesTreeSE2::edge_sequence_t&   edgesToRefine,
-    const TrajectoriesAndRobotShape&            ptgInfo)
+    const TrajectoriesAndRobotShape& ptgInfo, const double ptg_tolerance_dist)
 {
     const size_t nEdges = edgesToRefine.size();
     ASSERT_EQUAL_(inPath.size(), nEdges + 1);
@@ -53,7 +53,7 @@ void mpp::refine_trajectory(
         normalized_distance_t newNormDist = 0;
 
         const bool ok = ptg->inverseMap_WS2TP(
-            deltaNodes.x, deltaNodes.y, newK, newNormDist);
+            deltaNodes.x, deltaNodes.y, newK, newNormDist, ptg_tolerance_dist);
         if (!ok)
         {
             std::stringstream ss;
@@ -92,7 +92,7 @@ void mpp::refine_trajectory(
 void mpp::refine_trajectory(
     const std::vector<mpp::MotionPrimitivesTreeSE2::node_t>& inPath,
     std::vector<mpp::MotionPrimitivesTreeSE2::edge_t>&       edgesToRefine,
-    const TrajectoriesAndRobotShape&                         ptgInfo)
+    const TrajectoriesAndRobotShape& ptgInfo, const double ptg_tolerance_dist)
 {
     mpp::MotionPrimitivesTreeSE2::path_t     newPath;
     MotionPrimitivesTreeSE2::edge_sequence_t newEdges;
@@ -102,5 +102,5 @@ void mpp::refine_trajectory(
 
     // this will store the output directly in "edgesToRefine" since we use
     // pointers above:
-    refine_trajectory(newPath, newEdges, ptgInfo);
+    refine_trajectory(newPath, newEdges, ptgInfo, ptg_tolerance_dist);
 }
diff --git a/mrpt_path_planning/src/algos/viz.cpp b/mrpt_path_planning/src/algos/viz.cpp
@@ -5,6 +5,7 @@
  * ------------------------------------------------------------------------- */
 
 #include <mpp/algos/render_tree.h>
+#include <mpp/algos/trajectories.h>
 #include <mpp/algos/viz.h>
 #include <mrpt/gui/CDisplayWindow3D.h>
 #include <mrpt/opengl/CCylinder.h>
@@ -106,9 +107,6 @@ void mpp::viz_nav_plan_animation(
 
     glVehFrame->insert(glVeh);
 
-    // The path is referenced to the path planning "start pose", account for it:
-    glVehFrame->setPose(plan.originalInput.stateStart.pose);
-
     // Build opengl scene:
     {
         mrpt::gui::CDisplayWindow3DLocker dwl(*win, scene);