register callback when loading data

rdiankov · Jan 12, 2024 · 3ca4fb8 · 3ca4fb8
1 parent c2a410e
commit 3ca4fb8
Showing 1 changed file with 23 additions and 9 deletions.
diff --git a/src/libopenrave-core/environment-core.h b/src/libopenrave-core/environment-core.h
@@ -624,37 +624,43 @@ class Environment : public EnvironmentBase
             if (_IsColladaFile(path)) {
                 if( RaveParseColladaURI(shared_from_this(), filename, atts) ) {
                     UpdatePublishedBodies();
+                    _RegisterKinBodyCallbackOnModify();
                     return true;
                 }
             }
         }
         else if( _IsColladaFile(filename) ) {
             if( RaveParseColladaFile(shared_from_this(), filename, atts) ) {
                 UpdatePublishedBodies();
+                _RegisterKinBodyCallbackOnModify();
                 return true;
             }
         }
         else if( _IsJSONFile(filename) ) {
             _ClearRapidJsonBuffer();
             if( RaveParseJSONFile(shared_from_this(), filename, UFIM_Exact, atts, *_prLoadEnvAlloc) ) {
+                _RegisterKinBodyCallbackOnModify();
                 return true;
             }
         }
         else if( _IsMsgPackFile(filename) ) {
             _ClearRapidJsonBuffer();
             if( RaveParseMsgPackFile(shared_from_this(), filename, UFIM_Exact, atts, *_prLoadEnvAlloc) ) {
+                _RegisterKinBodyCallbackOnModify();
                 return true;
             }
         }
         else if (StringEndsWith(filename, ".json.gpg")) {
             _ClearRapidJsonBuffer();
             if( RaveParseEncryptedJSONFile(shared_from_this(), filename, UFIM_Exact, atts, *_prLoadEnvAlloc) ) {
+                _RegisterKinBodyCallbackOnModify();
                 return true;
             }
         }
         else if (StringEndsWith(filename, ".msgpack.gpg")) {
             _ClearRapidJsonBuffer();
             if( RaveParseEncryptedMsgPackFile(shared_from_this(), filename, UFIM_Exact, atts, *_prLoadEnvAlloc) ) {
+                _RegisterKinBodyCallbackOnModify();
                 return true;
             }
         }
@@ -663,6 +669,7 @@ class Environment : public EnvironmentBase
             if( RaveParseXFile(shared_from_this(), robot, filename, atts) ) {
                 _AddRobot(robot, IAM_AllowRenaming);
                 UpdatePublishedBodies();
+                _RegisterKinBodyCallbackOnModify();
                 return true;
             }
         }
@@ -671,13 +678,15 @@ class Environment : public EnvironmentBase
             if( !!pbody ) {
                 _AddKinBody(pbody,IAM_AllowRenaming);
                 UpdatePublishedBodies();
+                _RegisterKinBodyCallbackOnModify();
                 return true;
             }
         }
         else {
             if( _ParseXMLFile(OpenRAVEXMLParser::CreateInterfaceReader(shared_from_this(),atts,true), filename) ) {
                 if( OpenRAVEXMLParser::GetXMLErrorCount() == 0 ) {
                     UpdatePublishedBodies();
+                    _RegisterKinBodyCallbackOnModify();
                     return true;
                 }
             }
@@ -3344,15 +3353,7 @@ class Environment : public EnvironmentBase
 
     inline void RegisterCallbackOnModify(std::function<void(EnvironmentBaseInfoPtr)> callback) override {
         _callbackOnModify = callback;
-        for (size_t index=0;index<_vecbodies.size();index++) {
-            if (!!_vecbodies[index]) {
-                _vecbodies[index]->RegisterCallbackOnModify(
-                    [this](KinBody::KinBodyInfoPtr kinBodyInfo) {
-                        _MergeKinbodyDiff(kinBodyInfo);
-                    }
-                );
-            }
-        }
+        _RegisterKinBodyCallbackOnModify();
     }
 
 protected:
@@ -3446,6 +3447,19 @@ class Environment : public EnvironmentBase
         return pbody;
     }
 
+    void _RegisterKinBodyCallbackOnModify()
+    {
+        for (size_t index=0;index<_vecbodies.size();index++) {
+            if (!!_vecbodies[index]) {
+                _vecbodies[index]->RegisterCallbackOnModify(
+                    [this](KinBody::KinBodyInfoPtr kinBodyInfo) {
+                        _MergeKinbodyDiff(kinBodyInfo);
+                    }
+                );
+            }
+        }
+    }
+
     void _SetDefaultGravity()
     {
         if( !!_pPhysicsEngine ) {