From dcb8d06516299de4ca8bcf511bdfe9b8b5080588 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 6 Jun 2024 15:46:18 +0200 Subject: [PATCH] The light at the end of the tunnel --- src/creo2urdf/include/creo2urdf/Creo2Urdf.h | 1 - .../include/creo2urdf/ElementTreeManager.h | 10 +++--- src/creo2urdf/include/creo2urdf/Utils.h | 2 +- src/creo2urdf/src/Creo2Urdf.cpp | 18 ++++++---- src/creo2urdf/src/ElementTreeManager.cpp | 34 ++++++++++++------- src/creo2urdf/src/Sensorizer.cpp | 17 ++++++++-- 6 files changed, 55 insertions(+), 27 deletions(-) diff --git a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h index b9c3202..f87e96c 100644 --- a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h +++ b/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. */ }; diff --git a/src/creo2urdf/include/creo2urdf/ElementTreeManager.h b/src/creo2urdf/include/creo2urdf/ElementTreeManager.h index a7acafe..947427e 100644 --- a/src/creo2urdf/include/creo2urdf/ElementTreeManager.h +++ b/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& joint_info_map); + bool populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map& 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. diff --git a/src/creo2urdf/include/creo2urdf/Utils.h b/src/creo2urdf/include/creo2urdf/Utils.h index ff6372a..2f2f4d6 100644 --- a/src/creo2urdf/include/creo2urdf/Utils.h +++ b/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. diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index d655706..c2fddb9 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/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()) { diff --git a/src/creo2urdf/src/ElementTreeManager.cpp b/src/creo2urdf/src/ElementTreeManager.cpp index 8fe1399..afe48f9 100644 --- a/src/creo2urdf/src/ElementTreeManager.cpp +++ b/src/creo2urdf/src/ElementTreeManager.cpp @@ -24,13 +24,16 @@ ElementTreeManager::ElementTreeManager(pfcFeature_ptr feat, std::map& joint_info_map) +bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map& 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(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; } diff --git a/src/creo2urdf/src/Sensorizer.cpp b/src/creo2urdf/src/Sensorizer.cpp index 113ff78..5c769f5 100644 --- a/src/creo2urdf/src/Sensorizer.cpp +++ b/src/creo2urdf/src/Sensorizer.cpp @@ -119,12 +119,18 @@ void Sensorizer::assignTransformToFTSensor(const std::map& 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