trajectory: projection with additional info

source/Navigation/libraries/core/Trajectory.cpp

@@ -188,6 +188,22 @@ namespace armarx::nav::core
                 bestProj.wayPointAfter = wpAfter;
                 bestProj.projection.waypoint.pose = ip.Get(t);
 
+                bestProj.segment = [&]
+                {
+                    if (i == 0)
+                    {
+                        return Projection::Segment::FIRST;
+                    }
+
+                    if (i == (pts.size() - 1))
+                    {
+                        return Projection::Segment::FINAL;
+                    }
+
+                    return Projection::Segment::INTERMEDIATE;
+                }();
+
+
                 switch (velocityInterpolation)
                 {
                     case VelocityInterpolation::LinearInterpolation:
@@ -324,13 +340,9 @@ namespace armarx::nav::core
 
                 pointOnPath += dir * eps;
                 resampledPath.push_back(pointOnPath);
-
-                float d = (nav::conv::to2D(pointOnPath) - lineSegment.end).norm();
-
-                d += 1;
             }
 
-            // find the segment that is sufficiently away from the current pointOnPath
+            // find the segment that is sufficiently far away from the current pointOnPath
             // skip those segments that are too close
             // => find the first segment that penetrates the epsilon-region
             const float distanceEnd = (lineSegment.end - nav::conv::to2D(pointOnPath)).norm();
@@ -378,6 +390,12 @@ namespace armarx::nav::core
             resampledPath.push_back(pointOnPath);
         }
 
+        // remove first "waypoint" => this is the start
+        resampledPath.erase(resampledPath.begin());
+
+        // remove last "waypoint" => this is the end
+        resampledPath.pop_back();
+
         // TODO(fabian.reister): debug
         ARMARX_INFO << "Resampled path contains " << resampledPath.size() << " waypoints";
 

source/Navigation/libraries/core/Trajectory.h

@@ -43,6 +43,15 @@ namespace armarx::nav::core
 
         TrajectoryPoint wayPointBefore;
         TrajectoryPoint wayPointAfter;
+
+        enum class Segment
+        {
+            FIRST,
+            INTERMEDIATE,
+            FINAL
+        };
+
+        Segment segment;
     };
 
     enum class VelocityInterpolation