diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp index 8ee8b789cc933ba7f7a8afa86fa400aad9a40158..760af325a522618218a7d39ceaf45d9fa20fa65a 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp @@ -95,7 +95,6 @@ namespace armarx getProxyFromProperty(robotHead.kinematicUnitObserver, "KinematicUnitObserverName", false, "", false); robotHead.debugObserver = debugObserver; robotHead.fetchDatafields(); - robotHead.updateDebugObserverDatafields(); createRemoteGuiTab(); RemoteGui_startRunningTask(); @@ -122,6 +121,7 @@ namespace armarx ARMARX_VERBOSE << "Received object poses from '" << providerName << "'."; std::optional<IceUtil::Time> discardUpdatesUntil; + bool discardAll = false; { std::scoped_lock lock(robotHeadMutex); if (robotHead.checkHeadVelocity) @@ -129,19 +129,35 @@ namespace armarx if (robotHead.isMoving()) { robotHead.movementStarts(robotHead.discardIntervalAfterMoveMS); - robotHead.updateDebugObserverDatafields(); // ARMARX_IMPORTANT << "Ignoring pose update because robot head is moving! until " << robotHead.discardUpdatesUntil; - return; + discardAll = true; } - if (TimeUtil::GetTime() < robotHead.discardUpdatesUntil) + else if (TimeUtil::GetTime() < robotHead.discardUpdatesUntil) { - // ARMARX_IMPORTANT << "Ignoring pose update because robot head is moved until: " << robotHead.discardUpdatesUntil; - robotHead.updateDebugObserverDatafields(); - return; + discardAll = true; + // ARMARX_IMPORTANT << "Ignoring pose update because robot head has moved until: " << robotHead.discardUpdatesUntil; } - discardUpdatesUntil = robotHead.discardUpdatesUntil; - robotHead.updateDebugObserverDatafields(); + else + { + discardUpdatesUntil = robotHead.discardUpdatesUntil; + } + } + } + if (debugObserver) + { + StringVariantBaseMap map = + { + { "Discarding All Updates", new Variant(discardAll ? 1.f : 0.f) }, + }; + if (discardAll) + { + map["Proportion Updated Poses"] = new Variant(0.f); } + debugObserver->setDebugChannel(getName(), map); + } + if (discardAll) + { + return; } { std::scoped_lock lock(dataMutex); @@ -163,6 +179,7 @@ namespace armarx // Build new poses. objpose::ObjectPoseSeq newObjectPoses; + int numUpdated = 0; for (const objpose::data::ProvidedObjectPose& provided : providedPoses) { IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds); @@ -198,6 +215,7 @@ namespace armarx } else { + numUpdated++; objpose::ObjectPose& newPose = newObjectPoses.emplace_back(); newPose.fromProvidedPose(provided, robot); if (newPose.objectID.dataset().empty()) @@ -216,6 +234,15 @@ namespace armarx } } + if (debugObserver) + { + debugObserver->setDebugChannel(getName(), + { + { "Discarding All Updates", new Variant(discardAll ? 1 : 0) }, + { "Proportion Updated Poses", new Variant(static_cast<float>(numUpdated) / providedPoses.size()) } + }); + } + this->objectPoses[providerName] = newObjectPoses; handleProviderUpdate(providerName); } @@ -886,18 +913,6 @@ namespace armarx } } - void ObjectPoseObserver::RobotHeadMovement::updateDebugObserverDatafields() - { - if (debugObserver) - { - debugObserver->setDebugChannel(Logging::tag.tagName, - { - { "DicardUpdatesUntilMs", new Variant(discardUpdatesUntil.toMilliSeconds()) }, - { "NowMs", new Variant(TimeUtil::GetTime().toMilliSeconds()) }, - }); - } - } - void ObjectPoseObserver::createRemoteGuiTab() { diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h index cfc1b22b8dc34e72e110f79a726ea51c9e8d4da1..5022afbf2c92b7cba7a7c2455dbe26d0e3c1f88f 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h @@ -204,7 +204,6 @@ namespace armarx void movementStarts(IceUtil::Time discardInterval); void movementStops(long discardIntervalMs); void movementStops(IceUtil::Time discardInterval); - void updateDebugObserverDatafields(); }; RobotHeadMovement robotHead; std::mutex robotHeadMutex;