#include "amr_comunication/vda_5050/utils/pose.h"


bool vda_5050::Pose::SavePoseAsFile(const std::string& file_name)
{
  bool result = false;
  std::ofstream file(file_name);
  if (file.is_open())
  {
    file << x_ << " " << y_ << " " << yaw_ << "\n";
    file.close();
    result = true;
  }
  else
  {
    result = false;
  }
  return result;
}

bool vda_5050::Pose::LoadPoseFromFile(const std::string& file_name)
{
  bool result = false;
  std::ifstream file(file_name);
  if (file.is_open())
  {
    double x, y, yaw;
    file >> x >> y >> yaw;
    setPose(x, y, yaw);
    file.close();
    result = true;
  }
  else
  {
    result = false;
  }
  return result;
}

