fixed thruster.cc undefined behavior

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")
 
 #============================================================================

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

include/gz/sim/Server.hh

@@ -26,6 +26,8 @@
 #include <gz/sim/Export.hh>
 #include <gz/sim/ServerConfig.hh>
 #include <gz/sim/SystemPluginPtr.hh>
+#include <sdf/Element.hh>
+#include <sdf/Plugin.hh>
 
 namespace gz
 {
@@ -225,14 +227,60 @@ namespace gz
                   const SystemPluginPtr &_system,
                   const unsigned int _worldIndex = 0);
 
+      /// \brief Add a System to the server. The server must not be running when
+      /// calling this.
+      /// \param[in] _system system to be added
+      /// \param[in] _entity Entity of system to be added.
+      /// If _entity is std::nullopt, it will be added to the world entity.
+      /// \param[in] _sdf Pointer to the SDF element of a <plugin> tag with
+      /// configuration options for the system being added.
+      /// \param[in] _worldIndex Index of the world to query.
+      /// \return Whether the system was added successfully, or std::nullopt
+      /// if _worldIndex is invalid.
+      public: std::optional<bool> AddSystem(
+                  const SystemPluginPtr &_system,
+                  std::optional<Entity> _entity,
+                  std::optional<std::shared_ptr<const sdf::Element>> _sdf,
+                  const unsigned int _worldIndex = 0);
+
+      /// \brief Add a System to the server. The server must not be running when
+      /// calling this.
+      /// \param[in] _plugin system plugin to be added with any additional XML
+      /// contents.
+      /// \param[in] _entity Entity of system to be added.
+      /// If _entity is std::nullopt, it will be added to the world entity.
+      /// \param[in] _worldIndex Index of the world to query.
+      /// \return Whether the system was added successfully, or std::nullopt
+      /// if _worldIndex is invalid.
+      public: std::optional<bool> AddSystem(
+                  const sdf::Plugin &_plugin,
+                  std::optional<Entity> _entity,
+                  const unsigned int _worldIndex = 0);
+
+      /// \brief Add a System to the server. The server must not be running when
+      /// calling this.
+      /// \param[in] _system System to be added
+      /// \param[in] _worldIndex Index of the world to add to.
+      /// \return Whether the system was added successfully, or std::nullopt
+      /// if _worldIndex is invalid.
+      public: std::optional<bool> AddSystem(
+                  const std::shared_ptr<System> &_system,
+                  const unsigned int _worldIndex = 0);
+
       /// \brief Add a System to the server. The server must not be running when
       /// calling this.
       /// \param[in] _system System to be added
+      /// \param[in] _entity Entity of system to be added.
+      /// If _entity is std::nullopt, it will be added to the world entity.
+      /// \param[in] _sdf Pointer to the SDF element of a <plugin> tag with
+      /// configuration options for the system being added
       /// \param[in] _worldIndex Index of the world to add to.
       /// \return Whether the system was added successfully, or std::nullopt
       /// if _worldIndex is invalid.
       public: std::optional<bool> AddSystem(
                   const std::shared_ptr<System> &_system,
+                  std::optional<Entity> _entity,
+                  std::optional<std::shared_ptr<const sdf::Element>> _sdf,
                   const unsigned int _worldIndex = 0);
 
       /// \brief Get an Entity based on a name.

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/Server.cc

@@ -352,6 +352,29 @@ std::optional<size_t> Server::SystemCount(const unsigned int _worldIndex) const
 //////////////////////////////////////////////////
 std::optional<bool> Server::AddSystem(const SystemPluginPtr &_system,
                                       const unsigned int _worldIndex)
+{
+  return this->AddSystem(_system, std::nullopt, std::nullopt, _worldIndex);
+}
+
+//////////////////////////////////////////////////
+std::optional<bool> Server::AddSystem(const sdf::Plugin &_plugin,
+                                      std::optional<Entity> _entity,
+                                      const unsigned int _worldIndex)
+{
+  auto system = this->dataPtr->systemLoader->LoadPlugin(_plugin);
+  if (system)
+  {
+    return this->AddSystem(*system, _entity, _plugin.ToElement(), _worldIndex);
+  }
+  return false;
+}
+
+//////////////////////////////////////////////////
+std::optional<bool> Server::AddSystem(
+    const SystemPluginPtr &_system,
+    std::optional<Entity> _entity,
+    std::optional<std::shared_ptr<const sdf::Element>> _sdf,
+    const unsigned int _worldIndex)
 {
   // Check the current state, and return early if preconditions are not met.
   std::lock_guard<std::mutex> lock(this->dataPtr->runMutex);
@@ -364,7 +387,7 @@ std::optional<bool> Server::AddSystem(const SystemPluginPtr &_system,
 
   if (_worldIndex < this->dataPtr->simRunners.size())
   {
-    this->dataPtr->simRunners[_worldIndex]->AddSystem(_system);
+    this->dataPtr->simRunners[_worldIndex]->AddSystem(_system, _entity, _sdf);
     return true;
   }
 
@@ -374,6 +397,16 @@ std::optional<bool> Server::AddSystem(const SystemPluginPtr &_system,
 //////////////////////////////////////////////////
 std::optional<bool> Server::AddSystem(const std::shared_ptr<System> &_system,
                                       const unsigned int _worldIndex)
+{
+  return this->AddSystem(_system, std::nullopt, std::nullopt, _worldIndex);
+}
+
+//////////////////////////////////////////////////
+std::optional<bool> Server::AddSystem(
+    const std::shared_ptr<System> &_system,
+    std::optional<Entity> _entity,
+    std::optional<std::shared_ptr<const sdf::Element>> _sdf,
+    const unsigned int _worldIndex)
 {
   std::lock_guard<std::mutex> lock(this->dataPtr->runMutex);
   if (this->dataPtr->running)
@@ -384,7 +417,7 @@ std::optional<bool> Server::AddSystem(const std::shared_ptr<System> &_system,
 
   if (_worldIndex < this->dataPtr->simRunners.size())
   {
-    this->dataPtr->simRunners[_worldIndex]->AddSystem(_system);
+    this->dataPtr->simRunners[_worldIndex]->AddSystem(_system, _entity, _sdf);
     return true;
   }
 

src/Server_TEST.cc

@@ -625,17 +625,48 @@ TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RunOnceUnpaused))
   auto mockSystemPlugin = systemLoader.LoadPlugin(sdfPlugin);
   ASSERT_TRUE(mockSystemPlugin.has_value());
 
-  // Check that it was loaded
-  const size_t systemCount = *server.SystemCount();
-  EXPECT_TRUE(*server.AddSystem(mockSystemPlugin.value()));
-  EXPECT_EQ(systemCount + 1, *server.SystemCount());
-
   // Query the interface from the plugin
   auto system = mockSystemPlugin.value()->QueryInterface<sim::System>();
   EXPECT_NE(system, nullptr);
   auto mockSystem = dynamic_cast<sim::MockSystem*>(system);
   EXPECT_NE(mockSystem, nullptr);
 
+  Entity entity = server.EntityByName("default").value();
+  size_t configureCallCount = 0;
+  auto configureCallback = [&sdfPlugin, &entity, &configureCallCount](
+    const Entity &_entity,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    EntityComponentManager &,
+    EventManager &)
+  {
+    configureCallCount++;
+    EXPECT_EQ(entity, _entity);
+    EXPECT_EQ(sdfPlugin.ToElement()->ToString(""), _sdf->ToString(""));
+  };
+
+  mockSystem->configureCallback = configureCallback;
+  const size_t systemCount = *server.SystemCount();
+  EXPECT_TRUE(
+    *server.AddSystem(
+      mockSystemPlugin.value(), entity, sdfPlugin.ToElement()
+  ));
+
+  // Add pointer
+  auto mockSystemPtr = std::make_shared<MockSystem>();
+  mockSystemPtr->configureCallback = configureCallback;
+  EXPECT_TRUE(*server.AddSystem(mockSystemPtr, entity, sdfPlugin.ToElement()));
+
+  // Add an sdf::Plugin
+  EXPECT_TRUE(*server.AddSystem(sdfPlugin, entity));
+
+  // Fail if plugin is invalid
+  sdf::Plugin invalidPlugin("foo_plugin", "foo::systems::FooPlugin");
+  EXPECT_FALSE(*server.AddSystem(invalidPlugin, entity));
+
+  // Check that it was loaded
+  EXPECT_EQ(systemCount + 3, *server.SystemCount());
+  EXPECT_EQ(2u, configureCallCount);
+
   // No steps should have been executed
   EXPECT_EQ(0u, mockSystem->preUpdateCallCount);
   EXPECT_EQ(0u, mockSystem->updateCallCount);

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

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

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/odometry_publisher/OdometryPublisher.cc

@@ -278,6 +278,21 @@ void OdometryPublisher::PreUpdate(const gz::sim::UpdateInfo &_info,
 {
   GZ_PROFILE("OdometryPublisher::PreUpdate");
 
+  // \TODO(anyone) This is a temporary fix for
+  // gazebosim/gz-sim#2165 until gazebosim/gz-sim#2217 is resolved.
+  if (kNullEntity == this->dataPtr->model.Entity())
+  {
+    return;
+  }
+
+  if (!this->dataPtr->model.Valid(_ecm))
+  {
+    gzwarn << "OdometryPublisher model no longer valid. "
+           << "Disabling plugin." << std::endl;
+    this->dataPtr->model = Model(kNullEntity);
+    return;
+  }
+
   // \TODO(anyone) Support rewind
   if (_info.dt < std::chrono::steady_clock::duration::zero())
   {
@@ -301,6 +316,15 @@ void OdometryPublisher::PostUpdate(const UpdateInfo &_info,
     const EntityComponentManager &_ecm)
 {
   GZ_PROFILE("OdometryPublisher::PostUpdate");
+
+  // \TODO(anyone) This is a temporary fix for
+  // gazebosim/gz-sim#2165 until gazebosim/gz-sim#2217 is resolved.
+  if (kNullEntity == this->dataPtr->model.Entity())
+  {
+    return;
+  }
+
+
   // Nothing left to do if paused.
   if (_info.paused)
     return;