NavSat.cc

/*
 * Copyright 2021 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#include "sdf/NavSat.hh"
#include "sdf/parser.hh"
#include "Utils.hh"

using namespace sdf;
using namespace gz;

/// \brief Private navsat data.
class sdf::NavSat::Implementation
{
  /// \brief Noise values for the horizontal positioning sensor
  public: Noise horizontalPositionNoise;

  /// \brief Noise values for the vertical positioning sensor
  public: Noise verticalPositionNoise;

  /// \brief Noise values for the horizontal velocity sensor
  public: Noise horizontalVelocityNoise;

  /// \brief Noise values for the verical velocity sensor
  public: Noise verticalVelocityNoise;

  /// \brief The SDF element pointer used during load.
  public: sdf::ElementPtr sdf{nullptr};
};

//////////////////////////////////////////////////
NavSat::NavSat()
  : dataPtr(gz::utils::MakeImpl<Implementation>())
{
}

//////////////////////////////////////////////////
Errors NavSat::Load(ElementPtr _sdf)
{
  Errors errors;

  this->dataPtr->sdf = _sdf;

  // Check that sdf is a valid pointer
  if (!_sdf)
  {
    errors.push_back({ErrorCode::ELEMENT_MISSING,
        "Attempting to load NAVSAT, but the provided SDF "
        "element is null."});
    return errors;
  }

  // Check that the provided SDF element is a <navsat> element.
  // This is an error that cannot be recovered, so return an error.
  if (_sdf->GetName() != "navsat" && _sdf->GetName() != "gps")
  {
    errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE,
        "Attempting to load NAVSAT, but the provided SDF element is "
        "not a <navsat>."});
    return errors;
  }

  // Load navsat sensor properties
  if (_sdf->HasElement("position_sensing"))
  {
    sdf::ElementPtr elem = _sdf->GetElement("position_sensing");
    if (elem->HasElement("horizontal"))
    {
      sdf::ElementPtr horiz = elem->GetElement("horizontal");
      if (horiz->HasElement("noise"))
      {
        this->dataPtr->horizontalPositionNoise.Load(horiz->GetElement("noise"));
      }
    }
    if (elem->HasElement("vertical"))
    {
      sdf::ElementPtr vert = elem->GetElement("vertical");
      if (vert->HasElement("noise"))
      {
        this->dataPtr->verticalPositionNoise.Load(vert->GetElement("noise"));
      }
    }
  }
  if (_sdf->HasElement("velocity_sensing"))
  {
    sdf::ElementPtr elem = _sdf->GetElement("velocity_sensing");
    if (elem->HasElement("horizontal"))
    {
      sdf::ElementPtr horiz = elem->GetElement("horizontal");
      if (horiz->HasElement("noise"))
      {
        this->dataPtr->horizontalVelocityNoise.Load(horiz->GetElement("noise"));
      }
    }
    if (elem->HasElement("vertical"))
    {
      sdf::ElementPtr vert = elem->GetElement("vertical");
      if (vert->HasElement("noise"))
      {
        this->dataPtr->verticalVelocityNoise.Load(vert->GetElement("noise"));
      }
    }
  }

  return errors;
}

//////////////////////////////////////////////////
sdf::ElementPtr NavSat::Element() const
{
  return this->dataPtr->sdf;
}

//////////////////////////////////////////////////
const Noise &NavSat::HorizontalPositionNoise() const
{
  return this->dataPtr->horizontalPositionNoise;
}

//////////////////////////////////////////////////
void NavSat::SetHorizontalPositionNoise(const Noise &_noise)
{
  this->dataPtr->horizontalPositionNoise = _noise;
}

//////////////////////////////////////////////////
const Noise &NavSat::HorizontalVelocityNoise() const
{
  return this->dataPtr->horizontalVelocityNoise;
}

//////////////////////////////////////////////////
void NavSat::SetHorizontalVelocityNoise(const Noise &_noise)
{
  this->dataPtr->horizontalVelocityNoise = _noise;
}

//////////////////////////////////////////////////
const Noise &NavSat::VerticalPositionNoise() const
{
  return this->dataPtr->verticalPositionNoise;
}

//////////////////////////////////////////////////
void NavSat::SetVerticalPositionNoise(const Noise &_noise)
{
  this->dataPtr->verticalPositionNoise = _noise;
}

//////////////////////////////////////////////////
const Noise &NavSat::VerticalVelocityNoise() const
{
  return this->dataPtr->verticalVelocityNoise;
}

//////////////////////////////////////////////////
void NavSat::SetVerticalVelocityNoise(const Noise &_noise)
{
  this->dataPtr->verticalVelocityNoise = _noise;
}

//////////////////////////////////////////////////
bool NavSat::operator==(const NavSat &_navsat) const
{
  if (this->dataPtr->verticalPositionNoise != _navsat.VerticalPositionNoise())
    return false;
  if (this->dataPtr->horizontalPositionNoise !=
      _navsat.HorizontalPositionNoise())
    return false;
  if (this->dataPtr->verticalVelocityNoise != _navsat.VerticalVelocityNoise())
    return false;
  if (this->dataPtr->horizontalVelocityNoise !=
      _navsat.HorizontalVelocityNoise())
    return false;

  return true;
}

//////////////////////////////////////////////////
bool NavSat::operator!=(const NavSat &_navsat) const
{
  return !(*this == _navsat);
}

/////////////////////////////////////////////////
sdf::ElementPtr NavSat::ToElement() const
{
  sdf::Errors errors;
  auto result = this->ToElement(errors);
  sdf::throwOrPrintErrors(errors);
  return result;
}

/////////////////////////////////////////////////
sdf::ElementPtr NavSat::ToElement(sdf::Errors &_errors) const
{
  sdf::ElementPtr elem(new sdf::Element);
  sdf::initFile("navsat.sdf", ParserConfig::GlobalConfig(), elem, _errors);
  const auto defaultNoise = sdf::Noise();

  if (this->dataPtr->horizontalPositionNoise != defaultNoise)
  {
    auto el = elem->GetElement("position_sensing", _errors)->
        GetElement("horizontal", _errors)->GetElement("noise", _errors);
    el->Copy(
        this->dataPtr->horizontalPositionNoise.ToElement(_errors), _errors);
  }

  if (this->dataPtr->verticalPositionNoise != defaultNoise)
  {
    auto el = elem->GetElement("position_sensing", _errors)->
        GetElement("vertical", _errors)->GetElement("noise", _errors);
    el->Copy(this->dataPtr->verticalPositionNoise.ToElement(_errors), _errors);
  }

  if (this->dataPtr->horizontalVelocityNoise != defaultNoise)
  {
    auto el = elem->GetElement("velocity_sensing", _errors)->
        GetElement("horizontal", _errors)->GetElement("noise", _errors);
    el->Copy(
        this->dataPtr->horizontalVelocityNoise.ToElement(_errors), _errors);
  }

  if (this->dataPtr->verticalVelocityNoise != defaultNoise)
  {
    auto el = elem->GetElement("velocity_sensing", _errors)->
        GetElement("vertical", _errors)->GetElement("noise", _errors);
    el->Copy(this->dataPtr->verticalVelocityNoise.ToElement(_errors), _errors);
  }

  return elem;
}