Merge pull request #30 from baylessj/serialization

Add Serialization

.github/workflows/build.yml

@@ -8,8 +8,6 @@ jobs:
       - uses: actions/checkout@v2
         with:
           submodules: true
-      - name: Install LCOV dependency
-        run: sudo apt-get install lcov
       - name: Init CMake
         env:
           CC: gcc-9

.github/workflows/main.yml

@@ -28,10 +28,10 @@ jobs:
           CXX: g++-9
         run: cd build && make && cd ..
       - name: Test
-        run: ./build/tst/RobotSquiggles_tst
+        run: ./build/tst/squiggles_tst
       - name: Collect Coverage
         run: |
-          lcov --directory ./build/src/CMakeFiles/RobotSquiggles_lib.dir/ --capture --output-file coverage.info
+          lcov --directory ./build/src/CMakeFiles/squiggles_lib.dir/ --capture --output-file coverage.info
           lcov --remove coverage.info '/usr/*' --output-file coverage.info
           lcov --remove coverage.info '*/gtest/*' --output-file coverage.info
           lcov --remove coverage.info '*/googletest/*' --output-file coverage.info

.github/workflows/memcheck.yml

@@ -25,4 +25,4 @@ jobs:
         run: make all
       - name: check for leaks
         working-directory: build
-        run: valgrind --tool=memcheck --leak-check=full --leak-resolution=med --show-leak-kinds=all --undef-value-errors=yes --track-origins=yes  --error-exitcode=1 --show-reachable=no ./src/RobotSquiggles_run
+        run: valgrind --tool=memcheck --leak-check=full --leak-resolution=med --show-leak-kinds=all --undef-value-errors=yes --track-origins=yes  --error-exitcode=1 --show-reachable=no ./src/squiggles_run

CMakeLists.txt

@@ -18,6 +18,7 @@ set(SQUIGGLES_SOURCES include/constraints.hpp
   include/physicalmodel/passthroughmodel.hpp
   include/physicalmodel/physicalmodel.hpp
   include/physicalmodel/tankmodel.hpp
+  src/io.cpp
   src/main.cpp
   src/quinticpolynomial.cpp
   src/spline.cpp

include/geometry/controlvector.hpp

@@ -42,6 +42,16 @@ class ControlVector {
            ", 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;

include/geometry/pose.hpp

@@ -10,6 +10,8 @@
 #include <cmath>
 #include <string>
 
+#include "math/utils.hpp"
+
 namespace squiggles {
 class Pose {
   public:
@@ -46,6 +48,16 @@ class Pose {
            ", 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;

include/geometry/profilepoint.hpp

@@ -12,6 +12,7 @@
 #include <vector>
 
 #include "controlvector.hpp"
+#include "math/utils.hpp"
 
 namespace squiggles {
 struct ProfilePoint {
@@ -54,6 +55,41 @@ struct ProfilePoint {
            ", 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;

include/io.hpp

@@ -0,0 +1,37 @@
+/**
+ * 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 _SQUIGGLES_IO_HPP_
+#define _SQUIGGLES_IO_HPP_
+
+#include <optional>
+#include <vector>
+
+#include "geometry/profilepoint.hpp"
+
+namespace squiggles {
+/**
+ * Writes the path data to a CSV file.
+ */
+int serialize_path(std::ostream& out, std::vector<ProfilePoint> path);
+
+/**
+ * Converts CSV data into a path.
+ */
+std::optional<std::vector<ProfilePoint>> deserialize_path(std::istream& in);
+
+/**
+ * Converts CSV data from the Pathfinder library's format to a Squiggles path.
+ *
+ * NOTE: this code is copied almost exactly from Jaci Brunning's Pathfinder
+ * library. The source for this code can be found at:
+ * https://github.com/JaciBrunning/Pathfinder/blob/master/Pathfinder-Core/src/io.c
+ */
+std::optional<std::vector<ProfilePoint>>
+deserialize_pathfinder_path(std::istream& left, std::istream& right);
+} // namespace squiggles
+
+#endif

include/math/utils.hpp

@@ -7,6 +7,8 @@
 #ifndef _MATH_UTILS_HPP_
 #define _MATH_UTILS_HPP_
 
+#include <cmath>
+
 namespace squiggles {
 /**
  * Returns the sign value of the given value.
@@ -17,6 +19,11 @@ namespace squiggles {
 template <class T> inline int sgn(T v) {
   return (v > T(0)) - (v < T(0));
 }
+
+inline bool
+nearly_equal(const double& a, const double& b, double epsilon = 1e-5) {
+  return std::fabs(a - b) < epsilon;
+}
 } // namespace squiggles
 
 #endif

include/squiggles.hpp

@@ -16,6 +16,7 @@
 #include "physicalmodel/tankmodel.hpp"
 
 #include "constraints.hpp"
+#include "io.hpp"
 #include "spline.hpp"
 
 #endif

src/io.cpp

@@ -0,0 +1,168 @@
+/**
+ * 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.
+ */
+#include <iostream>
+#include <sstream>
+#include <string.h>
+
+#include "io.hpp"
+
+namespace squiggles {
+int serialize_path(std::ostream& out, std::vector<ProfilePoint> path) {
+  if (!out || path.size() == 0) {
+    return -1;
+  }
+
+  out << "x, y, yaw, vel, accel, jerk, curvature, time, wheels\n";
+  for (auto p : path) {
+    out << p.to_csv() << "\n";
+  }
+  return 0;
+}
+
+std::optional<std::vector<ProfilePoint>> deserialize_path(std::istream& in) {
+  if (!in) {
+    std::cout << "File does not exist!" << std::endl;
+    return std::nullopt;
+  }
+
+  std::vector<ProfilePoint> path;
+  std::string line;
+  bool header_found = false;
+  while (std::getline(in, line)) {
+    if (!header_found) {
+      // skip the first line with the field declarations
+      header_found = true;
+      continue;
+    }
+    // vector contents are [x, y, yaw, vel, acc, jerk, curv, time, wheels...]
+    std::vector<double> contents;
+    std::string token;
+    std::istringstream line_stream(line);
+    while (std::getline(line_stream, token, ',')) {
+      contents.emplace_back(stod(token));
+    }
+
+    if (contents.size() < 8) {
+      std::cout << "Error parsing Squiggles path: malformed CSV content";
+      return std::nullopt;
+    }
+
+    double x = contents[0];
+    double y = contents[1];
+    double yaw = contents[2];
+    double vel = contents[3];
+    double acc = contents[4];
+    double jerk = contents[5];
+    double curv = contents[6];
+    double time = contents[7];
+    // put the remaining items into the wheel velocities vector
+    std::vector<double> wheels;
+    if (contents.size() > 8) {
+      wheels = std::vector<double>(contents.begin() + 8, contents.end());
+    }
+
+    path.emplace_back(ProfilePoint(
+      ControlVector(Pose(x, y, yaw), vel, acc, jerk), wheels, curv, time));
+  }
+  return path;
+}
+
+static std::optional<std::vector<ProfilePoint>>
+deserialize_pathfinder_segment(std::istream& in) {
+  if (!in) {
+    std::cout << "File does not exist!" << std::endl;
+    return std::nullopt;
+  }
+
+  std::vector<ProfilePoint> path;
+  std::string line;
+  bool header_found = false;
+  while (std::getline(in, line)) {
+    if (!header_found) {
+      // skip the first line with the field declarations
+      header_found = true;
+      continue;
+    }
+    // vector contents are [dt, x, y, pos, vel, acc, jerk, yaw]
+    std::vector<double> contents;
+    std::string token;
+    std::istringstream line_stream(line);
+    while (std::getline(line_stream, token, ',')) {
+      contents.emplace_back(stod(token));
+    }
+
+    if (contents.size() < 8) {
+      std::cout << "Error parsing Squiggles path: malformed CSV content";
+      return std::nullopt;
+    }
+
+    double dt = contents[0];
+    double x = contents[1];
+    double y = contents[2];
+    // ignoring the path position in position 3
+    double vel = contents[4];
+    double acc = contents[5];
+    double jerk = contents[6];
+    double yaw = contents[7];
+
+    path.emplace_back(
+      ProfilePoint(ControlVector(Pose(x, y, yaw), vel, acc, jerk), {}, 0, dt));
+  }
+  return path;
+}
+
+static Pose wheels_to_pose(Pose right, double track_width) {
+  auto new_x = right.x - (track_width / 2.0) * std::cos(right.yaw);
+  auto new_y = right.y + (track_width / 2.0) * std::sin(right.yaw);
+  return Pose(new_x, new_y, right.yaw);
+}
+
+static double wheel_vels_to_linear(double left, double right) {
+  return (left + right) / 2.0;
+}
+
+static double
+wheel_vels_to_curvature(double left, double right, double track_width) {
+  return (right - left) / track_width;
+}
+
+std::optional<std::vector<ProfilePoint>>
+deserialize_pathfinder_path(std::istream& left, std::istream& right) {
+  auto left_path = deserialize_pathfinder_segment(left);
+  auto right_path = deserialize_pathfinder_segment(right);
+
+  if (!left_path || !right_path) {
+    return std::nullopt;
+  }
+
+  auto track_width =
+    left_path->begin()->vector.pose.dist(right_path->begin()->vector.pose);
+
+  std::vector<ProfilePoint> path(left_path->size());
+  for (std::size_t i = 0; i < left_path->size(); ++i) {
+    auto l_v = left_path.value()[i].vector.vel;
+    auto l_a = left_path.value()[i].vector.accel;
+    auto l_j = left_path.value()[i].vector.jerk;
+    auto r_v = right_path.value()[i].vector.vel;
+    auto r_a = right_path.value()[i].vector.accel;
+    auto r_j = right_path.value()[i].vector.jerk;
+
+    auto pose = wheels_to_pose(right_path.value()[i].vector.pose, track_width);
+    auto vel = wheel_vels_to_linear(l_v, r_v);
+    auto accel = wheel_vels_to_linear(l_a, r_a);
+    auto jerk = wheel_vels_to_linear(l_j, r_j);
+    auto curvature = wheel_vels_to_curvature(l_v, r_v, track_width);
+
+    path[i] = ProfilePoint(ControlVector(pose, vel, accel, jerk),
+                           {l_v, r_v},
+                           curvature,
+                           left_path.value()[i].time);
+  }
+
+  return path;
+}
+} // namespace squiggles

tst/CMakeLists.txt

@@ -1,6 +1,13 @@
 set(BINARY ${CMAKE_PROJECT_NAME}_tst)
 
-file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false *.hpp *.cpp)
+# file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false *.hpp *.cpp)
+set(TEST_SOURCES 
+    impose-limits-test.cpp
+    io-test.cpp
+    main.cpp
+    model-constraints-test.cpp
+    plan-path-test.cpp
+    shared.hpp)
 
 include_directories(.)