pose.hpp

Go to the documentation of this file.

 
#ifndef _GEOMETRY_POSE_HPP_
#define _GEOMETRY_POSE_HPP_
 
#include <cmath>
#include <string>
 
#include "math/utils.hpp"
 
namespace squiggles {
class Pose {
  public:
  Pose(double ix, double iy, double iyaw) : x(ix), y(iy), yaw(iyaw) {}
 
  Pose() = default;
 
  double dist(const Pose& other) const {
    return std::sqrt((x - other.x) * (x - other.x) +
                     (y - other.y) * (y - other.y));
  }
 
  std::string to_string() const {
    return "Pose: {x: " + std::to_string(x) + ", y: " + std::to_string(y) +
           ", yaw: " + std::to_string(yaw) + "}";
  }
 
  std::string to_csv() const {
    return std::to_string(x) + "," + std::to_string(y) + "," +
           std::to_string(yaw);
  }
 
  bool operator==(const Pose& other) const {
    return nearly_equal(x, other.x) && nearly_equal(y, other.y) &&
           nearly_equal(yaw, other.yaw);
  }
 
  double x;
  double y;
  double yaw;
};
} // namespace squiggles
 
#endif