From f8add56ea6635934576a404fbd2b89d26994e802 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 5 Apr 2024 11:49:01 -0700 Subject: [PATCH] Use SetComponentData to simplify code and improve coverage (#2360) Signed-off-by: Steve Peters --- .../ackermann_steering/AckermannSteering.cc | 57 +++--------------- src/systems/diff_drive/DiffDrive.cc | 26 ++------ .../joint_controller/JointController.cc | 12 +--- .../JointPositionController.cc | 12 +--- src/systems/mecanum_drive/MecanumDrive.cc | 53 +++------------- src/systems/thruster/Thruster.cc | 14 +---- .../velocity_control/VelocityControl.cc | 60 +++---------------- test/integration/wheel_slip.cc | 25 +++----- 8 files changed, 40 insertions(+), 219 deletions(-) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 0d468916e5..0548749c9d 100644 --- 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(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd( - {this->dataPtr->leftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->leftJointSpeed}); } for (Entity joint : this->dataPtr->rightJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->rightJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->rightJointSpeed}); } } // Update steering for (Entity joint : this->dataPtr->leftSteeringJoints) { - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd( - {this->dataPtr->leftSteeringJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd( - {this->dataPtr->leftSteeringJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->leftSteeringJointSpeed}); } for (Entity joint : this->dataPtr->rightSteeringJoints) { - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd( - {this->dataPtr->rightSteeringJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd( - {this->dataPtr->rightSteeringJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->rightSteeringJointSpeed}); } if (!this->dataPtr->steeringOnly) { diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 700e342743..04fc968279 100644 --- 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(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({this->dataPtr->leftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed}); - } + _ecm.SetComponentData(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(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->rightJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed}); - } + _ecm.SetComponentData(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 index 4c18ac7bdf..40f5caac33 100644 --- 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(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({targetVel})); - } - else - { - *vel = components::JointVelocityCmd({targetVel}); - } + _ecm.SetComponentData(joint, {targetVel}); } } } diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 80d36c2f9e..b2835160ff 100644 --- 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(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({targetVel})); - } - else - { - *vel = components::JointVelocityCmd({targetVel}); - } + _ecm.SetComponentData(joint, {targetVel}); } return; } diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index 6d77de4c77..2f585307c7 100644 --- 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(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->frontLeftJointSpeed}); } for (Entity joint : this->dataPtr->frontRightJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed})); - } - else - { - *vel = - components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->frontRightJointSpeed}); } for (Entity joint : this->dataPtr->backLeftJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->backLeftJointSpeed}); } for (Entity joint : this->dataPtr->backRightJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->backRightJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->backRightJointSpeed}); } } diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index e7663924b2..87aa50dcb4 100644 --- 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( - this->dataPtr->jointEntity); - if (velocityComp == nullptr) - { - _ecm.CreateComponent(this->dataPtr->jointEntity, - components::JointVelocityCmd({desiredPropellerAngVel})); - } - else - { - velocityComp->Data()[0] = desiredPropellerAngVel; - } + _ecm.SetComponentData( + this->dataPtr->jointEntity, {desiredPropellerAngVel}); angvel.set_data(desiredPropellerAngVel); } diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 68224b3bab..b5a1ccbde1 100644 --- 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( - 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( + this->dataPtr->model.Entity(), {this->dataPtr->angularVelocity}); // update linear velocity of model - auto modelLinearVel = - _ecm.Component( - 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( + 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(it->second); - if (!linkAngularVelComp) - { - _ecm.CreateComponent(it->second, - components::AngularVelocityCmd({angularVel})); - } - else - { - *linkAngularVelComp = components::AngularVelocityCmd(angularVel); - } + _ecm.SetComponentData( + 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(it->second); - if (!linkLinearVelComp) - { - _ecm.CreateComponent(it->second, - components::LinearVelocityCmd({linearVel})); - } - else - { - *linkLinearVelComp = components::LinearVelocityCmd(linearVel); - } + _ecm.SetComponentData( + it->second, {linearVel}); } else { diff --git a/test/integration/wheel_slip.cc b/test/integration/wheel_slip.cc index 08453c8933..fef0017caf 100644 --- 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(wheelRearLeftSpin0Entity); - auto wheelRearRightVelocity0Cmd = - ecm->Component(wheelRearRightSpin0Entity); - auto wheelRearLeftVelocity1Cmd = - ecm->Component(wheelRearLeftSpin1Entity); - auto wheelRearRightVelocity1Cmd = - ecm->Component(wheelRearRightSpin1Entity); - - *wheelRearLeftVelocity0Cmd = - components::JointVelocityCmd({angularSpeed}); - *wheelRearRightVelocity0Cmd = - components::JointVelocityCmd({angularSpeed}); - *wheelRearLeftVelocity1Cmd = - components::JointVelocityCmd({angularSpeed}); - *wheelRearRightVelocity1Cmd = - components::JointVelocityCmd({angularSpeed}); + ecm->SetComponentData( + wheelRearLeftSpin0Entity, {angularSpeed}); + ecm->SetComponentData( + wheelRearLeftSpin1Entity, {angularSpeed}); + ecm->SetComponentData( + wheelRearRightSpin0Entity, {angularSpeed}); + ecm->SetComponentData( + wheelRearRightSpin1Entity, {angularSpeed}); }); server.AddSystem(testSlipSystem.systemPtr); server.Run(true, 2000, false);