Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Updating ROS2 RobotImporter to use libsdformat #460

Merged
merged 15 commits into from
Aug 25, 2023
Merged

Updating ROS2 RobotImporter to use libsdformat #460

Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
25e5575
first attempt at using sdformat
mbalfour-amzn Jul 5, 2023
21affbd
Second revision of replacing the use of the urdfdom API with sdformat
lemonade-dm Aug 4, 2023
5b125f2
Updated the Link query logic in the RobotImporter
lemonade-dm Aug 16, 2023
4f6d296
Addressed feedback for URDF Import using SDFormat PR
lemonade-dm Aug 18, 2023
682f8e8
Added support to gather all Materials from each model tag within an SDF
lemonade-dm Aug 18, 2023
f0ed3d4
Fixed nullptr dereference for fixed joint parsing from SDF
lemonade-dm Aug 18, 2023
6a6418c
Updated the `CreatePrefabTemplateFromURDF` link hierarchy logic
lemonade-dm Aug 18, 2023
5eb10a8
Fixed RobotImporterWidget error logging when xacro succeeds and URDF …
lemonade-dm Aug 21, 2023
7a8414d
Added UnitTest for URDF links without inertia
lemonade-dm Aug 21, 2023
68f5bdc
Updated the Revolute joint maker code to not set a joint limit when t…
lemonade-dm Aug 21, 2023
1b862cd
JointsMaker and URDFPrefabMaker build fixes.
lemonade-dm Aug 22, 2023
9d21bc0
Fixed crash in CreatePrefabTemplateFromURDF for joint links
lemonade-dm Aug 23, 2023
b007b1d
Fixed assert in creation of ROS2 Robot Control due to an invalid Enti…
lemonade-dm Aug 24, 2023
6335e15
Updated Xacro process launch code to validate that the xacro command …
lemonade-dm Aug 25, 2023
ab097dd
Updated the Joint Visitor for SDF joints
lemonade-dm Aug 25, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 0 additions & 3 deletions Gems/ROS2/Code/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -140,9 +140,6 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS)
3rdParty::sdformat
)

find_package(urdfdom)
target_link_libraries(${gem_name}.Editor.Static PUBLIC urdfdom::urdfdom_model)

ly_add_target(
NAME ${gem_name}.Editor GEM_MODULE
NAMESPACE Gem
Expand Down
3 changes: 2 additions & 1 deletion Gems/ROS2/Code/FindROS2.cmake
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,10 @@

# Note that this does not find any ros2 package in particular, but determines whether a distro is sourced properly
# Can be extended to handle supported / unsupported distros
message(STATUS "Ros Distro is \"$ENV{ROS_DISTRO}\"")
if (NOT DEFINED ENV{ROS_DISTRO} OR NOT DEFINED ENV{AMENT_PREFIX_PATH})
message(WARNING, "To build ROS2 Gem a ROS distribution needs to be sourced, but none detected")
set(ROS2_FOUND FALSE)
return()
endif()
set(ROS2_FOUND TRUE)
set(ROS2_FOUND TRUE)
84 changes: 62 additions & 22 deletions Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,21 @@
*/

#include "ROS2RobotImporterEditorSystemComponent.h"
#include "RobotImporter/URDF/UrdfParser.h"
#include "RobotImporter/Utils/FilePath.h"
#include <RobotImporter/URDF/UrdfParser.h>
#include <RobotImporter/Utils/FilePath.h>
#include <RobotImporter/Utils/ErrorUtils.h>
#include "RobotImporterWidget.h"
#include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
#include <AzCore/Serialization/SerializeContext.h>
#include <AzCore/Utils/Utils.h>
#include <AzCore/std/chrono/chrono.h>
#include <AzCore/std/string/string.h>
#include <AzCore/StringFunc/StringFunc.h>
#include <AzToolsFramework/API/ViewPaneOptions.h>


#include <sdf/sdf.hh>

#if !defined(Q_MOC_RUN)
#include <QWindow>
#endif
Expand Down Expand Up @@ -83,26 +90,38 @@ namespace ROS2
{
if (filePath.empty())
{
AZ_Warning("ROS2EditorSystemComponent", false, "Path provided for prefab is empty");
AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "Path provided for prefab is empty");
return false;
}
if (Utils::IsFileXacro(filePath))
{
AZ_Warning("ROS2EditorSystemComponent", false, "XACRO formatted files are not supported");
AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "XACRO formatted files are not supported");
return false;
}

urdf::ModelInterfaceSharedPtr parsedUrdf = UrdfParser::ParseFromFile(filePath);
if (!parsedUrdf)
// Read the SDF Settings from the Settings Registry into a local struct
SdfAssetBuilderSettings sdfBuilderSettings;
sdfBuilderSettings.LoadSettings();
// Set the parser config settings for URDF content
sdf::ParserConfig parserConfig;
parserConfig.URDFSetPreserveFixedJoint(sdfBuilderSettings.m_urdfPreserveFixedJoints);

auto parsedUrdfOutcome = UrdfParser::ParseFromFile(filePath, parserConfig);
if (!parsedUrdfOutcome)
{
const auto log = UrdfParser::GetUrdfParsingLog();
AZ_Warning("ROS2EditorSystemComponent", false, "URDF parsing failed. Refer to %s", log.c_str());
const AZStd::string log = Utils::JoinSdfErrorsToString(parsedUrdfOutcome.error());

AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "URDF parsing failed with errors:\nRefer to %s",
log.c_str());
return false;
}

auto collidersNames = Utils::GetMeshesFilenames(parsedUrdf->getRoot(), false, true);
auto visualNames = Utils::GetMeshesFilenames(parsedUrdf->getRoot(), true, false);
auto meshNames = Utils::GetMeshesFilenames(parsedUrdf->getRoot(), true, true);
// Urdf Root has been parsed successfully retrieve it from the Outcome
const sdf::Root& parsedUrdfRoot = parsedUrdfOutcome.value();

auto collidersNames = Utils::GetMeshesFilenames(&parsedUrdfRoot, false, true);
auto visualNames = Utils::GetMeshesFilenames(&parsedUrdfRoot, true, false);
auto meshNames = Utils::GetMeshesFilenames(&parsedUrdfRoot, true, true);
AZStd::shared_ptr<Utils::UrdfAssetMap> urdfAssetsMapping = AZStd::make_shared<Utils::UrdfAssetMap>();
if (importAssetWithUrdf)
{
Expand All @@ -126,7 +145,7 @@ namespace ROS2

if (loopTime - loopStartTime > assetLoopTimeout)
{
AZ_Warning("ROS2EditorSystemComponent", false, "Loop waiting for assets timed out");
AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "Loop waiting for assets timed out");
break;
}

Expand All @@ -136,19 +155,19 @@ namespace ROS2
auto sourceAssetFullPath = asset.m_availableAssetInfo.m_sourceAssetGlobalPath;
if (sourceAssetFullPath.empty())
{
AZ_Warning("ROS2EditorSystemComponent", false, "Asset %s missing `sourceAssetFullPath`", name.c_str());
AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "Asset %s missing `sourceAssetFullPath`", name.c_str());
continue;
}
using namespace AzToolsFramework;
using namespace AzToolsFramework::AssetSystem;
AZ::Outcome<AssetSystem::JobInfoContainer> result = AZ::Failure();
AssetSystemJobRequestBus::BroadcastResult(
result, &AssetSystemJobRequestBus::Events::GetAssetJobsInfo, sourceAssetFullPath, true);
result, &AssetSystemJobRequestBus::Events::GetAssetJobsInfo, sourceAssetFullPath.Native(), true);

if (!result.IsSuccess())
{
assetProcessorFailed = true;
AZ_Error("ROS2EditorSystemComponent", false, "Asset System failed to reply with jobs infos");
AZ_Error("ROS2RobotImporterEditorSystemComponent", false, "Asset System failed to reply with jobs infos");
break;
}

Expand All @@ -157,34 +176,55 @@ namespace ROS2
{
if (job.m_status == JobStatus::Queued || job.m_status == JobStatus::InProgress)
{
AZ_Printf("ROS2EditorSystemComponent", "asset %s is being processed", sourceAssetFullPath.c_str());
AZ_Printf("ROS2RobotImporterEditorSystemComponent", "asset %s is being processed", sourceAssetFullPath.c_str());
allAssetProcessed = false;
}
else
{
AZ_Printf("ROS2EditorSystemComponent", "asset %s is done", sourceAssetFullPath.c_str());
AZ_Printf("ROS2RobotImporterEditorSystemComponent", "asset %s is done", sourceAssetFullPath.c_str());
}
}
}

if (allAssetProcessed && !assetProcessorFailed)
{
AZ_Printf("ROS2EditorSystemComponent", "All assets processed");
AZ_Printf("ROS2RobotImporterEditorSystemComponent", "All assets processed");
}
};

AZStd::string prefabName = AZStd::string(parsedUrdf->getName().c_str(), parsedUrdf->getName().size()) + ".prefab";
// Use the name of the first model tag in the SDF for the prefab
// Otherwise use the name of the first world tag in the SDF
AZStd::string prefabName;
if (const sdf::Model* model = parsedUrdfRoot.Model();
model != nullptr)
{
prefabName = AZStd::string(model->Name().c_str(), model->Name().size()) + ".prefab";
}

if (uint64_t urdfWorldCount = parsedUrdfRoot.WorldCount();
prefabName.empty() && urdfWorldCount > 0)
{
const sdf::World* parsedUrdfWorld = parsedUrdfRoot.WorldByIndex(0);
prefabName = AZStd::string(parsedUrdfWorld->Name().c_str(), parsedUrdfWorld->Name().size()) + ".prefab";
}

if (prefabName.empty())
{
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In the format itself, the world element is optional, since robots can be defined either with or without the world context (as a child element or as a sibling of world element).

image

Perhaps the converter always creates the robot inside a world element, but I am not sure whether we should rely on that it won't change. An example of correct SDF that this would skip:

<?xml version='1.0'?>
<sdf version='1.10'>
  <model name='my_model'>
    ...
  </model>
</sdf>

What do you think?

Copy link
Contributor Author

@lemonade-dm lemonade-dm Aug 17, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am thinking we could check if there is at least 1 model, otherwise the URDF wouldn't have any robot data within it.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

IMO, I'd be inclined to skip any validation like that for now. Technically, I think the sdf format supports files with no models, like one with just plugins and lights for example. Admittedly a file without models is probably not useful and maybe should generate a warning, but it seems a little bit wrong to me to disallow it completely. I won't be opposed if you decide to require it though.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Well we use the Model name or World name for the name of prefab that gets generated.
If want we can use the name of the URDF file itself and then let the URDFPrefabMaker code take of dealing with not having any models.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What I mean here is that there can be Models without any Worlds. So just checking for Worlds is not enough.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, I noticed that is was the case for URDF conversion.
It comes with only a root without any tags within it.

I have updated the logic, to use the name of the root tag if it exist, otherwise it uses the name of first tag as the prefab name.

AZ_Error("ROS2RobotImporterEditorSystemComponent", false, "URDF file converted to SDF %.*s contains no worlds."
" O3DE Prefab cannot be created", AZ_STRING_ARG(filePath));
return false;
}


const AZ::IO::Path prefabPathRelative(AZ::IO::Path("Assets") / "Importer" / prefabName);
const AZ::IO::Path prefabPath(AZ::IO::Path(AZ::Utils::GetProjectPath()) / prefabPathRelative);
AZStd::unique_ptr<URDFPrefabMaker> prefabMaker;
prefabMaker = AZStd::make_unique<URDFPrefabMaker>(filePath, parsedUrdf, prefabPath.String(), urdfAssetsMapping, useArticulation);
AZStd::unique_ptr<URDFPrefabMaker> prefabMaker = AZStd::make_unique<URDFPrefabMaker>(filePath, &parsedUrdfRoot, prefabPath.String(), urdfAssetsMapping, useArticulation);

auto prefabOutcome = prefabMaker->CreatePrefabFromURDF();

if (!prefabOutcome.IsSuccess())
{
AZ_Error("ROS2EditorSystemComponent", false, "Unable to create Prefab from URDF file %s", filePath.data());
AZ_Error("ROS2RobotImporterEditorSystemComponent", false, "Unable to create Prefab from URDF file %s", filePath.data());
return false;
}

Expand Down
1 change: 0 additions & 1 deletion Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@
#include <AzToolsFramework/Entity/EditorEntityContextBus.h>
#include <ROS2/RobotImporter/RobotImporterBus.h>
#include <RobotImporter/Utils/SourceAssetsStorage.h>
#include <urdf_parser/urdf_parser.h>
namespace ROS2
{

Expand Down
Loading