diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp index d6635696acd4f72d3c3342423233a459baf89af0..62dec875ea7ccb106b762edec6ae79e2341e27b0 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp +++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp @@ -226,107 +226,121 @@ namespace armarx::armem::server::obj::instance } } - if (!discard) + if (discard) { - // Update the entity. - stats.numUpdated++; + continue; + } - VirtualRobot::RobotPtr robot = - robots.get(provided.robotName, provided.providerName); + // Update the entity. + stats.numUpdated++; - if (not robot) - { - ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `" - << provided.robotName << "`."; - } + VirtualRobot::RobotPtr robot = robots.get(provided.robotName, provided.providerName); + if (not robot) + { + ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `" + << provided.robotName << "`."; + } - // robot may be null! + // robot may be null! - // Update the robot to obtain correct local -> global transformation - if (robot and robotSyncTimestamp != timestamp) - { - ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp)) - << "Failed to synchronize robot to timestamp " << timestamp << ". This is " - << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past."; + bool synchronized = false; - robotSyncTimestamp = timestamp; + // Update the robot to obtain correct local -> global transformation + if (robot and robotSyncTimestamp != timestamp) + { + synchronized = robots.reader->synchronizeRobot(*robot, timestamp); + if (not synchronized) + { + ARMARX_VERBOSE << "Failed to synchronize robot to timestamp " << timestamp + << " of provided object pose (" << provided.objectID + << "). This is " << (Clock::Now() - timestamp).toSecondsDouble() + << " seconds in the past."; + } + robotSyncTimestamp = timestamp; - // Apply calibration offset after synchronizing the robot. + // Apply calibration offset after synchronizing the robot. + { + if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode)) { - if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode)) - { - VirtualRobot::RobotNodePtr robotNode = - robot->getRobotNode(calibration.robotNode); + VirtualRobot::RobotNodePtr robotNode = + robot->getRobotNode(calibration.robotNode); - float value = robotNode->getJointValue(); - float newValue = value + calibration.offset; + float value = robotNode->getJointValue(); + float newValue = value + calibration.offset; - /* + /* * If newValue is out of the joint's limits, then the calibration would be ignored. * In that case, we expand the joint value limits of the local robot model to allow * for the calibrated value. * As this is just for perception (and not for controlling the robot), this should be fine^TM. */ - VirtualRobot::RobotNode::JointLimits limits = - robotNode->getJointLimits(); - bool limitsChanged = false; - if (newValue < limits.low) - { - limits.low = newValue; - limitsChanged = true; - } - if (newValue > limits.high) - { - limits.high = newValue; - limitsChanged = true; - } - if (limitsChanged) - { - robotNode->setJointLimits(limits.low, limits.high); - } - - robotNode->setJointValue(newValue); + VirtualRobot::RobotNode::JointLimits limits = robotNode->getJointLimits(); + bool limitsChanged = false; + if (newValue < limits.low) + { + limits.low = newValue; + limitsChanged = true; } + if (newValue > limits.high) + { + limits.high = newValue; + limitsChanged = true; + } + if (limitsChanged) + { + robotNode->setJointLimits(limits.low, limits.high); + } + + robotNode->setJointValue(newValue); } } + } - objpose::ObjectPose& newPose = newObjectPoses.emplace_back(); - if (provided.objectPoseFrame.empty()) - { - objpose::data::ProvidedObjectPose copy = provided; - copy.objectPoseFrame = armarx::GlobalFrame; - newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK. - } - else - { - newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK. - } + if (not synchronized) + { + /* + * We know nothing about the current robot configuration. So the behvaiour of the + * following code is the same as if we have no robot model at all. + */ + robot = nullptr; + } - if (previousPose && previousPose->attachment) - { - // Keep current attachment. - ARMARX_CHECK(!p.discardSnapshotsWhileAttached); - newPose.attachment = previousPose->attachment; - } + objpose::ObjectPose& newPose = newObjectPoses.emplace_back(); + if (provided.objectPoseFrame.empty()) + { + objpose::data::ProvidedObjectPose copy = provided; + copy.objectPoseFrame = armarx::GlobalFrame; + newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK. + } + else + { + newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK. + } - if (newPose.objectID.dataset().empty()) - { - // Try to find the data set. - const std::string dataset = - classNameToDatasetCache.get(newPose.objectID.className()); - if (!dataset.empty()) - { - newPose.objectID = { - dataset, newPose.objectID.className(), newPose.objectID.instanceName()}; - } - } - if (!provided.localOOBB) + if (previousPose && previousPose->attachment) + { + // Keep current attachment. + ARMARX_CHECK(!p.discardSnapshotsWhileAttached); + newPose.attachment = previousPose->attachment; + } + + if (newPose.objectID.dataset().empty()) + { + // Try to find the data set. + const std::string dataset = + classNameToDatasetCache.get(newPose.objectID.className()); + if (!dataset.empty()) { - // Try to load oobb from disk. - newPose.localOOBB = getObjectOOBB(newPose.objectID); + newPose.objectID = { + dataset, newPose.objectID.className(), newPose.objectID.instanceName()}; } } + if (!provided.localOOBB) + { + // Try to load oobb from disk. + newPose.localOOBB = getObjectOOBB(newPose.objectID); + } } commitObjectPoses(newObjectPoses, providerName);