Merge branch 'main' into namespace_migration

gazebosim · May 28, 2022 · 0e10296 · 0e10296
2 parents 422058f + 8a9c631
commit 0e10296
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 0 deletions.
diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh
@@ -435,6 +435,10 @@ namespace sdf
     /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is
     /// a private function, so we need to befriend the entire class.
     friend class Link;
+    /// \brief Allow Joint::SetPoseRelativeToGraph to call SetXmlParentName
+    /// and SetPoseRelativeToGraph, but Joint::SetPoseRelativeToGraph is
+    /// a private function, so we need to befriend the entire class.
+    friend class Joint;
 
     /// \brief Private data pointer.
     IGN_UTILS_IMPL_PTR(dataPtr)

diff --git a/src/Joint.cc b/src/Joint.cc
@@ -331,6 +331,11 @@ void Joint::SetPoseRelativeToGraph(
       axis->SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph);
     }
   }
+  for (auto &sensor : this->dataPtr->sensors)
+  {
+    sensor.SetXmlParentName(this->dataPtr->name);
+    sensor.SetPoseRelativeToGraph(_graph);
+  }
 }
 
 /////////////////////////////////////////////////

diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc
@@ -978,6 +978,7 @@ TEST(DOMJoint, Sensors)
   EXPECT_EQ("joint", joint->Name());
   EXPECT_EQ(1u, joint->SensorCount());
 
+  ignition::math::Pose3d pose;
   // Get the force_torque sensor
   const sdf::Sensor *forceTorqueSensor =
     joint->SensorByName("force_torque_sensor");
@@ -986,6 +987,12 @@ TEST(DOMJoint, Sensors)
   EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type());
   EXPECT_EQ(gz::math::Pose3d(10, 11, 12, 0, 0, 0),
       forceTorqueSensor->RawPose());
+  EXPECT_TRUE(
+      forceTorqueSensor->SemanticPose().Resolve(pose, "__model__").empty());
+  EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), pose);
+  EXPECT_TRUE(forceTorqueSensor->SemanticPose().Resolve(pose).empty());
+  EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), pose);
+
   auto forceTorqueSensorObj = forceTorqueSensor->ForceTorqueSensor();
   ASSERT_NE(nullptr, forceTorqueSensorObj);
   EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorObj->Frame());