The light at the end of the tunnel

src/creo2urdf/include/creo2urdf/Creo2Urdf.h

@@ -150,7 +150,6 @@ class Creo2Urdf : public pfcUICommandActionListener {
     std::string m_csv_path{ "" }; /**< Path to the CSV file containing joint information. */
     std::string m_output_path{ "" }; /**< Output path for the exported URDF file. */
     pfcModel_ptr m_root_asm_model_ptr{ nullptr }; /**< Handle to the Creo model. */
-    ElementTreeManager m_element_tree; /**< Manager for the element tree. */
     pfcSession_ptr m_session_ptr{ nullptr }; /**< Handle to the Creo session. */
 };
 

src/creo2urdf/include/creo2urdf/ElementTreeManager.h

@@ -64,7 +64,7 @@ class ElementTreeManager {
      * @param[out] joint_info_map A map containing joint information.
      * @return True if successful, false otherwise.
      */
-    bool populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map<std::string, JointInfo>& joint_info_map);
+    bool populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map<std::string, JointInfo>& joint_info_map, bool is_asm=false);
 
     /**
      * @brief Gets the constraint type between two assembled parts.
@@ -85,10 +85,10 @@ class ElementTreeManager {
     std::string getChildName();
 
 private:
-    wfcElementTree_ptr tree = nullptr; ///< Pointer to the ElementTree of the part as feature.
-    wfcWFeature_ptr wfeat = nullptr;   ///< Pointer to the part as feature.
-    pfcSolid_ptr parent_solid;         ///< Pointer to the parent solid.
-    pfcSolid_ptr child_solid;          ///< Pointer to the child solid.
+    wfcElementTree_ptr tree{ nullptr }; ///< Pointer to the ElementTree of the part as feature.
+    wfcWFeature_ptr wfeat{ nullptr };   ///< Pointer to the part as feature.
+    pfcSolid_ptr parent_solid{ nullptr };         ///< Pointer to the parent solid.
+    pfcSolid_ptr child_solid{ nullptr };          ///< Pointer to the child solid.
 
     /*
      * @brief Retrieves the name of a common datum for the given model item type.

src/creo2urdf/include/creo2urdf/Utils.h

@@ -186,7 +186,7 @@ struct FTSensorInfo {
     std::string frame{"sensor"}; ///< Frame associated with the FT sensor.
     std::string sensorName{""}; ///< Name of the FT sensor.
     std::string frameName{""}; ///< Name of the frame.
-    std::string linkName{" "}; ///< Name of the associated link.
+    std::string linkName{""}; ///< Name of the associated link.
     std::string exportedFrameName{""}; ///< Name of the exported frame.
     iDynTree::Transform parent_link_H_sensor{iDynTree::Transform::Identity()}; ///< 3D transform from parent link to sensor.
     iDynTree::Transform child_link_H_sensor{iDynTree::Transform::Identity()}; ///< 3D transform from child link to sensor.

src/creo2urdf/src/Creo2Urdf.cpp

@@ -33,12 +33,18 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod
         if (!component_handle) {
             return false;
         }
+        ElementTreeManager element_tree_manager;
 
-        printToMessageWindow("Processing " + string(component_handle->GetFullName()) + " Owner " + string(model_owner->GetFullName()));
+        //printToMessageWindow("Processing " + string(component_handle->GetFullName()) + " Owner " + string(model_owner->GetFullName()));
 
         auto type = component_handle->GetType();
         if (type == pfcMDL_ASSEMBLY) {
             auto sub_asm_component_list = component_handle->ListItems(pfcModelItemType::pfcITEM_FEATURE);
+            auto ok = element_tree_manager.populateJointInfoFromElementTree(asmItemAsFeat, joint_info_map);
+            if (!ok) {
+                // WE NEED TO FIX the failure, the paths are not cleared
+                //continue;
+            }
             iDynTree::Transform parentAsmCsys_H_asmCsys = iDynTree::Transform::Identity();
             bool ret{ false };
             std::tie(ret, parentAsmCsys_H_asmCsys) = getTransformFromPart(component_handle, "ASM_CSYS", scale);
@@ -47,10 +53,10 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod
                 //return false;
             }
             else {
-                printToMessageWindow("Got the transform from  " + string(model_owner->GetFullName()) + " (the parent assembly) to " + string(component_handle->GetFullName()), c2uLogLevel::INFO);
-                printToMessageWindow(parentAsmCsys_H_asmCsys.toString());
+                //printToMessageWindow("Got the transform from  " + string(model_owner->GetFullName()) + " (the parent assembly) to " + string(component_handle->GetFullName()), c2uLogLevel::INFO);
+                //printToMessageWindow(parentAsmCsys_H_asmCsys.toString());
             }
-            auto ok = processAsmItems(sub_asm_component_list, component_handle, parentAsmCsys_H_asmCsys);
+            ok = processAsmItems(sub_asm_component_list, component_handle, parentAsmCsys_H_asmCsys);
             if (!ok) {
                 return false;
             }
@@ -63,7 +69,7 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod
         seq->append(asmItemAsFeat->GetId());
 
 
-        m_element_tree.populateJointInfoFromElementTree(asmItemAsFeat, joint_info_map);
+        element_tree_manager.populateJointInfoFromElementTree(asmItemAsFeat, joint_info_map);
 
         pfcComponentPath_ptr comp_path = pfcCreateComponentPath(pfcAssembly::cast(model_owner), seq);
 
@@ -245,7 +251,7 @@ void Creo2Urdf::OnCommand() {
         auto parent_link_name = joint_info.second.parent_link_name;
         auto child_link_name = joint_info.second.child_link_name;
         auto axis_name = joint_info.second.datum_name;
-        auto joint_name = getRenameElementFromConfig(parent_link_name + "--" + child_link_name);
+        auto joint_name = getRenameElementFromConfig(joint_info.first);
 
         // This handles the case of a "cut" assembly, where we have an axis but we miss the child link.
         if (child_link_name.empty() || link_info_map.find(parent_link_name) == link_info_map.end() || link_info_map.find(child_link_name) == link_info_map.end()) {

src/creo2urdf/src/ElementTreeManager.cpp

@@ -24,13 +24,16 @@ ElementTreeManager::ElementTreeManager(pfcFeature_ptr feat, std::map<std::string
 
 ElementTreeManager::~ElementTreeManager() {}
 
-bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map<std::string, JointInfo>& joint_info_map)
+bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map<std::string, JointInfo>& joint_info_map, bool is_asm)
 {
     wfeat = wfcWFeature::cast(feat);
 
     try
     {
         tree = wfeat->GetElementTree(nullptr, wfcFEAT_EXTRACT_NO_OPTS);
+        std::string joint_name = std::to_string(feat->GetId()) + ".xml";
+        //printToMessageWindow("Element tree extracted for feature " + std::to_string(feat->GetId()), c2uLogLevel::INFO);
+        tree->WriteElementTreeToFile(wfcELEMTREE_XML, joint_name.c_str());
     }
     xcatchbegin
     xcatchcip(pfcXToolkitInvalidType)
@@ -41,11 +44,16 @@ bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, s
 
     JointInfo joint;
 
-    if (!retrieveSolidReferences())
+
+    if (!retrieveSolidReferences()) {
+        printToMessageWindow("Could not retrieve solid references!", c2uLogLevel::WARN);
         return false;
+    }
 
     joint.child_link_name = getChildName();
     joint.parent_link_name = getParentName();
+    std::string joint_name = joint.parent_link_name + "--" + joint.child_link_name;
+    printToMessageWindow("JOINT !! " + joint_name, c2uLogLevel::INFO);
     joint.type = proAsmCompSetType_to_JointType.at(static_cast<ProAsmcompSetType>(getConstraintType()));
 
     if (joint.type == JointType::Revolute)
@@ -72,7 +80,7 @@ bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, s
         return false;
     }
 
-    joint_info_map.insert({ joint.datum_name, joint });
+    joint_info_map.insert({ joint_name, joint });
 
     return true;
 }
@@ -126,8 +134,9 @@ string ElementTreeManager::getConstraintDatum(pfcFeature_ptr feat, pfcComponentC
 
 std::string ElementTreeManager::getParentName()
 {
-    if (tree == nullptr)
+    if (!tree || !parent_solid)
     {
+        printToMessageWindow("Tree or parent solid is null!", c2uLogLevel::WARN);
         return "";
     }
     try{
@@ -143,13 +152,14 @@ std::string ElementTreeManager::getParentName()
 
 std::string ElementTreeManager::getChildName()
 {
-    if (tree == nullptr)
+    if (!tree || !child_solid)
     {
+        printToMessageWindow("Tree or child solid is null!", c2uLogLevel::WARN);
         return "";
     }
     try {
 
-    return std::string(child_solid->GetFullName());
+        return std::string(child_solid->GetFullName());
     }
     xcatchbegin
     xcatchcip(defaultEx)
@@ -185,18 +195,18 @@ bool ElementTreeManager::retrieveSolidReferences()
 
         for (int m = 0; m < extrefs->getarraysize(); m++) {
             auto extref = extrefs->get(m)->GetAsmcomponents()->GetPathToRef()->GetLeaf();
-
-            if (std::string(extref->GetFullName()) != retrievePartName())
-            {
+            // While defining a constraint the first part is the parent link and the second part is the child link
+            if (extref && !parent_solid) {
                 parent_solid = extref;
             }
-            if (std::string(extref->GetFullName()) == retrievePartName())
-            {
+            else if (extref && !child_solid && parent_solid) {
                 child_solid = extref;
             }
+            else {
+                break;
+            }
         }
     }
-
     return true;
 }
 

src/creo2urdf/src/Sensorizer.cpp

@@ -119,12 +119,18 @@ void Sensorizer::assignTransformToFTSensor(const std::map<std::string, ExportedF
             f.second.child_link_H_sensor = exported_frame_info_map.at(f.second.frameName).linkFrame_H_additionalFrame * exported_frame_info_map.at(f.second.frameName).additionalTransformation;
         }
         else {
-            if (joint_info_map.find(f.second.frameName) == joint_info_map.end())
+
+            auto joint_it = std::find_if(joint_info_map.begin(), joint_info_map.end(),
+                [f](const std::pair<std::string, JointInfo>& pair) {
+                    return pair.second.datum_name == f.second.frameName;
+                });
+
+            if (joint_it == joint_info_map.end())
             {
                 continue;
             }
 
-            JointInfo j_info = joint_info_map.at(f.second.frameName);
+            JointInfo j_info = joint_it->second;
 
             LinkInfo parent_l_info = link_info_map.at(j_info.parent_link_name);
             LinkInfo child_l_info = link_info_map.at(j_info.child_link_name);
@@ -261,6 +267,13 @@ void Sensorizer::assignTransformToSensors(const std::map<std::string, ExportedFr
                     break;
                 }
             }
+
+            if (link_info_map.find(cad_link_name) == link_info_map.end())
+			{
+				printToMessageWindow("Sensorizer: link " + cad_link_name + " not found in the link info map, sensor "+ s.sensorName + " skipped.", c2uLogLevel::WARN);
+				continue;
+			}
+
             auto link_info = link_info_map.at(cad_link_name);
             std::tie(ret, csys_H_additionalFrame) = getTransformFromPart(link_info.modelhdl, s.frameName, scale);
             if (!ret)