Use SetComponentData to simplify code and improve coverage (#2360)

Signed-off-by: Steve Peters <[email protected]>
gazebosim · Apr 5, 2024 · f8add56 · f8add56
1 parent 6884e66
commit f8add56
Show file tree

Hide file tree

Showing 8 changed files with 40 additions and 219 deletions.
diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc
@@ -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)
  {

diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc
@@ -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)
@@ -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

diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc
@@ -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});
  }
  }
 }

diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc
@@ -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;
  }

diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc
@@ -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});
  }
 }
 

diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc
@@ -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);
  }
 

diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc
@@ -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
@@ -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
  {
@@ -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
  {

diff --git a/test/integration/wheel_slip.cc b/test/integration/wheel_slip.cc
@@ -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);