Merge pull request #2363 from gazebosim/scpeters/merge_8_main

Merge gz-sim8 ➡️  main

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

src/MeshInertiaCalculator.cc

@@ -218,6 +218,11 @@ std::optional<gz::math::Inertiald> 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<Triangle> meshTriangles;
   gz::math::MassMatrix3d meshMassMatrix;
   gz::math::Pose3d centreOfMass;

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

src/gui/plugins/apply_force_torque/ApplyForceTorque.cc

@@ -19,7 +19,6 @@
 #include <mutex>
 #include <string>
 
-#include <gz/common/MeshManager.hh>
 #include <gz/common/MouseEvent.hh>
 #include <gz/gui/Application.hh>
 #include <gz/gui/GuiEvents.hh>

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)

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
   {

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)
   {
@@ -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<components::JointPosition>(

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

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<std::mutex> 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<std::mutex> 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<std::mutex> timeLock(this->timeMutex);
+
   for (const auto & sensorId : this->updatedSensorIds)
   {
     auto *sensor =

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});
     }
   }
 }

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;
   }

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});
   }
 }
 

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);
   }