diff --git a/Changelog.md b/Changelog.md index 8e8cc35c3b..e225a617c2 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,23 @@ ## 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 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/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 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 { diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 3fe8a5f60d..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) { @@ -968,11 +927,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( 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/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 = 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/thruster.cc b/test/integration/thruster.cc index 3a365ea69f..d8de8a61f9 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,10 +245,9 @@ 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); } EXPECT_LT(sleep, maxSleep); @@ -331,13 +331,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") { 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);