Skip to content

Commit

Permalink
refactor hook, revert changes in articulation link creation
Browse files Browse the repository at this point in the history 
Signed-off-by: Jan Hanca <[email protected]>
  • Loading branch information
@jhanca-robotecai
jhanca-robotecai committed Nov 29, 2023
1 parent a76378f commit 972433b
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 29 deletions.
58 changes: 34 additions & 24 deletions Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2SkidSteeringModelHook.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,10 @@
#include <RobotControl/ROS2RobotControlComponent.h>
#include <RobotImporter/SDFormat/ROS2ModelPluginHooks.h>
#include <RobotImporter/SDFormat/ROS2SDFormatHooksUtils.h>
#include <Source/EditorArticulationLinkComponent.h>
#include <Source/EditorHingeJointComponent.h>
#include <VehicleDynamics/ModelComponents/SkidSteeringModelComponent.h>
#include <VehicleDynamics/Utilities.h>

#include <sdf/Joint.hh>
#include <sdf/Link.hh>
Expand Down Expand Up @@ -58,6 +60,19 @@ namespace ROS2::SDFormat
jointComponent->Deactivate();
return;
}

PhysX::EditorArticulationLinkComponent* articulationComponent =
entity->FindComponent<PhysX::EditorArticulationLinkComponent>();
if (articulationComponent != nullptr)
{
const auto wheelComponentPtr = HooksUtils::CreateComponent<VehicleDynamics::WheelControllerComponent>(*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.");
Expand All @@ -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
{
Expand All @@ -97,8 +108,6 @@ namespace ROS2::SDFormat
jointNameLeft.c_str(),
jointNameRight.c_str());
}

return axle;
};

if (element->HasElement("wheelSeparation") && element->HasElement("wheelDiameter"))
Expand All @@ -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<std::string>("leftJoint"),
element->Get<std::string>("rightJoint"),
element->Get<float>("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<std::string>("leftFrontJoint"),
element->Get<std::string>("rightFrontJoint"),
element->Get<float>("wheelDiameter"),
"Front"));
"Front");
}
if (element->HasElement("leftRearJoint") && element->HasElement("rightRearJoint"))
{
configuration.m_axles.emplace_back(configureAxle(
addAxle(
element->Get<std::string>("leftRearJoint"),
element->Get<std::string>("rightRearJoint"),
element->Get<float>("wheelDiameter"),
"Rear"));
"Rear");
}
}
}
Expand All @@ -157,17 +166,18 @@ namespace ROS2::SDFormat
}
else
{
constexpr float epsilon = 0.001f;
AZ_Warning(
"ROS2SkidSteeringModelPluginHook",
fabsf(configuration.m_wheelbase - wheelSeparation->Get<float>()) < 0.001f,
fabsf(configuration.m_wheelbase - wheelSeparation->Get<float>()) < epsilon,
"Different wheel separation distances in one model are not supported.");
}

configuration.m_axles.emplace_back(configureAxle(
addAxle(
jointLeft->Get<std::string>(),
jointRight->Get<std::string>(),
wheelDiameter->Get<float>(),
AZStd::to_string(dataCount)));
AZStd::to_string(dataCount));

jointLeft = jointLeft->GetNextElement("left_joint");
jointRight = jointRight->GetNextElement("right_joint");
Expand Down
10 changes: 5 additions & 5 deletions Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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<PhysX::EditorArticulationLinkComponent>(articulationLinkConfiguration);
Expand Down

0 comments on commit 972433b

Please sign in to comment.