Merge branch 'main' into merge_8_main_20240822

gazebosim · Aug 23, 2024 · 931c4dc · 931c4dc
2 parents 3505389 + 40d966b
commit 931c4dc
Showing 1 changed file with 24 additions and 2 deletions.
diff --git a/src/DopplerVelocityLog.cc b/src/DopplerVelocityLog.cc
@@ -1731,12 +1731,34 @@ namespace gz
               samplePointInSensorFrame;
 
           // Transform sample point to the environmental data frame
-          const gz::math::Vector3d samplePointInDataFrame =
+          const std::optional<gz::math::CoordinateVector3>
+              samplePointInDataFrameCoordVec =
               this->worldState->origin.PositionTransform(
-                  samplePointInWorldFrame,
+                  gz::math::CoordinateVector3::Metric(samplePointInWorldFrame),
                   gz::math::SphericalCoordinates::GLOBAL,
                   this->waterVelocityReference);
 
+          if (!samplePointInDataFrameCoordVec.has_value())
+            continue;
+
+          gz::math::Vector3d samplePointInDataFrame;
+          if (samplePointInDataFrameCoordVec->IsSpherical())
+          {
+            samplePointInDataFrame.Set(
+                samplePointInDataFrameCoordVec->Lat()->Radian(),
+                samplePointInDataFrameCoordVec->Lon()->Radian(),
+                *samplePointInDataFrameCoordVec->Z());
+          }
+          else if (samplePointInDataFrameCoordVec->IsMetric())
+          {
+            samplePointInDataFrame =
+                *samplePointInDataFrameCoordVec->AsMetricVector();
+          }
+          else
+          {
+            continue;
+          }
+
           // Sample water velocity in the world frame at sample point
           const gz::math::Vector3d sampledVelocityInWorldFrame =
               this->waterVelocity->LookUp(samplePointInDataFrame);