visualization: visualizing trajectory

source/armarx/navigation/util/CMakeLists.txt

@@ -9,6 +9,7 @@ armarx_add_library(util
         ArViz
         # Simox
         Simox::VirtualRobot
+        armarx_navigation::core
     SOURCES
         ./util.cpp
         ./Visualization.cpp

source/armarx/navigation/util/Visualization.cpp

@@ -21,7 +21,7 @@ namespace armarx::navigation::util
     }
 
     void
-    Visualization::visualizePath(std::vector<Eigen::Vector3f>& path)
+    Visualization::visualize(const core::Trajectory& trajectory)
     {
         // TODO(fabian.reister): more finegrained locking
         std::lock_guard g{paramsMutex};
@@ -31,34 +31,34 @@ namespace armarx::navigation::util
             return;
         }
 
-        if (path.size() < 2)
+        if (trajectory.poses().size() < 2)
         {
             ARMARX_WARNING << "Unable to visualize path with less than two segments";
             return;
         }
 
-        viz::Layer pathLayer = arviz.layer("path");
-        for (size_t i = 1; i < path.size(); i++)
-        {
-            Eigen::Vector3f prevPose = Eigen::Vector3f::Zero();
-            prevPose.head<2>() = path.at(i - 1).head<2>();
-            Eigen::Vector3f currentPose = Eigen::Vector3f::Zero();
-            currentPose.head<2>() = path.at(i).head<2>();
-            viz::Arrow a = viz::Arrow("path_segment_" + std::to_string(i));
-            a.color(viz::Color::orange());
-            a.width(25);
-            a.fromTo(prevPose, currentPose);
-            pathLayer.add(a);
-        }
-
-        arviz.commit({pathLayer});
-
-        visualizeTrajectory(path);
+        // viz::Layer pathLayer = arviz.layer("path");
+        // for (size_t i = 1; i < path.size(); i++)
+        // {
+        //     Eigen::Vector3f prevPose = Eigen::Vector3f::Zero();
+        //     prevPose.head<2>() = path.at(i - 1).head<2>();
+        //     Eigen::Vector3f currentPose = Eigen::Vector3f::Zero();
+        //     currentPose.head<2>() = path.at(i).head<2>();
+        //     viz::Arrow a = viz::Arrow("path_segment_" + std::to_string(i));
+        //     a.color(viz::Color::orange());
+        //     a.width(25);
+        //     a.fromTo(prevPose, currentPose);
+        //     pathLayer.add(a);
+        // }
+
+        // arviz.commit({pathLayer});
+
+        visualizeTrajectory(trajectory);
     }
 
 
     void
-    Visualization::visualizeTrajectory(std::vector<Eigen::Vector3f>& path)
+    Visualization::visualizeTrajectory(const core::Trajectory& trajectory)
     {
         if (!params.enableTrajectoryVisualization)
         {
@@ -71,39 +71,38 @@ namespace armarx::navigation::util
         robotVis.show();
         robotLayer.add(robotVis);
 
-        for (size_t i = 1; i < path.size(); i++)
-        {
-            const Eigen::Vector3f& prevPose = path.at(i - 1);
-            const Eigen::Vector3f& currentPose = path.at(i);
-
-            Eigen::Vector3f diff = (currentPose - prevPose);
+        const auto resampledTrajectory = trajectory.resample(10);
+        // const auto duration = trajectory.duration(core::VelocityInterpolation::LastWaypoint);
 
-            const float steps = std::ceil(diff.head<2>().norm() / 10.0f);
-            diff /= steps;
+        const float speedup = 1.0;
 
-            std::lock_guard<std::mutex> lock(mutex);
+        ARMARX_INFO << "Visualizing trajectory";
 
-            for (size_t j = 0; j < steps; j++)
-            {
-                Eigen::Vector2f c = prevPose.head<2>() + diff.head<2>() * j;
-                float alpha = prevPose.z() + diff.z() * j;
+        const auto points = resampledTrajectory.points();
+        for (size_t i = 0; i < (points.size() - 1); i++)
+        {
+            const auto& wp = points.at(i);
+            robotVis.pose(wp.waypoint.pose.matrix());
 
-                Eigen::Affine3f m(Eigen::Affine3f::Identity());
-                m.translate(Eigen::Vector3f(c.x(), c.y(), 0.0));
-                m.rotate(Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitZ()));
+            std::lock_guard<std::mutex> lock(mutex);
+            arviz.commit({robotLayer});
 
-                robotVis.pose(m.matrix());
+            const float distance =
+                (wp.waypoint.pose.translation() - points.at(i + 1).waypoint.pose.translation())
+                    .norm();
 
-                arviz.commit({robotLayer});
+            ARMARX_CHECK(wp.velocity > 0.F);
+            const float velocity = 200;
+            const int dt = static_cast<int>(speedup * distance / velocity); // [ms]
 
-                std::this_thread::sleep_for(std::chrono::milliseconds(10));
-            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(dt));
         }
 
+        ARMARX_INFO << "Done visualizing trajectory";
+
         robotVis.hide();
 
         std::lock_guard<std::mutex> lock(mutex);
-
         arviz.commit({robotLayer});
     }
 

source/armarx/navigation/util/Visualization.h

@@ -8,6 +8,7 @@
 #include "RobotAPI/components/ArViz/Client/ScopedClient.h"
 #include <RobotAPI/components/ArViz/Client/Client.h>
 
+#include "armarx/navigation/core/Trajectory.h"
 #include <armarx/navigation/core/basic_types.h>
 
 namespace armarx::navigation::util
@@ -43,7 +44,7 @@ namespace armarx::navigation::util
         void failed();
 
 
-        void visualizePath(std::vector<Eigen::Vector3f>& path);
+        void visualize(const core::Trajectory& trajectory);
 
         void
         updateParams(const Params& params)
@@ -53,7 +54,7 @@ namespace armarx::navigation::util
         }
 
     private:
-        void visualizeTrajectory(std::vector<Eigen::Vector3f>& path);
+        void visualizeTrajectory(const core::Trajectory& trajectory);
 
         void asyncHideDelayed();