NjointCartesianWaypointController: using global pose info directly from provided VirtualRobot

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp

@@ -129,8 +129,8 @@ namespace armarx
         }
         //visu
         {
-            _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
-            _tripFakeRobotGP.commitWrite();
+            _tripRt2NonRtRobotGP.getWriteBuffer().setIdentity();
+            _tripRt2NonRtRobotGP.commitWrite();
         }
     }
 
@@ -281,6 +281,9 @@ namespace armarx
             }
         }
         _tripRt2NonRt.commitWrite();
+
+        _tripRt2NonRtRobotGP.getWriteBuffer() = _rtRobot->getGlobalPose();
+        _tripRt2NonRtRobotGP.commitWrite();
     }
 
     void
@@ -412,17 +415,13 @@ namespace armarx
     NJointCartesianWaypointController::setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
                                                                        const Ice::Current&)
     {
-        std::lock_guard g{_tripFakeRobotGPWriteMutex};
-        _tripFakeRobotGP.getWriteBuffer() = p;
-        _tripFakeRobotGP.commitWrite();
+        ; // No longer used ...
     }
 
     void
     NJointCartesianWaypointController::resetVisualizationRobotGlobalPose(const Ice::Current&)
     {
-        std::lock_guard g{_tripFakeRobotGPWriteMutex};
-        _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
-        _tripFakeRobotGP.commitWrite();
+        ; // No longer used ...
     }
 
     void
@@ -477,7 +476,7 @@ namespace armarx
             std::lock_guard g{_tripRt2NonRtMutex};
             const auto& buf = _tripRt2NonRt.getUpToDateReadBuffer();
 
-            const Eigen::Matrix4f fakeGP = _tripFakeRobotGP.getUpToDateReadBuffer();
+            const Eigen::Matrix4f fakeGP = _tripRt2NonRtRobotGP.getUpToDateReadBuffer();
             const Eigen::Matrix4f gp = std::isfinite(fakeGP(0, 0)) ? fakeGP : buf.rootPose;
 
             if (buf.tcp != buf.tcpTarg)

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h

@@ -146,8 +146,7 @@ namespace armarx
         mutable std::recursive_mutex _tripRt2NonRtMutex;
         TripleBuffer<RtToNonRtData> _tripRt2NonRt;
 
-        mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
-        TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
+        TripleBuffer<Eigen::Matrix4f> _tripRt2NonRtRobotGP;
 
         //publish data
         std::atomic_size_t _publishWpsNum{0};