Derive velocities from spline

src/camera/cv_camera.cc

@@ -33,7 +33,13 @@ CVCamera::CVCamera(const CameraConstant& c, std::optional<std::string> log_path)
   LOG(INFO) << c.pipeline.value();
 
   backup_image_ = cv::imread("/bos/constants/dont_worry_about_it.jpg");
-  if (c.frame_height.has_value() && c.frame_width.has_value()) {
+  if (backup_image_.empty()) {
+    LOG(WARNING) << "Backup image /bos/constants/dont_worry_about_it.jpg not "
+                    "found; using black placeholder";
+    int w = c.frame_width.value_or(640);
+    int h = c.frame_height.value_or(480);
+    backup_image_ = cv::Mat::zeros(h, w, CV_8UC3);
+  } else if (c.frame_height.has_value() && c.frame_width.has_value()) {
     cv::resize(backup_image_, backup_image_,
                cv::Size(c.frame_width.value(), c.frame_height.value()));
   }

src/pathing/CMakeLists.txt

@@ -1,2 +1,5 @@
-add_library(pathing pathfinding.cc splines.cc controller.cc)
+add_library(pathing pathfinding.cc splines.cc velocity_profile.cc controller.cc)
 target_link_libraries(pathing PUBLIC wpilibc wpiutil ntcore nlohmann_json::nlohmann_json utils localization ${OpenCV_LIBS})
+
+add_executable(pathing_simulator simulator.cc)
+target_link_libraries(pathing_simulator PRIVATE pathing utils)

src/pathing/controller.cc

@@ -1,29 +1,22 @@
 #include <networktables/BooleanTopic.h>
 #include <networktables/DoubleTopic.h>
-#include <networktables/NetworkTableInstance.h>
 #include <networktables/StructTopic.h>
-#include <algorithm>
 #include <chrono>
 #include <cmath>
-#include <fstream>
-#include <nlohmann/json.hpp>
-#include <thread>
 #include "splines.h"
 #include "src/localization/position_receiver.h"
-#include "src/pathing/splines.h"
 #include "src/utils/log.h"
+#include "src/utils/pch.h"
+#include "velocity_profile.h"
 
 namespace pathing {
 
 auto RunController(
     const std::string& navgrid_path = "/root/bos/constants/navgrid.json",
     bool verbose = false) -> void {
-  const int lookahead_offset_ = 50;
-  const double speed_ = 1.0;
 
   std::ifstream file(navgrid_path);
   if (!file.is_open()) {
-    LOG(FATAL) << "Failed to open navgrid.json" << std::endl;
     LOG(FATAL) << "Failed to open navgrid.json";
     return;
   }
@@ -53,17 +46,17 @@ auto RunController(
 
   auto vx_pub = inst.GetDoubleTopic("/pathing/vx").Publish();
   auto vy_pub = inst.GetDoubleTopic("/pathing/vy").Publish();
-  auto next_pose_sub =
-      inst.GetStructTopic<frc::Pose2d>("/pathing/nextPose").Publish();
-
-  std::vector<frc::Pose2d> spline_points;
+  SplineResult result;
+  std::vector<std::pair<double, double>> velocity_profile;
+  int prev_closest_idx = -1;
 
   while (true) {
     if (!enabled_sub.Get()) {
       vx_pub.Set(0.0);
       vy_pub.Set(0.0);
 
-      spline_points.clear();
+      result.points.clear();
+      velocity_profile.clear();
       std::this_thread::sleep_for(std::chrono::milliseconds(100));
       continue;
     }
@@ -90,25 +83,25 @@ auto RunController(
     if (current_pose.Translation().Distance(t2d).value() < nodeSizeMeters) {
       vx_pub.Set(0.0);
       vy_pub.Set(0.0);
-      spline_points.clear();
+      result.points.clear();
+      velocity_profile.clear();
       std::this_thread::sleep_for(std::chrono::milliseconds(100));
       continue;
     }
 
-    if (spline_points.empty()) {
-      std::vector<frc::Pose2d> new_spline =
-          createSpline(grid, start_pt, target_pt, nodeSizeMeters);
-      if (!new_spline.empty()) {
-        spline_points = new_spline;
+    if (result.points.empty()) {
+      result = CreateSpline(grid, start_pt, target_pt, nodeSizeMeters);
+      velocity_profile = CreateVelocityProfile(result);
+      if (!result.points.empty()) {
         if (verbose) {
-          for (const auto& p : spline_points) {
+          for (const auto& p : result.points) {
             LOG(INFO) << "spline pt: " << p.X().value() << " " << p.Y().value();
           }
         }
       }
     }
 
-    if (spline_points.empty()) {
+    if (result.points.empty() || velocity_profile.empty()) {
       vx_pub.Set(0.0);
       vy_pub.Set(0.0);
       std::this_thread::sleep_for(std::chrono::milliseconds(100));
@@ -119,10 +112,10 @@ auto RunController(
       int closest_idx = 0;
 
       // TODO: this can be optimizeed by only searching from the previous closest index instead of the entire spline
-      frc::Translation2d first2d(spline_points[0].X(), spline_points[0].Y());
+      frc::Translation2d first2d(result.points[0].X(), result.points[0].Y());
       double best_dist = current_pose.Translation().Distance(first2d).value();
-      for (int i = 1; i < (int)spline_points.size(); ++i) {
-        frc::Translation2d t2d(spline_points[i].X(), spline_points[i].Y());
+      for (int i = 1; i < (int)result.points.size(); ++i) {
+        frc::Translation2d t2d(result.points[i].X(), result.points[i].Y());
         double d = current_pose.Translation().Distance(t2d).value();
         if (d < best_dist) {
           best_dist = d;
@@ -135,44 +128,57 @@ auto RunController(
 
       if (verbose) {
         LOG(INFO) << "Closeset idx: " << closest_idx
-                  << " Spline size: " << spline_points.size();
+                  << " Spline size: " << result.points.size();
       }
 
-      int lookahead_idx = std::min(closest_idx + lookahead_offset_,
-                                   (int)spline_points.size() - 1);
-
-      frc::Pose2d lookahead = spline_points[lookahead_idx];
-
-      if (verbose) {
-        LOG(INFO) << "current " << current_pose.X().value() << " "
-                  << current_pose.Y().value();
-      }
-
-      double dx = lookahead.X().value() - current_pose.X().value();
-      double dy = lookahead.Y().value() - current_pose.Y().value();
-      if (verbose) {
-        LOG(INFO) << "dx " << dx << " vy " << dy;
-        LOG(INFO) << "looakhead " << lookahead.X().value() << " "
-                  << lookahead.Y().value();
+      if (prev_closest_idx >= 0 && closest_idx < prev_closest_idx) {
+        vx_pub.Set(0.0);
+        vy_pub.Set(0.0);
+        inst.Flush();
+        result.points.clear();
+        velocity_profile.clear();
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        continue;
       }
+      if (prev_closest_idx >= 0 && closest_idx == prev_closest_idx &&
+          closest_idx >= static_cast<int>(result.params.size()) - 5) {
+        double dx = target_pose.X().value() - current_pose.X().value();
+        double dy = target_pose.Y().value() - current_pose.Y().value();
+        double dist = std::hypot(dx, dy);
+
+        while (dist > 0.001) {
+          double vx = (dx / dist) * 1.0;
+          double vy = (dy / dist) * 1.0;
+
+          vx_pub.Set(vx);
+          vy_pub.Set(vy);
+          if (verbose) {
+            LOG(INFO) << "linear approach vx " << vx << " vy " << vy;
+          }
+          inst.Flush();
+          std::this_thread::sleep_for(std::chrono::milliseconds(20));
 
-      next_pose_sub.Set(lookahead);
-
-      double dist = std::hypot(dx, dy);
-      if (verbose) {
-        LOG(INFO) << "dist " << dist;
-      }
-      if (dist < 1e-6) {
+          current_pose = current_sub.Get();
+          dx = target_pose.X().value() - current_pose.X().value();
+          dy = target_pose.Y().value() - current_pose.Y().value();
+          dist = std::hypot(dx, dy);
+        }
         vx_pub.Set(0.0);
         vy_pub.Set(0.0);
+        inst.Flush();
+        result.points.clear();
+        velocity_profile.clear();
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
         continue;
       }
+      prev_closest_idx = closest_idx;
+
+      if (closest_idx >= static_cast<int>(result.params.size())) {
+        closest_idx = static_cast<int>(result.params.size()) - 1;
+      }
 
-      double vx = dx * speed_;
-      double vy = dy * speed_;
+      const auto [vx, vy] = velocity_profile[closest_idx];
 
-      // NOTE: we need to test whether to divide vx and vy by dist to normalize speeds or not,
       vx_pub.Set(vx);
       vy_pub.Set(vy);
       if (verbose) {

src/pathing/pathfinding.cc

@@ -9,13 +9,59 @@
 
 namespace pathing {
 
+auto BFSFirstFreeCell(std::vector<std::vector<Node>>& field, Point start_point)
+    -> Node {
+  int sx = start_point.x;
+  int sy = start_point.y;
+
+  field[sy][sx].visited = true;
+
+  std::deque<Point> queue;
+  queue.push_back(start_point);
+
+  std::vector<std::pair<int, int>> dirs = {{-1, -1}, {-1, 0}, {-1, 1}, {0, -1},
+                                           {0, 1},   {1, -1}, {1, 0},  {1, 1}};
+
+  while (!queue.empty()) {
+    const Point current_point = queue.front();
+    queue.pop_front();
+
+    Node& current = field[current_point.y][current_point.x];
+
+    for (auto [dy, dx] : dirs) {
+      int nx = current.x + dx;
+      int ny = current.y + dy;
+
+      if (nx >= 0 && nx < (int)field[0].size() && ny >= 0 &&
+          ny < (int)field.size()) {
+        if (!field[ny][nx].visited) {
+          field[ny][nx].visited = true;
+          if (!field[ny][nx].obstacle) {
+            return field[ny][nx];
+          }
+          queue.push_back({.x = (uint)nx, .y = (uint)ny});
+        }
+      }
+    }
+  }
+
+  return field[sy][sx];
+}
+
 auto BFS(std::vector<std::vector<Node>>& field, Point start_point,
          Point end_point) -> std::vector<Node> {
 
   int sx = start_point.x;
   int sy = start_point.y;
+  if (field[sy][sx].obstacle) {
+    Node adjusted = BFSFirstFreeCell(field, start_point);
+    sx = adjusted.x;
+    sy = adjusted.y;
+  }
 
   Node* start = &field[sy][sx];
+  start_point.x = sx;
+  start_point.y = sy;
   start->visited = true;
   start->cost = 0;
 
@@ -85,6 +131,9 @@ auto BFS(std::vector<std::vector<Node>>& field, Point start_point,
       field[rcurrent->y][rcurrent->x].path = true;
     }
   }
+  if (rcurrent != nullptr) {
+    rpath.push_back(*rcurrent);
+  }
   std::reverse(rpath.begin(), rpath.end());
 
   std::cout << "BFS path (" << start_point.x << "," << start_point.y << ") -> ("

src/pathing/pathfinding.h

@@ -2,14 +2,15 @@
 
 #include <sys/types.h>
 #include <cfloat>
+#include <cmath>
 #include <string>
 #include <vector>
 
 namespace pathing {
 
 struct Node {
   uint x, y;
-  double cost = DBL_MAX;
+  double cost = INFINITY;
   bool visited = false;
   bool obstacle = false;
   char readable;
@@ -25,6 +26,9 @@ struct Point {
   uint x, y;
 };
 
+auto BFSFirstFreeCell(std::vector<std::vector<Node>>& field, Point start_point)
+    -> Node;
+
 auto BFS(std::vector<std::vector<Node>>& field, Point start_point,
          Point end_point) -> std::vector<Node>;