Fix calibration offset in object instance segment

• Fix calibration offset being lost after a second synchronization
• Fix calibration offset being lost due to hitting the joint limits of the robot model

With @daab

source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp

@@ -51,6 +51,15 @@
 namespace armarx::armem::server::obj::instance
 {
 
+    void Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+    {
+        defs->optional(robotName, prefix + "robotName",
+                       "Name of robot whose note can be calibrated.\n"
+                       "If not given, the 'fallbackName' is used.");
+        defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
+        defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
+    }
+
     Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
         SpecializedCoreSegment(memoryToIceAdapter,
                                objects::instaceSegmentID.coreSegmentName,
@@ -150,6 +159,7 @@ namespace armarx::armem::server::obj::instance
     Segment::CommitStats Segment::commitObjectPoses(
         const std::string& providerName,
         const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+        const Calibration& calibration,
         std::optional<armem::Time> discardUpdatesUntil)
     {
         CommitStats stats;
@@ -209,6 +219,43 @@ namespace armarx::armem::server::obj::instance
                 {
                     ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp));
                     robotSyncTimestamp = timestamp;
+
+
+                    // Apply calibration offset after synchronizing the robot.
+                    {
+                        if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
+                        {
+                            VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode(calibration.robotNode);
+
+                            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);
+                        }
+                    }
                 }
 
                 objpose::ObjectPose& newPose = newObjectPoses.emplace_back();