updated pathplanner

src/main/cpp/RobotContainer.cpp

@@ -129,8 +129,7 @@ frc2::CommandPtr RobotContainer::IntakeNote() {
 }
 
 frc2::Command* RobotContainer::GetAutonomousCommand() {
-  // return autos.GetSelectedCommand();
-  return nullptr;
+  return autos.GetSelectedCommand();
 }
 
 SwerveSubsystem& RobotContainer::GetSwerveSubsystem() {

src/main/cpp/pathplanner/lib/auto/CommandUtil.cpp

@@ -53,8 +53,14 @@ frc2::CommandPtr CommandUtil::commandFromJson(const wpi::json &json,
 }
 
 frc2::CommandPtr CommandUtil::waitCommandFromJson(const wpi::json &json) {
-	auto waitTime = units::second_t(json.at("waitTime").get<double>());
-	return frc2::cmd::Wait(waitTime);
+	auto waitJson = json.at("waitTime");
+	if (waitJson.is_number()) {
+		return frc2::cmd::Wait(units::second_t { waitJson.get<double>() });
+	} else {
+		// Field is not a number, probably a choreo expression
+		return frc2::cmd::Wait(units::second_t {
+				waitJson.at("val").get<double>() });
+	}
 }
 
 frc2::CommandPtr CommandUtil::namedCommandFromJson(const wpi::json &json) {

src/main/cpp/pathplanner/lib/auto/NamedCommands.cpp

@@ -4,15 +4,19 @@
 
 using namespace pathplanner;
 
-std::unordered_map<std::string, std::shared_ptr<frc2::Command>> NamedCommands::namedCommands;
-
 frc2::CommandPtr NamedCommands::getCommand(std::string name) {
 	if (NamedCommands::hasCommand(name)) {
 		return CommandUtil::wrappedEventCommand(
-				NamedCommands::namedCommands.at(name));
+				NamedCommands::GetNamedCommands().at(name));
 	}
 	FRC_ReportError(frc::warn::Warning,
 			"PathPlanner attempted to create a command '{}' that has not been registered with NamedCommands::registerCommand",
 			name);
 	return frc2::cmd::None();
 }
+
+std::unordered_map<std::string, std::shared_ptr<frc2::Command>>& NamedCommands::GetNamedCommands() {
+	static std::unordered_map<std::string, std::shared_ptr<frc2::Command>> *namedCommands =
+			new std::unordered_map<std::string, std::shared_ptr<frc2::Command>>();
+	return *namedCommands;
+}

src/main/cpp/pathplanner/lib/commands/FollowPathCommand.cpp

@@ -1,37 +1,45 @@
 #include "pathplanner/lib/commands/FollowPathCommand.h"
+#include "pathplanner/lib/path/PathPlannerPath.h"
+#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h"
+#include "pathplanner/lib/commands/PathPlannerAuto.h"
 
 using namespace pathplanner;
 
 FollowPathCommand::FollowPathCommand(std::shared_ptr<PathPlannerPath> path,
 		std::function<frc::Pose2d()> poseSupplier,
 		std::function<frc::ChassisSpeeds()> speedsSupplier,
-		std::function<void(frc::ChassisSpeeds)> output,
-		std::unique_ptr<PathFollowingController> controller,
-		ReplanningConfig replanningConfig, std::function<bool()> shouldFlipPath,
+		std::function<void(frc::ChassisSpeeds, std::vector<DriveFeedforward>)> output,
+		std::shared_ptr<PathFollowingController> controller,
+		RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
 		frc2::Requirements requirements) : m_originalPath(path), m_poseSupplier(
 		poseSupplier), m_speedsSupplier(speedsSupplier), m_output(output), m_controller(
-		std::move(controller)), m_replanningConfig(replanningConfig), m_shouldFlipPath(
-		shouldFlipPath) {
+		controller), m_robotConfig(robotConfig), m_shouldFlipPath(
+		shouldFlipPath), m_eventScheduler() {
 	AddRequirements(requirements);
 
-	auto &&driveRequirements = GetRequirements();
+	auto driveRequirements = GetRequirements();
+	auto eventReqs = EventScheduler::getSchedulerRequirements(m_originalPath);
 
-	for (EventMarker &marker : m_originalPath->getEventMarkers()) {
-		auto reqs = marker.getCommand()->GetRequirements();
-
-		for (auto &&requirement : reqs) {
-			if (driveRequirements.find(requirement)
-					!= driveRequirements.end()) {
-				throw FRC_MakeError(frc::err::CommandIllegalUse,
-						"Events that are triggered during path following cannot require the drive subsystem");
-			}
+	for (auto requirement : eventReqs) {
+		if (driveRequirements.find(requirement) != driveRequirements.end()) {
+			throw FRC_MakeError(frc::err::CommandIllegalUse,
+					"Events that are triggered during path following cannot require the drive subsystem");
 		}
+	}
 
-		AddRequirements(reqs);
+	AddRequirements(eventReqs);
+
+	m_path = m_originalPath;
+	// Ensure the ideal trajectory is generated
+	auto idealTraj = m_path->getIdealTrajectory(m_robotConfig);
+	if (idealTraj.has_value()) {
+		m_trajectory = idealTraj.value();
 	}
 }
 
 void FollowPathCommand::Initialize() {
+	PathPlannerAuto::currentPathName = m_originalPath->name;
+
 	if (m_shouldFlipPath() && !m_originalPath->preventFlipping) {
 		m_path = m_originalPath->flipPath();
 	} else {
@@ -43,68 +51,53 @@ void FollowPathCommand::Initialize() {
 
 	m_controller->reset(currentPose, currentSpeeds);
 
-	frc::ChassisSpeeds fieldSpeeds =
-			frc::ChassisSpeeds::FromRobotRelativeSpeeds(currentSpeeds,
-					currentPose.Rotation());
-	frc::Rotation2d currentHeading = frc::Rotation2d(fieldSpeeds.vx(),
-			fieldSpeeds.vy());
-	frc::Rotation2d targetHeading = (m_path->getPoint(1).position
-			- m_path->getPoint(0).position).Angle();
-	frc::Rotation2d headingError = currentHeading - targetHeading;
-	bool onHeading = units::math::hypot(currentSpeeds.vx, currentSpeeds.vy)
-			< 0.25_mps || units::math::abs(headingError.Degrees()) < 30_deg;
-
-	if (!m_path->isChoreoPath() && m_replanningConfig.enableInitialReplanning
-			&& (currentPose.Translation().Distance(m_path->getPoint(0).position)
-					> 0.25_m || !onHeading)) {
-		replanPath(currentPose, currentSpeeds);
+	auto linearVel = units::math::hypot(currentSpeeds.vx, currentSpeeds.vy);
+
+	if (m_path->getIdealStartingState().has_value()) {
+		// Check if we match the ideal starting state
+		bool idealVelocity = units::math::abs(
+				linearVel
+						- m_path->getIdealStartingState().value().getVelocity())
+				<= 0.25_mps;
+		bool idealRotation =
+				!m_robotConfig.isHolonomic
+						|| units::math::abs(
+								(currentPose.Rotation()
+										- m_path->getIdealStartingState().value().getRotation()).Degrees())
+								<= 30_deg;
+		if (idealVelocity && idealRotation) {
+			// We can use the ideal trajectory
+			m_trajectory = m_path->getIdealTrajectory(m_robotConfig).value();
+		} else {
+			// We need to regenerate
+			m_trajectory = m_path->generateTrajectory(currentSpeeds,
+					currentPose.Rotation(), m_robotConfig);
+		}
 	} else {
-		m_generatedTrajectory = m_path->getTrajectory(currentSpeeds,
-				currentPose.Rotation());
-		PathPlannerLogging::logActivePath (m_path);
-		PPLibTelemetry::setCurrentPath(m_path);
+		// No ideal starting state, generate the trajectory
+		m_trajectory = m_path->generateTrajectory(currentSpeeds,
+				currentPose.Rotation(), m_robotConfig);
 	}
 
-	// Initialize marker stuff
-	m_currentEventCommands.clear();
-	m_untriggeredEvents.clear();
-
-	const auto &eventCommands = m_generatedTrajectory.getEventCommands();
+	PathPlannerLogging::logActivePath (m_path);
+	PPLibTelemetry::setCurrentPath(m_path);
 
-	m_untriggeredEvents.insert(m_untriggeredEvents.end(), eventCommands.begin(),
-			eventCommands.end());
+	m_eventScheduler.initialize(m_trajectory);
 
 	m_timer.Reset();
 	m_timer.Start();
 }
 
 void FollowPathCommand::Execute() {
 	units::second_t currentTime = m_timer.Get();
-	PathPlannerTrajectory::State targetState = m_generatedTrajectory.sample(
-			currentTime);
-	if (m_controller->isHolonomic()) {
+	PathPlannerTrajectoryState targetState = m_trajectory.sample(currentTime);
+	if (!m_controller->isHolonomic() && m_path->isReversed()) {
 		targetState = targetState.reverse();
 	}
 
 	frc::Pose2d currentPose = m_poseSupplier();
 	frc::ChassisSpeeds currentSpeeds = m_speedsSupplier();
 
-	if (!m_path->isChoreoPath() && m_replanningConfig.enableDynamicReplanning) {
-		units::meter_t previousError = units::math::abs(
-				m_controller->getPositionalError());
-		units::meter_t currentError = currentPose.Translation().Distance(
-				targetState.position);
-
-		if (currentError
-				>= m_replanningConfig.dynamicReplanningTotalErrorThreshold
-				|| currentError - previousError
-						>= m_replanningConfig.dynamicReplanningErrorSpikeThreshold) {
-			replanPath(currentPose, currentSpeeds);
-			m_timer.Reset();
-			targetState = m_generatedTrajectory.sample(0_s);
-		}
-	}
-
 	units::meters_per_second_t currentVel = units::math::hypot(currentSpeeds.vx,
 			currentSpeeds.vy);
 
@@ -115,76 +108,36 @@ void FollowPathCommand::Execute() {
 	PPLibTelemetry::setCurrentPose(currentPose);
 	PathPlannerLogging::logCurrentPose(currentPose);
 
-	if (m_controller->isHolonomic()) {
-		PPLibTelemetry::setTargetPose(targetState.getTargetHolonomicPose());
-		PathPlannerLogging::logTargetPose(targetState.getTargetHolonomicPose());
-	} else {
-		PPLibTelemetry::setTargetPose(targetState.getDifferentialPose());
-		PathPlannerLogging::logTargetPose(targetState.getDifferentialPose());
-	}
+	PPLibTelemetry::setTargetPose(targetState.pose);
+	PathPlannerLogging::logTargetPose(targetState.pose);
 
-	PPLibTelemetry::setVelocities(currentVel, targetState.velocity,
+	PPLibTelemetry::setVelocities(currentVel, targetState.linearVelocity,
 			currentSpeeds.omega, targetSpeeds.omega);
-	PPLibTelemetry::setPathInaccuracy(m_controller->getPositionalError());
-
-	m_output(targetSpeeds);
-
-	if (!m_untriggeredEvents.empty()
-			&& m_timer.HasElapsed(m_untriggeredEvents[0].first)) {
-		// Time to trigger this event command
-		auto event = m_untriggeredEvents[0];
 
-		for (std::pair<std::shared_ptr<frc2::Command>, bool> &runningCommand : m_currentEventCommands) {
-			if (!runningCommand.second) {
-				continue;
-			}
+	m_output(targetSpeeds, targetState.feedforwards);
 
-			if (!frc2::RequirementsDisjoint(runningCommand.first.get(),
-					event.second.get())) {
-				runningCommand.first->End(true);
-				runningCommand.second = false;
-			}
-		}
-
-		event.second->Initialize();
-		m_currentEventCommands.emplace_back(event.second, true);
-
-		m_untriggeredEvents.pop_front();
-	}
-
-	// Run event marker commands
-	for (std::pair<std::shared_ptr<frc2::Command>, bool> &runningCommand : m_currentEventCommands) {
-		if (!runningCommand.second) {
-			continue;
-		}
-
-		runningCommand.first->Execute();
-		if (runningCommand.first->IsFinished()) {
-			runningCommand.first->End(false);
-			runningCommand.second = false;
-		}
-	}
+	m_eventScheduler.execute(currentTime);
 }
 
 bool FollowPathCommand::IsFinished() {
-	return m_timer.HasElapsed(m_generatedTrajectory.getTotalTime());
+	return m_timer.HasElapsed(m_trajectory.getTotalTime());
 }
 
 void FollowPathCommand::End(bool interrupted) {
 	m_timer.Stop();
+	PathPlannerAuto::currentPathName = "";
 
 	// Only output 0 speeds when ending a path that is supposed to stop, this allows interrupting
 	// the command to smoothly transition into some auto-alignment routine
 	if (!interrupted && m_path->getGoalEndState().getVelocity() < 0.1_mps) {
-		m_output(frc::ChassisSpeeds());
+		std::vector < DriveFeedforward > ff;
+		for (size_t m = 0; m < m_robotConfig.numModules; m++) {
+			ff.emplace_back(DriveFeedforward { });
+		}
+		m_output(frc::ChassisSpeeds(), ff);
 	}
 
 	PathPlannerLogging::logActivePath(nullptr);
 
-	// End markers
-	for (std::pair<std::shared_ptr<frc2::Command>, bool> &runningCommand : m_currentEventCommands) {
-		if (runningCommand.second) {
-			runningCommand.first->End(true);
-		}
-	}
+	m_eventScheduler.end();
 }