From 6cfcd07c346a72546963ed56ade48daa828c2072 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 15 Mar 2024 10:18:16 -0500 Subject: [PATCH 1/8] Prepare for 8.2.0 (#2335) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 17 +++++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 21080bd6d9..f131a19a66 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-sim8 VERSION 8.1.0) +project(gz-sim8 VERSION 8.2.0) set (GZ_DISTRIBUTION "Harmonic") #============================================================================ diff --git a/Changelog.md b/Changelog.md index 16a29e826f..0a1349e075 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,22 @@ ## Gazebo Sim 8.x +### Gazebo Sim 8.2.0 (2024-03-14) + +1. Add reference to joint_controller.md tutorial. + * [Pull request #2333](https://github.com/gazebosim/gz-sim/pull/2333) + +1. Fix wget in maritime tutorials + * [Pull request #2330](https://github.com/gazebosim/gz-sim/pull/2330) + +1. Add entity and sdf parameters to Server's AddSystem interface + * [Pull request #2324](https://github.com/gazebosim/gz-sim/pull/2324) + +1. Add entity validation to OdometryPublisher + * [Pull request #2326](https://github.com/gazebosim/gz-sim/pull/2326) + +1. Fix typo in Joint.hh + * [Pull request #2310](https://github.com/gazebosim/gz-sim/pull/2310) + ### Gazebo Sim 8.1.0 (2024-02-06) 1. Add tutorial for using components in systems From 5faaf5ea710f178ee15e0fb79d4a5f4b9990fe73 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 22 Mar 2024 10:10:54 -0700 Subject: [PATCH 2/8] Publish step size in world stats topic (#2340) Signed-off-by: Ian Chen --- src/SimulationRunner.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 4ee08be76a..a5ba51e96c 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -463,6 +463,8 @@ void SimulationRunner::PublishStats() msg.set_paused(this->currentInfo.paused); + msgs::Set(msg.mutable_step_size(), this->currentInfo.dt); + if (this->Stepping()) { // (deprecated) Remove this header in Gazebo H From 5944188294511a60b829f88851edeaded0b72a84 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 25 Mar 2024 09:13:35 -0700 Subject: [PATCH 3/8] Check null mesh (#2341) Signed-off-by: Ian Chen --- src/MeshInertiaCalculator.cc | 5 +++++ src/gui/plugins/apply_force_torque/ApplyForceTorque.cc | 1 - .../VisualizationCapabilities.cc | 9 ++++++++- src/rendering/SceneManager.cc | 10 ++++++++-- 4 files changed, 21 insertions(+), 4 deletions(-) diff --git a/src/MeshInertiaCalculator.cc b/src/MeshInertiaCalculator.cc index 23d5e95310..47b7c3c230 100644 --- a/src/MeshInertiaCalculator.cc +++ b/src/MeshInertiaCalculator.cc @@ -218,6 +218,11 @@ std::optional MeshInertiaCalculator::operator() // Load the Mesh gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance(); mesh = meshManager->Load(fullPath); + if (!mesh) + { + gzerr << "Failed to load mesh: " << fullPath << std::endl; + return std::nullopt; + } std::vector meshTriangles; gz::math::MassMatrix3d meshMassMatrix; gz::math::Pose3d centreOfMass; diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc index 24a607d20f..a2ca6ce79f 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc @@ -19,7 +19,6 @@ #include #include -#include #include #include #include diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc index 82e1492853..c5d24bc7ab 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -1245,7 +1245,14 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry( gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance(); descriptor.mesh = meshManager->Load(descriptor.meshName); - geom = this->scene->CreateMesh(descriptor); + if (descriptor.mesh) + { + geom = this->scene->CreateMesh(descriptor); + } + else + { + gzerr << "Failed to load mesh: " << descriptor.meshName << std::endl; + } scale = _geom.MeshShape()->Scale(); } else if (_geom.Type() == sdf::GeometryType::HEIGHTMAP) diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index ab5d0ee92b..d94e6aa8b7 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -813,8 +813,14 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, rendering::MeshDescriptor descriptor; descriptor.meshName = name; descriptor.mesh = meshManager->MeshByName(name); - - geom = this->dataPtr->scene->CreateMesh(descriptor); + if (descriptor.mesh) + { + geom = this->dataPtr->scene->CreateMesh(descriptor); + } + else + { + gzerr << "Unable to find the polyline mesh: " << name << std::endl; + } } else { From 1d5114bb0614b995220a9fa7d30d8d5d2da576a0 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Thu, 28 Mar 2024 01:26:14 +0800 Subject: [PATCH 4/8] Fixed turning error in ackermann steering (#2342) This is a fix to the error seen in Ackermann Steering's mode. The steps to reproduce this error are described in issue #2314. Signed-off-by: Saurabh Kamat --- src/systems/ackermann_steering/AckermannSteering.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 3fe8a5f60d..0d468916e5 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -968,11 +968,11 @@ void AckermannSteeringPrivate::UpdateAngle( double leftSteeringJointAngle = atan((2.0 * this->wheelBase * sin(ang)) / \ - ((2.0 * this->wheelBase * cos(ang)) + \ + ((2.0 * this->wheelBase * cos(ang)) - \ (1.0 * this->wheelSeparation * sin(ang)))); double rightSteeringJointAngle = atan((2.0 * this->wheelBase * sin(ang)) / \ - ((2.0 * this->wheelBase * cos(ang)) - \ + ((2.0 * this->wheelBase * cos(ang)) + \ (1.0 * this->wheelSeparation * sin(ang)))); auto leftSteeringPos = _ecm.Component( From 760ba21f685f65f436a0107103a68bcf4170a17b Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Thu, 28 Mar 2024 23:09:30 +0800 Subject: [PATCH 5/8] Added mutex to protect stored time variables (#2345) Signed-off-by: Saurabh Kamat --- src/systems/dvl/DopplerVelocityLogSystem.cc | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/systems/dvl/DopplerVelocityLogSystem.cc b/src/systems/dvl/DopplerVelocityLogSystem.cc index 103311ed6b..fbf5d7d873 100644 --- a/src/systems/dvl/DopplerVelocityLogSystem.cc +++ b/src/systems/dvl/DopplerVelocityLogSystem.cc @@ -220,6 +220,9 @@ class gz::sim::systems::DopplerVelocityLogSystem::Implementation /// \brief Current simulation time. public: std::chrono::steady_clock::duration nextUpdateTime{ std::chrono::steady_clock::duration::max()}; + + /// \brief Mutex to protect current simulation times + public: std::mutex timeMutex; }; using namespace gz; @@ -351,6 +354,8 @@ void DopplerVelocityLogSystem::Implementation::DoPostUpdate( return true; }); + std::lock_guard timeLock(this->timeMutex); + if (!this->perStepRequests.empty() || ( !_info.paused && this->nextUpdateTime <= _info.simTime)) { @@ -611,6 +616,9 @@ void DopplerVelocityLogSystem::Implementation::OnRender() } auto closestUpdateTime = std::chrono::steady_clock::duration::max(); + + std::lock_guard timeLock(this->timeMutex); + for (const auto & [_, sensorId] : this->sensorIdPerEntity) { gz::sensors::Sensor *sensor = @@ -635,6 +643,9 @@ void DopplerVelocityLogSystem::Implementation::OnRender() void DopplerVelocityLogSystem::Implementation::OnPostRender() { GZ_PROFILE("DopplerVelocityLogSystem::Implementation::OnPostRender"); + + std::lock_guard timeLock(this->timeMutex); + for (const auto & sensorId : this->updatedSensorIds) { auto *sensor = From b9138fb12b0cef77621ed3a5eba577ebe4f7abd0 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Tue, 2 Apr 2024 05:41:32 +0530 Subject: [PATCH 6/8] Fixed undefined behavior in thruster.cc (#2350) --------- Signed-off-by: Gaurav Kumar Co-authored-by: Arjo Chakravarty --- test/integration/thruster.cc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 3a365ea69f..c4d4ec4d08 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -211,6 +211,7 @@ void ThrusterTest::TestWorld(const std::string &_world, // Check no movement because of invalid commands fixture.Server()->Run(true, 100, false); + ASSERT_FALSE(modelPoses.empty()); EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X()); EXPECT_EQ(100u, modelPoses.size()); EXPECT_EQ(100u, propellerAngVels.size()); @@ -244,8 +245,8 @@ void ThrusterTest::TestWorld(const std::string &_world, // Check movement if (_namespace != "lowbattery") { - for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep; - ++sleep) + for (sleep = 0; (modelPoses.empty() || modelPoses.back().Pos().X() < 5.0) && + sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); fixture.Server()->Run(true, 100, false); @@ -331,13 +332,11 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_NEAR(0.0, angVel.Y(), _baseTol); EXPECT_NEAR(0.0, angVel.Z(), _baseTol); } - modelPoses.clear(); propellerAngVels.clear(); propellerLinVels.clear(); // Make sure that when the deadband is disabled // commands below the deadband should create a movement - auto latest_pose = modelPoses.back(); msgs::Boolean db_msg; if (_namespace == "deadband") { From 6884e666a51ead68f9e53bdcf7a64daa24d5279f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 5 Apr 2024 04:49:40 +0800 Subject: [PATCH 7/8] Remove unessecary sleep (#2357) Im not why this sleep was her in the first place. But it does not seem to be needed. Signed-off-by: Arjo Chakravarty --- test/integration/thruster.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index c4d4ec4d08..d8de8a61f9 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -248,7 +248,6 @@ void ThrusterTest::TestWorld(const std::string &_world, for (sleep = 0; (modelPoses.empty() || modelPoses.back().Pos().X() < 5.0) && sleep < maxSleep; ++sleep) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); fixture.Server()->Run(true, 100, false); } EXPECT_LT(sleep, maxSleep); From f8add56ea6635934576a404fbd2b89d26994e802 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 5 Apr 2024 11:49:01 -0700 Subject: [PATCH 8/8] 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);