Integrate main with the java side by nanoticity · Pull Request #110 · frc971/bos

nanoticity · 2026-02-28T00:10:59Z

cherry picking from the other pr, that one kinda got cooked

Signed-off-by: Nano <nanoticity@gmail.com>

Copilot

Pull request overview

This PR integrates the C++ pathing controller with the robot's main process (main_bot_main.cc), enabling on-the-fly velocity sending to the Java side via NetworkTables. It introduces a new Controller class and a VelocitySender class in the pathing subsystem, along with a standalone binary and a library target in CMake.

Changes:

Added pathing::Controller and pathing::VelocitySender classes to compute and publish velocity commands over NetworkTables.
Integrated the pathing controller into main_bot_main.cc via a poll loop that starts/stops the controller based on a NetworkTables Enabled flag.
Updated CMakeLists.txt files to build the new controller library and pathing_controller_main executable.

Reviewed changes

Copilot reviewed 8 out of 8 changed files in this pull request and generated 14 comments.

Show a summary per file

File	Description
`src/pathing/velocity_sender.h`	Declares `VelocitySender` class to publish velocity arrays over NT
`src/pathing/velocity_sender.cc`	Implements `VelocitySender` constructor and `Send()` method
`src/pathing/controller.h`	Declares `Controller` class with NT subscribers and `running_` flag
`src/pathing/controller.cc`	Implements path-following loop reading from NT and sending velocities
`src/pathing/controller_main.cc`	Standalone binary that runs the same enable/disable loop as `main_bot_main.cc`
`src/pathing/CMakeLists.txt`	Adds `controller` library and `pathing_controller_main` executable targets
`src/main_bot_main.cc`	Embeds the pathing controller enable/disable loop into the main bot process
`src/CMakeLists.txt`	Links `controller` library into `main_bot_main`

src/main_bot_main.cc

src/pathing/velocity_sender.cc

src/pathing/controller_main.cc

src/pathing/controller.h

src/pathing/velocity_sender.cc

src/main_bot_main.cc

src/pathing/velocity_sender.cc

src/pathing/controller.cc

Copilot

Pull request overview

Copilot reviewed 7 out of 7 changed files in this pull request and generated 9 comments.

Copilot · 2026-02-28T00:57:36Z

src/pathing/controller.h

+ private:
+  nt::NetworkTableInstance instance_;
+  nt::StructSubscriber<frc::Pose2d> current_pose_sub_;
+  nt::StructSubscriber<frc::Pose2d> target_pose_sub_;


running_ is used in controller.cc (lines 53, 82, 117, 132) as an std::atomic member but it is never declared in the Controller class definition in controller.h. This will cause a compilation failure. An std::atomic<bool> running_{true}; member must be added to the private section of the Controller class.

Suggested change

nt::StructSubscriber<frc::Pose2d> target_pose_sub_;

nt::StructSubscriber<frc::Pose2d> target_pose_sub_;

std::atomic<bool> running_{true}; // Controls the running state of the controller loop

Copilot · 2026-02-28T00:57:36Z

src/main_bot_main.cc

@@ -71,4 +76,35 @@ auto main() -> int {
  LOG(INFO) << "Started estimators";

  left_thread.join();


right_thread (created before this block) is never joined or detached, while only left_thread is joined at line 78. This means if the program ever terminates abnormally, the destructor of a joinable std::thread will call std::terminate. The thread should be either joined (alongside left_thread) or detached.

Suggested change

left_thread.join();

left_thread.join();

right_thread.join(); // Ensure right_thread is also joined before destruction

Copilot · 2026-02-28T00:57:36Z

src/main_bot_main.cc

+
+  std::this_thread::sleep_for(std::chrono::hours::max());


The line std::this_thread::sleep_for(std::chrono::hours::max()); at line 109 is unreachable code because the while(true) loop above it at line 87 never exits (no break exists in the loop). This dead code should be removed.

Suggested change

std::this_thread::sleep_for(std::chrono::hours::max());

Copilot · 2026-02-28T00:57:37Z

src/pathing/velocity_sender.cc

+  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();
+  }


VelocitySender::Send uses mutex_.try_lock() which silently drops velocity commands if the mutex is already held. Since Send() is called only from the single Controller::Send() thread in the current design, there is no other concurrent caller to contend with, making the non-blocking try_lock unnecessary. More importantly, if the lock genuinely can't be acquired, velocity commands are silently lost — the robot will continue at its last commanded velocity, which is a safety concern. Using a blocking std::lock_guard<std::mutex> (as done in the analogous PositionSender::Send) would be safer and more consistent with the rest of the codebase.

Copilot · 2026-02-28T00:57:37Z

src/pathing/velocity_sender.h

+#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>


velocity_sender.h includes several headers that are not used in the class itself: <frc/geometry/Pose2d.h>, <networktables/BooleanTopic.h>, <networktables/DoubleTopic.h>, <networktables/NetworkTable.h>, and <networktables/StructTopic.h>. Only <networktables/DoubleArrayTopic.h>, <networktables/NetworkTableInstance.h>, and <mutex> (from pch.h) are actually needed. Unused includes increase compile times and create unnecessary coupling.

Suggested change

#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 <networktables/DoubleArrayTopic.h>

#include <networktables/NetworkTableInstance.h>

Copilot · 2026-02-28T00:57:37Z

src/pathing/CMakeLists.txt

+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})


controller_main.cc is referenced as a source file for the pathing_controller_main executable but this file does not exist in the repository. The build will fail when CMake tries to compile this target.

Suggested change

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})

# The pathing_controller_main target was removed because controller_main.cc does not exist in the repository.

Copilot · 2026-02-28T00:57:37Z

src/main_bot_main.cc

  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) {


The pathing control loop (starting at line 87) is placed after left_thread.join() at line 78. Since RunLocalization runs an infinite loop (the localization threads never terminate normally), left_thread.join() will block forever and the pathing control loop will never execute. The pathing logic should be started before joining threads, or the join should be restructured (e.g., using detach() or running the control loop in its own thread).

Copilot · 2026-02-28T00:57:37Z

src/pathing/controller.cc

+    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;
+      }
+    }


The lookahead point search at lines 96–103 only advances idx when a pose is at least lookahead distance away, but it never resets idx when a new Send() call is made (it's a local size_t idx = 0 initialized once per Send() call, which is fine). However, the inner for loop can fail to find any pose beyond lookahead distance (e.g., when the robot is already past all nearby poses), in which case idx is unchanged from its last value. The guard if (idx >= poses.size()) idx = poses.size() - 1; on line 105 only catches an out-of-bounds index, not a stale idx pointing to an already-passed waypoint. This could cause the controller to continually target a waypoint behind the robot, creating oscillation. Consider advancing idx to the closest not-yet-visited waypoint as a fallback.

Copilot · 2026-02-28T00:57:38Z

src/pathing/controller.h

+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StructTopic.h>
+#include <atomic>
+#include <cstdint>


<cstdint> is included in controller.h but is not used anywhere in either controller.h or controller.cc. This unused include should be removed.

Suggested change

#include <cstdint>

nanoticity added 3 commits February 28, 2026 00:02

Make the timestamping system cleaner, integrate with java code

9a7c6af

Signed-off-by: Nano <nanoticity@gmail.com>

Integrate into main so that pathing runs whever main gets ran

4111749

Add velocity_cender.h

778a0fc

Copilot AI review requested due to automatic review settings February 28, 2026 00:11

Copilot started reviewing on behalf of nanoticity February 28, 2026 00:11 View session

Copilot AI reviewed Feb 28, 2026

View reviewed changes

Copilot comments fixes

38d190f

nanoticity requested a review from Copilot February 28, 2026 00:53

Copilot started reviewing on behalf of nanoticity February 28, 2026 00:54 View session

Copilot AI reviewed Feb 28, 2026

View reviewed changes

	nt::StructSubscriber<frc::Pose2d> target_pose_sub_;
	nt::StructSubscriber<frc::Pose2d> target_pose_sub_;
	std::atomic<bool> running_{true}; // Controls the running state of the controller loop

		@@ -71,4 +76,35 @@ auto main() -> int {
		LOG(INFO) << "Started estimators";

		left_thread.join();

	left_thread.join();
	left_thread.join();
	right_thread.join(); // Ensure right_thread is also joined before destruction

		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})

	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})
	# The pathing_controller_main target was removed because controller_main.cc does not exist in the repository.

Conversation

nanoticity commented Feb 28, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Uh oh!

Copilot AI Feb 28, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 28, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 28, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 28, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 28, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 28, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 28, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 28, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 28, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants