Merge branch 'gz-sim8' into merge_7_8_20240731

src/SystemManager.cc

@@ -25,6 +25,7 @@
 #include "SystemInternal.hh"
 #include "gz/sim/components/SystemPluginInfo.hh"
 #include "gz/sim/Conversions.hh"
+#include "gz/sim/Util.hh"
 #include "SystemManager.hh"
 
 using namespace gz;
@@ -339,6 +340,11 @@ bool SystemManager::EntitySystemAddService(const msgs::EntityPlugin_V &_req,
 {
   std::lock_guard<std::mutex> lock(this->systemsMsgMutex);
   this->systemsToAdd.push_back(_req);
+
+  // The response is set to true to indicate that the service request is
+  // handled but it does not necessarily mean the system is added
+  // successfully
+  // \todo(iche033) Return false if system is not added successfully?
   _res.set_data(true);
   return true;
 }
@@ -398,8 +404,21 @@ void SystemManager::ProcessPendingEntitySystems()
 
     if (req.plugins().empty())
     {
-      gzwarn << "Unable to add plugins to Entity: '" << entity
-             << "'. No plugins specified." << std::endl;
+      gzerr << "Unable to add plugins to Entity: '" << entity
+            << "'. No plugins specified." << std::endl;
+       continue;
+    }
+
+    // set to world entity if entity id is not specified in the request.
+    if (entity == kNullEntity || entity == 0u)
+    {
+      entity = worldEntity(*this->entityCompMgr);
+    }
+    // otherwise check if entity exists before attempting to load the plugin.
+    else if (!this->entityCompMgr->HasEntity(entity))
+    {
+      gzerr << "Unable to add plugins to Entity: '" << entity
+            << "'. Entity does not exist." << std::endl;
        continue;
     }
 

src/systems/battery_plugin/LinearBatteryPlugin.cc

@@ -480,6 +480,9 @@ void LinearBatteryPlugin::PreUpdate(
 {
   GZ_PROFILE("LinearBatteryPlugin::PreUpdate");
 
+  if (!this->dataPtr->battery)
+    return;
+
   // Recalculate the total power load among consumers
   double total_power_load = this->dataPtr->initialPowerLoad;
   _ecm.Each<components::BatteryPowerLoad>(
@@ -546,6 +549,9 @@ void LinearBatteryPlugin::Update(const UpdateInfo &_info,
 {
   GZ_PROFILE("LinearBatteryPlugin::Update");
 
+  if (!this->dataPtr->battery)
+    return;
+
   // \TODO(anyone) Support rewind
   if (_info.dt < std::chrono::steady_clock::duration::zero())
   {
@@ -610,6 +616,9 @@ void LinearBatteryPlugin::PostUpdate(const UpdateInfo &_info,
   if (_info.paused || !this->dataPtr->statePub)
     return;
 
+  if (!this->dataPtr->battery)
+    return;
+
   // Publish battery state
   msgs::BatteryState msg;
   msg.mutable_header()->mutable_stamp()->CopyFrom(

src/systems/user_commands/UserCommands.cc

@@ -583,6 +583,12 @@ UserCommands::~UserCommands() = default;
 bool UserCommandsInterface::HasContactSensor(const Entity _collision)
 {
   auto *linkEntity = ecm->Component<components::ParentEntity>(_collision);
+
+  if (linkEntity == nullptr)
+  {
+    return false;
+  }
+
   auto allLinkSensors =
     ecm->EntitiesByComponents(components::Sensor(),
       components::ParentEntity(*linkEntity));