From fdc79d372bd8babbf9cf7e5e445b04c594cb037c Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 22 Feb 2021 14:00:44 +0100
Subject: [PATCH] Move code to separate file
---
.../ObjectPoseObserver/CMakeLists.txt | 4 +
.../ObjectPoseObserver/ObjectPoseObserver.cpp | 81 -----------------
.../ObjectPoseObserver/ObjectPoseObserver.h | 43 ++-------
.../detail/ObjectPoseDecay.cpp | 7 ++
.../detail/ObjectPoseDecay.h | 24 +++++
.../detail/RobotHeadMovement.cpp | 89 +++++++++++++++++++
.../detail/RobotHeadMovement.h | 46 ++++++++++
7 files changed, 175 insertions(+), 119 deletions(-)
create mode 100644 source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
create mode 100644 source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
create mode 100644 source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
create mode 100644 source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
index 96c82550e..fd526dd81 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
@@ -12,6 +12,8 @@ set(COMPONENT_LIBS
set(SOURCES
ObjectPoseObserver.cpp
+ detail/ObjectPoseDecay.cpp
+ detail/RobotHeadMovement.cpp
plugins/ObjectPoseProviderPlugin.cpp
plugins/ObjectPoseClientPlugin.cpp
@@ -19,6 +21,8 @@ set(SOURCES
)
set(HEADERS
ObjectPoseObserver.h
+ detail/ObjectPoseDecay.h
+ detail/RobotHeadMovement.h
plugins/ObjectPoseProviderPlugin.h
plugins/ObjectPoseClientPlugin.h
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 634600034..71021ee3d 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -831,87 +831,6 @@ namespace armarx
}
- void ObjectPoseObserver::RobotHeadMovement::fetchDatafields()
- {
- if (kinematicUnitObserver)
- {
- for (const std::string& jointName : jointNames)
- {
- std::string error = "";
- try
- {
- DatafieldRefBasePtr datafield = kinematicUnitObserver->getDatafieldRefByName(jointVelocitiesChannelName, jointName);
- DatafieldRefPtr casted = DatafieldRefPtr::dynamicCast(datafield);
- if (casted)
- {
- jointVelocitiesDatafields.push_back(casted);
- }
- }
- catch (const InvalidDatafieldException& e)
- {
- error = e.what();
- }
- catch (const InvalidChannelException& e)
- {
- error = e.what();
- }
- if (error.size() > 0)
- {
- ARMARX_WARNING << "Could not get datafield for joint '" << jointName << "' in channel '" << jointVelocitiesChannelName << "': \n "
- << error;
- }
- }
- }
- else
- {
- ARMARX_INFO << "Cannot fetch joint velocity datafields because there is no kinematic unit observer.";
- }
- }
-
- bool ObjectPoseObserver::RobotHeadMovement::isMoving() const
- {
- for (DatafieldRefPtr df : jointVelocitiesDatafields)
- {
- if (df)
- {
- float jointVelocity = df->getFloat();
- // ARMARX_IMPORTANT << "Is " << df->datafieldName << " " << VAROUT(std::abs(jointVelocity)) << " > " << VAROUT(maxJointVelocity) << "?";
- if (std::abs(jointVelocity) > maxJointVelocity)
- {
- return true;
- }
- }
- }
- return false;
- }
-
- void ObjectPoseObserver::RobotHeadMovement::movementStarts(long discardIntervalMs)
- {
- return movementStarts(IceUtil::Time::milliSeconds(discardIntervalMs));
- }
- void ObjectPoseObserver::RobotHeadMovement::movementStarts(IceUtil::Time discardInterval)
- {
- discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
- }
- void ObjectPoseObserver::RobotHeadMovement::movementStops(long discardIntervalMs)
- {
- return movementStops(IceUtil::Time::milliSeconds(discardIntervalMs));
- }
- void ObjectPoseObserver::RobotHeadMovement::movementStops(IceUtil::Time discardInterval)
- {
- if (discardInterval.toMilliSeconds() < 0)
- {
- // Stop discarding.
- discardUpdatesUntil = IceUtil::Time::milliSeconds(-1);
- }
- else
- {
- // Basically the same as starting.
- discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
- }
- }
-
-
void ObjectPoseObserver::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index 6f4cb8a0c..09c839d29 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -40,8 +40,10 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
+#include <RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h>
+#include <RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h>
+#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
namespace armarx
@@ -185,45 +187,10 @@ namespace armarx
Data data;
- struct Decay
- {
- /// Time for an object pose to decay.
- float durationSeconds = 20.0;
- /// Frequency of updating the current decay.
- float updateFrequencyHz = 10.0;
-
- SimpleRunningTask<>::pointer_type updateTask;
- };
- Decay decay;
-
+ objpose::ObjectPoseDecay decay;
- class RobotHeadMovement : public armarx::Logging
- {
- public:
-
- bool checkHeadVelocity = true;
-
- std::string jointVelocitiesChannelName = "jointvelocities";
- std::vector<std::string> jointNames = {"Neck_1_Yaw", "Neck_2_Pitch"};
- float maxJointVelocity = 0.05f;
- int discardIntervalAfterMoveMS = 100;
-
- KinematicUnitObserverInterfacePrx kinematicUnitObserver;
- std::vector<DatafieldRefPtr> jointVelocitiesDatafields;
- IceUtil::Time discardUpdatesUntil = IceUtil::Time::seconds(-1);
-
- DebugObserverInterfacePrx debugObserver;
-
- void fetchDatafields();
- bool isMoving() const;
-
- void movementStarts(long discardIntervalMs);
- void movementStarts(IceUtil::Time discardInterval);
- void movementStops(long discardIntervalMs);
- void movementStops(IceUtil::Time discardInterval);
- };
- RobotHeadMovement robotHead;
+ objpose::RobotHeadMovement robotHead;
std::mutex robotHeadMutex;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
new file mode 100644
index 000000000..c716765ab
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
@@ -0,0 +1,7 @@
+#include "ObjectPoseDecay.h"
+
+
+namespace armarx::objpose
+{
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
new file mode 100644
index 000000000..a68223beb
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+
+namespace armarx::objpose
+{
+
+ class ObjectPoseDecay
+ {
+ public:
+
+ /// Time for an object pose to decay.
+ float durationSeconds = 20.0;
+ /// Frequency of updating the current decay.
+ float updateFrequencyHz = 10.0;
+
+ // bool removeDecayedObjects = false;
+
+ SimpleRunningTask<>::pointer_type updateTask;
+
+ };
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
new file mode 100644
index 000000000..c0c68e6d1
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
@@ -0,0 +1,89 @@
+#include "RobotHeadMovement.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+
+namespace armarx::objpose
+{
+
+ void RobotHeadMovement::fetchDatafields()
+ {
+ if (kinematicUnitObserver)
+ {
+ for (const std::string& jointName : jointNames)
+ {
+ std::string error = "";
+ try
+ {
+ DatafieldRefBasePtr datafield = kinematicUnitObserver->getDatafieldRefByName(jointVelocitiesChannelName, jointName);
+ DatafieldRefPtr casted = DatafieldRefPtr::dynamicCast(datafield);
+ if (casted)
+ {
+ jointVelocitiesDatafields.push_back(casted);
+ }
+ }
+ catch (const InvalidDatafieldException& e)
+ {
+ error = e.what();
+ }
+ catch (const InvalidChannelException& e)
+ {
+ error = e.what();
+ }
+ if (error.size() > 0)
+ {
+ ARMARX_WARNING << "Could not get datafield for joint '" << jointName << "' in channel '" << jointVelocitiesChannelName << "': \n "
+ << error;
+ }
+ }
+ }
+ else
+ {
+ ARMARX_INFO << "Cannot fetch joint velocity datafields because there is no kinematic unit observer.";
+ }
+ }
+
+ bool RobotHeadMovement::isMoving() const
+ {
+ for (DatafieldRefPtr df : jointVelocitiesDatafields)
+ {
+ if (df)
+ {
+ float jointVelocity = df->getFloat();
+ // ARMARX_IMPORTANT << "Is " << df->datafieldName << " " << VAROUT(std::abs(jointVelocity)) << " > " << VAROUT(maxJointVelocity) << "?";
+ if (std::abs(jointVelocity) > maxJointVelocity)
+ {
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+
+ void RobotHeadMovement::movementStarts(long discardIntervalMs)
+ {
+ return movementStarts(IceUtil::Time::milliSeconds(discardIntervalMs));
+ }
+ void RobotHeadMovement::movementStarts(IceUtil::Time discardInterval)
+ {
+ discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
+ }
+ void RobotHeadMovement::movementStops(long discardIntervalMs)
+ {
+ return movementStops(IceUtil::Time::milliSeconds(discardIntervalMs));
+ }
+ void RobotHeadMovement::movementStops(IceUtil::Time discardInterval)
+ {
+ if (discardInterval.toMilliSeconds() < 0)
+ {
+ // Stop discarding.
+ discardUpdatesUntil = IceUtil::Time::milliSeconds(-1);
+ }
+ else
+ {
+ // Basically the same as starting.
+ discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
+ }
+ }
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
new file mode 100644
index 000000000..b0345e7c3
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
@@ -0,0 +1,46 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <IceUtil/Time.h>
+
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
+
+
+namespace armarx::objpose
+{
+ class RobotHeadMovement : public armarx::Logging
+ {
+ public:
+
+ bool checkHeadVelocity = true;
+
+ std::string jointVelocitiesChannelName = "jointvelocities";
+ std::vector<std::string> jointNames = {"Neck_1_Yaw", "Neck_2_Pitch"};
+ float maxJointVelocity = 0.05f;
+ int discardIntervalAfterMoveMS = 100;
+
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver;
+ std::vector<DatafieldRefPtr> jointVelocitiesDatafields;
+ IceUtil::Time discardUpdatesUntil = IceUtil::Time::seconds(-1);
+
+ DebugObserverInterfacePrx debugObserver;
+
+ void fetchDatafields();
+ bool isMoving() const;
+
+ void movementStarts(long discardIntervalMs);
+ void movementStarts(IceUtil::Time discardInterval);
+ void movementStops(long discardIntervalMs);
+ void movementStops(IceUtil::Time discardInterval);
+ };
+
+
+
+
+}
--
GitLab