diff --git a/Pathfinding/grids.json b/Pathfinding/grids.json index fce25cb..ac02045 100644 --- a/Pathfinding/grids.json +++ b/Pathfinding/grids.json @@ -10,6 +10,22 @@ { "type": "SHAPE", "shapes": [ + { + "type": "RECTANGLE", + "x": 6.0, + "y": 3.0, + "width": 10.0, + "height": 1.0, + "rotation": 0.0, + "inverted": false + }, + { + "inverted": false, + "type": "CIRCLE", + "x": 4.0, + "y": 4.0, + "radius": 2.5 + }, { "type": "RECTANGLE", "x": 8.2296, @@ -22,4 +38,4 @@ ] } ] -} +} \ No newline at end of file diff --git a/Pathfinding/src/main/java/com/swrobotics/pathfinding/core/grid/Grid.java b/Pathfinding/src/main/java/com/swrobotics/pathfinding/core/grid/Grid.java index 110c55f..e25641d 100644 --- a/Pathfinding/src/main/java/com/swrobotics/pathfinding/core/grid/Grid.java +++ b/Pathfinding/src/main/java/com/swrobotics/pathfinding/core/grid/Grid.java @@ -185,6 +185,8 @@ public boolean canEdgePass(Point p1, Point p2) { protected void invalidateLineOfSightCache() { if (sightCache != null) sightCache.invalidate(); + if (parent != null) + parent.invalidateLineOfSightCache(); } public boolean lineOfSight(Point a, Point b) { diff --git a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java index 7481e57..d828b8c 100644 --- a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java +++ b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.swrobotics.messenger.client.MessengerClient; import com.swrobotics.robot.commands.DefaultDriveCommand; import com.swrobotics.robot.control.ControlBoard; +import com.swrobotics.robot.logging.FieldView; import com.swrobotics.robot.subsystems.swerve.SwerveDrive; import com.swrobotics.taskmanager.filesystem.FileSystemAPI; @@ -53,7 +54,7 @@ public RobotContainer() { // FIXME: Update at kickoff - drive = new SwerveDrive(FieldInfo.CHARGED_UP_2023); + drive = new SwerveDrive(FieldInfo.CHARGED_UP_2023, messenger); controlboard = new ControlBoard(this); drive.setDefaultCommand(new DefaultDriveCommand(drive, controlboard)); @@ -65,6 +66,8 @@ public RobotContainer() { autoSelector.addDefaultOption("Drive forward", new PrintCommand("FIXME!!")); autoSelector.addOption("Example other auto", new PrintCommand("Example Auto")); + + FieldView.publish(); } public Command getAutonomousCommand() { diff --git a/Robot/src/main/java/com/swrobotics/robot/logging/FieldView.java b/Robot/src/main/java/com/swrobotics/robot/logging/FieldView.java new file mode 100644 index 0000000..fbd16ff --- /dev/null +++ b/Robot/src/main/java/com/swrobotics/robot/logging/FieldView.java @@ -0,0 +1,19 @@ +package com.swrobotics.robot.logging; + +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public final class FieldView { + private static final Field2d field = new Field2d(); + + public static final FieldObject2d robotPose = field.getRobotObject(); + public static final FieldObject2d visionEstimates = field.getObject("Vision estimates"); + public static final FieldObject2d aprilTagPoses = field.getObject("AprilTag poses"); + public static final FieldObject2d pathPlannerPath = field.getObject("PathPlanner path"); + public static final FieldObject2d pathPlannerSetpoint = field.getObject("PathPlanner setpoint"); + + public static void publish() { + SmartDashboard.putData("Field View", field); + } +} diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java index 0077530..7f2b2c7 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java @@ -1,5 +1,7 @@ package com.swrobotics.robot.subsystems.swerve; +import com.swrobotics.messenger.client.MessengerClient; +import com.swrobotics.robot.logging.FieldView; import org.littletonrobotics.junction.Logger; import com.kauailabs.navx.frc.AHRS; @@ -47,7 +49,7 @@ public final class SwerveDrive extends SubsystemBase { private SwerveModulePosition[] prevPositions; private Rotation2d prevGyroAngle; - public SwerveDrive(FieldInfo fieldInfo) { + public SwerveDrive(FieldInfo fieldInfo, MessengerClient msg) { gyro = new AHRS(SPI.Port.kMXP); modules = new SwerveModule[INFOS.length]; @@ -82,15 +84,18 @@ public SwerveDrive(FieldInfo fieldInfo) { new PIDConstants(8.0), new PIDConstants(4.0, 0.0), minMax, Math.hypot(HALF_SPACING, HALF_SPACING), new ReplanningConfig(), 0.020), this); - Pathfinding.setPathfinder(new LocalADStar()); // TODO: Theta* +// Pathfinding.setPathfinder(new LocalADStar()); // TODO: Theta* + Pathfinding.setPathfinder(new ThetaStarPathfinder(msg)); PathPlannerLogging.setLogActivePathCallback( (activePath) -> { Logger.recordOutput( - "Drive/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + "Drive/Trajectory", activePath.toArray(new Pose2d[0])); + FieldView.pathPlannerPath.setPoses(activePath); }); PathPlannerLogging.setLogTargetPoseCallback( (targetPose) -> { Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + FieldView.pathPlannerSetpoint.setPose(targetPose); }); } @@ -119,7 +124,9 @@ public SwerveModulePosition[] getCurrentModulePositions() { } // TODO: Better way of selecting between manual/auto input + // TODO: Split some of this into periodic(), since drive is not guaranteed to be called every time public void drive(ChassisSpeeds robotRelSpeeds) { + System.out.println("Driving: " + robotRelSpeeds); robotRelSpeeds = ChassisSpeeds.discretize(robotRelSpeeds, 0.020); SwerveModuleState[] targetStates = kinematics.getStates(robotRelSpeeds); SwerveModulePosition[] positions = new SwerveModulePosition[modules.length]; diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java index 46eb28c..dd6934d 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java @@ -2,6 +2,7 @@ import com.swrobotics.lib.field.FieldInfo; import com.swrobotics.mathlib.MathUtil; +import com.swrobotics.robot.logging.FieldView; import com.swrobotics.robot.subsystems.tagtracker.TagTrackerInput; import com.swrobotics.robot.utils.GeometryUtil; import edu.wpi.first.math.Matrix; @@ -11,9 +12,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; import java.util.List; @@ -30,10 +28,6 @@ public final class SwerveEstimator { private final TreeMap updates = new TreeMap<>(); private final Matrix q; - private final FieldObject2d robotPoseObj; - private final FieldObject2d visionPosesObj; - private final FieldObject2d tagPosesObj; - public SwerveEstimator(FieldInfo field) { tagTracker = new TagTrackerInput( field, @@ -48,12 +42,6 @@ public SwerveEstimator(FieldInfo field) { for (int i = 0; i < 3; i++) { q.set(i, 0, MathUtil.square(STATE_STD_DEVS[i])); } - - Field2d debugField = new Field2d(); - robotPoseObj = debugField.getObject("Robot"); - visionPosesObj = debugField.getObject("Vision estimates"); - tagPosesObj = debugField.getObject("AprilTag poses"); - SmartDashboard.putData("Localization", debugField); } public Pose2d getEstimatedPose() { @@ -76,7 +64,7 @@ public void update(Twist2d driveTwist) { for (Pose3d tagPose3d : tagTracker.getEnvironment().getAllPoses()) { tagPoses.add(tagPose3d.toPose2d()); } - tagPosesObj.setPoses(tagPoses); + FieldView.aprilTagPoses.setPoses(tagPoses); for (TagTrackerInput.VisionUpdate visionUpdate : visionData) { double timestamp = visionUpdate.timestamp; @@ -125,14 +113,14 @@ private void update() { } // Debug field stuffs - robotPoseObj.setPose(latestPose); + FieldView.robotPose.setPose(latestPose); List visionPoses = new ArrayList<>(); for (PoseUpdate update : updates.values()) { for (TagTrackerInput.VisionUpdate visionUpdate : update.visionUpdates) { visionPoses.add(visionUpdate.estPose); } } - visionPosesObj.setPoses(visionPoses); + FieldView.visionEstimates.setPoses(visionPoses); } private int compareStdDevs(TagTrackerInput.VisionUpdate u1, TagTrackerInput.VisionUpdate u2) { diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/ThetaStarPathfinder.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/ThetaStarPathfinder.java new file mode 100644 index 0000000..1c621de --- /dev/null +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/ThetaStarPathfinder.java @@ -0,0 +1,154 @@ +package com.swrobotics.robot.subsystems.swerve; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.pathfinding.Pathfinder; +import com.swrobotics.messenger.client.MessageReader; +import com.swrobotics.messenger.client.MessengerClient; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; + +import java.util.ArrayList; +import java.util.List; + +public final class ThetaStarPathfinder implements Pathfinder { + private static final String MSG_SET_POS = "Pathfinder:SetPos"; + private static final String MSG_SET_GOAL = "Pathfinder:SetGoal"; + private static final String MSG_PATH = "Pathfinder:Path"; + + private static final double CORRECT_TARGET_TOL = 0.1524 + 0.1; + + // Directly taken from PPLib + private static final double SMOOTHING_ANCHOR_PCT = 0.8; + private static final double SMOOTHING_CONTROL_PCT = 0.33; + + private final MessengerClient msg; + + private Translation2d start, goal; + + private boolean newPathAvail; + private List pathBezier; + + public ThetaStarPathfinder(MessengerClient msg) { + this.msg = msg; + msg.addHandler(MSG_PATH, this::onPath); + + newPathAvail = false; + start = goal = new Translation2d(); + } + + private void onPath(String type, MessageReader reader) { + System.out.println("Path received"); + boolean pathValid = reader.readBoolean(); + if (!pathValid) { + // FIXME: If this happens, the path following command never ends + // this effectively disables the drive base until robot is disabled + System.out.println("It was invalid :("); + return; + } + + int count = reader.readInt(); + if (count < 2) { + System.out.println("Not enough points???"); + return; + } + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < count; i++) { + double x = reader.readDouble(); + double y = reader.readDouble(); + + pathPoints.add(new Translation2d(x, y)); + } + System.out.println("Path points: " + pathPoints); + + // Check if path is to correct target + // In case of latency returning path for previous target + Translation2d lastPoint = pathPoints.get(pathPoints.size() - 1); + if (lastPoint.minus(goal).getNorm() > CORRECT_TARGET_TOL) { + System.out.println("Wrong target... old path? want to go to " + goal); + return; + } + + // Replace start and end with actual points + pathPoints.set(0, start); + pathPoints.set(pathPoints.size() - 1, goal); + + // Calculate Bezier points + // Taken from PPLib LocalADStar#extractPath() + pathBezier = new ArrayList<>(); + pathBezier.add(pathPoints.get(0)); + pathBezier.add(pathPoints + .get(1) + .minus(pathPoints.get(0)) + .times(SMOOTHING_CONTROL_PCT) + .plus(pathPoints.get(0))); + for (int i = 1; i < pathPoints.size() - 1; i++) { + Translation2d last = pathPoints.get(i - 1); + Translation2d current = pathPoints.get(i); + Translation2d next = pathPoints.get(i + 1); + + Translation2d anchor1 = current.minus(last).times(SMOOTHING_ANCHOR_PCT).plus(last); + Translation2d anchor2 = current.minus(next).times(SMOOTHING_ANCHOR_PCT).plus(next); + + double controlDist = anchor1.getDistance(anchor2) * SMOOTHING_CONTROL_PCT; + + Translation2d prevControl1 = last.minus(anchor1).times(SMOOTHING_CONTROL_PCT).plus(anchor1); + Translation2d nextControl1 = + new Translation2d(controlDist, anchor1.minus(prevControl1).getAngle()).plus(anchor1); + + Translation2d prevControl2 = + new Translation2d(controlDist, anchor2.minus(next).getAngle()).plus(anchor2); + Translation2d nextControl2 = next.minus(anchor2).times(SMOOTHING_CONTROL_PCT).plus(anchor2); + + pathBezier.add(prevControl1); + pathBezier.add(anchor1); + pathBezier.add(nextControl1); + + pathBezier.add(prevControl2); + pathBezier.add(anchor2); + pathBezier.add(nextControl2); + } + pathBezier.add( + pathPoints + .get(pathPoints.size() - 2) + .minus(pathPoints.get(pathPoints.size() - 1)) + .times(SMOOTHING_CONTROL_PCT) + .plus(pathPoints.get(pathPoints.size() - 1))); + pathBezier.add(pathPoints.get(pathPoints.size() - 1)); + + newPathAvail = true; + System.out.println("Bezierified into " + pathBezier); + } + + @Override + public boolean isNewPathAvailable() { + return newPathAvail; + } + + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!newPathAvail) + return null; + newPathAvail = false; + return new PathPlannerPath(pathBezier, constraints, goalEndState); + } + + @Override + public void setStartPosition(Translation2d startPosition) { + start = startPosition; + msg.prepare(MSG_SET_POS).addDouble(startPosition.getX()).addDouble(startPosition.getY()).send(); + } + + @Override + public void setGoalPosition(Translation2d goalPosition) { + goal = goalPosition; + msg.prepare(MSG_SET_GOAL).addDouble(goalPosition.getX()).addDouble(goalPosition.getY()).send(); + } + + @Override + public void setDynamicObstacles(List> obs, Translation2d currentRobotPos) { + // Not supported (yet) + } +} diff --git a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/FieldViewTool.java b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/FieldViewTool.java index 8ef1a11..bf438d5 100644 --- a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/FieldViewTool.java +++ b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/FieldViewTool.java @@ -105,7 +105,7 @@ public FieldViewTool(ShuffleLog log, SmartDashboard smartDashboard, NetworkTable layers = new ArrayList<>(); layers.add(new MeterGridLayer()); // TODO-Kickoff: Field vector layer 2024 - layers.add(new Field2dLayer(smartDashboard)); +// layers.add(new Field2dLayer(smartDashboard)); // FIXME: For some reason this causes ClassNotFoundException layers.add(new PathfindingLayer(msg, this)); TagTrackerLayer tagTrackerLayer = new TagTrackerLayer(); layers.add(tagTrackerLayer);