From 18d350d9f5263efa99b4711b7efc9d7ecf93003e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 11 Feb 2022 12:36:17 +0100 Subject: [PATCH] Fix spherical coordinates FromLocal calls (#428) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix spherical coordinates FromLocal calls. Signed-off-by: Carlos Agüero --- .../include/vrx_gazebo/scoring_plugin.hh | 4 ++++ vrx_gazebo/src/perception_scoring_plugin.cc | 10 +++------- vrx_gazebo/src/scoring_plugin.cc | 11 ++++++++++ .../src/stationkeeping_scoring_plugin.cc | 20 ++++++------------- vrx_gazebo/src/wayfinding_scoring_plugin.cc | 8 +------- vrx_gazebo/src/wildlife_scoring_plugin.cc | 11 ++++++---- .../include/wave_gazebo_plugins/Wavefield.hh | 12 +++++------ 7 files changed, 38 insertions(+), 38 deletions(-) diff --git a/vrx_gazebo/include/vrx_gazebo/scoring_plugin.hh b/vrx_gazebo/include/vrx_gazebo/scoring_plugin.hh index 815863e66..85011bfe2 100644 --- a/vrx_gazebo/include/vrx_gazebo/scoring_plugin.hh +++ b/vrx_gazebo/include/vrx_gazebo/scoring_plugin.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -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; diff --git a/vrx_gazebo/src/perception_scoring_plugin.cc b/vrx_gazebo/src/perception_scoring_plugin.cc index f61cc778a..d19804083 100644 --- a/vrx_gazebo/src/perception_scoring_plugin.cc +++ b/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(); diff --git a/vrx_gazebo/src/scoring_plugin.cc b/vrx_gazebo/src/scoring_plugin.cc index b8fc46692..2484e76ca 100644 --- a/vrx_gazebo/src/scoring_plugin.cc +++ b/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; diff --git a/vrx_gazebo/src/stationkeeping_scoring_plugin.cc b/vrx_gazebo/src/stationkeeping_scoring_plugin.cc index f50e08d1f..0d9215722 100644 --- a/vrx_gazebo/src/stationkeeping_scoring_plugin.cc +++ b/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(); diff --git a/vrx_gazebo/src/wayfinding_scoring_plugin.cc b/vrx_gazebo/src/wayfinding_scoring_plugin.cc index e345509ec..dd1247b11 100644 --- a/vrx_gazebo/src/wayfinding_scoring_plugin.cc +++ b/vrx_gazebo/src/wayfinding_scoring_plugin.cc @@ -22,7 +22,6 @@ #include #include #include -#include #include #include #include @@ -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(); diff --git a/vrx_gazebo/src/wildlife_scoring_plugin.cc b/vrx_gazebo/src/wildlife_scoring_plugin.cc index 6a5a443d0..5076465c0 100644 --- a/vrx_gazebo/src/wildlife_scoring_plugin.cc +++ b/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 orientation = pose.Rot(); // Fill the GeoPoseStamped message. diff --git a/wave_gazebo_plugins/include/wave_gazebo_plugins/Wavefield.hh b/wave_gazebo_plugins/include/wave_gazebo_plugins/Wavefield.hh index 3f438dc4b..7b931ae7c 100644 --- a/wave_gazebo_plugins/include/wave_gazebo_plugins/Wavefield.hh +++ b/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.