From 33c65906af397d477c9360ca6d22ced4f8df1f9a Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 10 Oct 2023 23:29:49 +0200
Subject: [PATCH] NjointCartesianWaypointController: using global pose info
directly from provided VirtualRobot
---
.../NJointCartesianWaypointController.cpp | 17 ++++++++---------
.../NJointCartesianWaypointController.h | 3 +--
2 files changed, 9 insertions(+), 11 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 697b087cc..08870cfcc 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/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)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index dce6aed44..59734617b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/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};
--
GitLab