Integrate main with the java side

src/CMakeLists.txt

@@ -28,6 +28,6 @@ add_executable(turret_bot_main turret_bot_main.cc)
 target_link_libraries(turret_bot_main PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils yolo gamepiece)
 
 add_executable(main_bot_main main_bot_main.cc)
-target_link_libraries(main_bot_main PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils)
+target_link_libraries(main_bot_main PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils controller)
 
 set(CMAKE_CXX_CLANG_TIDY "")

src/main_bot_main.cc

@@ -5,6 +5,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"
 
@@ -14,12 +15,16 @@ auto main() -> int {
   utils::StartNetworktables();
   // TODO configure vision bot camera paths
 
-  std::string log_path = frc::DataLogManager::GetLogDir();
+  // NT for enabling/disabling pathing controller
+  auto instance = nt::NetworkTableInstance::GetDefault();
+  auto table = instance.GetTable("Pathing");
+  auto enabled_entry = table->GetEntry("Enabled");
 
-  LOG(INFO) << "Starting cameras with right camera disabled";
+  LOG(INFO) << "Starting cameras";
   // camera::CameraSource front_camera = camera::CameraSource(
-  //     "Front", std::make_unique<camera::CVCamera>(
-  //                  camera_constants[Camera::MAIN_ROBOT_FRONT_CAMERA]));
+  //     "front",
+  //     std::make_unique<camera::CVCamera>(
+  //         camera::camera_constants[camera::Camera::MAIN_ROBOT_FRONT_CAMERA]));
 
   camera::CameraSource left_camera = camera::CameraSource(
       "Left", std::make_unique<camera::CVCamera>(
@@ -71,4 +76,35 @@ auto main() -> int {
   LOG(INFO) << "Started estimators";
 
   left_thread.join();
+
+  // Main control loop for pathing controller
+  std::unique_ptr<pathing::Controller> controller;
+  std::thread controller_thread;
+  bool last_enabled = false;
+
+  LOG(INFO) << "Entering main control loop";
+
+  while (true) {
+    bool enabled = enabled_entry.GetBoolean(false);
+
+    if (enabled && !last_enabled) {
+      LOG(INFO) << "Starting pathing controller";
+      controller = std::make_unique<pathing::Controller>();
+      controller_thread = std::thread([&controller]() { controller->Send(); });
+    } else if (!enabled && last_enabled) {
+      LOG(INFO) << "Stopping pathing controller";
+      if (controller) {
+        controller->Stop();
+        if (controller_thread.joinable()) {
+          controller_thread.join();
+        }
+        controller.reset();
+      }
+    }
+
+    last_enabled = enabled;
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+
+  std::this_thread::sleep_for(std::chrono::hours::max());
 }

src/pathing/CMakeLists.txt

@@ -1,2 +1,8 @@
 add_executable(simulator simulator.cc pathing.cc)
-target_link_libraries(simulator PRIVATE ${OpenCV_LIBS} nlohmann_json::nlohmann_json ntcore wpilibc wpimath)
+target_link_libraries(simulator PRIVATE ${OpenCV_LIBS} nlohmann_json::nlohmann_json ntcore wpilibc wpimath)
+
+add_library(controller controller.cc velocity_sender.cc pathing.cc)
+target_link_libraries(controller PRIVATE ${OpenCV_LIBS} nlohmann_json::nlohmann_json ntcore wpilibc wpimath)
+
+add_executable(pathing_controller_main controller_main.cc)
+target_link_libraries(pathing_controller_main PRIVATE controller ntcore wpilibc wpimath nlohmann_json::nlohmann_json ${OpenCV_LIBS})

src/pathing/controller.cc

@@ -0,0 +1,135 @@
+#include "src/pathing/controller.h"
+#include <chrono>
+#include <fstream>
+#include <nlohmann/json.hpp>
+#include <thread>
+#include "src/pathing/pathing.h"
+#include "src/pathing/velocity_sender.h"
+
+namespace pathing {
+
+Controller::Controller() : instance_(nt::NetworkTableInstance::GetDefault()) {
+
+  auto table = instance_.GetTable("Pathing");
+
+  current_pose_sub_ =
+      table->GetStructTopic<frc::Pose2d>("CurrentPose").Subscribe({});
+
+  target_pose_sub_ =
+      table->GetStructTopic<frc::Pose2d>("TargetPose").Subscribe({});
+}
+
+void Controller::Send() {
+
+  VelocitySender sender;
+
+  std::ifstream file("/root/bos/constants/navgrid.json");
+  nlohmann::json data = nlohmann::json::parse(file);
+
+  int W = data["grid"][0].size();
+  int H = data["grid"].size();
+  double nodeSize = data["nodeSizeMeters"];
+
+  std::vector<std::vector<bool>> gridData(H, std::vector<bool>(W));
+
+  for (int y = 0; y < H; y++)
+    for (int x = 0; x < W; x++)
+      gridData[y][x] = data["grid"][y][x];
+
+  cv::Mat grid = initializeGrid(gridData);
+
+  frc::Pose2d start = current_pose_sub_.Get();
+  frc::Pose2d target = target_pose_sub_.Get();
+
+  int sx = start.X().value() / nodeSize;
+  int sy = start.Y().value() / nodeSize;
+  int tx = target.X().value() / nodeSize;
+  int ty = target.Y().value() / nodeSize;
+
+  auto poses = createSpline(grid, sx, sy, tx, ty, nodeSize);
+
+  if (poses.empty()) {
+    sender.Send(0, 0);
+    running_.store(false);
+    return;
+  }
+
+  std::vector<double> dist(poses.size(), 0);
+
+  for (size_t i = 1; i < poses.size(); i++) {
+    double dx = poses[i].X().value() - poses[i - 1].X().value();
+    double dy = poses[i].Y().value() - poses[i - 1].Y().value();
+    dist[i] = dist[i - 1] + std::hypot(dx, dy);
+  }
+
+  double total = dist.back();
+
+  constexpr double maxV = 5.0;
+  constexpr double maxA = 3.0;
+  constexpr double lookahead = 0.5;
+  constexpr double tol = 0.1;
+
+  std::vector<double> speed(poses.size());
+
+  for (size_t i = 0; i < poses.size(); i++) {
+    double a = std::sqrt(2 * maxA * dist[i]);
+    double d = std::sqrt(2 * maxA * (total - dist[i]));
+    speed[i] = std::min({a, d, maxV});
+  }
+
+  size_t idx = 0;
+
+  while (running_) {
+
+    frc::Pose2d pose = current_pose_sub_.Get();
+    double rx = pose.X().value();
+    double ry = pose.Y().value();
+
+    double goalDist = std::hypot(poses.back().X().value() - rx,
+                                 poses.back().Y().value() - ry);
+
+    if (goalDist < tol) {
+      sender.Send(0, 0);
+      break;
+    }
+
+    for (size_t i = idx; i < poses.size(); i++) {
+      double d =
+          std::hypot(poses[i].X().value() - rx, poses[i].Y().value() - ry);
+      if (d >= lookahead) {
+        idx = i;
+        break;
+      }
+    }
+
+    if (idx >= poses.size())
+      idx = poses.size() - 1;
+
+    double txp = poses[idx].X().value();
+    double typ = poses[idx].Y().value();
+
+    double dx = txp - rx;
+    double dy = typ - ry;
+    double mag = std::hypot(dx, dy);
+
+    if (mag < 1e-6) {
+      sender.Send(0, 0);
+      if (!running_)
+        break;
+      continue;
+    }
+
+    double vx = dx / mag * speed[idx];
+    double vy = dy / mag * speed[idx];
+
+    sender.Send(vx, vy);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  }
+}
+
+void Controller::Stop() {
+  running_.store(false);
+}
+
+}  // namespace pathing

src/pathing/controller.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <frc/geometry/Pose2d.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StructTopic.h>
+#include <atomic>
+#include <cstdint>
+
+namespace pathing {
+class Controller {
+ public:
+  Controller();
+  void Send();
+  void Stop();
+
+ private:
+  nt::NetworkTableInstance instance_;
+  nt::StructSubscriber<frc::Pose2d> current_pose_sub_;
+  nt::StructSubscriber<frc::Pose2d> target_pose_sub_;
+};
+
+}  // namespace pathing

src/pathing/velocity_sender.cc

@@ -0,0 +1,26 @@
+#include "src/pathing/velocity_sender.h"
+#include <frc/Timer.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/StructTopic.h>
+
+namespace pathing {
+
+VelocitySender::VelocitySender()
+    : instance_(nt::NetworkTableInstance::GetDefault()) {
+  std::shared_ptr<nt::NetworkTable> table =
+      instance_.GetTable("Orin/OTFVelocity/");
+
+  nt::DoubleArrayTopic vel_topic = table->GetDoubleArrayTopic("Velocity");
+  vel_publisher_ = vel_topic.Publish();
+}
+
+void VelocitySender::Send(const double ax, const double ay) {
+  if (mutex_.try_lock()) {
+    double timestamp = frc::Timer::GetFPGATimestamp().to<double>() +
+                       instance_.GetServerTimeOffset().value_or(0) / 1000000.0;
+    std::vector<double> vel_array = {ax, ay, timestamp};
+    vel_publisher_.Set(vel_array);
+    mutex_.unlock();
+  }
+}
+}  // namespace pathing

src/pathing/velocity_sender.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <frc/geometry/Pose2d.h>
+#include <networktables/BooleanTopic.h>
+#include <networktables/DoubleArrayTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StructTopic.h>
+#include "src/utils/pch.h"
+
+namespace pathing {
+class VelocitySender {
+ public:
+  VelocitySender();
+  void Send(const double ax, const double ay);
+
+ private:
+  nt::NetworkTableInstance instance_;
+
+  nt::DoubleArrayPublisher vel_publisher_;
+
+  std::mutex mutex_;
+};
+}  // namespace pathing