Merge branch 'gazebosim:gz-sim8' into gz-sim8

gazebosim · Apr 10, 2024 · 5cc1bdd · 5cc1bdd
2 parents f27f2ad + 6505cf2
commit 5cc1bdd
Show file tree

Hide file tree

Showing 10 changed files with 50 additions and 220 deletions.
diff --git 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<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)
   {

diff --git 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<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

diff --git 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<components::JointVelocityCmd>(joint);
-
-      if (vel == nullptr)
-      {
-        _ecm.CreateComponent(
-            joint, components::JointVelocityCmd({targetVel}));
-      }
-      else
-      {
-        *vel = components::JointVelocityCmd({targetVel});
-      }
+      _ecm.SetComponentData<components::JointVelocityCmd>(joint, {targetVel});
     }
   }
 }

diff --git 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<components::JointVelocityCmd>(joint);
-
-      if (vel == nullptr)
-      {
-        _ecm.CreateComponent(
-            joint, components::JointVelocityCmd({targetVel}));
-      }
-      else
-      {
-        *vel = components::JointVelocityCmd({targetVel});
-      }
+      _ecm.SetComponentData<components::JointVelocityCmd>(joint, {targetVel});
     }
     return;
   }

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

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

diff --git 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<components::AngularVelocityCmd>(
-      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<components::AngularVelocityCmd>(
+    this->dataPtr->model.Entity(), {this->dataPtr->angularVelocity});
 
   // update linear velocity of model
-  auto modelLinearVel =
-    _ecm.Component<components::LinearVelocityCmd>(
-      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<components::LinearVelocityCmd>(
+    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<components::AngularVelocityCmd>(it->second);
-      if (!linkAngularVelComp)
-      {
-        _ecm.CreateComponent(it->second,
-            components::AngularVelocityCmd({angularVel}));
-      }
-      else
-      {
-        *linkAngularVelComp = components::AngularVelocityCmd(angularVel);
-      }
+      _ecm.SetComponentData<components::AngularVelocityCmd>(
+        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<components::LinearVelocityCmd>(it->second);
-      if (!linkLinearVelComp)
-      {
-        _ecm.CreateComponent(it->second,
-            components::LinearVelocityCmd({linearVel}));
-      }
-      else
-      {
-        *linkLinearVelComp = components::LinearVelocityCmd(linearVel);
-      }
+      _ecm.SetComponentData<components::LinearVelocityCmd>(
+        it->second, {linearVel});
     }
     else
     {

diff --git 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);

diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc
@@ -721,6 +721,16 @@ TEST_F(TriggeredPublisherTest,
 
   std::string service = "/srv-test";
   node.Advertise(service, srvEchoCb);
+  {
+    // This block of code is here because service requests from a previous test
+    // might interfere with this test. We sleep a small amount time and reset
+    // `recvCount` to 0. This ensures that the service requets from the previous
+    // test are discarded properly.
+    // TODO(azeey) Remove once
+    // https://github.com/gazebosim/gz-transport/issues/491 is resolved.
+    GZ_SLEEP_MS(2000);
+    recvCount = 0;
+  }
 
   const std::size_t pubCount{10};
   for (std::size_t i = 0; i < pubCount; ++i)

diff --git 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<components::JointVelocityCmd>(wheelRearLeftSpin0Entity);
-      auto wheelRearRightVelocity0Cmd =
-        ecm->Component<components::JointVelocityCmd>(wheelRearRightSpin0Entity);
-      auto wheelRearLeftVelocity1Cmd =
-        ecm->Component<components::JointVelocityCmd>(wheelRearLeftSpin1Entity);
-      auto wheelRearRightVelocity1Cmd =
-        ecm->Component<components::JointVelocityCmd>(wheelRearRightSpin1Entity);
-
-      *wheelRearLeftVelocity0Cmd =
-        components::JointVelocityCmd({angularSpeed});
-      *wheelRearRightVelocity0Cmd =
-        components::JointVelocityCmd({angularSpeed});
-      *wheelRearLeftVelocity1Cmd =
-        components::JointVelocityCmd({angularSpeed});
-      *wheelRearRightVelocity1Cmd =
-        components::JointVelocityCmd({angularSpeed});
+        ecm->SetComponentData<components::JointVelocityCmd>(
+          wheelRearLeftSpin0Entity, {angularSpeed});
+        ecm->SetComponentData<components::JointVelocityCmd>(
+          wheelRearLeftSpin1Entity, {angularSpeed});
+        ecm->SetComponentData<components::JointVelocityCmd>(
+          wheelRearRightSpin0Entity, {angularSpeed});
+        ecm->SetComponentData<components::JointVelocityCmd>(
+          wheelRearRightSpin1Entity, {angularSpeed});
       });
   server.AddSystem(testSlipSystem.systemPtr);
   server.Run(true, 2000, false);