Skip to content

Commit

Permalink
Use SetComponentData to simplify code and improve coverage (#2360)
Browse files Browse the repository at this point in the history 
Signed-off-by: Steve Peters <[email protected]>
  • Loading branch information
@scpeters
scpeters authored Apr 5, 2024
1 parent 6884e66 commit f8add56
Show file tree
Hide file tree
Showing 8 changed files with 40 additions and 219 deletions.
57 changes: 8 additions & 49 deletions src/systems/ackermann_steering/AckermannSteering.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -608,70 +608,29 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info,
for (Entity joint : this->dataPtr->leftJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(
joint, components::JointVelocityCmd(
{this->dataPtr->leftJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(
joint, {this->dataPtr->leftJointSpeed});
}

for (Entity joint : this->dataPtr->rightJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->rightJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(
joint, {this->dataPtr->rightJointSpeed});
}
}

// Update steering
for (Entity joint : this->dataPtr->leftSteeringJoints)
{
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(
joint, components::JointVelocityCmd(
{this->dataPtr->leftSteeringJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd(
{this->dataPtr->leftSteeringJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(
joint, {this->dataPtr->leftSteeringJointSpeed});
}

for (Entity joint : this->dataPtr->rightSteeringJoints)
{
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd(
{this->dataPtr->rightSteeringJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd(
{this->dataPtr->rightSteeringJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(
joint, {this->dataPtr->rightSteeringJointSpeed});
}
if (!this->dataPtr->steeringOnly)
{
Expand Down
26 changes: 4 additions & 22 deletions src/systems/diff_drive/DiffDrive.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -459,17 +459,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info,
continue;

// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(
joint, components::JointVelocityCmd({this->dataPtr->leftJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->leftJointSpeed});
}

for (Entity joint : this->dataPtr->rightJoints)
Expand All @@ -479,17 +470,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info,
continue;

// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->rightJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->rightJointSpeed});
}

// Create the left and right side joint position components if they
Expand Down
12 changes: 1 addition & 11 deletions src/systems/joint_controller/JointController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -344,17 +344,7 @@ void JointController::PreUpdate(const UpdateInfo &_info,
// Update joint velocity
for (Entity joint : this->dataPtr->jointEntities)
{
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(
joint, components::JointVelocityCmd({targetVel}));
}
else
{
*vel = components::JointVelocityCmd({targetVel});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint, {targetVel});
}
}
}
Expand Down
12 changes: 1 addition & 11 deletions src/systems/joint_position_controller/JointPositionController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -461,17 +461,7 @@ void JointPositionController::PreUpdate(
for (Entity joint : this->dataPtr->jointEntities)
{
// Update velocity command.
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(
joint, components::JointVelocityCmd({targetVel}));
}
else
{
*vel = components::JointVelocityCmd({targetVel});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint, {targetVel});
}
return;
}
Expand Down
53 changes: 8 additions & 45 deletions src/systems/mecanum_drive/MecanumDrive.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -426,66 +426,29 @@ void MecanumDrive::PreUpdate(const gz::sim::UpdateInfo &_info,
for (Entity joint : this->dataPtr->frontLeftJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->frontLeftJointSpeed});
}

for (Entity joint : this->dataPtr->frontRightJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed}));
}
else
{
*vel =
components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->frontRightJointSpeed});
}

for (Entity joint : this->dataPtr->backLeftJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->backLeftJointSpeed});
}

for (Entity joint : this->dataPtr->backRightJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->backRightJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->backRightJointSpeed});
}
}

Expand Down
14 changes: 2 additions & 12 deletions src/systems/thruster/Thruster.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -740,18 +740,8 @@ void Thruster::PreUpdate(
// Velocity control
else
{
auto velocityComp =
_ecm.Component<gz::sim::components::JointVelocityCmd>(
this->dataPtr->jointEntity);
if (velocityComp == nullptr)
{
_ecm.CreateComponent(this->dataPtr->jointEntity,
components::JointVelocityCmd({desiredPropellerAngVel}));
}
else
{
velocityComp->Data()[0] = desiredPropellerAngVel;
}
_ecm.SetComponentData<gz::sim::components::JointVelocityCmd>(
this->dataPtr->jointEntity, {desiredPropellerAngVel});
angvel.set_data(desiredPropellerAngVel);
}

Expand Down
60 changes: 8 additions & 52 deletions src/systems/velocity_control/VelocityControl.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -197,38 +197,12 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info,
return;

// update angular velocity of model
auto modelAngularVel =
_ecm.Component<components::AngularVelocityCmd>(
this->dataPtr->model.Entity());

if (modelAngularVel == nullptr)
{
_ecm.CreateComponent(
this->dataPtr->model.Entity(),
components::AngularVelocityCmd({this->dataPtr->angularVelocity}));
}
else
{
*modelAngularVel =
components::AngularVelocityCmd({this->dataPtr->angularVelocity});
}
_ecm.SetComponentData<components::AngularVelocityCmd>(
this->dataPtr->model.Entity(), {this->dataPtr->angularVelocity});

// update linear velocity of model
auto modelLinearVel =
_ecm.Component<components::LinearVelocityCmd>(
this->dataPtr->model.Entity());

if (modelLinearVel == nullptr)
{
_ecm.CreateComponent(
this->dataPtr->model.Entity(),
components::LinearVelocityCmd({this->dataPtr->linearVelocity}));
}
else
{
*modelLinearVel =
components::LinearVelocityCmd({this->dataPtr->linearVelocity});
}
_ecm.SetComponentData<components::LinearVelocityCmd>(
this->dataPtr->model.Entity(), {this->dataPtr->linearVelocity});

// If there are links, create link components
// If the link hasn't been identified yet, look for it
Expand Down Expand Up @@ -262,17 +236,8 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info,
auto it = this->dataPtr->links.find(linkName);
if (it != this->dataPtr->links.end())
{
auto linkAngularVelComp =
_ecm.Component<components::AngularVelocityCmd>(it->second);
if (!linkAngularVelComp)
{
_ecm.CreateComponent(it->second,
components::AngularVelocityCmd({angularVel}));
}
else
{
*linkAngularVelComp = components::AngularVelocityCmd(angularVel);
}
_ecm.SetComponentData<components::AngularVelocityCmd>(
it->second, {angularVel});
}
else
{
Expand All @@ -286,17 +251,8 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info,
auto it = this->dataPtr->links.find(linkName);
if (it != this->dataPtr->links.end())
{
auto linkLinearVelComp =
_ecm.Component<components::LinearVelocityCmd>(it->second);
if (!linkLinearVelComp)
{
_ecm.CreateComponent(it->second,
components::LinearVelocityCmd({linearVel}));
}
else
{
*linkLinearVelComp = components::LinearVelocityCmd(linearVel);
}
_ecm.SetComponentData<components::LinearVelocityCmd>(
it->second, {linearVel});
}
else
{
Expand Down
25 changes: 8 additions & 17 deletions test/integration/wheel_slip.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -526,23 +526,14 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TricyclesUphill))
testSlipSystem.OnPreUpdate([&](const UpdateInfo &,
EntityComponentManager &)
{
auto wheelRearLeftVelocity0Cmd =
ecm->Component<components::JointVelocityCmd>(wheelRearLeftSpin0Entity);
auto wheelRearRightVelocity0Cmd =
ecm->Component<components::JointVelocityCmd>(wheelRearRightSpin0Entity);
auto wheelRearLeftVelocity1Cmd =
ecm->Component<components::JointVelocityCmd>(wheelRearLeftSpin1Entity);
auto wheelRearRightVelocity1Cmd =
ecm->Component<components::JointVelocityCmd>(wheelRearRightSpin1Entity);

*wheelRearLeftVelocity0Cmd =
components::JointVelocityCmd({angularSpeed});
*wheelRearRightVelocity0Cmd =
components::JointVelocityCmd({angularSpeed});
*wheelRearLeftVelocity1Cmd =
components::JointVelocityCmd({angularSpeed});
*wheelRearRightVelocity1Cmd =
components::JointVelocityCmd({angularSpeed});
ecm->SetComponentData<components::JointVelocityCmd>(
wheelRearLeftSpin0Entity, {angularSpeed});
ecm->SetComponentData<components::JointVelocityCmd>(
wheelRearLeftSpin1Entity, {angularSpeed});
ecm->SetComponentData<components::JointVelocityCmd>(
wheelRearRightSpin0Entity, {angularSpeed});
ecm->SetComponentData<components::JointVelocityCmd>(
wheelRearRightSpin1Entity, {angularSpeed});
});
server.AddSystem(testSlipSystem.systemPtr);
server.Run(true, 2000, false);
Expand Down

0 comments on commit f8add56

Please sign in to comment.