ros_conversions: new types

source/armarx/navigation/local_planning/ros_conversions.cpp

 #include "ros_conversions.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/Duration.h"
 
+#include "armarx/navigation/core/Trajectory.h"
 #include <armarx/navigation/conversions/eigen.h>
 
+#include <range/v3/view/zip.hpp>
+
 namespace armarx::navigation::conv
 {
     teb_local_planner::PoseSE2
@@ -41,13 +47,13 @@ namespace armarx::navigation::conv
     }
 
     teb_local_planner::TimedElasticBand
-    toRos(const core::Trajectory& trajectory)
+    toRos(const core::GlobalTrajectory& trajectory)
     {
         teb_local_planner::TimedElasticBand teb;
 
         bool first = true;
         teb_local_planner::PoseSE2 lastPose;
-        for (const core::TrajectoryPoint& point : trajectory.points())
+        for (const core::GlobalTrajectoryPoint& point : trajectory.points())
         {
             teb_local_planner::PoseSE2 pose = toRos(point.waypoint.pose);
             teb.addPose(pose);
@@ -67,27 +73,27 @@ namespace armarx::navigation::conv
         return teb;
     }
 
-    core::Trajectory
+    core::LocalTrajectory
     fromRos(const teb_local_planner::TimedElasticBand& teb)
     {
-        core::TrajectoryPoints trajectory;
+        core::LocalTrajectory trajectory;
         trajectory.reserve(teb.poses().size());
 
-        teb_local_planner::PoseSequence::const_iterator poseIt = teb.poses().begin();
-        for (const teb_local_planner::VertexTimeDiff* timeDiff : teb.timediffs())
-        {
-            Eigen::Vector2d distance = poseIt[1]->pose().position() - poseIt[0]->pose().position();
-            float velocity = distance.norm() / timeDiff->dt();
+        // TODO this timestamp should be given via the argument list
+        DateTime timestamp = Clock::Now();
 
-            core::Pose pose = fromRos(poseIt[0]->pose());
+        ARMARX_CHECK_EQUAL(teb.poses().size(), teb.timediffs().size());
+
+        for (const auto& [poseVertex, timediff]: ranges::views::zip(teb.poses(), teb.timediffs()))
+        {
+            const core::Pose pose = fromRos(poseVertex->pose());
+            const Duration dt = Duration::SecondsDouble(timediff->dt());
+            timestamp += dt;
 
-            trajectory.push_back({.waypoint = {pose}, .velocity = velocity});
-            poseIt++;
+            trajectory.push_back(core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {pose}});
         }
-        core::Pose end = fromRos(teb.poses().end()[0]->pose());
-        trajectory.push_back({.waypoint = {end}, .velocity = 0});
 
-        return {trajectory};
+        return trajectory;
     }
 
 

source/armarx/navigation/local_planning/ros_conversions.h

@@ -36,9 +36,9 @@ namespace armarx::navigation::conv
 
     geometry_msgs::Twist toRos(const core::Twist& velocity);
 
-    teb_local_planner::TimedElasticBand toRos(const core::Trajectory& trajectory);
+    teb_local_planner::TimedElasticBand toRos(const core::GlobalTrajectory& trajectory);
 
-    core::Trajectory fromRos(const teb_local_planner::TimedElasticBand& teb);
+    core::LocalTrajectory fromRos(const teb_local_planner::TimedElasticBand& teb);
 
 
 } // namespace armarx::navigation::conv