ROS2SDFormatHooksUtils.cpp 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277
  1. /*
  2. * Copyright (c) Contributors to the Open 3D Engine Project.
  3. * For complete copyright and license terms please see the LICENSE at the root of this distribution.
  4. *
  5. * SPDX-License-Identifier: Apache-2.0 OR MIT
  6. *
  7. */
  8. #include "ROS2SDFormatHooksUtils.h"
  9. #include <AzToolsFramework/ToolsComponents/TransformComponent.h>
  10. #include <ROS2/Communication/TopicConfiguration.h>
  11. #include <ROS2/Frame/ROS2FrameEditorComponent.h>
  12. #include <ROS2/Utilities/ROS2Names.h>
  13. #include <RobotImporter/Utils/RobotImporterUtils.h>
  14. #include <RobotImporter/Utils/TypeConversions.h>
  15. #include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
  16. #include <sdf/Element.hh>
  17. #include <sdf/Joint.hh>
  18. namespace ROS2::SDFormat
  19. {
  20. // temporarily disable import hooks for sensors and models for https://github.com/o3de/sig-simulation/pull/96
  21. // void HooksUtils::AddTopicConfiguration(
  22. // SensorConfiguration& sensorConfig, const AZStd::string& topic, const AZStd::string& messageType, const AZStd::string& configName)
  23. // {
  24. // TopicConfiguration config;
  25. // config.m_topic = topic;
  26. // config.m_type = messageType;
  27. // sensorConfig.m_publishersConfigurations.insert(AZStd::make_pair(configName, config));
  28. // }
  29. AZ::EntityId HooksUtils::GetJointEntityId(
  30. const std::string& jointName, const sdf::Model& sdfModel, const CreatedEntitiesMap& createdEntities)
  31. {
  32. const auto jointPtr = sdfModel.JointByName(jointName);
  33. if (jointPtr != nullptr)
  34. {
  35. const auto linkName(jointPtr->ChildName().c_str());
  36. const auto linkPtr = sdfModel.LinkByName(linkName);
  37. if (linkPtr != nullptr && createdEntities.contains(linkPtr))
  38. {
  39. const auto& entityResult = createdEntities.at(linkPtr);
  40. return entityResult.IsSuccess() ? entityResult.GetValue() : AZ::EntityId();
  41. }
  42. }
  43. return AZ::EntityId();
  44. }
  45. void HooksUtils::EnableMotor(const AZ::EntityId& entityId)
  46. {
  47. AZ::Entity* entity = nullptr;
  48. AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, entityId);
  49. if (entity == nullptr)
  50. {
  51. AZ_Warning("HooksUtils", false, "Cannot switch on motor in wheel joint. Entity was not created successfully.");
  52. return;
  53. }
  54. // Enable motor in hinge joint (only if articulations are not enabled)
  55. PhysX::EditorHingeJointComponent* jointComponent = entity->FindComponent<PhysX::EditorHingeJointComponent>();
  56. if (jointComponent != nullptr)
  57. {
  58. entity->Activate();
  59. if (entity->GetState() == AZ::Entity::State::Active)
  60. {
  61. PhysX::EditorJointRequestBus::Event(
  62. AZ::EntityComponentIdPair(entityId, jointComponent->GetId()),
  63. &PhysX::EditorJointRequests::SetBoolValue,
  64. PhysX::JointsComponentModeCommon::ParameterNames::EnableMotor,
  65. true);
  66. entity->Deactivate();
  67. }
  68. }
  69. }
  70. void HooksUtils::SetSensorEntityTransform(AZ::Entity& entity, const sdf::Sensor& sdfSensor)
  71. {
  72. const auto sensorSemanticPose = sdfSensor.SemanticPose();
  73. AZ::Transform tf = Utils::GetLocalTransformURDF(sensorSemanticPose);
  74. auto* transformInterface = entity.FindComponent<AzToolsFramework::Components::TransformComponent>();
  75. if (transformInterface)
  76. {
  77. AZ_Trace(
  78. "CreatePrefabFromUrdfOrSdf",
  79. "Setting transform %s to [%f %f %f] [%f %f %f %f]\n",
  80. sdfSensor.Name().c_str(),
  81. tf.GetTranslation().GetX(),
  82. tf.GetTranslation().GetY(),
  83. tf.GetTranslation().GetZ(),
  84. tf.GetRotation().GetX(),
  85. tf.GetRotation().GetY(),
  86. tf.GetRotation().GetZ(),
  87. tf.GetRotation().GetW());
  88. transformInterface->SetLocalTM(tf);
  89. }
  90. else
  91. {
  92. AZ_Trace(
  93. "CreatePrefabFromUrdfOrSdf", "Setting transform failed: %s does not have transform interface\n", sdfSensor.Name().c_str());
  94. }
  95. }
  96. namespace HooksUtils::PluginParser
  97. {
  98. // Inserts name (key) and value (val) of given parameter to map.
  99. void ParseRegularContent(const sdf::Element& content, HooksUtils::PluginParams& remappings)
  100. {
  101. const AZStd::string contentName = content.GetName().c_str();
  102. const sdf::ParamPtr contentValuePtr = content.GetValue();
  103. if (!contentValuePtr || contentName.empty())
  104. {
  105. AZ_Warning("PluginParser", false, "Encountered invalid (empty) remapping while parsing URDF/SDF plugin.");
  106. return;
  107. }
  108. const AZStd::string contentValue = contentValuePtr->GetAsString().c_str();
  109. if (!contentValue.empty())
  110. {
  111. remappings[contentName] = contentValue;
  112. }
  113. }
  114. // Parses parameters present in ros element, inserting them to the map.
  115. void ParseRos2Remapping(const sdf::Element& rosContent, HooksUtils::PluginParams& remappings)
  116. {
  117. if (rosContent.GetName() != "remapping" && rosContent.GetName() != "argument")
  118. {
  119. // parameter other than remapping or argument can be handled as regular parameter
  120. ParseRegularContent(rosContent, remappings);
  121. return;
  122. }
  123. const sdf::ParamPtr contentValuePtr = rosContent.GetValue();
  124. if (!contentValuePtr)
  125. {
  126. AZ_Warning("PluginParser", false, "ROS 2 content in parsing URDF/SDF plugin data not available.");
  127. return;
  128. }
  129. AZStd::string contentValue = contentValuePtr->GetAsString().c_str();
  130. if (contentValue.find_last_of('=') == AZStd::string::npos || contentValue.find_last_of(':') == AZStd::string::npos)
  131. {
  132. AZ_Warning("PluginParser", false, "Encountered invalid remapping while parsing URDF/SDF plugin.");
  133. return;
  134. }
  135. // get new name of the topic
  136. int startVal = contentValue.find_last_of('=');
  137. if (contentValue.find_last_of('/') != AZStd::string::npos && contentValue.find_last_of('/') > startVal)
  138. {
  139. startVal = contentValue.find_last_of("/");
  140. }
  141. startVal += 1;
  142. if (startVal >= contentValue.size())
  143. {
  144. AZ_Warning("PluginParser", false, "Encountered invalid (empty) remapping while parsing URDF/SDF plugin.");
  145. return;
  146. }
  147. AZStd::string newTopic = contentValue.substr(startVal, contentValue.size() - startVal);
  148. // get previous name of the topic
  149. contentValue = contentValue.substr(0, contentValue.find_first_of(':'));
  150. int startKey = contentValue.find_last_of('/') != std::string::npos ? contentValue.find_last_of('/') + 1 : 0;
  151. if (startKey >= contentValue.size())
  152. {
  153. AZ_Warning("PluginParser", false, "Encountered invalid (empty) remapping while parsing URDF/SDF plugin.");
  154. return;
  155. }
  156. AZStd::string prevTopic = contentValue.substr(startKey, contentValue.size() - startKey);
  157. if (ROS2Names::ValidateTopic(newTopic).IsSuccess() && ROS2Names::ValidateTopic(prevTopic).IsSuccess())
  158. {
  159. remappings[prevTopic] = newTopic;
  160. }
  161. else
  162. {
  163. AZ_Warning("PluginParser", false, "Encountered invalid topic name while parsing URDF/SDF plugin.");
  164. }
  165. }
  166. } // namespace HooksUtils::PluginParser
  167. ROS2FrameConfiguration HooksUtils::GetFrameConfiguration(const HooksUtils::PluginParams& pluginParams)
  168. {
  169. ROS2FrameConfiguration frameConfiguration;
  170. const static AZStd::vector<AZStd::string> namespaceRemapNames = { "robotNamespace", "namespace" };
  171. const AZStd::string remappedNamespace = HooksUtils::ValueOfAny(pluginParams, namespaceRemapNames);
  172. if (!ROS2Names::ValidateNamespace(remappedNamespace).IsSuccess())
  173. {
  174. AZ_Warning("PluginParser", false, "Encountered invalid namespace name while parsing URDF/SDF plugin.");
  175. }
  176. else if (!remappedNamespace.empty())
  177. {
  178. frameConfiguration.m_namespaceConfiguration.SetNamespace(remappedNamespace, NamespaceConfiguration::NamespaceStrategy::Custom);
  179. }
  180. if (pluginParams.contains("frameName"))
  181. {
  182. frameConfiguration.m_frameName = pluginParams.at("frameName");
  183. }
  184. else if (pluginParams.contains("frame_name"))
  185. {
  186. frameConfiguration.m_frameName = pluginParams.at("frame_name");
  187. }
  188. return frameConfiguration;
  189. }
  190. HooksUtils::PluginParams HooksUtils::GetPluginParams(const sdf::Plugins& plugins)
  191. {
  192. HooksUtils::PluginParams remappings;
  193. if (plugins.empty())
  194. {
  195. return remappings;
  196. }
  197. const auto plugin = plugins[0];
  198. for (const auto& content : plugin.Contents())
  199. {
  200. std::string contentName = content->GetName();
  201. if (contentName == "ros")
  202. {
  203. // when ros tag is present, parse it's elements and insert them into the map
  204. auto rosContent = content->GetFirstElement();
  205. while (rosContent != nullptr)
  206. {
  207. PluginParser::ParseRos2Remapping(*rosContent, remappings);
  208. rosContent = rosContent->GetNextElement();
  209. }
  210. }
  211. else
  212. {
  213. PluginParser::ParseRegularContent(*content, remappings);
  214. }
  215. }
  216. return remappings;
  217. }
  218. AZStd::string HooksUtils::ValueOfAny(
  219. const HooksUtils::PluginParams& pluginParams, const AZStd::vector<AZStd::string>& paramNames, const AZStd::string& defaultVal)
  220. {
  221. for (const auto& paramName : paramNames)
  222. {
  223. if (pluginParams.contains(paramName))
  224. {
  225. const AZStd::string paramValue = pluginParams.at(paramName);
  226. return AZ::IO::PathView(paramValue).Filename().String();
  227. }
  228. }
  229. return defaultVal;
  230. }
  231. AZStd::string HooksUtils::GetTopicName(const PluginParams& pluginParams, sdf::ElementPtr element, const AZStd::string& defaultVal)
  232. {
  233. if (element->HasElement("topic"))
  234. {
  235. return element->Get<std::string>("topic").c_str();
  236. }
  237. const static AZStd::vector<AZStd::string> remapParamNames = { "topicName", "out" };
  238. return ValueOfAny(pluginParams, remapParamNames, defaultVal);
  239. }
  240. float HooksUtils::GetFrequency(const PluginParams& pluginParams, const float defaultVal)
  241. {
  242. const static AZStd::vector<AZStd::string> frequencyParamNames = { "updateRate", "update_rate" };
  243. return AZStd::stof(ValueOfAny(pluginParams, frequencyParamNames, AZStd::to_string(defaultVal)));
  244. }
  245. } // namespace ROS2::SDFormat