123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277 |
- /*
- * Copyright (c) Contributors to the Open 3D Engine Project.
- * For complete copyright and license terms please see the LICENSE at the root of this distribution.
- *
- * SPDX-License-Identifier: Apache-2.0 OR MIT
- *
- */
- #include "ROS2SDFormatHooksUtils.h"
- #include <AzToolsFramework/ToolsComponents/TransformComponent.h>
- #include <ROS2/Communication/TopicConfiguration.h>
- #include <ROS2/Frame/ROS2FrameEditorComponent.h>
- #include <ROS2/Utilities/ROS2Names.h>
- #include <RobotImporter/Utils/RobotImporterUtils.h>
- #include <RobotImporter/Utils/TypeConversions.h>
- #include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
- #include <sdf/Element.hh>
- #include <sdf/Joint.hh>
- namespace ROS2::SDFormat
- {
- // temporarily disable import hooks for sensors and models for https://github.com/o3de/sig-simulation/pull/96
- // void HooksUtils::AddTopicConfiguration(
- // SensorConfiguration& sensorConfig, const AZStd::string& topic, const AZStd::string& messageType, const AZStd::string& configName)
- // {
- // TopicConfiguration config;
- // config.m_topic = topic;
- // config.m_type = messageType;
- // sensorConfig.m_publishersConfigurations.insert(AZStd::make_pair(configName, config));
- // }
- AZ::EntityId HooksUtils::GetJointEntityId(
- const std::string& jointName, const sdf::Model& sdfModel, const CreatedEntitiesMap& createdEntities)
- {
- const auto jointPtr = sdfModel.JointByName(jointName);
- if (jointPtr != nullptr)
- {
- const auto linkName(jointPtr->ChildName().c_str());
- const auto linkPtr = sdfModel.LinkByName(linkName);
- if (linkPtr != nullptr && createdEntities.contains(linkPtr))
- {
- const auto& entityResult = createdEntities.at(linkPtr);
- return entityResult.IsSuccess() ? entityResult.GetValue() : AZ::EntityId();
- }
- }
- return AZ::EntityId();
- }
- void HooksUtils::EnableMotor(const AZ::EntityId& entityId)
- {
- AZ::Entity* entity = nullptr;
- AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, entityId);
- if (entity == nullptr)
- {
- AZ_Warning("HooksUtils", false, "Cannot switch on motor in wheel joint. Entity was not created successfully.");
- return;
- }
- // Enable motor in hinge joint (only if articulations are not enabled)
- PhysX::EditorHingeJointComponent* jointComponent = entity->FindComponent<PhysX::EditorHingeJointComponent>();
- if (jointComponent != nullptr)
- {
- entity->Activate();
- if (entity->GetState() == AZ::Entity::State::Active)
- {
- PhysX::EditorJointRequestBus::Event(
- AZ::EntityComponentIdPair(entityId, jointComponent->GetId()),
- &PhysX::EditorJointRequests::SetBoolValue,
- PhysX::JointsComponentModeCommon::ParameterNames::EnableMotor,
- true);
- entity->Deactivate();
- }
- }
- }
- void HooksUtils::SetSensorEntityTransform(AZ::Entity& entity, const sdf::Sensor& sdfSensor)
- {
- const auto sensorSemanticPose = sdfSensor.SemanticPose();
- AZ::Transform tf = Utils::GetLocalTransformURDF(sensorSemanticPose);
- auto* transformInterface = entity.FindComponent<AzToolsFramework::Components::TransformComponent>();
- if (transformInterface)
- {
- AZ_Trace(
- "CreatePrefabFromUrdfOrSdf",
- "Setting transform %s to [%f %f %f] [%f %f %f %f]\n",
- sdfSensor.Name().c_str(),
- tf.GetTranslation().GetX(),
- tf.GetTranslation().GetY(),
- tf.GetTranslation().GetZ(),
- tf.GetRotation().GetX(),
- tf.GetRotation().GetY(),
- tf.GetRotation().GetZ(),
- tf.GetRotation().GetW());
- transformInterface->SetLocalTM(tf);
- }
- else
- {
- AZ_Trace(
- "CreatePrefabFromUrdfOrSdf", "Setting transform failed: %s does not have transform interface\n", sdfSensor.Name().c_str());
- }
- }
- namespace HooksUtils::PluginParser
- {
- // Inserts name (key) and value (val) of given parameter to map.
- void ParseRegularContent(const sdf::Element& content, HooksUtils::PluginParams& remappings)
- {
- const AZStd::string contentName = content.GetName().c_str();
- const sdf::ParamPtr contentValuePtr = content.GetValue();
- if (!contentValuePtr || contentName.empty())
- {
- AZ_Warning("PluginParser", false, "Encountered invalid (empty) remapping while parsing URDF/SDF plugin.");
- return;
- }
- const AZStd::string contentValue = contentValuePtr->GetAsString().c_str();
- if (!contentValue.empty())
- {
- remappings[contentName] = contentValue;
- }
- }
- // Parses parameters present in ros element, inserting them to the map.
- void ParseRos2Remapping(const sdf::Element& rosContent, HooksUtils::PluginParams& remappings)
- {
- if (rosContent.GetName() != "remapping" && rosContent.GetName() != "argument")
- {
- // parameter other than remapping or argument can be handled as regular parameter
- ParseRegularContent(rosContent, remappings);
- return;
- }
- const sdf::ParamPtr contentValuePtr = rosContent.GetValue();
- if (!contentValuePtr)
- {
- AZ_Warning("PluginParser", false, "ROS 2 content in parsing URDF/SDF plugin data not available.");
- return;
- }
- AZStd::string contentValue = contentValuePtr->GetAsString().c_str();
- if (contentValue.find_last_of('=') == AZStd::string::npos || contentValue.find_last_of(':') == AZStd::string::npos)
- {
- AZ_Warning("PluginParser", false, "Encountered invalid remapping while parsing URDF/SDF plugin.");
- return;
- }
- // get new name of the topic
- int startVal = contentValue.find_last_of('=');
- if (contentValue.find_last_of('/') != AZStd::string::npos && contentValue.find_last_of('/') > startVal)
- {
- startVal = contentValue.find_last_of("/");
- }
- startVal += 1;
- if (startVal >= contentValue.size())
- {
- AZ_Warning("PluginParser", false, "Encountered invalid (empty) remapping while parsing URDF/SDF plugin.");
- return;
- }
- AZStd::string newTopic = contentValue.substr(startVal, contentValue.size() - startVal);
- // get previous name of the topic
- contentValue = contentValue.substr(0, contentValue.find_first_of(':'));
- int startKey = contentValue.find_last_of('/') != std::string::npos ? contentValue.find_last_of('/') + 1 : 0;
- if (startKey >= contentValue.size())
- {
- AZ_Warning("PluginParser", false, "Encountered invalid (empty) remapping while parsing URDF/SDF plugin.");
- return;
- }
- AZStd::string prevTopic = contentValue.substr(startKey, contentValue.size() - startKey);
- if (ROS2Names::ValidateTopic(newTopic).IsSuccess() && ROS2Names::ValidateTopic(prevTopic).IsSuccess())
- {
- remappings[prevTopic] = newTopic;
- }
- else
- {
- AZ_Warning("PluginParser", false, "Encountered invalid topic name while parsing URDF/SDF plugin.");
- }
- }
- } // namespace HooksUtils::PluginParser
- ROS2FrameConfiguration HooksUtils::GetFrameConfiguration(const HooksUtils::PluginParams& pluginParams)
- {
- ROS2FrameConfiguration frameConfiguration;
- const static AZStd::vector<AZStd::string> namespaceRemapNames = { "robotNamespace", "namespace" };
- const AZStd::string remappedNamespace = HooksUtils::ValueOfAny(pluginParams, namespaceRemapNames);
- if (!ROS2Names::ValidateNamespace(remappedNamespace).IsSuccess())
- {
- AZ_Warning("PluginParser", false, "Encountered invalid namespace name while parsing URDF/SDF plugin.");
- }
- else if (!remappedNamespace.empty())
- {
- frameConfiguration.m_namespaceConfiguration.SetNamespace(remappedNamespace, NamespaceConfiguration::NamespaceStrategy::Custom);
- }
- if (pluginParams.contains("frameName"))
- {
- frameConfiguration.m_frameName = pluginParams.at("frameName");
- }
- else if (pluginParams.contains("frame_name"))
- {
- frameConfiguration.m_frameName = pluginParams.at("frame_name");
- }
- return frameConfiguration;
- }
- HooksUtils::PluginParams HooksUtils::GetPluginParams(const sdf::Plugins& plugins)
- {
- HooksUtils::PluginParams remappings;
- if (plugins.empty())
- {
- return remappings;
- }
- const auto plugin = plugins[0];
- for (const auto& content : plugin.Contents())
- {
- std::string contentName = content->GetName();
- if (contentName == "ros")
- {
- // when ros tag is present, parse it's elements and insert them into the map
- auto rosContent = content->GetFirstElement();
- while (rosContent != nullptr)
- {
- PluginParser::ParseRos2Remapping(*rosContent, remappings);
- rosContent = rosContent->GetNextElement();
- }
- }
- else
- {
- PluginParser::ParseRegularContent(*content, remappings);
- }
- }
- return remappings;
- }
- AZStd::string HooksUtils::ValueOfAny(
- const HooksUtils::PluginParams& pluginParams, const AZStd::vector<AZStd::string>& paramNames, const AZStd::string& defaultVal)
- {
- for (const auto& paramName : paramNames)
- {
- if (pluginParams.contains(paramName))
- {
- const AZStd::string paramValue = pluginParams.at(paramName);
- return AZ::IO::PathView(paramValue).Filename().String();
- }
- }
- return defaultVal;
- }
- AZStd::string HooksUtils::GetTopicName(const PluginParams& pluginParams, sdf::ElementPtr element, const AZStd::string& defaultVal)
- {
- if (element->HasElement("topic"))
- {
- return element->Get<std::string>("topic").c_str();
- }
- const static AZStd::vector<AZStd::string> remapParamNames = { "topicName", "out" };
- return ValueOfAny(pluginParams, remapParamNames, defaultVal);
- }
- float HooksUtils::GetFrequency(const PluginParams& pluginParams, const float defaultVal)
- {
- const static AZStd::vector<AZStd::string> frequencyParamNames = { "updateRate", "update_rate" };
- return AZStd::stof(ValueOfAny(pluginParams, frequencyParamNames, AZStd::to_string(defaultVal)));
- }
- } // namespace ROS2::SDFormat
|