From 40d966b6fda6da198709214d982be201e9faf2c5 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@openrobotics.org>
Date: Fri, 23 Aug 2024 10:16:32 -0700
Subject: [PATCH] Fix SphericalCoordinate deprecation warning in DVL sensor
 (#460)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
---
 src/DopplerVelocityLog.cc | 26 ++++++++++++++++++++++++--
 1 file changed, 24 insertions(+), 2 deletions(-)

diff --git a/src/DopplerVelocityLog.cc b/src/DopplerVelocityLog.cc
index 4a01b73b..3f6854e2 100644
--- 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);