Backport nonbreaking changes from ionic (#2552)

* Model.hh: add ModelByName and ModelCount APIs
  Backport nested model accessor APIs from #2494.
* Add WrenchMeasured component
* Physics.cc: fix typo in comment
* sensor_TEST.py: move PreUpdate callback to Update
  The test's PreUpdate callback assumes that it executes
  after the ForceTorque::PreUpdate, so just move it to
  Update to gurantee it. Also fix a spelling error in
  the callback variable names.
* fix spelling in on_post_update_cb
* backport minor fixes to force-torque test
* Include System.hh from some systems, add comment

Signed-off-by: Steve Peters <[email protected]>

Author: scpeters

include/gz/sim/Model.hh

@@ -137,6 +137,14 @@ namespace gz
       public: sim::Entity LinkByName(const EntityComponentManager &_ecm,
           const std::string &_name) const;
 
+      /// \brief Get the ID of a nested model entity which is an immediate
+      /// child of this model.
+      /// \param[in] _ecm Entity-component manager.
+      /// \param[in] _name Nested model name.
+      /// \return Nested model entity.
+      public: sim::Entity ModelByName(const EntityComponentManager &_ecm,
+          const std::string &_name) const;
+
       /// \brief Get all joints which are immediate children of this model.
       /// \param[in] _ecm Entity-component manager.
       /// \return All joints in this model.
@@ -167,6 +175,12 @@ namespace gz
       /// \return Number of links in this model.
       public: uint64_t LinkCount(const EntityComponentManager &_ecm) const;
 
+      /// \brief Get the number of nested models which are immediate children
+      /// of this model.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Number of nested models in this model.
+      public: uint64_t ModelCount(const EntityComponentManager &_ecm) const;
+
       /// \brief Set a command to change the model's pose.
       /// \param[in] _ecm Entity-component manager.
       /// \param[in] _pose New model pose.

include/gz/sim/components/WrenchMeasured.hh

@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef GZ_SIM_COMPONENTS_WRENCHMEASURED_HH_
+#define GZ_SIM_COMPONENTS_WRENCHMEASURED_HH_
+
+#include <gz/msgs/wrench.pb.h>
+
+#include <gz/sim/components/Factory.hh>
+#include <gz/sim/components/Component.hh>
+#include <gz/sim/components/Serialization.hh>
+#include <gz/sim/config.hh>
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+
+namespace components
+{
+/// \brief Wrench measured by a ForceTorqueSensor in SI units (Nm for torque,
+/// N for force).
+/// The wrench is expressed in the Sensor frame and the force component is
+/// applied at the sensor origin.
+/// \note The term Wrench is used here to mean a pair of 3D vectors representing
+/// torque and force quantities expressed in a given frame and where the force
+/// is applied at the origin of the frame. This is different from the Wrench
+/// used in screw theory.
+/// \note The value of force_offset in msgs::Wrench is ignored for this
+/// component. The force is assumed to be applied at the origin of the sensor
+/// frame.
+using WrenchMeasured =
+    Component<msgs::Wrench, class WrenchMeasuredTag,
+              serializers::MsgSerializer>;
+GZ_SIM_REGISTER_COMPONENT("gz_sim_components.WrenchMeasured",
+                          WrenchMeasured)
+}  // namespace components
+}
+}
+}
+
+#endif

python/test/sensor_TEST.py

@@ -33,11 +33,11 @@ def test_model(self):
         file_path = os.path.dirname(os.path.realpath(__file__))
         fixture = TestFixture(os.path.join(file_path, 'joint_test.sdf'))
 
-        def on_post_udpate_cb(_info, _ecm):
+        def on_post_update_cb(_info, _ecm):
             self.post_iterations += 1
 
-        def on_pre_udpate_cb(_info, _ecm):
-            self.pre_iterations += 1
+        def on_update_cb(_info, _ecm):
+            self.iterations += 1
             world_e = world_entity(_ecm)
             self.assertNotEqual(K_NULL_ENTITY, world_e)
             w = World(world_e)
@@ -53,19 +53,19 @@ def on_pre_udpate_cb(_info, _ecm):
             # Pose Test
             self.assertEqual(Pose3d(0, 1, 0, 0, 0, 0), sensor.pose(_ecm))
             # Topic Test
-            if self.pre_iterations <= 1:
+            if self.iterations <= 1:
                 self.assertEqual(None, sensor.topic(_ecm))
             else:
                 self.assertEqual('sensor_topic_test', sensor.topic(_ecm))
             # Parent Test
             self.assertEqual(j.entity(), sensor.parent(_ecm))
 
-        def on_udpate_cb(_info, _ecm):
-            self.iterations += 1
+        def on_pre_update_cb(_info, _ecm):
+            self.pre_iterations += 1
 
-        fixture.on_post_update(on_post_udpate_cb)
-        fixture.on_update(on_udpate_cb)
-        fixture.on_pre_update(on_pre_udpate_cb)
+        fixture.on_post_update(on_post_update_cb)
+        fixture.on_update(on_update_cb)
+        fixture.on_pre_update(on_pre_update_cb)
         fixture.finalize()
 
         server = fixture.server()

src/Model.cc

@@ -148,6 +148,16 @@ Entity Model::LinkByName(const EntityComponentManager &_ecm,
       components::Link());
 }
 
+//////////////////////////////////////////////////
+Entity Model::ModelByName(const EntityComponentManager &_ecm,
+    const std::string &_name) const
+{
+  return _ecm.EntityByComponents(
+      components::ParentEntity(this->dataPtr->id),
+      components::Name(_name),
+      components::Model());
+}
+
 //////////////////////////////////////////////////
 std::vector<Entity> Model::Joints(const EntityComponentManager &_ecm) const
 {
@@ -184,6 +194,12 @@ uint64_t Model::LinkCount(const EntityComponentManager &_ecm) const
   return this->Links(_ecm).size();
 }
 
+//////////////////////////////////////////////////
+uint64_t Model::ModelCount(const EntityComponentManager &_ecm) const
+{
+  return this->Models(_ecm).size();
+}
+
 //////////////////////////////////////////////////
 void Model::SetWorldPoseCmd(EntityComponentManager &_ecm,
     const math::Pose3d &_pose)

src/SystemManager.cc

@@ -307,6 +307,7 @@ const std::vector<ISystemConfigure *>& SystemManager::SystemsConfigure()
   return this->systemsConfigure;
 }
 
+//////////////////////////////////////////////////
 const std::vector<ISystemConfigureParameters *>&
 SystemManager::SystemsConfigureParameters()
 {

src/systems/force_torque/ForceTorque.cc

@@ -45,6 +45,7 @@
 #include "gz/sim/components/Sensor.hh"
 #include "gz/sim/components/World.hh"
 #include "gz/sim/EntityComponentManager.hh"
+#include "gz/sim/System.hh"
 #include "gz/sim/Util.hh"
 
 using namespace gz;

src/systems/physics/Physics.cc

@@ -93,6 +93,7 @@
 
 #include "gz/sim/EntityComponentManager.hh"
 #include "gz/sim/Model.hh"
+#include "gz/sim/System.hh"
 #include "gz/sim/Util.hh"
 
 // Components
@@ -432,7 +433,7 @@ class gz::sim::systems::PhysicsPrivate
                       }
                       return true;
                     }};
-  /// \brief msgs::Contacts equality comparison function.
+  /// \brief msgs::Wrench equality comparison function.
   public: std::function<bool(const msgs::Wrench &, const msgs::Wrench &)>
           wrenchEql{
           [](const msgs::Wrench &_a, const msgs::Wrench &_b)

src/systems/user_commands/UserCommands.cc

@@ -80,6 +80,7 @@
 #include "gz/sim/EntityComponentManager.hh"
 #include "gz/sim/Model.hh"
 #include "gz/sim/SdfEntityCreator.hh"
+#include "gz/sim/System.hh"
 #include "gz/sim/Util.hh"
 #include "gz/sim/World.hh"
 #include "gz/sim/components/ContactSensorData.hh"

test/integration/force_torque_system.cc

@@ -43,7 +43,7 @@ class ForceTorqueTest : public InternalFixture<::testing::Test>
 };
 
 /////////////////////////////////////////////////
-TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeight))
+TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeightTopic))
 {
   using namespace std::chrono_literals;
   // Start server
@@ -59,8 +59,8 @@ TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeight))
 
   // Having iters exactly in sync with update rate can lead to a race condition
   // in the test between simulation and transport
-  size_t iters = 999u;
-  size_t updates = 100u;
+  const size_t iters = 999u;
+  const size_t updates = 100u;
 
   std::vector<msgs::Wrench> wrenches;
   wrenches.reserve(updates);

test/integration/model.cc

@@ -164,6 +164,30 @@ TEST_F(ModelIntegrationTest, SourceFilePath)
   EXPECT_EQ("/tmp/path", model.SourceFilePath(ecm));
 }
 
+//////////////////////////////////////////////////
+TEST_F(ModelIntegrationTest, ModelByName)
+{
+  EntityComponentManager ecm;
+
+  // Model
+  auto eModel = ecm.CreateEntity();
+  Model model(eModel);
+  EXPECT_EQ(eModel, model.Entity());
+  EXPECT_EQ(0u, model.ModelCount(ecm));
+
+  // Nested Model
+  auto eNestedModel = ecm.CreateEntity();
+  ecm.CreateComponent<components::Model>(eNestedModel, components::Model());
+  ecm.CreateComponent<components::ParentEntity>(eNestedModel,
+      components::ParentEntity(eModel));
+  ecm.CreateComponent<components::Name>(eNestedModel,
+      components::Name("nested_model_name"));
+
+  // Check model
+  EXPECT_EQ(eNestedModel, model.ModelByName(ecm, "nested_model_name"));
+  EXPECT_EQ(1u, model.ModelCount(ecm));
+}
+
 //////////////////////////////////////////////////
 TEST_F(ModelIntegrationTest, LinkByName)
 {