diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2SkidSteeringModelHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2SkidSteeringModelHook.cpp index 967ba926e3..bce2ff31d0 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2SkidSteeringModelHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2SkidSteeringModelHook.cpp @@ -23,7 +23,7 @@ namespace ROS2::SDFormat { namespace Parser { - VehicleDynamics::VehicleConfiguration GetConfiguration( + VehicleDynamics::VehicleConfiguration CreateVehicleConfiguration( const sdf::ElementPtr element, const sdf::Model& sdfModel, const CreatedEntitiesMap& createdEntities) { VehicleDynamics::VehicleConfiguration configuration; @@ -47,7 +47,7 @@ namespace ROS2::SDFormat else { AZ_Warning( - "ROS2SkidSteeringModelPluginHook", + "CreateVehicleConfiguration", false, "Cannot find entity ID for one of the joints: %s or %s", jointNameLeft.c_str(), @@ -113,7 +113,7 @@ namespace ROS2::SDFormat { constexpr float epsilon = 0.001f; AZ_Warning( - "ROS2SkidSteeringModelPluginHook", + "CreateVehicleConfiguration", fabsf(configuration.m_wheelbase - wheelSeparation->Get()) < epsilon, "Different wheel separation distances in one model are not supported."); } @@ -130,7 +130,7 @@ namespace ROS2::SDFormat wheelDiameter = wheelDiameter->GetNextElement("wheel_diameter"); } AZ_Warning( - "ROS2SkidSteeringModelPluginHook", + "CreateVehicleConfiguration", wheelPairs == configuration.m_axles.size(), "VehicleConfiguration parsing might be incorrect: expected %d axles, found %d.", wheelPairs, @@ -138,14 +138,12 @@ namespace ROS2::SDFormat } AZ_Warning( - "ROS2SkidSteeringModelPluginHook", - !configuration.m_axles.empty(), - "VehicleConfiguration parsing error: cannot find any axles."); + "CreateVehicleConfiguration", !configuration.m_axles.empty(), "VehicleConfiguration parsing error: cannot find any axles."); return configuration; } - VehicleDynamics::SkidSteeringModelLimits GetModelLimits(const sdf::ElementPtr element) + VehicleDynamics::SkidSteeringModelLimits CreateModelLimits(const sdf::ElementPtr element) { VehicleDynamics::SkidSteeringModelLimits modelLimits; if (element->HasElement("wheelAcceleration")) @@ -158,7 +156,7 @@ namespace ROS2::SDFormat } else { - AZ_Warning("ROS2SkidSteeringModelPluginHook", false, "VehicleConfiguration parsing error: cannot determine model limits."); + AZ_Warning("CreateModelLimits", false, "VehicleConfiguration parsing error: cannot determine model limits."); } return modelLimits; @@ -177,8 +175,9 @@ namespace ROS2::SDFormat { // Parse parameters const sdf::ElementPtr element = sdfPlugin.Element(); - VehicleDynamics::VehicleConfiguration vehicleConfiguration = Parser::GetConfiguration(element, sdfModel, createdEntities); - VehicleDynamics::SkidSteeringModelLimits modelLimits = Parser::GetModelLimits(element); + VehicleDynamics::VehicleConfiguration vehicleConfiguration = + Parser::CreateVehicleConfiguration(element, sdfModel, createdEntities); + VehicleDynamics::SkidSteeringModelLimits modelLimits = Parser::CreateModelLimits(element); // Create required components HooksUtils::CreateComponent(entity); diff --git a/Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.cpp b/Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.cpp index bb0279db69..5af4cc1099 100644 --- a/Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.cpp +++ b/Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.cpp @@ -61,15 +61,6 @@ namespace ROS2::VehicleDynamics } } - SkidSteeringModelLimits::SkidSteeringModelLimits( - const float linearLimit, const float angularLimit, const float linearAcceleration, const float angularAcceleration) - { - m_linearLimit = AZStd::clamp(linearLimit, 0.0f, 100.0f); - m_angularLimit = AZStd::clamp(angularLimit, 0.0f, 10.0f); - m_linearAcceleration = AZStd::clamp(linearAcceleration, 0.0f, 100.0f); - m_angularAcceleration = AZStd::clamp(angularAcceleration, 0.0f, 100.0f); - } - VehicleInputs SkidSteeringModelLimits::LimitState(const VehicleInputs& inputState) const { VehicleInputs ret = inputState; diff --git a/Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.h b/Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.h index 04074f0c01..fe9bc59399 100644 --- a/Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.h +++ b/Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.h @@ -21,8 +21,6 @@ namespace ROS2::VehicleDynamics public: AZ_RTTI(SkidSteeringModelLimits, "{23420EFB-BB62-48C7-AD37-E50580A53C39}", VehicleModelLimits); SkidSteeringModelLimits() = default; - SkidSteeringModelLimits( - const float linearLimit, const float angularLimit, const float linearAcceleration, const float angularAcceleration); static void Reflect(AZ::ReflectContext* context);