diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp index 2844a7cbb457cfb2060d67dff99a4f1c83b84168..bd8d091c83a5358b79caca978e007a8af3115892 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp @@ -378,6 +378,8 @@ namespace armarx objpose::AttachObjectToRobotNodeOutput ObjectPoseObserver::attachObjectToRobotNode( const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&) { + std::scoped_lock lock(dataMutex, robotMutex); + objpose::AttachObjectToRobotNodeOutput output; output.success = false; // We are not successful until proven otherwise. @@ -400,12 +402,9 @@ namespace armarx } std::string frameName = input.frameName; + // Find object pose. - std::optional<objpose::ObjectPose> currentObjectPose; - { - std::scoped_lock lock(dataMutex); - currentObjectPose = findObjectPose(input.providerName, objectID); - } + objpose::ObjectPose* currentObjectPose = findObjectPose(input.providerName, objectID); if (!currentObjectPose) { ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName @@ -424,7 +423,6 @@ namespace armarx } else { - std::scoped_lock lock(robotMutex); RobotState::synchronizeLocalClone(robot); armarx::FramedPose framed(currentObjectPose->objectPoseGlobal, armarx::GlobalFrame, agent->getName()); @@ -451,17 +449,45 @@ namespace armarx objpose::DetachObjectFromRobotNodeOutput ObjectPoseObserver::detachObjectFromRobotNode( const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&) { + std::scoped_lock lock(dataMutex); objpose::DetachObjectFromRobotNodeOutput output; - output.wasAttached = true; + + ObjectID objectID = armarx::fromIce(input.objectID); + { + std::scoped_lock lock(dataMutex); + objpose::ObjectPose* objectPose = findObjectPose(input.providerName, objectID); + if (!objectPose) + { + output.wasAttached = false; + } + else + { + output.wasAttached = bool(objectPose->attachment); + objectPose->attachment = std::nullopt; + } + } return output; } objpose::DetachAllObjectsFromRobotNodesOutput ObjectPoseObserver::detachAllObjectsFromRobotNodes(const Ice::Current&) { + std::scoped_lock lock(dataMutex); objpose::DetachAllObjectsFromRobotNodesOutput output; output.numDetached = 0; + for (auto& [prov, poses] : objectPoses) + { + for (auto& pose : poses) + { + if (pose.attachment) + { + pose.attachment = std::nullopt; + output.numDetached++; + } + } + } + return output; } @@ -564,18 +590,16 @@ namespace armarx return oobb; } - - std::optional<objpose::ObjectPose> ObjectPoseObserver::findObjectPose( - const std::string& providerName, ObjectID& objectID) + objpose::ObjectPose* ObjectPoseObserver::findObjectPose(const std::string& providerName, ObjectID& objectID) { - std::optional<objpose::ObjectPose> pose; + objpose::ObjectPose* pose = nullptr; if (!providerName.empty()) { pose = findObjectPoseByID(objectPoses.at(providerName), objectID); } else { - for (const auto& [_, poses] : objectPoses) + for (auto& [_, poses] : objectPoses) { pose = findObjectPoseByID(poses, objectID); if (pose) @@ -587,17 +611,16 @@ namespace armarx return pose; } - - std::optional<objpose::ObjectPose> ObjectPoseObserver::findObjectPoseByID(const objpose::ObjectPoseSeq& objectPoses, const ObjectID& id) + objpose::ObjectPose* ObjectPoseObserver::findObjectPoseByID(objpose::ObjectPoseSeq& objectPoses, const ObjectID& id) { - for (const objpose::ObjectPose& pose : objectPoses) + for (objpose::ObjectPose& pose : objectPoses) { if (pose.objectID == id) { - return pose; + return &pose; } } - return std::nullopt; + return nullptr; } objpose::ObjectPose ObjectPoseObserver::applyAttachment( diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h index b0827f2e7890b5ecea02b6c8cafaae064b01e69a..c0223807553b06d76ceafce234a3d363a9828874 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h @@ -137,10 +137,11 @@ namespace armarx std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id); - std::optional<objpose::ObjectPose> findObjectPose(const std::string& providerName, ObjectID& objectID); - static std::optional<objpose::ObjectPose> findObjectPoseByID(const objpose::ObjectPoseSeq& objectPoses, const ObjectID& id); - static objpose::ObjectPose applyAttachment(const objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr robot); + objpose::ObjectPose* findObjectPose(const std::string& providerName, ObjectID& objectID); + static objpose::ObjectPose* findObjectPoseByID(objpose::ObjectPoseSeq& objectPoses, const ObjectID& id); + + static objpose::ObjectPose applyAttachment(const objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr robot); void toIceWithAttachments(const objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent, objpose::data::ObjectPoseSeq& result, bool& synchronized);