diff --git a/source/armarx/navigation/util/CMakeLists.txt b/source/armarx/navigation/util/CMakeLists.txt
index 551e4727d11d9136304b17f56d4745a990400356..5021b515ef9f54b4bfd425fd8f64232f6343c986 100644
--- a/source/armarx/navigation/util/CMakeLists.txt
+++ b/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
diff --git a/source/armarx/navigation/util/Visualization.cpp b/source/armarx/navigation/util/Visualization.cpp
index dc02922e92e2728169e2fe09f0e3ec1fa92da38d..0f121fe60d28a3a9f108d2a1fb1f6dc8b93dc908 100644
--- a/source/armarx/navigation/util/Visualization.cpp
+++ b/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});
}
diff --git a/source/armarx/navigation/util/Visualization.h b/source/armarx/navigation/util/Visualization.h
index ff403829323b8b2afb52278d4ca02543bf76d3a1..cf8d7beb733c1eb0286bb5ff62591688397a2ffd 100644
--- a/source/armarx/navigation/util/Visualization.h
+++ b/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();