From 972433bb24d2ca2b09beb568e21af452abbbfd43 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Wed, 29 Nov 2023 15:39:34 +0100 Subject: [PATCH] refactor hook, revert changes in articulation link creation Signed-off-by: Jan Hanca --- .../Hooks/ROS2SkidSteeringModelHook.cpp | 58 +++++++++++-------- .../RobotImporter/URDF/ArticulationsMaker.cpp | 10 ++-- 2 files changed, 39 insertions(+), 29 deletions(-) diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2SkidSteeringModelHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2SkidSteeringModelHook.cpp index 123cc785db..fa905e144a 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2SkidSteeringModelHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2SkidSteeringModelHook.cpp @@ -11,8 +11,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -58,6 +60,19 @@ namespace ROS2::SDFormat jointComponent->Deactivate(); return; } + + PhysX::EditorArticulationLinkComponent* articulationComponent = + entity->FindComponent(); + if (articulationComponent != nullptr) + { + const auto wheelComponentPtr = HooksUtils::CreateComponent(*entity); + AZ_Warning( + "ROS2SkidSteeringModelPluginHook", + wheelComponentPtr != nullptr, + "Cannot create WheelControllerComponent in articulation link."); + + return; + } } AZ_Warning("ROS2SkidSteeringModelPluginHook", false, "Cannot switch on motor in wheel joint. Joint does not exist."); @@ -67,26 +82,22 @@ namespace ROS2::SDFormat const sdf::ElementPtr element, const sdf::Model& sdfModel, const CreatedEntitiesMap& createdEntities) { VehicleDynamics::VehicleConfiguration configuration; - auto configureAxle = [&sdfModel, &createdEntities]( - const std::string& jointNameLeft, - const std::string& jointNameRight, - const float wheelDiameter, - AZStd::string tag) -> VehicleDynamics::AxleConfiguration + auto addAxle = [&sdfModel, &createdEntities, &configuration]( + const std::string& jointNameLeft, + const std::string& jointNameRight, + const float wheelDiameter, + AZStd::string tag) -> void { - VehicleDynamics::AxleConfiguration axle; - axle.m_axleTag = AZStd::move(tag); - axle.m_isSteering = false; // Skid steering model does not have any steering wheels. - axle.m_isDrive = true; - axle.m_wheelRadius = wheelDiameter / 2.0f; - const auto entityIdLeft = GetAxleWheelId(jointNameLeft, sdfModel, createdEntities); const auto entityIdRight = GetAxleWheelId(jointNameRight, sdfModel, createdEntities); if (entityIdLeft.IsValid() && entityIdRight.IsValid()) { - axle.m_axleWheels.emplace_back(AZStd::move(entityIdLeft)); - axle.m_axleWheels.emplace_back(AZStd::move(entityIdRight)); EnableMotor(entityIdLeft); EnableMotor(entityIdRight); + constexpr bool steering = false; // Skid steering model does not have any steering wheels. + constexpr bool drive = true; + configuration.m_axles.emplace_back(ROS2::VehicleDynamics::Utilities::Create2WheelAxle( + entityIdLeft, entityIdRight, AZStd::move(tag), wheelDiameter / 2.0f, steering, drive)); } else { @@ -97,8 +108,6 @@ namespace ROS2::SDFormat jointNameLeft.c_str(), jointNameRight.c_str()); } - - return axle; }; if (element->HasElement("wheelSeparation") && element->HasElement("wheelDiameter")) @@ -109,30 +118,30 @@ namespace ROS2::SDFormat { // ROS 1 version libgazebo_ros_skid_steer_drive.so if (element->HasElement("leftJoint") && element->HasElement("rightJoint")) { - configuration.m_axles.emplace_back(configureAxle( + addAxle( element->Get("leftJoint"), element->Get("rightJoint"), element->Get("wheelDiameter"), - "")); + ""); } } { // ROS 1 version libgazebo_ros_diff_drive.so if (element->HasElement("leftFrontJoint") && element->HasElement("rightFrontJoint")) { - configuration.m_axles.emplace_back(configureAxle( + addAxle( element->Get("leftFrontJoint"), element->Get("rightFrontJoint"), element->Get("wheelDiameter"), - "Front")); + "Front"); } if (element->HasElement("leftRearJoint") && element->HasElement("rightRearJoint")) { - configuration.m_axles.emplace_back(configureAxle( + addAxle( element->Get("leftRearJoint"), element->Get("rightRearJoint"), element->Get("wheelDiameter"), - "Rear")); + "Rear"); } } } @@ -157,17 +166,18 @@ namespace ROS2::SDFormat } else { + constexpr float epsilon = 0.001f; AZ_Warning( "ROS2SkidSteeringModelPluginHook", - fabsf(configuration.m_wheelbase - wheelSeparation->Get()) < 0.001f, + fabsf(configuration.m_wheelbase - wheelSeparation->Get()) < epsilon, "Different wheel separation distances in one model are not supported."); } - configuration.m_axles.emplace_back(configureAxle( + addAxle( jointLeft->Get(), jointRight->Get(), wheelDiameter->Get(), - AZStd::to_string(dataCount))); + AZStd::to_string(dataCount)); jointLeft = jointLeft->GetNextElement("left_joint"); jointRight = jointRight->GetNextElement("right_joint"); diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp index 42704e1f09..2c88b4fe8a 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp @@ -28,7 +28,8 @@ namespace ROS2 } }; } // namespace - ArticulationCfg& AddToArticulationConfig(ArticulationCfg& articulationLinkConfiguration, const sdf::Joint* joint) + ArticulationCfg& AddToArticulationConfig( + ArticulationCfg& articulationLinkConfiguration, const sdf::Joint* joint, const bool isWheelEntity) { if (!joint) { @@ -84,9 +85,7 @@ namespace ROS2 articulationLinkConfiguration.m_linearLimitUpper = jointAxis->Lower(); } } - articulationLinkConfiguration.m_motorConfiguration.m_driveDamping = jointAxis->Damping(); - articulationLinkConfiguration.m_motorConfiguration.m_driveForceLimit = jointAxis->Effort(); - articulationLinkConfiguration.m_motorConfiguration.m_driveStiffness = jointAxis->Stiffness(); + articulationLinkConfiguration.m_motorConfiguration.m_useMotor = isWheelEntity; } else { @@ -128,7 +127,8 @@ namespace ROS2 AZStd::string linkName(link->Name().c_str(), link->Name().size()); for (const sdf::Joint* joint : Utils::GetJointsForChildLink(model, linkName, getNestedModelJoints)) { - articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, joint); + const bool isWheelEntity = Utils::IsWheelURDFHeuristics(model, link); + articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, joint, isWheelEntity); } entity->CreateComponent(articulationLinkConfiguration);