diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp index 416674e3da8ddb63d403bea83f1d918950443099..a742a5640dff766664addaac4869535679a2169e 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp @@ -132,39 +132,38 @@ namespace armarx const std::string& providerName, const objpose::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&) { ARMARX_VERBOSE << "Received object poses from '" << providerName << "'."; - long syncedTimestamp = -1; + objpose::ObjectPoseSeq objectPoses; - for (const auto& provided : providedPoses) { - objpose::ObjectPose& pose = objectPoses.emplace_back(); - pose.objectType = provided.objectType; - pose.objectID = provided.objectID; - - pose.objectPoseOriginal = provided.objectPose; - pose.objectPoseOriginalFrame = provided.objectPoseFrame; + std::scoped_lock lock(dataMutex); + RobotState::synchronizeLocalClone("robot"); - armarx::PosePtr p = armarx::PosePtr::dynamicCast(provided.objectPose); - ARMARX_CHECK_NOT_NULL(p); + for (const auto& provided : providedPoses) { - std::scoped_lock robotMutex; - if (syncedTimestamp != pose.timestampMicroSeconds) + objpose::ObjectPose& pose = objectPoses.emplace_back(); + pose.objectType = provided.objectType; + pose.objectID = provided.objectID; + + pose.objectPoseOriginal = provided.objectPose; + pose.objectPoseOriginalFrame = provided.objectPoseFrame; + + armarx::PosePtr p = armarx::PosePtr::dynamicCast(provided.objectPose); + ARMARX_CHECK_NOT_NULL(p); { - RobotState::synchronizeLocalClone("robot", pose.timestampMicroSeconds); - syncedTimestamp = pose.timestampMicroSeconds; + armarx::FramedPose framed(p->toEigen(), provided.objectPoseFrame, robot->getName()); + framed.changeFrame(robot, robot->getRootNode()->getName()); + pose.objectPoseRobot = new Pose(framed.toEigen()); + framed.changeToGlobal(robot); + pose.objectPoseGlobal = new Pose(framed.toEigen()); + + pose.robotConfig = robot->getConfig()->getRobotNodeJointValueMap(); + pose.robotPose = new Pose(robot->getGlobalPose()); } - armarx::FramedPose framed(p->toEigen(), provided.objectPoseFrame, robot->getName()); - framed.changeFrame(robot, robot->getRootNode()->getName()); - pose.objectPoseRobot = new Pose(framed.toEigen()); - framed.changeToGlobal(robot); - pose.objectPoseGlobal = new Pose(framed.toEigen()); - - pose.robotConfig = robot->getConfig()->getRobotNodeJointValueMap(); - pose.robotPose = new Pose(robot->getGlobalPose()); - } - pose.confidence = provided.confidence; - pose.timestampMicroSeconds = provided.timestampMicroSeconds; - pose.providerName = provided.providerName; + pose.confidence = provided.confidence; + pose.timestampMicroSeconds = provided.timestampMicroSeconds; + pose.providerName = provided.providerName; + } } {