Initial commit - test serial

SerialTest/include/okapi/squiggles/geometry/controlvector.hpp

@@ -0,0 +1,62 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _GEOMETRY_CONTROL_VECTOR_HPP_
+#define _GEOMETRY_CONTROL_VECTOR_HPP_
+
+#include <cmath>
+#include <string>
+
+#include "pose.hpp"
+
+namespace squiggles {
+class ControlVector {
+  public:
+  /**
+   * A vector used to specify a state along a hermite spline.
+   *
+   * @param ipose The 2D position and heading.
+   * @param ivel The velocity component of the vector.
+   * @param iaccel The acceleration component of the vector.
+   * @param ijerk The jerk component of the vector.
+   */
+  ControlVector(Pose ipose,
+                double ivel = std::nan(""),
+                double iaccel = 0.0,
+                double ijerk = 0.0)
+    : pose(ipose), vel(ivel), accel(iaccel), jerk(ijerk) {}
+
+  ControlVector() = default;
+
+  /**
+   * Serializes the Control Vector data for debugging.
+   *
+   * @return The Control Vector data.
+   */
+  std::string to_string() const {
+    return "ControlVector: {" + pose.to_string() +
+           ", v: " + std::to_string(vel) + ", a: " + std::to_string(accel) +
+           ", j: " + std::to_string(jerk) + "}";
+  }
+
+  std::string to_csv() const {
+    return pose.to_csv() + "," + std::to_string(vel) + "," +
+           std::to_string(accel) + "," + std::to_string(jerk);
+  }
+
+  bool operator==(const ControlVector& other) const {
+    return pose == other.pose && nearly_equal(vel, other.vel) &&
+           nearly_equal(accel, other.accel) && nearly_equal(jerk, other.jerk);
+  }
+
+  Pose pose;
+  double vel;
+  double accel;
+  double jerk;
+};
+} // namespace squiggles
+
+#endif

SerialTest/include/okapi/squiggles/geometry/pose.hpp

@@ -0,0 +1,67 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _GEOMETRY_POSE_HPP_
+#define _GEOMETRY_POSE_HPP_
+
+#include <cmath>
+#include <string>
+
+#include "math/utils.hpp"
+
+namespace squiggles {
+class Pose {
+  public:
+  /**
+   * Specifies a point and heading in 2D space.
+   *
+   * @param ix The x position of the point in meters.
+   * @param iy The y position of the point in meters.
+   * @param iyaw The heading at the point in radians.
+   */
+  Pose(double ix, double iy, double iyaw) : x(ix), y(iy), yaw(iyaw) {}
+
+  Pose() = default;
+
+  /**
+   * Calculates the Euclidean distance between this pose and another.
+   *
+   * @param other The point from which the distance will be calculated.
+   *
+   * @return The distance between this pose and Other.
+   */
+  double dist(const Pose& other) const {
+    return std::sqrt((x - other.x) * (x - other.x) +
+                     (y - other.y) * (y - other.y));
+  }
+
+  /**
+   * Serializes the Pose data for debugging.
+   *
+   * @return The Pose data.
+   */
+  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

SerialTest/include/okapi/squiggles/geometry/profilepoint.hpp

@@ -0,0 +1,100 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#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 {
+  /**
+   * Defines a state along a motion profiled path.
+   *
+   * @param ivector The pose and associated dynamics at this state in the path.
+   * @param iwheel_velocities The component of the robot's velocity provided by
+   *                          each wheel in meters per second.
+   * @param icurvature The degree to which the curve deviates from a straight
+   *                   line at this point in 1 / meters.
+   * @param itime The timestamp for this state relative to the start of the
+   *              path in seconds.
+   */
+  ProfilePoint(ControlVector ivector,
+               std::vector<double> iwheel_velocities,
+               double icurvature,
+               double itime)
+    : vector(ivector),
+      wheel_velocities(iwheel_velocities),
+      curvature(icurvature),
+      time(itime) {}
+
+  ProfilePoint() = default;
+
+  /**
+   * Serializes the Profile Point data for debugging.
+   *
+   * @return The Profile Point data.
+   */
+  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