Skip to content

Commit

Permalink
report error instead of throwing when AutoBuilder is configured multi…
Browse files Browse the repository at this point in the history 
…ple times
  • Loading branch information
@mjansen4857
mjansen4857 committed Jan 24, 2024
1 parent f652244 commit 6803be2
Show file tree
Hide file tree
Showing 3 changed files with 30 additions and 35 deletions.
10 changes: 5 additions & 5 deletions pathplannerlib-python/pathplannerlib/auto.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
PathfindThenFollowPathLTV
from .geometry_util import flipFieldPose
import os
from wpilib import getDeployDirectory
from wpilib import getDeployDirectory, reportError
import json
from commands2.command import Command
from commands2.subsystem import Subsystem
Expand Down Expand Up @@ -172,7 +172,7 @@ def configureHolonomic(pose_supplier: Callable[[], Pose2d], reset_pose: Callable
:param drive_subsystem: the subsystem for the robot's drive
"""
if AutoBuilder._configured:
raise RuntimeError('AutoBuilder has already been configured. Please only configure AutoBuilder once')
reportError('AutoBuilder has already been configured. This is likely in error.', True)

AutoBuilder._pathFollowingCommandBuilder = lambda path: FollowPathHolonomic(
path,
Expand Down Expand Up @@ -233,7 +233,7 @@ def configureRamsete(pose_supplier: Callable[[], Pose2d], reset_pose: Callable[[
:param drive_subsystem: the subsystem for the robot's drive
"""
if AutoBuilder._configured:
raise RuntimeError('AutoBuilder has already been configured. Please only configure AutoBuilder once')
reportError('AutoBuilder has already been configured. This is likely in error.', True)

AutoBuilder._pathFollowingCommandBuilder = lambda path: FollowPathRamsete(
path,
Expand Down Expand Up @@ -296,7 +296,7 @@ def configureLTV(pose_supplier: Callable[[], Pose2d], reset_pose: Callable[[Pose
:param drive_subsystem: the subsystem for the robot's drive
"""
if AutoBuilder._configured:
raise RuntimeError('AutoBuilder has already been configured. Please only configure AutoBuilder once')
reportError('AutoBuilder has already been configured. This is likely in error.', True)

AutoBuilder._pathFollowingCommandBuilder = lambda path: FollowPathLTV(
path,
Expand Down Expand Up @@ -351,7 +351,7 @@ def configureCustom(path_following_command_builder: Callable[[PathPlannerPath],
:param reset_pose: a consumer for resetting the robot's pose
"""
if AutoBuilder._configured:
raise RuntimeError('AutoBuilder has already been configured. Please only configure AutoBuilder once')
reportError('AutoBuilder has already been configured. This is likely in error.', True)

AutoBuilder._pathFollowingCommandBuilder = path_following_command_builder
AutoBuilder._getPose = pose_supplier
Expand Down
31 changes: 13 additions & 18 deletions pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
Expand Down Expand Up @@ -60,7 +61,6 @@ public class AutoBuilder {
* @param shouldFlipPath Supplier that determines if paths should be flipped to the other side of
* the field. This will maintain a global blue alliance origin.
* @param driveSubsystem the subsystem for the robot's drive
* @throws AutoBuilderException if AutoBuilder has already been configured
*/
public static void configureHolonomic(
Supplier<Pose2d> poseSupplier,
Expand All @@ -71,8 +71,8 @@ public static void configureHolonomic(
BooleanSupplier shouldFlipPath,
Subsystem driveSubsystem) {
if (configured) {
throw new AutoBuilderException(
"Auto builder has already been configured. Please only configure auto builder once");
DriverStation.reportError(
"Auto builder has already been configured. This is likely in error.", true);
}

AutoBuilder.pathFollowingCommandBuilder =
Expand Down Expand Up @@ -128,7 +128,6 @@ public static void configureHolonomic(
* @param shouldFlipPath Supplier that determines if paths should be flipped to the other side of
* the field. This will maintain a global blue alliance origin.
* @param driveSubsystem the subsystem for the robot's drive
* @throws AutoBuilderException if AutoBuilder has already been configured
*/
public static void configureRamsete(
Supplier<Pose2d> poseSupplier,
Expand All @@ -139,8 +138,8 @@ public static void configureRamsete(
BooleanSupplier shouldFlipPath,
Subsystem driveSubsystem) {
if (configured) {
throw new AutoBuilderException(
"Auto builder has already been configured. Please only configure auto builder once");
DriverStation.reportError(
"Auto builder has already been configured. This is likely in error.", true);
}

AutoBuilder.pathFollowingCommandBuilder =
Expand Down Expand Up @@ -198,7 +197,6 @@ public static void configureRamsete(
* @param shouldFlipPath Supplier that determines if paths should be flipped to the other side of
* the field. This will maintain a global blue alliance origin.
* @param driveSubsystem the subsystem for the robot's drive
* @throws AutoBuilderException if AutoBuilder has already been configured
*/
public static void configureRamsete(
Supplier<Pose2d> poseSupplier,
Expand All @@ -211,8 +209,8 @@ public static void configureRamsete(
BooleanSupplier shouldFlipPath,
Subsystem driveSubsystem) {
if (configured) {
throw new AutoBuilderException(
"Auto builder has already been configured. Please only configure auto builder once");
DriverStation.reportError(
"Auto builder has already been configured. This is likely in error.", true);
}

AutoBuilder.pathFollowingCommandBuilder =
Expand Down Expand Up @@ -274,7 +272,6 @@ public static void configureRamsete(
* @param shouldFlipPath Supplier that determines if paths should be flipped to the other side of
* the field. This will maintain a global blue alliance origin.
* @param driveSubsystem the subsystem for the robot's drive
* @throws AutoBuilderException if AutoBuilder has already been configured
*/
public static void configureLTV(
Supplier<Pose2d> poseSupplier,
Expand All @@ -286,8 +283,8 @@ public static void configureLTV(
BooleanSupplier shouldFlipPath,
Subsystem driveSubsystem) {
if (configured) {
throw new AutoBuilderException(
"Auto builder has already been configured. Please only configure auto builder once");
DriverStation.reportError(
"Auto builder has already been configured. This is likely in error.", true);
}

AutoBuilder.pathFollowingCommandBuilder =
Expand Down Expand Up @@ -348,7 +345,6 @@ public static void configureLTV(
* @param shouldFlipPath Supplier that determines if paths should be flipped to the other side of
* the field. This will maintain a global blue alliance origin.
* @param driveSubsystem the subsystem for the robot's drive
* @throws AutoBuilderException if AutoBuilder has already been configured
*/
public static void configureLTV(
Supplier<Pose2d> poseSupplier,
Expand All @@ -362,8 +358,8 @@ public static void configureLTV(
BooleanSupplier shouldFlipPath,
Subsystem driveSubsystem) {
if (configured) {
throw new AutoBuilderException(
"Auto builder has already been configured. Please only configure auto builder once");
DriverStation.reportError(
"Auto builder has already been configured. This is likely in error.", true);
}

AutoBuilder.pathFollowingCommandBuilder =
Expand Down Expand Up @@ -423,15 +419,14 @@ public static void configureLTV(
* @param pathFollowingCommandBuilder a function that builds a command to follow a given path
* @param poseSupplier a supplier for the robot's current pose
* @param resetPose a consumer for resetting the robot's pose
* @throws AutoBuilderException if AutoBuilder has already been configured
*/
public static void configureCustom(
Function<PathPlannerPath, Command> pathFollowingCommandBuilder,
Supplier<Pose2d> poseSupplier,
Consumer<Pose2d> resetPose) {
if (configured) {
throw new AutoBuilderException(
"Auto builder has already been configured. Please only configure auto builder once");
DriverStation.reportError(
"Auto builder has already been configured. This is likely in error.", true);
}

AutoBuilder.pathFollowingCommandBuilder = pathFollowingCommandBuilder;
Expand Down
24 changes: 12 additions & 12 deletions pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/AutoBuilder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,8 @@ void AutoBuilder::configureHolonomic(std::function<frc::Pose2d()> poseSupplier,
HolonomicPathFollowerConfig config,
std::function<bool()> shouldFlipPath, frc2::Subsystem *driveSubsystem) {
if (m_configured) {
throw std::runtime_error(
"Auto builder has already been configured. Please only configure auto builder once");
FRC_ReportError(frc::err::Error,
"Auto builder has already been configured. This is likely in error.");
}

AutoBuilder::m_pathFollowingCommandBuilder = [poseSupplier,
Expand Down Expand Up @@ -85,8 +85,8 @@ void AutoBuilder::configureRamsete(std::function<frc::Pose2d()> poseSupplier,
ReplanningConfig replanningConfig, std::function<bool()> shouldFlipPath,
frc2::Subsystem *driveSubsystem) {
if (m_configured) {
throw std::runtime_error(
"Auto builder has already been configured. Please only configure auto builder once");
FRC_ReportError(frc::err::Error,
"Auto builder has already been configured. This is likely in error.");
}

AutoBuilder::m_pathFollowingCommandBuilder = [poseSupplier, speedsSupplier,
Expand Down Expand Up @@ -128,8 +128,8 @@ void AutoBuilder::configureRamsete(std::function<frc::Pose2d()> poseSupplier,
ReplanningConfig replanningConfig, std::function<bool()> shouldFlipPath,
frc2::Subsystem *driveSubsystem) {
if (m_configured) {
throw std::runtime_error(
"Auto builder has already been configured. Please only configure auto builder once");
FRC_ReportError(frc::err::Error,
"Auto builder has already been configured. This is likely in error.");
}

AutoBuilder::m_pathFollowingCommandBuilder =
Expand Down Expand Up @@ -172,8 +172,8 @@ void AutoBuilder::configureLTV(std::function<frc::Pose2d()> poseSupplier,
units::second_t dt, ReplanningConfig replanningConfig,
std::function<bool()> shouldFlipPath, frc2::Subsystem *driveSubsystem) {
if (m_configured) {
throw std::runtime_error(
"Auto builder has already been configured. Please only configure auto builder once");
FRC_ReportError(frc::err::Error,
"Auto builder has already been configured. This is likely in error.");
}

AutoBuilder::m_pathFollowingCommandBuilder =
Expand Down Expand Up @@ -217,8 +217,8 @@ void AutoBuilder::configureLTV(std::function<frc::Pose2d()> poseSupplier,
ReplanningConfig replanningConfig, std::function<bool()> shouldFlipPath,
frc2::Subsystem *driveSubsystem) {
if (m_configured) {
throw std::runtime_error(
"Auto builder has already been configured. Please only configure auto builder once");
FRC_ReportError(frc::err::Error,
"Auto builder has already been configured. This is likely in error.");
}

AutoBuilder::m_pathFollowingCommandBuilder = [poseSupplier, speedsSupplier,
Expand Down Expand Up @@ -256,8 +256,8 @@ void AutoBuilder::configureCustom(
std::function<frc::Pose2d()> poseSupplier,
std::function<void(frc::Pose2d)> resetPose) {
if (m_configured) {
throw std::runtime_error(
"Auto builder has already been configured. Please only configure auto builder once");
FRC_ReportError(frc::err::Error,
"Auto builder has already been configured. This is likely in error.");
}

AutoBuilder::m_pathFollowingCommandBuilder = pathFollowingCommandBuilder;
Expand Down

0 comments on commit 6803be2

Please sign in to comment.