Skip to content

Commit fd15cb6

Browse files
committed
Obstacle avoidance reintroduced
1 parent 1fc8844 commit fd15cb6

File tree

5 files changed

+24
-15
lines changed

5 files changed

+24
-15
lines changed

app/src/main/java/org/team2363/helixnavigator/document/HPath.java

Lines changed: 13 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
import org.team2363.helixtrajectory.HolonomicPath;
1515
import org.team2363.helixtrajectory.HolonomicWaypoint;
1616
import org.team2363.helixtrajectory.InitialGuessPoint;
17+
import org.team2363.helixtrajectory.Obstacle;
1718

1819
import com.jlbabilino.json.DeserializedJSONConstructor;
1920
import com.jlbabilino.json.DeserializedJSONObjectValue;
@@ -205,33 +206,38 @@ public final HTrajectory getTrajectory() {
205206
return trajectory.get();
206207
}
207208

208-
public HolonomicPath toPath() {
209+
public HolonomicPath toPath(List<Obstacle> obstacles) {
209210
List<HolonomicWaypoint> htWaypoints = new ArrayList<>();
210211
int i = 0;
212+
boolean foundFirstWaypoint = false;
211213
while (waypoints.get(i).isInitialGuess()) {
212214
i++;
213215
}
214216
while (i < waypoints.size()) {
215-
int waypointIndex = i;
216-
i++;
217217
List<InitialGuessPoint> initialGuessPoints = new ArrayList<>();
218218
while (i < waypoints.size() && waypoints.get(i).isInitialGuess()) {
219219
initialGuessPoints.add(((HInitialGuessWaypoint) waypoints.get(i)).toInitialGuessPoint());
220220
i++;
221221
}
222+
int waypointIndex = i;
222223
switch (waypoints.get(waypointIndex).getWaypointType()) {
223224
case SOFT:
224-
htWaypoints.add(((HSoftWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPoints));
225+
htWaypoints.add(((HSoftWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPoints, obstacles));
225226
break;
226227
case HARD:
227-
htWaypoints.add(((HHardWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPoints));
228+
htWaypoints.add(((HHardWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPoints, obstacles));
228229
break;
229230
case CUSTOM:
230-
htWaypoints.add(((HCustomWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPoints));
231+
htWaypoints.add(((HCustomWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPoints, obstacles));
231232
break;
232233
default:
233-
break;
234+
break;
235+
}
236+
if (!foundFirstWaypoint) {
237+
obstacles = List.of();
238+
foundFirstWaypoint = true;
234239
}
240+
i++;
235241
}
236242
return new HolonomicPath(htWaypoints);
237243
}

app/src/main/java/org/team2363/helixnavigator/document/waypoint/HCustomWaypoint.java

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44

55
import org.team2363.helixtrajectory.HolonomicWaypoint;
66
import org.team2363.helixtrajectory.InitialGuessPoint;
7+
import org.team2363.helixtrajectory.Obstacle;
78

89
import com.jlbabilino.json.DeserializedJSONConstructor;
910
import com.jlbabilino.json.DeserializedJSONObjectValue;
@@ -200,9 +201,9 @@ public final int getControlIntervalCount() {
200201
return controlIntervalCount.get();
201202
}
202203

203-
public HolonomicWaypoint toWaypoint(List<InitialGuessPoint> initialGuessPoints) {
204+
public HolonomicWaypoint toWaypoint(List<InitialGuessPoint> initialGuessPoints, List<Obstacle> obstacles) {
204205
return new HolonomicWaypoint(getX(), getY(), getHeading(), getVelocityX(), getVelocityY(), getAngularVelocity(),
205206
isXConstrained(), isYConstrained(), isHeadingConstrained(), isVelocityXConstrained(),
206-
isVelocityYConstrained(), isVelocityMagnitudeConstrained(), isAngularVelocityConstrained(), getControlIntervalCount(), initialGuessPoints, List.of());
207+
isVelocityYConstrained(), isVelocityMagnitudeConstrained(), isAngularVelocityConstrained(), getControlIntervalCount(), initialGuessPoints, obstacles);
207208
}
208209
}

app/src/main/java/org/team2363/helixnavigator/document/waypoint/HHardWaypoint.java

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44

55
import org.team2363.helixtrajectory.HolonomicWaypoint;
66
import org.team2363.helixtrajectory.InitialGuessPoint;
7+
import org.team2363.helixtrajectory.Obstacle;
78

89
import com.jlbabilino.json.DeserializedJSONConstructor;
910
import com.jlbabilino.json.DeserializedJSONObjectValue;
@@ -53,7 +54,7 @@ public final double getHeading() {
5354
return heading.get();
5455
}
5556

56-
public HolonomicWaypoint toWaypoint(List<InitialGuessPoint> initialGuessPoints) {
57-
return new HolonomicWaypoint(getX(), getY(), getHeading(), 0.0, 0.0, 0.0, true, true, true, false, false, false, false, 100, initialGuessPoints, List.of());
57+
public HolonomicWaypoint toWaypoint(List<InitialGuessPoint> initialGuessPoints, List<Obstacle> obstacles) {
58+
return new HolonomicWaypoint(getX(), getY(), getHeading(), 0.0, 0.0, 0.0, true, true, true, false, false, false, false, 100, initialGuessPoints, obstacles);
5859
}
5960
}

app/src/main/java/org/team2363/helixnavigator/document/waypoint/HSoftWaypoint.java

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44

55
import org.team2363.helixtrajectory.HolonomicWaypoint;
66
import org.team2363.helixtrajectory.InitialGuessPoint;
7+
import org.team2363.helixtrajectory.Obstacle;
78

89
import com.jlbabilino.json.DeserializedJSONConstructor;
910

@@ -23,7 +24,7 @@ public boolean isSoft() {
2324
return true;
2425
}
2526

26-
public HolonomicWaypoint toWaypoint(List<InitialGuessPoint> initialGuessPoints) {
27-
return new HolonomicWaypoint(getX(), getY(), 0.0, 0.0, 0.0, 0.0, true, true, false, false, false, false, false, 100, initialGuessPoints, List.of());
27+
public HolonomicWaypoint toWaypoint(List<InitialGuessPoint> initialGuessPoints, List<Obstacle> obstacles) {
28+
return new HolonomicWaypoint(getX(), getY(), 0.0, 0.0, 0.0, 0.0, true, true, false, false, false, false, false, 100, initialGuessPoints, obstacles);
2829
}
2930
}

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -69,17 +69,17 @@ public TrajectoryToolBar(DocumentManager documentManager) {
6969
HDocument hDocument = this.documentManager.getDocument();
7070
HPath hPath = this.documentManager.getDocument().getSelectedPath();
7171
SwerveDrivetrain drive = hDocument.getRobotConfiguration().toDrive();
72-
HolonomicPath path = hPath.toPath();
7372
List<Obstacle> obstacles = new ArrayList<>(hPath.getObstacles().size());
7473
for (int i = 0; i < hPath.getObstacles().size(); i++) {
7574
obstacles.add(hPath.getObstacles().get(i).toObstacle());
7675
}
76+
HolonomicPath path = hPath.toPath(obstacles);
7777
try {
78+
System.out.println("Path optimizing: " + path.toString());
7879
HolonomicTrajectory traj = OptimalTrajectoryGenerator.generate(drive, path);
7980
hPath.setTrajectory(HTrajectory.fromTrajectory(traj));
8081
} catch (InvalidPathException | PluginLoadException | TrajectoryGenerationException e) {
8182
System.out.println("Error generating path: " + e.getMessage() + System.lineSeparator());
82-
System.out.println("Path optimizing: " + path.toString());
8383
}
8484
}
8585
});

0 commit comments

Comments
 (0)