Draft: use arviz to visualize the target traj

Uncommitted changes on i61pc043. Can be finished, and probably needs to be tested

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";