Visualize velocity along LocalTrajectory in ArViz

source/armarx/navigation/server/introspection/ArvizIntrospector.cpp

@@ -3,8 +3,6 @@
 #include <iterator>
 #include <string>
 
-#include <range/v3/view/enumerate.hpp>
-
 #include <Eigen/Geometry>
 
 #include <SimoxUtility/algorithm/apply.hpp>
@@ -26,6 +24,7 @@
 #include <armarx/navigation/core/types.h>
 #include <armarx/navigation/server/StackResult.h>
 #include <armarx/navigation/util/Visualization.h>
+#include <range/v3/view/enumerate.hpp>
 
 namespace armarx::navigation::server
 {
@@ -61,7 +60,6 @@ namespace armarx::navigation::server
 
         drawLocalTrajectory(result.trajectory);
         arviz.commit(layers);
-
     }
 
     // void
@@ -168,7 +166,32 @@ namespace armarx::navigation::server
 
         layer.add(viz::Path("path").points(points).color(simox::Color::green()));
 
+
+        // Visualize trajectory speed
+        auto velLayer = arviz.layer("local_planner_velocity");
+
+        simox::ColorMap cm = simox::color::cmaps::inferno();
+        cm.set_vmin(0);
+        cm.set_vmax(1);
+
+        for (size_t i = 0; i < trajectory.size() - 1; i++)
+        {
+            const core::LocalTrajectoryPoint start = trajectory.at(i);
+            const core::LocalTrajectoryPoint end = trajectory.at(i + 1);
+
+            const Duration dT = end.timestamp - start.timestamp;
+            const Eigen::Vector3f distance = end.pose.translation() - start.pose.translation();
+            const float speed = distance.norm() / 1000 / dT.toSecondsDouble();
+
+            const Eigen::Vector3f pos = start.pose.translation() + distance / 2;
+            const simox::Color color = cm.at(speed);
+
+            velLayer.add(
+                viz::Sphere("velocity_" + std::to_string(i)).position(pos).radius(50).color(color));
+        }
+
         layers.emplace_back(std::move(layer));
+        layers.emplace_back(std::move(velLayer));
     }
 
     void