Skip to content

Commit f75ab9e

Browse files
jlbabilinojlbabilino
committed
Improved trajectory generation concurrency
1 parent fd15cb6 commit f75ab9e

File tree

3 files changed

+75
-29
lines changed

3 files changed

+75
-29
lines changed

‎app/build.gradle.kts

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ repositories {
2323
url = uri("https://maven.pkg.github.com/jlbabilino/HelixTrajectory")
2424
credentials {
2525
username = "jlbabilino"
26-
password = "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"
26+
password = "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"
2727
}
2828
}
2929
}
@@ -38,7 +38,7 @@ dependencies {
3838
implementation("systems.uom:systems-quantity:2.1")
3939
// implementation "org.djunits:djunits:4.01.07"
4040

41-
implementation("org.team2363:helixtrajectory:0.0.0-pre4")
41+
implementation("org.team2363:helixtrajectory:0.0.0-pre5")
4242
}
4343

4444
application {

‎app/src/main/java/org/team2363/helixnavigator/document/DocumentActions.java

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,14 @@
1414
import org.team2363.helixnavigator.document.waypoint.HWaypoint;
1515
import org.team2363.helixnavigator.ui.prompts.RobotConfigDialog;
1616
import org.team2363.helixnavigator.ui.prompts.TransformDialog;
17+
import org.team2363.helixtrajectory.HolonomicPath;
18+
import org.team2363.helixtrajectory.HolonomicTrajectory;
19+
import org.team2363.helixtrajectory.InvalidPathException;
20+
import org.team2363.helixtrajectory.Obstacle;
21+
import org.team2363.helixtrajectory.OptimalTrajectoryGenerator;
22+
import org.team2363.helixtrajectory.PluginLoadException;
23+
import org.team2363.helixtrajectory.SwerveDrivetrain;
24+
import org.team2363.helixtrajectory.TrajectoryGenerationException;
1725

1826
import com.jlbabilino.json.InvalidJSONTranslationConfiguration;
1927
import com.jlbabilino.json.JSONArray;
@@ -27,8 +35,12 @@
2735
import com.jlbabilino.json.JSONSerializerException;
2836

2937
import javafx.beans.property.BooleanProperty;
38+
import javafx.beans.property.ObjectProperty;
3039
import javafx.beans.property.SimpleBooleanProperty;
40+
import javafx.beans.property.SimpleObjectProperty;
3141
import javafx.beans.value.ObservableValue;
42+
import javafx.concurrent.Service;
43+
import javafx.concurrent.Task;
3244
import javafx.geometry.Point2D;
3345
import javafx.scene.input.Clipboard;
3446
import javafx.scene.input.ClipboardContent;
@@ -382,6 +394,66 @@ public void paste() {
382394
}
383395
}
384396

397+
private static class TrajectoryGenerationService extends Service<HolonomicTrajectory> {
398+
399+
// private final ObjectProperty<SwerveDrivetrain> drive = new SimpleObjectProperty<>(this, "drive", null);
400+
// private final ObjectProperty<HolonomicPath> path = new SimpleObjectProperty<>(this, "path", null);
401+
private SwerveDrivetrain drive = null;
402+
private HolonomicPath path = null;
403+
404+
TrajectoryGenerationService() {
405+
}
406+
407+
@Override
408+
protected Task<HolonomicTrajectory> createTask() {
409+
Task<HolonomicTrajectory> optimizeTask = new Task<HolonomicTrajectory>() {
410+
@Override
411+
protected HolonomicTrajectory call() throws PluginLoadException, InvalidPathException, TrajectoryGenerationException {
412+
System.out.println("Path optimizing: " + path.toString());
413+
if (drive != null && path != null) {
414+
return OptimalTrajectoryGenerator.generate(drive, path);
415+
} else {
416+
throw new TrajectoryGenerationException("No path specified for optimization service.");
417+
}
418+
// System.out.println("Error generating path: " + e.getMessage() + System.lineSeparator());
419+
}
420+
};
421+
return optimizeTask;
422+
}
423+
}
424+
425+
private static final TrajectoryGenerationService SERVICE = new TrajectoryGenerationService();
426+
public void generateTrajectory() {
427+
if (documentManager.getIsDocumentOpen() && documentManager.getDocument().isPathSelected()) {
428+
HDocument hDocument = documentManager.getDocument();
429+
HPath hPath = documentManager.getDocument().getSelectedPath();
430+
SwerveDrivetrain drive = hDocument.getRobotConfiguration().toDrive();
431+
List<Obstacle> obstacles = new ArrayList<>(hPath.getObstacles().size());
432+
for (int i = 0; i < hPath.getObstacles().size(); i++) {
433+
obstacles.add(hPath.getObstacles().get(i).toObstacle());
434+
}
435+
HolonomicPath path = hPath.toPath(obstacles);
436+
SERVICE.drive = drive;
437+
SERVICE.path = path;
438+
SERVICE.setOnSucceeded(workerState -> {
439+
HolonomicTrajectory traj = (HolonomicTrajectory) workerState.getSource().getValue();
440+
hPath.setTrajectory(HTrajectory.fromTrajectory(traj));
441+
});
442+
if (!SERVICE.isRunning()) {
443+
SERVICE.restart();
444+
}
445+
}
446+
}
447+
public void toggleTrajectoryGeneration() {
448+
if (SERVICE.isRunning()) {
449+
System.out.println("Cancelling trajopt");
450+
SERVICE.cancel();
451+
} else {
452+
System.out.println("Restarting trajopt");
453+
generateTrajectory();
454+
}
455+
}
456+
385457
private static final Rotate ROTATE_90_CLOCKWISE = new Rotate(-90);
386458
private static final Rotate ROTATE_90_COUNTERCLOCKWISE = new Rotate(90);
387459
private static final Rotate ROTATE_180 = new Rotate(180);

‎app/src/main/java/org/team2363/helixnavigator/ui/editor/toolbar/TrajectoryToolBar.java

Lines changed: 1 addition & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,6 @@
44

55
import java.io.File;
66
import java.io.IOException;
7-
import java.util.ArrayList;
8-
import java.util.List;
97

108
import javax.measure.quantity.Time;
119

@@ -14,14 +12,6 @@
1412
import org.team2363.helixnavigator.document.HPath;
1513
import org.team2363.helixnavigator.document.HTrajectory;
1614
import org.team2363.helixnavigator.global.Standards;
17-
import org.team2363.helixtrajectory.HolonomicPath;
18-
import org.team2363.helixtrajectory.HolonomicTrajectory;
19-
import org.team2363.helixtrajectory.InvalidPathException;
20-
import org.team2363.helixtrajectory.Obstacle;
21-
import org.team2363.helixtrajectory.OptimalTrajectoryGenerator;
22-
import org.team2363.helixtrajectory.PluginLoadException;
23-
import org.team2363.helixtrajectory.SwerveDrivetrain;
24-
import org.team2363.helixtrajectory.TrajectoryGenerationException;
2515
import org.team2363.lib.ui.validation.UnitTextField;
2616

2717
import com.jlbabilino.json.InvalidJSONTranslationConfiguration;
@@ -65,23 +55,7 @@ public TrajectoryToolBar(DocumentManager documentManager) {
6555
getItems().addAll(importTraj, exportTraj, generateTraj, timestampSlider, animateButton);
6656

6757
generateTraj.setOnAction(event -> {
68-
if (this.documentManager.getIsDocumentOpen() && this.documentManager.getDocument().isPathSelected()) {
69-
HDocument hDocument = this.documentManager.getDocument();
70-
HPath hPath = this.documentManager.getDocument().getSelectedPath();
71-
SwerveDrivetrain drive = hDocument.getRobotConfiguration().toDrive();
72-
List<Obstacle> obstacles = new ArrayList<>(hPath.getObstacles().size());
73-
for (int i = 0; i < hPath.getObstacles().size(); i++) {
74-
obstacles.add(hPath.getObstacles().get(i).toObstacle());
75-
}
76-
HolonomicPath path = hPath.toPath(obstacles);
77-
try {
78-
System.out.println("Path optimizing: " + path.toString());
79-
HolonomicTrajectory traj = OptimalTrajectoryGenerator.generate(drive, path);
80-
hPath.setTrajectory(HTrajectory.fromTrajectory(traj));
81-
} catch (InvalidPathException | PluginLoadException | TrajectoryGenerationException e) {
82-
System.out.println("Error generating path: " + e.getMessage() + System.lineSeparator());
83-
}
84-
}
58+
documentManager.actions().toggleTrajectoryGeneration();
8559
});
8660
importTraj.setOnAction(event -> {
8761
if (this.documentManager.getIsDocumentOpen() && this.documentManager.getDocument().isPathSelected()) {

0 commit comments

Comments
 (0)