From 1270672cb7cf42317a9745d15d480b5287395822 Mon Sep 17 00:00:00 2001 From: jasondaming Date: Mon, 15 Feb 2021 21:53:41 -0600 Subject: [PATCH 1/4] adding WaypointUtil. linting isn't working --- .../first/wpilibj/waypoint/WaypointUtil.java | 152 ++++++++++++++++++ 1 file changed, 152 insertions(+) create mode 100644 wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java new file mode 100644 index 00000000000..03b9993dbaa --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java @@ -0,0 +1,152 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.waypoint; + +import edu.wpi.first.pathweaver.path.Path; +import edu.wpi.first.pathweaver.path.wpilib.WpilibPath; +import org.apache.commons.csv.CSVFormat; +import org.apache.commons.csv.CSVParser; +import org.apache.commons.csv.CSVPrinter; +import org.apache.commons.csv.CSVRecord; + +import java.io.BufferedWriter; +import java.io.IOException; +import java.io.Reader; +import java.nio.file.Files; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.logging.Level; +import java.util.logging.Logger; + +import javafx.geometry.Point2D; + +public final class WaypointUtil { + private static final Logger LOGGER = Logger.getLogger(WaypointUtil.class.getName()); + + private WaypointUtil() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Exports path object to csv file. + * + * @param fileLocation the directory and filename to write to + * @param path Path object to save + * + * @return true if successful file write was preformed + */ + public static boolean export(String fileLocation, Path path, Double yoffset) { + try ( + BufferedWriter writer = Files.newBufferedWriter(Paths.get(fileLocation + path.getPathName())); + + CSVPrinter csvPrinter = new CSVPrinter(writer, CSVFormat.DEFAULT + .withHeader("X", "Y", "Tangent X", "Tangent Y", "Fixed Theta", "Reversed", "Name")) + ) { + for (Waypoint wp : path.getWaypoints()) { + double xPos = wp.getX(); + double yPos = wp.getY() + yoffset; + double tangentX = wp.getTangentX(); + double tangentY = wp.getTangentY(); + String name = wp.getName(); + csvPrinter.printRecord(xPos, yPos, tangentX, tangentY, wp.isLockTangent(), wp.isReversed(), name); + } + csvPrinter.flush(); + } catch (IOException except) { + LOGGER.log(Level.WARNING, "Could not save Path file", except); + return false; + } + return true; + } + + /** + * Imports Path object from disk. + * + * @param fileLocation Folder with path file + * @param fileName Name of path file + * + * @return Path object saved in Path file + */ + public static Path importPath(String fileLocation, String fileName) { + try(Reader reader = Files.newBufferedReader(java.nio.file.Path.of(fileLocation, fileName)); + CSVParser csvParser = new CSVParser(reader, CSVFormat.DEFAULT + .withFirstRecordAsHeader() + .withIgnoreHeaderCase() + .withTrim())) { + ArrayList waypoints = new ArrayList<>(); + for (CSVRecord csvRecord : csvParser) { + Point2D position = new Point2D( + Double.parseDouble(csvRecord.get("X")), + Double.parseDouble(csvRecord.get("Y")) + ); + Point2D tangent = new Point2D( + Double.parseDouble(csvRecord.get("Tangent X")), + Double.parseDouble(csvRecord.get("Tangent Y")) + ); + boolean locked = Boolean.parseBoolean(csvRecord.get("Fixed Theta")); + boolean reversed = Boolean.parseBoolean(csvRecord.get("Reversed")); + Waypoint point = new Waypoint(position, tangent, locked, reversed); + if (csvRecord.isMapped("Name")) { + String name = csvRecord.get("Name"); + point.setName(name); + } + waypoints.add(point); + } + return new WpilibPath(waypoints, fileName); + } catch (IOException except) { + LOGGER.log(Level.WARNING, "Could not read Path file", except); + return null; + } + } + + /** + * Imports Path object and creates a trajectory with that info. + * + * @param fileLocation Folder with path file + * @param fileName Name of path file + * + * @return Trajectory created from the waypoints in the file. + */ + public static Trajectory importPathToTrajectory(String fileLocation, String fileName, TrajectoryConfig config) { + try(Reader reader = Files.newBufferedReader(java.nio.file.Path.of(fileLocation, fileName)); + CSVParser csvParser = new CSVParser(reader, CSVFormat.DEFAULT + .withFirstRecordAsHeader() + .withIgnoreHeaderCase() + .withTrim())) { + int loop = 0; + Pose2d start = new Pose2d(); + Pose2d end = new Pose2d(); + var interiorWaypoints = new ArrayList(); + for (CSVRecord csvRecord : csvParser) { + if (loop == 0) { + start = createPoseWaypoint(csvRecord); + } + else if (loop == csvParser.Length - 1) { + end = createPoseWaypoint(csvRecord); + } + else { + interiorWaypoints.add(createTranslationWaypoint(csvRecord)); + } + config.setReversed(Boolean.parseBoolean(csvRecord.get("Reversed"))); + } + return TrajectoryGenerator.generateTrajectory( + start, + interiorWaypoints, + end, + config); + } catch (IOException except) { + LOGGER.log(Level.WARNING, "Could not read Path file", except); + return null; + } + } + + private static Pose2d createPoseWaypoint(CSVRecord csvRecord) { + return new Pose2d(new Translation2d(Double.parseDouble(csvRecord.get("X")), Double.parseDouble(csvRecord.get("Y"))), + new Rotation2d(Double.parseDouble(csvRecord.get("Tangent X")), Double.parseDouble(csvRecord.get("Tangent Y")))); + } + + private static Translation2d createTranslationWaypoint(CSVRecord csvRecord) { + return new Translation2d(Double.parseDouble(csvRecord.get("X")), Double.parseDouble(csvRecord.get("Y"))); + } +} From e928ad3e89b0a202c00c5e5e4f6b9baffd01090b Mon Sep 17 00:00:00 2001 From: jasondaming Date: Tue, 16 Feb 2021 19:20:47 -0600 Subject: [PATCH 2/4] read in the csv old school --- .../first/wpilibj/waypoint/WaypointUtil.java | 184 ++++++++---------- 1 file changed, 83 insertions(+), 101 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java index 03b9993dbaa..1b70a4b5d27 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java @@ -4,23 +4,22 @@ package edu.wpi.first.wpilibj.waypoint; -import edu.wpi.first.pathweaver.path.Path; -import edu.wpi.first.pathweaver.path.wpilib.WpilibPath; -import org.apache.commons.csv.CSVFormat; -import org.apache.commons.csv.CSVParser; -import org.apache.commons.csv.CSVPrinter; -import org.apache.commons.csv.CSVRecord; - -import java.io.BufferedWriter; +import java.io.BufferedReader; +import java.io.File; +import java.io.FileReader; import java.io.IOException; -import java.io.Reader; -import java.nio.file.Files; -import java.nio.file.Paths; import java.util.ArrayList; import java.util.logging.Level; import java.util.logging.Logger; -import javafx.geometry.Point2D; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.spline.Spline.ControlVector; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator.ControlVectorList; public final class WaypointUtil { private static final Logger LOGGER = Logger.getLogger(WaypointUtil.class.getName()); @@ -30,70 +29,49 @@ private WaypointUtil() { } /** - * Exports path object to csv file. + * Imports Path object and creates a trajectory with that info. * - * @param fileLocation the directory and filename to write to - * @param path Path object to save + * @param filepath Full path to the file + * @param config TrajectoryConfig that sets the contraints for the Trajectory to be generated * - * @return true if successful file write was preformed + * @return Trajectory created from the waypoints in the file. */ - public static boolean export(String fileLocation, Path path, Double yoffset) { - try ( - BufferedWriter writer = Files.newBufferedWriter(Paths.get(fileLocation + path.getPathName())); - - CSVPrinter csvPrinter = new CSVPrinter(writer, CSVFormat.DEFAULT - .withHeader("X", "Y", "Tangent X", "Tangent Y", "Fixed Theta", "Reversed", "Name")) - ) { - for (Waypoint wp : path.getWaypoints()) { - double xPos = wp.getX(); - double yPos = wp.getY() + yoffset; - double tangentX = wp.getTangentX(); - double tangentY = wp.getTangentY(); - String name = wp.getName(); - csvPrinter.printRecord(xPos, yPos, tangentX, tangentY, wp.isLockTangent(), wp.isReversed(), name); - } - csvPrinter.flush(); - } catch (IOException except) { - LOGGER.log(Level.WARNING, "Could not save Path file", except); - return false; - } - return true; - } + public static Trajectory importPathToCubicTrajectory(String filepath, TrajectoryConfig config) { + try (FileReader fr = new FileReader(new File(filepath)); + BufferedReader reader = new BufferedReader(fr)) { + + var interiorWaypoints = new ArrayList(); + Pose2d start = new Pose2d(); + Pose2d end = new Pose2d(); + int loop = 0; + String line; + String lastline = ""; - /** - * Imports Path object from disk. - * - * @param fileLocation Folder with path file - * @param fileName Name of path file - * - * @return Path object saved in Path file - */ - public static Path importPath(String fileLocation, String fileName) { - try(Reader reader = Files.newBufferedReader(java.nio.file.Path.of(fileLocation, fileName)); - CSVParser csvParser = new CSVParser(reader, CSVFormat.DEFAULT - .withFirstRecordAsHeader() - .withIgnoreHeaderCase() - .withTrim())) { - ArrayList waypoints = new ArrayList<>(); - for (CSVRecord csvRecord : csvParser) { - Point2D position = new Point2D( - Double.parseDouble(csvRecord.get("X")), - Double.parseDouble(csvRecord.get("Y")) - ); - Point2D tangent = new Point2D( - Double.parseDouble(csvRecord.get("Tangent X")), - Double.parseDouble(csvRecord.get("Tangent Y")) - ); - boolean locked = Boolean.parseBoolean(csvRecord.get("Fixed Theta")); - boolean reversed = Boolean.parseBoolean(csvRecord.get("Reversed")); - Waypoint point = new Waypoint(position, tangent, locked, reversed); - if (csvRecord.isMapped("Name")) { - String name = csvRecord.get("Name"); - point.setName(name); + while ((line = reader.readLine()) != null) { + if (loop == 0) { + if (line != "X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name") { + throw new RuntimeException("this isn’t a PathWeaver csv file"); + } + } + else if (loop == 1) { + start = createPoseWaypoint(line); + } + else if (loop == 2) { + ; // skip the second line because we are logging last + } + else { + interiorWaypoints.add(createTranslationWaypoint(lastline)); } - waypoints.add(point); + lastline = line; + loop++; } - return new WpilibPath(waypoints, fileName); + end = createPoseWaypoint(lastline); + + return TrajectoryGenerator.generateTrajectory( + start, + interiorWaypoints, + end, + config); } catch (IOException except) { LOGGER.log(Level.WARNING, "Could not read Path file", except); return null; @@ -103,37 +81,31 @@ public static Path importPath(String fileLocation, String fileName) { /** * Imports Path object and creates a trajectory with that info. * - * @param fileLocation Folder with path file - * @param fileName Name of path file + * @param filepath Full path to the file + * @param config TrajectoryConfig that sets the contraints for the Trajectory to be generated * * @return Trajectory created from the waypoints in the file. */ - public static Trajectory importPathToTrajectory(String fileLocation, String fileName, TrajectoryConfig config) { - try(Reader reader = Files.newBufferedReader(java.nio.file.Path.of(fileLocation, fileName)); - CSVParser csvParser = new CSVParser(reader, CSVFormat.DEFAULT - .withFirstRecordAsHeader() - .withIgnoreHeaderCase() - .withTrim())) { + public static Trajectory importPathToQuinticTrajectory(String filepath, TrajectoryConfig config) { + try (FileReader fr = new FileReader(new File(filepath)); + BufferedReader reader = new BufferedReader(fr)) { + + ControlVectorList controlVectors = new ControlVectorList(); int loop = 0; - Pose2d start = new Pose2d(); - Pose2d end = new Pose2d(); - var interiorWaypoints = new ArrayList(); - for (CSVRecord csvRecord : csvParser) { - if (loop == 0) { - start = createPoseWaypoint(csvRecord); - } - else if (loop == csvParser.Length - 1) { - end = createPoseWaypoint(csvRecord); - } - else { - interiorWaypoints.add(createTranslationWaypoint(csvRecord)); - } - config.setReversed(Boolean.parseBoolean(csvRecord.get("Reversed"))); + String line; + + while ((line = reader.readLine()) != null) { + if (loop == 0) { + if (line != "X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name") { + throw new RuntimeException("this isn’t a PathWeaver csv file"); + } + } + else { + controlVectors.add(createControlVector(line)); + } + loop++; } - return TrajectoryGenerator.generateTrajectory( - start, - interiorWaypoints, - end, + return TrajectoryGenerator.generateTrajectory(controlVectors, config); } catch (IOException except) { LOGGER.log(Level.WARNING, "Could not read Path file", except); @@ -141,12 +113,22 @@ else if (loop == csvParser.Length - 1) { } } - private static Pose2d createPoseWaypoint(CSVRecord csvRecord) { - return new Pose2d(new Translation2d(Double.parseDouble(csvRecord.get("X")), Double.parseDouble(csvRecord.get("Y"))), - new Rotation2d(Double.parseDouble(csvRecord.get("Tangent X")), Double.parseDouble(csvRecord.get("Tangent Y")))); + private static Pose2d createPoseWaypoint(String input) { + String[] arrOfStr = input.split(",", 0); + // 8.21m is the Height of the field PathWeaver and traj use different starting points + return new Pose2d(new Translation2d(Double.parseDouble(arrOfStr[0]), 8.21 + Double.parseDouble(arrOfStr[1])), + new Rotation2d(Double.parseDouble(arrOfStr[2]), Double.parseDouble(arrOfStr[3]))); + } + + private static Translation2d createTranslationWaypoint(String input) { + String[] arrOfStr = input.split(",", 0); + return new Translation2d(Double.parseDouble(arrOfStr[0]), Double.parseDouble(arrOfStr[1])); } - private static Translation2d createTranslationWaypoint(CSVRecord csvRecord) { - return new Translation2d(Double.parseDouble(csvRecord.get("X")), Double.parseDouble(csvRecord.get("Y"))); + private static ControlVector createControlVector(String input) { + String[] arrOfStr = input.split(",", 0); + double[] x = new double[] {Double.parseDouble(arrOfStr[0]), Double.parseDouble(arrOfStr[2])}; + double[] y = new double[] {Double.parseDouble(arrOfStr[1]), Double.parseDouble(arrOfStr[3])}; + return new ControlVector(x, y); } } From ab76eea0f63c836072bc08353ba9a91e46df3940 Mon Sep 17 00:00:00 2001 From: jasondaming Date: Wed, 17 Feb 2021 07:21:33 -0600 Subject: [PATCH 3/4] equals and removed logger --- .../first/wpilibj/waypoint/WaypointUtil.java | 20 +++++-------------- 1 file changed, 5 insertions(+), 15 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java index 1b70a4b5d27..254ddf60164 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java @@ -9,8 +9,6 @@ import java.io.FileReader; import java.io.IOException; import java.util.ArrayList; -import java.util.logging.Level; -import java.util.logging.Logger; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; @@ -22,8 +20,6 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator.ControlVectorList; public final class WaypointUtil { - private static final Logger LOGGER = Logger.getLogger(WaypointUtil.class.getName()); - private WaypointUtil() { throw new UnsupportedOperationException("This is a utility class!"); } @@ -36,7 +32,7 @@ private WaypointUtil() { * * @return Trajectory created from the waypoints in the file. */ - public static Trajectory importPathToCubicTrajectory(String filepath, TrajectoryConfig config) { + public static Trajectory importPathToCubicTrajectory(String filepath, TrajectoryConfig config) throws IOException { try (FileReader fr = new FileReader(new File(filepath)); BufferedReader reader = new BufferedReader(fr)) { @@ -49,7 +45,7 @@ public static Trajectory importPathToCubicTrajectory(String filepath, Trajectory while ((line = reader.readLine()) != null) { if (loop == 0) { - if (line != "X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name") { + if (!line.equals("X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name")) { throw new RuntimeException("this isn’t a PathWeaver csv file"); } } @@ -57,7 +53,7 @@ else if (loop == 1) { start = createPoseWaypoint(line); } else if (loop == 2) { - ; // skip the second line because we are logging last + ; // skip the second line because we are logging the lastline and at 2 lastline is start } else { interiorWaypoints.add(createTranslationWaypoint(lastline)); @@ -72,9 +68,6 @@ else if (loop == 2) { interiorWaypoints, end, config); - } catch (IOException except) { - LOGGER.log(Level.WARNING, "Could not read Path file", except); - return null; } } @@ -86,7 +79,7 @@ else if (loop == 2) { * * @return Trajectory created from the waypoints in the file. */ - public static Trajectory importPathToQuinticTrajectory(String filepath, TrajectoryConfig config) { + public static Trajectory importPathToQuinticTrajectory(String filepath, TrajectoryConfig config) throws IOException { try (FileReader fr = new FileReader(new File(filepath)); BufferedReader reader = new BufferedReader(fr)) { @@ -96,7 +89,7 @@ public static Trajectory importPathToQuinticTrajectory(String filepath, Trajecto while ((line = reader.readLine()) != null) { if (loop == 0) { - if (line != "X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name") { + if (!line.equals("X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name")) { throw new RuntimeException("this isn’t a PathWeaver csv file"); } } @@ -107,9 +100,6 @@ public static Trajectory importPathToQuinticTrajectory(String filepath, Trajecto } return TrajectoryGenerator.generateTrajectory(controlVectors, config); - } catch (IOException except) { - LOGGER.log(Level.WARNING, "Could not read Path file", except); - return null; } } From 0782cdfc416937022d1bfc9b1767ff91b199822e Mon Sep 17 00:00:00 2001 From: jasondaming Date: Wed, 17 Feb 2021 12:12:28 -0600 Subject: [PATCH 4/4] removed height adjustment --- .../main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java index 254ddf60164..2c1d4322b0f 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/waypoint/WaypointUtil.java @@ -105,8 +105,7 @@ public static Trajectory importPathToQuinticTrajectory(String filepath, Trajecto private static Pose2d createPoseWaypoint(String input) { String[] arrOfStr = input.split(",", 0); - // 8.21m is the Height of the field PathWeaver and traj use different starting points - return new Pose2d(new Translation2d(Double.parseDouble(arrOfStr[0]), 8.21 + Double.parseDouble(arrOfStr[1])), + return new Pose2d(new Translation2d(Double.parseDouble(arrOfStr[0]), Double.parseDouble(arrOfStr[1])), new Rotation2d(Double.parseDouble(arrOfStr[2]), Double.parseDouble(arrOfStr[3]))); }