From 12921e9c893912b37a69b6997fc8aa6c2f339972 Mon Sep 17 00:00:00 2001 From: rmheuer <63077980+rmheuer@users.noreply.github.com> Date: Sat, 2 Dec 2023 20:22:46 -0600 Subject: [PATCH] [Robot] Send dynamic obstacles --- Pathfinding/config.json | 2 +- Pathfinding/src/main.rs | 14 +++++++-- .../com/swrobotics/robot/RobotContainer.java | 2 +- .../robot/control/ControlBoard.java | 23 ++++++++++++++ .../swerve/ThetaStarPathfinder.java | 27 ++++++++++++++++- .../tool/field/path/PathfindingLayer.java | 30 +++++++++++++++++-- .../tool/field/path/shape/Shape.java | 11 ++++--- 7 files changed, 97 insertions(+), 12 deletions(-) diff --git a/Pathfinding/config.json b/Pathfinding/config.json index 9e87340..86ce8c1 100644 --- a/Pathfinding/config.json +++ b/Pathfinding/config.json @@ -11,4 +11,4 @@ "origin_y": 1.0 }, "env_file": "environment.json" -} \ No newline at end of file +} diff --git a/Pathfinding/src/main.rs b/Pathfinding/src/main.rs index 94ecd2f..cbf6def 100644 --- a/Pathfinding/src/main.rs +++ b/Pathfinding/src/main.rs @@ -58,6 +58,7 @@ impl PathfinderTask { MSG_GET_SHAPES, MSG_SET_SHAPE, MSG_REMOVE_SHAPE, + MSG_SET_DYN_SHAPES, ]); let shapes = env.shapes; @@ -189,7 +190,14 @@ impl PathfinderTask { MSG_SET_SHAPE => self.on_set_shape(data).await, MSG_REMOVE_SHAPE => self.on_remove_shape(data).await, MSG_SET_DYN_SHAPES => { - self.on_set_dyn_shapes(data); + self.on_set_dyn_shapes(&mut data); + + let start_x = data.get_f64(); + let start_y = data.get_f64(); + start_pos = Vec2f { + x: start_x, + y: start_y, + }; needs_recalc = true; } _ => {} @@ -376,11 +384,11 @@ impl PathfinderTask { self.update_shapes().await; } - fn on_set_dyn_shapes(&mut self, mut data: Bytes) { + fn on_set_dyn_shapes(&mut self, data: &mut Bytes) { let count = data.get_i32(); self.dyn_shapes.clear(); for _ in 0..count { - if let Some(shape) = Self::read_shape(&mut data) { + if let Some(shape) = Self::read_shape(data) { self.dyn_shapes.push(shape); } else { self.update_grid(); diff --git a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java index d828b8c..d40014f 100644 --- a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java +++ b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java @@ -41,7 +41,7 @@ public class RobotContainer { public final MessengerClient messenger; private final ControlBoard controlboard; - private final SwerveDrive drive; + public final SwerveDrive drive; public RobotContainer() { // Turn off joystick warnings in sim diff --git a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java index fb88f43..e0c6ccf 100644 --- a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java +++ b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java @@ -2,18 +2,24 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.pathfinding.Pathfinding; import com.swrobotics.lib.input.XboxController; import com.swrobotics.mathlib.MathUtil; import com.swrobotics.mathlib.Vec2d; import com.swrobotics.robot.RobotContainer; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; +import java.util.ArrayList; +import java.util.List; + public class ControlBoard { private static final double DEADBAND = 0.15; @@ -47,6 +53,23 @@ public ControlBoard(RobotContainer robot) { operator = new XboxController(1, DEADBAND); driver.a.onFalling(pathfindingCommand); + driver.b.onFalling(() -> { + List> rects = new ArrayList<>(); + + // Generate some random rectangles + for (int i = 0; i < 3; i++) { + double cx = Math.random() * 16; + double cy = Math.random() * 8; + double w = Math.random() * 4; + double h = Math.random() * 4; + + rects.add(new Pair<>( + new Translation2d(cx - w/2, cy - h/2), + new Translation2d(cx + w/2, cy + h/2))); + } + + Pathfinding.setDynamicObstacles(rects, robot.drive.getEstimatedPose().getTranslation()); + }); // Congigure triggers // driver.start.onFalling(robot.drive::resetGyro); 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 index bddec15..85c14f7 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/ThetaStarPathfinder.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/ThetaStarPathfinder.java @@ -4,6 +4,7 @@ import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.pathfinding.Pathfinder; +import com.swrobotics.messenger.client.MessageBuilder; import com.swrobotics.messenger.client.MessageReader; import com.swrobotics.messenger.client.MessengerClient; import edu.wpi.first.math.Pair; @@ -22,6 +23,7 @@ private enum PathStatus { private static final String MSG_SET_ENDPOINTS = "Pathfinder:SetEndpoints"; private static final String MSG_PATH = "Pathfinder:Path"; + private static final String MSG_SET_DYN_SHAPES = "Pathfinder:SetDynamicShapes"; private static final double CORRECT_TARGET_TOL = 0.1524 + 0.1; private static final double TIMEOUT = 1; // Seconds @@ -188,6 +190,29 @@ public void setGoalPosition(Translation2d goalPosition) { @Override public void setDynamicObstacles(List> obs, Translation2d currentRobotPos) { - // Not supported (yet) + MessageBuilder builder = msg.prepare(MSG_SET_DYN_SHAPES); + builder.addInt(obs.size()); + for (Pair axisAlignedBB : obs) { + Translation2d to = axisAlignedBB.getFirst(); + Translation2d from = axisAlignedBB.getSecond(); + + Translation2d center = to.plus(from.minus(to).times(0.5)); + double width = Math.abs(from.getX() - to.getX()); + double height = Math.abs(from.getY() - to.getY()); + + builder.addByte((byte) 1); // Is rectangle + builder.addDouble(center.getX()); + builder.addDouble(center.getY()); + builder.addDouble(width); + builder.addDouble(height); + builder.addDouble(0); // Rotation + builder.addBoolean(false); // Inverted + } + + // Also send current position for recalculation + builder.addDouble(currentRobotPos.getX()); + builder.addDouble(currentRobotPos.getY()); + + builder.send(); } } diff --git a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/path/PathfindingLayer.java b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/path/PathfindingLayer.java index ab8b731..0f33bd8 100644 --- a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/path/PathfindingLayer.java +++ b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/path/PathfindingLayer.java @@ -122,7 +122,7 @@ private void onShapes(String type, MessageReader reader) { int count = reader.readInt(); shapes = new ArrayList<>(); for (int i = 0; i < count; i++) { - Shape shape = Shape.read(reader); + Shape shape = Shape.read(reader, true); shapes.add(shape); } @@ -149,9 +149,10 @@ private void onSetDynShapes(String type, MessageReader reader) { dynShapes.clear(); int count = reader.readInt(); for (int i = 0; i < count; i++) { - Shape shape = Shape.read(reader); + Shape shape = Shape.read(reader, false); dynShapes.add(shape); } + needsRefreshCellData = true; } @Override @@ -465,6 +466,31 @@ public void showGui() { } ImGui.endTable(); + + if (ImGui.treeNodeEx("Dynamic Shapes", ImGuiTreeNodeFlags.DefaultOpen)) { + int i = 0; + for (Shape shape : dynShapes) { + if (shape instanceof Rectangle r) { + if (ImGui.treeNodeEx("Rectangle##" + i++)) { + ImGui.text("X: " + r.x.get()); + ImGui.text("Y: " + r.y.get()); + ImGui.text("Width: " + r.width.get()); + ImGui.text("Height: " + r.height.get()); + ImGui.text("Rotation: " + r.rotation.get()); + ImGui.text("Inverted: " + r.inverted.get()); + ImGui.treePop(); + } + } else if (shape instanceof Circle c) { + if (ImGui.treeNodeEx("Circle##" + i++)) { + ImGui.text("X: " + c.x.get()); + ImGui.text("Y: " + c.y.get()); + ImGui.text("Radius: " + c.radius.get()); + ImGui.treePop(); + } + } + } + ImGui.treePop(); + } } } } diff --git a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/path/shape/Shape.java b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/path/shape/Shape.java index 7370360..82d700a 100644 --- a/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/path/shape/Shape.java +++ b/ShuffleLog/src/main/java/com/swrobotics/shufflelog/tool/field/path/shape/Shape.java @@ -21,10 +21,13 @@ public UUID getId() { protected abstract void readContent(MessageReader reader); - public static Shape read(MessageReader reader) { - long idMsb = reader.readLong(); - long idLsb = reader.readLong(); - UUID id = new UUID(idMsb, idLsb); + public static Shape read(MessageReader reader, boolean hasUuid) { + UUID id = null; + if (hasUuid) { + long idMsb = reader.readLong(); + long idLsb = reader.readLong(); + id = new UUID(idMsb, idLsb); + } byte type = reader.readByte(); Shape shape;