From d6172fab5129cb047700981a9541bd39d540db7c Mon Sep 17 00:00:00 2001
From: armar6-demo <armar6-demo@kit.edu>
Date: Thu, 24 Nov 2022 15:43:33 +0100
Subject: [PATCH] use arviz to visualize the target traj
---
.../TrajectoryProcessing.cpp | 15 +++++++++++++--
1 file changed, 13 insertions(+), 2 deletions(-)
diff --git a/source/armar6/skills/statecharts/Armar6WipingDemoGroup/TrajectoryProcessing.cpp b/source/armar6/skills/statecharts/Armar6WipingDemoGroup/TrajectoryProcessing.cpp
index e4765b185..fdbac52ae 100644
--- a/source/armar6/skills/statecharts/Armar6WipingDemoGroup/TrajectoryProcessing.cpp
+++ b/source/armar6/skills/statecharts/Armar6WipingDemoGroup/TrajectoryProcessing.cpp
@@ -29,6 +29,7 @@
#include <algorithm>
#include <fftw3.h>
#include <RobotAPI/libraries/core/Trajectory.h>
+#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
// just for debugging
#include <iostream>
@@ -353,14 +354,24 @@ void TrajectoryProcessing::run()
double timeCnt = 0.0;
TrajectoryPtr traj = new Trajectory();
- getDebugDrawerTopic()->clearAll();
+ // getDebugDrawerTopic()->clearAll();
+
+ viz::Client arviz = viz::Client::createFromTopic("TargetWipingTraj", getArvizTopic());
+ viz::Layer targetTrajLayer = arviz.layer("target wiping traj");
for (std::size_t i = 0; i < trajStartPoint.size(); i++)
{
// draw desired trajectory
FramedPositionPtr desiredPositionPtr = FramedPosition(trajStartPoint[i]->toEigen(), robot->getRootNode()->getName(), robot->getName()).toGlobal(robot);
desiredPositionPtr->y += 500;
- this->getDebugDrawerTopic()->setSphereVisu("WipingTraj", "desTrajectory" + i, desiredPositionPtr, DrawColor {0.0f, 0.0f, 0.0f, 1.0}, 5);
+ {
+ targetTrajLayer.add(viz::Sphere("target " + std::to_string(i)).position(desiredPositionPtr->toEigen()).radius(10)
+ .color(simox::Color::blue()));
+ arviz.commit(targetTrajLayer);
+ }
+
+
+ // this->getDebugDrawerTopic()->setSphereVisu("WipingTraj", "desTrajectory" + i, desiredPositionPtr, DrawColor {0.0f, 0.0f, 0.0f, 1.0}, 5);
// output the trajectory into a .csv file
// myfile1 << timeCnt << "," << trajStartPoint[i]->x << "," << trajStartPoint[i]->y << "," << 0 << "," << 1 << "," << 0 << "," << 0 << "," << 0 << "\n";
--
GitLab