Fix bug that caused iDynTree::ModelParserOptions to be ignored

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

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();

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.");

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; }
 
@@ -133,6 +133,7 @@ namespace iDynTree {
             return false;
         }
 
+        std::cerr << "m_options.addSensorFramesAsAdditionalFrame:" << m_options.addSensorFramesAsAdditionalFrames << std::endl;
         if (m_options.addSensorFramesAsAdditionalFrames)
         {
             if (!addSensorFramesAsAdditionalFramesToModel(m_model)) {

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;
 }