Fix spherical coordinates FromLocal calls (#428)

* Fix spherical coordinates FromLocal calls.

Signed-off-by: Carlos Agüero <[email protected]>

vrx_gazebo/include/vrx_gazebo/scoring_plugin.hh

@@ -26,6 +26,7 @@
 #include <gazebo/common/Events.hh>
 #include <gazebo/common/Plugin.hh>
 #include <gazebo/common/Time.hh>
+#include <ignition/math/SphericalCoordinates.hh>
 #include <gazebo/physics/World.hh>
 #include <sdf/sdf.hh>
 #include <gazebo/transport/transport.hh>
@@ -233,6 +234,9 @@ class ScoringPlugin : public gazebo::WorldPlugin
   /// \brief Silent mode enabled?
   protected: bool silent = false;
 
+  /// \brief Spherical coordinates conversions.
+  protected: ignition::math::SphericalCoordinates sc;
+
   /// \brief gazebo node pointer
   private: gazebo::transport::NodePtr gzNode;
 

vrx_gazebo/src/perception_scoring_plugin.cc

@@ -407,13 +407,9 @@ void PerceptionScoringPlugin::OnAttempt(
       // Snippet from UUV Simulator SphericalCoordinatesROSInterfacePlugin.cc
       ignition::math::Vector3d scVec(_msg->pose.position.latitude,
         _msg->pose.position.longitude, 0);
-      #if GAZEBO_MAJOR_VERSION >= 8
-        ignition::math::Vector3d cartVec =
-        this->world->SphericalCoords()->LocalFromSpherical(scVec);
-      #else
-        ignition::math::Vector3d cartVec =
-        this->world->GetSphericalCoordinates()->LocalFromSpherical(scVec);
-      #endif
+      ignition::math::Vector3d cartVec =
+        this->sc.LocalFromSphericalPosition(scVec);
+
       // Get current pose of the current object
       #if GAZEBO_MAJOR_VERSION >= 8
         ignition::math::Pose3d truePose = obj.modelPtr->WorldPose();

vrx_gazebo/src/scoring_plugin.cc

@@ -43,6 +43,17 @@ void ScoringPlugin::Load(gazebo::physics::WorldPtr _world,
     return;
   }
 
+  // Initialize spherical coordinates.
+#if GAZEBO_MAJOR_VERSION >= 8
+  auto gzSC = _world->SphericalCoords();
+#else
+  auto gzSC = _world->GetSphericalCoordinates();
+#endif
+  this->sc.SetLatitudeReference(gzSC->LatitudeReference());
+  this->sc.SetLongitudeReference(gzSC->LongitudeReference());
+  this->sc.SetElevationReference(gzSC->GetElevationReference());
+  this->sc.SetHeadingOffset(gzSC->HeadingOffset());
+
   this->readyTime.Set(this->initialStateDuration);
   this->runningTime = this->readyTime + this->readyStateDuration;
   this->finishTime = this->runningTime + this->runningStateDuration;

vrx_gazebo/src/stationkeeping_scoring_plugin.cc

@@ -66,14 +66,8 @@ void StationkeepingScoringPlugin::Load(gazebo::physics::WorldPtr _world,
     // Convert lat/lon to local
     // Snippet from UUV Simulator SphericalCoordinatesROSInterfacePlugin.cc
     ignition::math::Vector3d scVec(this->goalLat, this->goalLon, 0.0);
-
-#if GAZEBO_MAJOR_VERSION >= 8
-    ignition::math::Vector3d cartVec =
-      _world->SphericalCoords()->LocalFromSpherical(scVec);
-#else
     ignition::math::Vector3d cartVec =
-      _world->GetSphericalCoordinates()->LocalFromSpherical(scVec);
-#endif
+      this->sc.LocalFromSphericalPosition(scVec);
 
     // Store local 2D location and yaw
     this->goalX = cartVec.X();
@@ -93,13 +87,11 @@ void StationkeepingScoringPlugin::Load(gazebo::physics::WorldPtr _world,
     // Snippet from UUV Simulator SphericalCoordinatesROSInterfacePlugin.cc
     ignition::math::Vector3d cartVec(this->goalX, this->goalY, xyz.Z());
 
-#if GAZEBO_MAJOR_VERSION >= 8
-    ignition::math::Vector3d scVec =
-      _world->SphericalCoords()->SphericalFromLocal(cartVec);
-#else
-    ignition::math::Vector3d scVec =
-      _world->GetSphericalCoordinates()->SphericalFromLocal(cartVec);
-#endif
+    auto in = ignition::math::SphericalCoordinates::CoordinateType::GLOBAL;
+    auto out = ignition::math::SphericalCoordinates::CoordinateType::SPHERICAL;
+    auto scVec = this->sc.PositionTransform(cartVec, in, out);
+    scVec.X(IGN_RTOD(scVec.X()));
+    scVec.Y(IGN_RTOD(scVec.Y()));
 
     // Store spherical 2D location
     this->goalLat = scVec.X();

vrx_gazebo/src/wayfinding_scoring_plugin.cc

@@ -22,7 +22,6 @@
 #include <std_msgs/String.h>
 #include <cmath>
 #include <gazebo/common/Console.hh>
-#include <gazebo/common/SphericalCoordinates.hh>
 #include <ignition/math/Quaternion.hh>
 #include <ignition/math/Vector3.hh>
 #include <gazebo/physics/Model.hh>
@@ -72,13 +71,8 @@ void WayfindingScoringPlugin::Load(gazebo::physics::WorldPtr _world,
     //  snippet from UUV Simulator SphericalCoordinatesROSInterfacePlugin.cc
     ignition::math::Vector3d scVec(latlonyaw.X(), latlonyaw.Y(), 0.0);
 
-#if GAZEBO_MAJOR_VERSION >= 8
     ignition::math::Vector3d cartVec =
-      _world->SphericalCoords()->LocalFromSpherical(scVec);
-#else
-    ignition::math::Vector3d cartVec =
-      _world->GetSphericalCoordinates()->LocalFromSpherical(scVec);
-#endif
+      this->sc.LocalFromSphericalPosition(scVec);
 
     cartVec.Z() = latlonyaw.Z();
 

vrx_gazebo/src/wildlife_scoring_plugin.cc

@@ -461,13 +461,16 @@ void WildlifeScoringPlugin::PublishAnimalLocations()
     // Conversion from Gazebo Cartesian coordinates to spherical.
 #if GAZEBO_MAJOR_VERSION >= 8
     const ignition::math::Pose3d pose = buoy.link->WorldPose();
-    const ignition::math::Vector3d latlon =
-      this->world->SphericalCoords()->SphericalFromLocal(pose.Pos());
 #else
     const ignition::math::Pose3d pose = buoy.link->GetWorldPose().Ign();
-    const ignition::math::Vector3d latlon =
-      this->world->GetSphericalCoordinates()->SphericalFromLocal(pose.Pos());
 #endif
+
+    auto in = ignition::math::SphericalCoordinates::CoordinateType::GLOBAL;
+    auto out = ignition::math::SphericalCoordinates::CoordinateType::SPHERICAL;
+    auto latlon = this->sc.PositionTransform(pose.Pos(), in, out);
+    latlon.X(IGN_RTOD(latlon.X()));
+    latlon.Y(IGN_RTOD(latlon.Y()));
+
     const ignition::math::Quaternion<double> orientation = pose.Rot();
 
     // Fill the GeoPoseStamped message.

wave_gazebo_plugins/include/wave_gazebo_plugins/Wavefield.hh

@@ -42,7 +42,7 @@ namespace asv
   class WaveParametersPrivate;
 
   /// \brief A class to manage the parameters for generating a wave
-    /// in a wave field.
+  /// in a wave field.
   class WaveParameters
   {
     /// \brief Destructor.
@@ -185,19 +185,19 @@ namespace asv
   class WavefieldSampler
   {
     /// \brief Compute the depth at a point directly
-        /// (no sampling or interpolation).
+    /// (no sampling or interpolation).
     ///
     /// This method solves for (x, y) that when input into the
-        /// Gerstner wave function
+    /// Gerstner wave function
     /// gives the coordinates of the supplied parameter
-        /// _point (_point.x(), _point.y()),
+    /// _point (_point.x(), _point.y()),
     /// and also computes the wave height pz at this point.
     /// The depth h = pz - point.z().
     /// This is a numerical method that uses a multi-variate
-        /// Newton solver to solve
+    /// Newton solver to solve
     /// the two dimensional non-linear system. In general it is not as fast as
     /// sampling from a discretised wave field with an efficient
-        /// line intersection algorithm.
+    /// line intersection algorithm.
     ///
     /// \param[in] _waveParams  Gerstner wave parameters.
     /// \param[in] _point       The point at which we want the depth.