profilepoint.hpp

Go to the documentation of this file.

 
#ifndef _GEOMETRY_PROFILE_POINT_HPP_
#define _GEOMETRY_PROFILE_POINT_HPP_
 
#include <iostream>
#include <string>
#include <vector>
 
#include "controlvector.hpp"
#include "math/utils.hpp"
 
namespace squiggles {
struct ProfilePoint {
  ProfilePoint(ControlVector ivector,
               std::vector<double> iwheel_velocities,
               double icurvature,
               double itime)
    : vector(ivector),
      wheel_velocities(iwheel_velocities),
      curvature(icurvature),
      time(itime) {}
 
  ProfilePoint() = default;
 
  std::string to_string() const {
    std::string wheels = "{";
    for (auto& w : wheel_velocities) {
      wheels += std::to_string(w);
      wheels += ", ";
    }
    wheels += "}";
    return "ProfilePoint: {" + vector.to_string() + ", wheels: " + wheels +
           ", k: " + std::to_string(curvature) +
           ", t: " + std::to_string(time) + "}";
  }
 
  std::string to_csv() const {
    std::string wheels = "";
    for (auto& w : wheel_velocities) {
      wheels += ",";
      wheels += std::to_string(w);
    }
    return vector.to_csv() + "," + std::to_string(curvature) + "," +
           std::to_string(time) + wheels;
  }
 
  bool operator==(const ProfilePoint& other) const {
    for (std::size_t i = 0; i < wheel_velocities.size(); ++i) {
      if (!nearly_equal(wheel_velocities[i], other.wheel_velocities[i])) {
        return false;
      }
    }
    return vector == other.vector && nearly_equal(curvature, other.curvature) &&
           nearly_equal(time, other.time);
  }
 
  friend std::ostream& operator<<(std::ostream& os, const ProfilePoint& p) {
    return os << "ProfilePoint(ControlVector(Pose(" +
                   std::to_string(p.vector.pose.x) + "," +
                   std::to_string(p.vector.pose.y) + "," +
                   std::to_string(p.vector.pose.yaw) + ")," +
                   std::to_string(p.vector.vel) + "," +
                   std::to_string(p.vector.accel) + "," +
                   std::to_string(p.vector.jerk) + "),{" +
                   std::to_string(p.wheel_velocities[0]) + "," +
                   std::to_string(p.wheel_velocities[1]) + "}," +
                   std::to_string(p.curvature) + "," + std::to_string(p.time) +
                   "),";
    // return os << p.to_string();
  }
 
  ControlVector vector;
  std::vector<double> wheel_velocities;
  double curvature;
  double time;
};
} // namespace squiggles
 
#endif