Fix bug that caused iDynTree::ModelParserOptions to be ignored (robot…

…ology#1191)
flferretti · Dec 23, 2024 · ca140ca · ca140ca
1 parent b690cf7
commit ca140ca
Show file tree

Hide file tree

Showing 5 changed files with 69 additions and 10 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -3,7 +3,7 @@
 
 cmake_minimum_required(VERSION 3.16)
 
-project(iDynTree VERSION 12.3.2
+project(iDynTree VERSION 12.3.3
                  LANGUAGES C CXX)
 
 # Disable in source build, unless Eclipse is used

diff --git a/src/model_io/codecs/include/private/URDFDocument.h b/src/model_io/codecs/include/private/URDFDocument.h
@@ -43,7 +43,7 @@ class iDynTree::URDFDocument: public iDynTree::XMLDocument {
     } m_buffers;
 
 public:
-    explicit URDFDocument(XMLParserState& parserState);
+    explicit URDFDocument(XMLParserState& parserState, const iDynTree::ModelParserOptions& parserOptions);
     virtual ~URDFDocument();
 
     iDynTree::ModelParserOptions& options();

diff --git a/src/model_io/codecs/src/ModelLoader.cpp b/src/model_io/codecs/src/ModelLoader.cpp
@@ -81,9 +81,11 @@ namespace iDynTree
     {
         // Allocate parser
         std::shared_ptr<XMLParser> parser = std::make_shared<XMLParser>();
-        parser->setDocumentFactory([](XMLParserState& state){ 
-            return std::shared_ptr<XMLDocument>(new URDFDocument(state)); 
-            });
+        auto parserOptions =  this->m_pimpl->m_options;
+        auto documentFactoryWithOptions = [parserOptions](XMLParserState& state){
+            return std::shared_ptr<XMLDocument>(new URDFDocument(state, parserOptions));
+            };
+        parser->setDocumentFactory(documentFactoryWithOptions);
         parser->setPackageDirs(packageDirs);
         if (!parser->parseXMLFile(filename)) {
             reportError("ModelLoader", "loadModelFromFile", "Error in parsing model from URDF.");
@@ -106,9 +108,12 @@ namespace iDynTree
     {
         // Allocate parser
         std::shared_ptr<XMLParser> parser = std::make_shared<XMLParser>();
-        parser->setDocumentFactory([](XMLParserState& state){
-            return std::shared_ptr<XMLDocument>(new URDFDocument(state)); 
-            });
+        auto parserOptions =  this->m_pimpl->m_options;
+        auto documentFactoryWithOptions = [parserOptions](XMLParserState& state){
+            return std::shared_ptr<XMLDocument>(new URDFDocument(state, parserOptions));
+            };
+
+        parser->setDocumentFactory(documentFactoryWithOptions);
         parser->setPackageDirs(packageDirs);
         if (!parser->parseXMLString(modelString)) {
             reportError("ModelLoader", "loadModelFromString", "Error in parsing model from URDF.");

diff --git a/src/model_io/codecs/src/URDFDocument.cpp b/src/model_io/codecs/src/URDFDocument.cpp
@@ -27,8 +27,8 @@ namespace iDynTree {
                                            const std::unordered_map<std::string, MaterialElement::MaterialInfo>& materialDatabase,
                                            ModelSolidShapes &modelGeometries);
 
-    URDFDocument::URDFDocument(XMLParserState& parserState)
-    : XMLDocument(parserState) {}
+    URDFDocument::URDFDocument(XMLParserState& parserState, const iDynTree::ModelParserOptions& options)
+    : XMLDocument(parserState), m_options(options) {}
 
     iDynTree::ModelParserOptions& URDFDocument::options() { return m_options; }
 

diff --git a/src/model_io/codecs/tests/URDFModelImportUnitTest.cpp b/src/model_io/codecs/tests/URDFModelImportUnitTest.cpp
@@ -255,6 +255,59 @@ void checkDuplicateJointsReturnsError() {
 
 }
 
+void checkaddSensorFramesAsAdditionalFramesOption() {
+    std::string urdf = R"(
+  <robot name="robot">
+  <link name="link_1">
+    <inertial>
+      <mass value="1"/>
+    </inertial>
+  </link>
+  <link name="link_2">
+    <inertial>
+      <mass value="2"/>
+    </inertial>
+  </link>
+  <joint name="joint_1" type="fixed">
+    <origin xyz="0.0 0.0 0.0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <limit lower="-1.0" upper="1.0"/>
+  </joint>
+  <sensor name="l_leg_ft" type="force_torque">
+    <parent joint="joint_1"/>
+    <force_torque>
+      <frame>sensor</frame>
+      <measure_direction>child_to_parent</measure_direction>
+    </force_torque>
+    <origin rpy="2.220446049250313e-16 -2.220446049250314e-16 2.220446049250313e-16" xyz="0.0 -1.3877787807814457e-17 0.0"/>
+  </sensor>
+</robot>
+)";
+
+    // Let's first load with the option disabled
+    {
+        iDynTree::ModelLoader mdlLoader;
+        iDynTree::ModelParserOptions parserOptions;
+        parserOptions.addSensorFramesAsAdditionalFrames = false;
+        mdlLoader.setParsingOptions(parserOptions);
+        ASSERT_IS_TRUE(mdlLoader.loadModelFromString(urdf));
+        ASSERT_IS_FALSE(mdlLoader.model().isFrameNameUsed("l_leg_ft"));
+    }
+
+    // Then with the option enabled
+    {
+        iDynTree::ModelLoader mdlLoader;
+        iDynTree::ModelParserOptions parserOptions;
+        parserOptions.addSensorFramesAsAdditionalFrames = true;
+        mdlLoader.setParsingOptions(parserOptions);
+        ASSERT_IS_TRUE(mdlLoader.loadModelFromString(urdf));
+        ASSERT_IS_TRUE(mdlLoader.model().isFrameNameUsed("l_leg_ft"));
+    }
+
+}
+
 int main()
 {
     checkURDF(getAbsModelPath("/simple_model.urdf"),1,0,0,1, 1, 0, "link1");
@@ -270,6 +323,7 @@ int main()
     checkLimitsForJointsAreDefinedFromFileName(getAbsModelPath("iCubGenova02.urdf"));
 
     checkLoadReducedModelOrderIsKept(getAbsModelPath("iCubGenova02.urdf"));
+    checkaddSensorFramesAsAdditionalFramesOption();
 
     return EXIT_SUCCESS;
 }