Pathing rewrite

src/CMakeLists.txt

@@ -22,10 +22,10 @@ add_subdirectory(test)
 add_subdirectory(pathing)
 
 add_executable(main_bot_main main_bot_main.cc)
-target_link_libraries(main_bot_main PRIVATE camera localization utils)
+target_link_libraries(main_bot_main PRIVATE camera localization utils pathing)
 
 add_executable(second_bot_main second_bot_main.cc)
-target_link_libraries(second_bot_main PRIVATE camera localization utils)
+target_link_libraries(second_bot_main PRIVATE camera localization utils pathing)
 
 add_executable(unambiguous_second unambiguous_second.cc)
 target_link_libraries(unambiguous_second PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils)

src/localization/position_receiver.cc

@@ -1,17 +1,29 @@
 #include "src/localization/position_receiver.h"
+#include <frc/geometry/Pose3d.h>
+#include <networktables/DoubleArrayTopic.h>
+#include <units/length.h>
 
 namespace localization {
 
 PositionReceiver::PositionReceiver() {
   auto instance = nt::NetworkTableInstance::GetDefault();
-  std::shared_ptr<nt::NetworkTable> table = instance.GetTable("");
-  nt::StructTopic<frc::Pose3d> pose3d_topic =
-      table->GetStructTopic<frc::Pose3d>("Pose3d");
-  pose3d_subscriber_ = pose3d_topic.Subscribe({});
+  // auto pose_table = instance.GetTable("/Orin/PoseEstimate/main_bot_left");
+  // nt::StructTopic<frc::Pose3d> pose3d_topic =
+  //     pose_table->GetStructTopic<frc::Pose3d>("Pose3d");
+  // pose3d_subscriber_ = pose3d_topic.Subscribe({});
+
+  auto pose_table = instance.GetTable("/Pose");
+  nt::DoubleArrayTopic pose3d_topic =
+      pose_table->GetDoubleArrayTopic("robotPose");
+  auto pose3d_subscriber_ = pose3d_topic.Subscribe({});
 }
 
-auto PositionReceiver::Get() -> frc::Pose3d {
-  return pose3d_subscriber_.Get();
+auto PositionReceiver::Get() -> frc::Pose2d {
+  frc::Pose2d pose(units::meter_t{pose3d_subscriber_.Get()[0]},
+                   units::meter_t{pose3d_subscriber_.Get()[1]},
+                   frc::Rotation2d());
+
+  return pose;
 }
 
-}  // namespace localization
+}  // namespace localization

src/localization/position_receiver.h

@@ -1,6 +1,8 @@
 #pragma once
 
 #include <frc/geometry/Pose3d.h>
+#include <networktables/DoubleArrayTopic.h>
+#include <networktables/DoubleTopic.h>
 #include <networktables/StructTopic.h>
 #include "src/utils/pch.h"
 
@@ -9,11 +11,11 @@ namespace localization {
 class PositionReceiver {
  public:
   PositionReceiver();
-  auto Get() -> frc::Pose3d;
+  auto Get() -> frc::Pose2d;
 
  private:
-  nt::StructSubscriber<frc::Pose3d> pose3d_subscriber_;
+  nt::DoubleArraySubscriber pose3d_subscriber_;
 
   std::mutex mutex_;
 };
-}  // namespace localization
+}  // namespace localization

src/main_bot_main.cc

@@ -7,6 +7,7 @@
 #include "src/localization/opencv_apriltag_detector.h"
 #include "src/localization/run_localization.h"
 #include "src/localization/square_solver.h"
+#include "src/pathing/controller.h"
 #include "src/utils/camera_utils.h"
 #include "src/utils/nt_utils.h"
 
@@ -25,7 +26,6 @@ auto main() -> int {
         "Front", std::make_unique<camera::CVCamera>(
                      camera_constants.at("main_bot_front")));
     cv::Mat front_camera_frame = front_camera->GetFrame();
-
     std::vector<std::unique_ptr<localization::IPositionSender>> front_sender;
     front_sender.emplace_back(
         std::make_unique<localization::NetworkTableSender>(
@@ -95,6 +95,13 @@ auto main() -> int {
 
   LOG(INFO) << "Started estimators";
 
+  std::thread pathing_thread(pathing::RunController,
+                             "/bos/constants/navgrid.json");
+
+  LOG(INFO) << "pathing started";
+
   // TODO find better way
   left_thread.join();
+  right_thread.join();
+  pathing_thread.join();
 }

src/pathing/CMakeLists.txt

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

src/pathing/bfs_tester.cc

@@ -0,0 +1,105 @@
+#include <algorithm>
+#include <iostream>
+#include <string>
+#include "nlohmann/json.hpp"
+#include "src/pathing/pathfinding.h"
+#include "src/pathing/splines.h"
+#include "src/utils/log.h"
+#include "src/utils/pch.h"
+
+auto main(int argc, char* argv[]) -> int {
+
+  std::vector<double> result = pathing::knot_vector(15, 3);
+  for (double res : result) {
+    std::cout << res << std::endl;
+  }
+  std::flush(std::cout);
+  std::vector<std::string> argstrings(&argv[1], &argv[argc]);
+
+  std::ifstream file("/root/bos/constants/navgrid.json");
+  if (!file.is_open()) {
+    LOG(FATAL) << "Failed to open navgrid.json" << std::endl;
+    return 0;
+  }
+
+  nlohmann::json data = nlohmann::json::parse(file);
+  file.close();
+
+  const int GRID_H = data["grid"].size();
+  const int GRID_W = data["grid"][0].size();
+  double nodeSizeMeters = data["nodeSizeMeters"];
+  LOG(INFO) << nodeSizeMeters;
+
+  std::vector<std::vector<pathing::Node>> field(
+      GRID_H, std::vector<pathing::Node>(GRID_W));
+  for (int y = 0; y < GRID_H; ++y) {
+    for (int x = 0; x < GRID_W; ++x) {
+      field[y][x].x = x;
+      field[y][x].y = y;
+      field[y][x].obstacle = !data["grid"][y][x];
+    }
+  }
+
+  cv::namedWindow("Pathing", cv::WINDOW_AUTOSIZE);
+  cv::Mat mat(GRID_W, GRID_H, CV_8UC3);
+
+  while (true) {
+
+    for (int y = 0; y < GRID_H; ++y) {
+      for (int x = 0; x < GRID_W; ++x) {
+        pathing::Node n = {.x = x, .y = y};
+        n.obstacle = data["grid"][y][x];
+        n.readble = '-';
+        if (n.obstacle) {
+          n.readble = '#';
+        }
+        field[y][x] = n;
+      }
+    }
+
+    double vision_estimate_x = 2.21643;
+    double vision_estimate_y = 5.21322;
+
+    pathing::Point start = {.x = (int)(vision_estimate_x / nodeSizeMeters),
+                            .y = (int)(vision_estimate_y / nodeSizeMeters)};
+    pathing::Point end = {.x = 22, .y = 14};
+    std::vector<pathing::Node> result = pathing::BFS(field, start, end);
+    if (std::count(argstrings.begin(), argstrings.end(), "-v")) {
+      std::string readable = "";
+      for (pathing::Node n : result) {
+        std::string intermediate = "";
+        intermediate.append("(");
+        intermediate.append(std::to_string(n.x));
+        intermediate.append(", ");
+        intermediate.append(std::to_string(n.y));
+        intermediate.append("), ");
+        readable.append(intermediate);
+        field[n.y][n.x].readble = '*';
+      }
+      std::cout << readable << std::endl;
+
+      for (int y = 0; y < GRID_H; ++y) {
+        for (int x = 0; x < GRID_W; ++x) {
+          std::cout << field[y][x].readble;
+        }
+        std::cout << std::endl;
+      }
+    }
+    for (int y = 0; y < 15; y++) {
+      for (int x = 0; x < 15; x++) {
+        mat.at<cv::Vec3b>(y, x) = field[y][x].obstacle
+                                      ? cv::Vec3b(0, 0, 0)
+                                      : cv::Vec3b(255, 255, 255);
+
+        if (field[y][x].path) {
+          mat.at<cv::Vec3b>(y, x) = cv::Vec3b(0, 255, 0);
+        }
+      }
+    }
+
+    cv::Mat display;
+    // cv::resize(mat, display, cv::Size(450, 450), 0, 0, cv::INTER_NEAREST);
+    // cv::imshow("Grid", display);
+    // cv::waitKey();
+  }
+}

src/pathing/controller.cc

@@ -0,0 +1,146 @@
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StructTopic.h>
+#include <networktables/BooleanTopic.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"
+
+namespace pathing {
+
+auto RunController(const std::string& navgrid_path =
+                       "/root/bos/constants/navgrid.json") -> void {
+  const int lookahead_offset_ = 2;
+
+  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;
+  }
+
+  nlohmann::json data = nlohmann::json::parse(file);
+  file.close();
+
+  const int GRID_H = data["grid"].size();
+  const int GRID_W = data["grid"][0].size();
+  double nodeSizeMeters = data["nodeSizeMeters"];
+
+  std::vector<std::vector<pathing::Node>> grid(
+    GRID_H, std::vector<pathing::Node>(GRID_W));
+  for (int y = 0; y < GRID_H; ++y) {
+    for (int x = 0; x < GRID_W; ++x) {
+      grid[y][x].x = x;
+      grid[y][x].y = y;
+      grid[y][x].obstacle = data["grid"][y][x];
+    }
+  }
+  nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
+  auto current_sub =
+      inst.GetStructTopic<frc::Pose3d>("/Orin/PoseEstimate/main_bot_left/Pose3d")
+          .Subscribe({});
+  auto target_sub =
+      inst.GetStructTopic<frc::Pose2d>("/pathing/target").Subscribe({});
+
+  auto enabled_sub = inst.GetBooleanTopic("/pathing/enabled").Subscribe(false);
+
+  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;
+
+  while (true) {
+    if (!enabled_sub.Get()) {
+      vx_pub.Set(0.0);
+      vy_pub.Set(0.0);
+      spline_points.clear();
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      continue;
+    }
+    frc::Pose3d current_pose = current_sub.Get();
+    frc::Pose2d target_pose = target_sub.Get();
+
+    Point start_pt{
+        .x = (int)(current_pose.X().value() / nodeSizeMeters),
+        .y = (int)(current_pose.Y().value() / nodeSizeMeters)};
+    Point target_pt{
+        .x = (int)(target_pose.X().value() / nodeSizeMeters),
+        .y = (int)(target_pose.Y().value() / nodeSizeMeters)};
+    start_pt.x = std::clamp(start_pt.x, 0, GRID_W - 1);
+    start_pt.y = std::clamp(st  art_pt.y, 0, GRID_H - 1);
+    target_pt.x = std::clamp(target_pt.x, 0, GRID_W - 1);
+    target_pt.y = std::clamp(target_pt.y, 0, GRID_H - 1);
+
+    frc::Translation3d t3d(target_pose.X(), target_pose.Y(), 0_m);
+    if (current_pose.Translation().Distance(t3d).value() < nodeSizeMeters) {
+      vx_pub.Set(0.0);
+      vy_pub.Set(0.0);
+      spline_points.clear();
+      std::this_thread::sleep_for(std::chrono::milliseconds(20));
+      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 (spline_points.empty()) {
+      vx_pub.Set(0.0);
+      vy_pub.Set(0.0);
+      std::this_thread::sleep_for(std::chrono::milliseconds(20));
+      continue;
+    }
+
+    int closest_idx = 0;
+    {
+      frc::Translation3d first3d(spline_points[0].X(), spline_points[0].Y(), 0_m);
+      double best_dist = current_pose.Translation().Distance(first3d).value();
+      for (int i = 1; i < (int)spline_points.size(); ++i) {
+        frc::Translation3d p3d(spline_points[i].X(), spline_points[i].Y(), 0_m);
+        double d = current_pose.Translation().Distance(p3d).value();
+        if (d < best_dist) {
+          best_dist = d;
+          closest_idx = i;
+        }
+      }
+    }
+
+    int lookahead_idx =
+        std::min(closest_idx + lookahead_offset_, (int)spline_points.size() - 1);
+
+    frc::Pose2d lookahead = spline_points[lookahead_idx];
+    double dx = lookahead.X().value() - current_pose.X().value();
+    double dy = lookahead.Y().value() - current_pose.Y().value();
+
+    next_pose_sub.Set(lookahead);
+
+    double dist = std::sqrt(dx * dx + dy * dy);
+    if (dist < 1e-6) {
+      vx_pub.Set(0.0);
+      vy_pub.Set(0.0);
+      std::this_thread::sleep_for(std::chrono::milliseconds(20));
+      continue;
+    }
+
+    double vx = (dx / dist);
+    double vy = (dy / dist);
+
+    vx_pub.Set(vx);
+    vy_pub.Set(vy);
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  }
+}
+
+}  // namespace pathing

src/pathing/controller.h

@@ -0,0 +1,8 @@
+#include <string>
+namespace pathing {
+
+#pragma once
+
+auto RunController(const std::string& navgrid_path) -> void;
+
+}  // namespace pathing