From 5a505b03346e7315d8ad0afa9ba123472fc1bffb Mon Sep 17 00:00:00 2001
From: Christoph Pohl <christoph.pohl@kit.edu>
Date: Thu, 10 Mar 2022 15:06:53 +0100
Subject: [PATCH] Add getCurrentWaypointIndex interface function for
NJointCartesianWaypointController
---
.../NJointControllers/NJointCartesianWaypointController.cpp | 5 +++++
.../NJointControllers/NJointCartesianWaypointController.h | 1 +
.../units/RobotUnit/NJointCartesianWaypointController.ice | 1 +
3 files changed, 7 insertions(+)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 6ad411f67..4658f41d7 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -468,4 +468,9 @@ namespace armarx
}
}
}
+
+ int NJointCartesianWaypointController::getCurrentWaypointIndex(const Ice::Current&)
+ {
+ return _publishWpsCur;
+ }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index 9ed0b775c..4933edd9d 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -51,6 +51,7 @@ namespace armarx
public:
bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
+ int getCurrentWaypointIndex(const Ice::Current& = Ice::emptyCurrent) override;
void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
void setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
index 712399c5f..bf00909d6 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
@@ -51,6 +51,7 @@ module armarx
{
idempotent bool hasReachedTarget();
idempotent bool hasReachedForceLimit();
+ idempotent int getCurrentWaypointIndex();
void setConfig(NJointCartesianWaypointControllerRuntimeConfig cfg);
void setWaypoints(Eigen::Matrix4fSeq wps);
--
GitLab