Adjust to review

Signed-off-by: Michał <[email protected]>

Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp

@@ -7,12 +7,16 @@
  */
 
 #include "FixURDF.h"
+#include <AzCore/XML/rapidxml.h>
 #include <AzCore/XML/rapidxml_print.h>
-#include <AzCore/std/containers/unordered_set.h>
+#include <AzCore/std/containers/unordered_map.h>
 #include <iostream>
 
 namespace ROS2::Utils
 {
+    //! Modifies a parsed URDF in memory to add missing inertia to links, which prevents SDF error 19.
+    //! @param urdf URDF to modify.
+    //! @returns a list of names of links that were modified.
     AZStd::vector<AZStd::string> AddMissingInertiaToLinks(AZ::rapidxml::xml_node<>* urdf)
     {
         AZStd::vector<AZStd::string> modifiedLinks;
@@ -64,17 +68,22 @@ namespace ROS2::Utils
         return modifiedLinks;
     }
 
-    AZStd::vector<AZStd::string> ChangeDuplications(AZ::rapidxml::xml_node<>* urdf)
+    //! Handles a case of multiple joints and the link sharing a common names which causes SDF error2 (but is fine in URDF)
+    //! Function will add a suffix "_dup" to the name of the joint if it is also the name of a link.
+    //! If there are name collisions in links, this will not be able to fix it, the SDF parser will throw an error.
+    //! @param urdf URDF to modify.
+    //! @returns a list of links that were modified
+    AZStd::vector<AZStd::string> RenameDuplicatedJoints(AZ::rapidxml::xml_node<>* urdf)
     {
         using namespace AZ::rapidxml;
         AZStd::vector<AZStd::string> modifiedLinks;
-        AZStd::unordered_set<AZStd::string> linkAndJointsName;
+        AZStd::unordered_map<AZStd::string, unsigned int> linkAndJointsName;
         for (xml_node<>* link = urdf->first_node("link"); link; link = link->next_sibling("link"))
         {
             auto* name = link->first_attribute("name");
             if (name)
             {
-                linkAndJointsName.insert(name->value());
+                linkAndJointsName.insert(AZStd::make_pair(name->value(), 0));
             }
         }
         for (xml_node<>* joint = urdf->first_node("joint"); joint; joint = joint->next_sibling("joint"))
@@ -84,14 +93,15 @@ namespace ROS2::Utils
             {
                 if (linkAndJointsName.contains(name->value()))
                 {
-                    auto newName = AZStd::string(name->value()) + "_joint_dup";
+                    unsigned int& count = linkAndJointsName[name->value()];
+                    auto newName = AZStd::string::format("%s_dup%d", name->value(), count);
                     name->value(urdf->document()->allocate_string(newName.c_str()));
-                    linkAndJointsName.insert(newName);
+                    count++;
                     modifiedLinks.push_back(AZStd::move(newName));
                 }
                 else
                 {
-                    linkAndJointsName.insert(name->value());
+                    linkAndJointsName.insert(AZStd::make_pair(name->value(), 0));
                 }
             }
         }
@@ -117,7 +127,7 @@ namespace ROS2::Utils
             }
         }
 
-        auto renames = ChangeDuplications(urdf);
+        auto renames = RenameDuplicatedJoints(urdf);
         if (renames.size())
         {
             AZ_Warning("ROS2", false, "Renamed duplicated links and joints: ");
@@ -130,7 +140,6 @@ namespace ROS2::Utils
 
         std::string xmlDocString;
         AZ::rapidxml::print(std::back_inserter(xmlDocString), *urdf, 0);
-
         return { xmlDocString, modifiedElements };
     }
 } // namespace ROS2::Utils

Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h

@@ -8,26 +8,17 @@
 
 #pragma once
 
-#include <AzCore/XML/rapidxml.h>
 #include <AzCore/std/containers/vector.h>
 #include <AzCore/std/string/string.h>
 
 namespace ROS2
 {
     namespace Utils
     {
-        //! Modifies a parsed URDF in memory to add missing inertia to links, which prevents SDF error 19.
-        //! @param urdf URDF to modify.
-        //! @returns a list of names of links that were modified.
-        AZStd::vector<AZStd::string> AddMissingInertiaToLinks(AZ::rapidxml::xml_node<>* urdf);
-
-        //! Modifies names of links and joints to be unique, which means that a set of both contains no duplicates. This prevents SDF
-        //! error 2.
-        //! @param urdf URDF to modify.
-        //! @returns a list of links that were modified
-        AZStd::vector<AZStd::string> ChangeDuplications(AZ::rapidxml::xml_node<>* urdf);
-
-        //! Modifies in memory URDF to add missing elements
+        //! Modifies in memory URDF to increase chance of successful conversion to SDF.
+        //! It does the following:
+        //! - Adds missing inertia to links of mass 1 kg and identity inertia matrix.
+        //! - Renames joints that have the same name as a link.
         //! @param urdf URDF to modify.
         //! @returns a modified URDF and a list of XML element that were modified
         AZStd::pair<std::string, AZStd::vector<AZStd::string>> ModifyURDFInMemory(const std::string& data);

Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp

@@ -342,7 +342,8 @@ namespace ROS2
         if (currentPage() == m_xacroParamsPage)
         {
             m_params = m_xacroParamsPage->GetXacroParameters();
-            if (const bool isFileUrdfOrXacro = Utils::IsFileXacro(m_urdfPath) || Utils::IsFileUrdf(m_urdfPath); isFileUrdfOrXacro)
+            if (const bool isFileUrdfOrXacro = Utils::IsFileXacro(m_urdfPath) || Utils::IsFileUrdf(m_urdfPath);
+                isFileUrdfOrXacro)
             {
                 OpenUrdf();
             }
@@ -372,7 +373,7 @@ namespace ROS2
     {
         if ((currentPage() == m_fileSelectPage && m_params.empty()) || currentPage() == m_xacroParamsPage)
         {
-            if (m_parsedUrdf.Model() != nullptr && m_checkUrdfPage->isWarning())
+            if (m_parsedUrdf.Model() != nullptr && !m_checkUrdfPage->isWarning())
             {
                 return m_xacroParamsPage->nextId();
             }