[Robot] Send dynamic obstacles

Pathfinding/config.json

@@ -11,4 +11,4 @@
     "origin_y": 1.0
   },
   "env_file": "environment.json"
-}
+}

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();

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

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<Pair<Translation2d, Translation2d>> 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);

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<Pair<Translation2d, Translation2d>> obs, Translation2d currentRobotPos) {
-        // Not supported (yet)
+        MessageBuilder builder = msg.prepare(MSG_SET_DYN_SHAPES);
+        builder.addInt(obs.size());
+        for (Pair<Translation2d, Translation2d> 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();
     }
 }

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();
+                }
             }
         }
     }

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;