Resolve "Follow-up from "Add TEB to CMake config""

Closes #14 (closed)

source/armarx/navigation/local_planning/ros_conversions.cpp

@@ -7,6 +7,7 @@
 #include "armarx/navigation/core/basic_types.h"
 #include <armarx/navigation/conversions/eigen.h>
 
+#include <range/v3/view/drop.hpp>
 #include <range/v3/view/zip.hpp>
 
 namespace armarx::navigation::conv
@@ -82,23 +83,36 @@ namespace armarx::navigation::conv
     core::LocalTrajectory
     fromRos(const teb_local_planner::TimedElasticBand& teb)
     {
+        const auto& tebPoses = teb.poses();
+
+        if(tebPoses.empty())
+        {
+            return {};
+        }
+
         core::LocalTrajectory trajectory;
-        trajectory.reserve(teb.poses().size());
+        trajectory.reserve(tebPoses.size());
 
         // TODO this timestamp should be given via the argument list
         DateTime timestamp = Clock::Now();
 
-        ARMARX_CHECK_EQUAL(teb.poses().size(), teb.timediffs().size());
+        ARMARX_CHECK_EQUAL(tebPoses.size(), teb.timediffs().size()-1);
+
+        // add the first pose at this timestamp
+        trajectory.push_back(core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {fromRos(tebPoses.front()->pose())}});
 
-        for (const auto& [poseVertex, timediff]: ranges::views::zip(teb.poses(), teb.timediffs()))
+        // all consecutive poses
+        for (const auto& [poseVertex, timediff]: ranges::views::zip(tebPoses | ranges::views::drop(1), teb.timediffs()))
         {
             const core::Pose pose = fromRos(poseVertex->pose());
             const Duration dt = Duration::SecondsDouble(timediff->dt());
-            timestamp += dt;
 
+            timestamp += dt; 
             trajectory.push_back(core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {pose}});
         }
 
+        ARMARX_CHECK_EQUAL(trajectory.size(), tebPoses.size());
+
         return trajectory;
     }