Circular Obstacles

src-tauri/Cargo.toml

@@ -15,11 +15,10 @@ license = "BSD-3-Clause"
 tauri-build = { version = "1.4", features = [] }
 
 [dependencies]
-tauri = { version = "1.4", features = [ "window-set-title", "path-all", "dialog-confirm", "dialog-save", "dialog-open", "fs-all", "shell-open", "devtools"] }
+tauri = { version = "1.4", features = [ "dialog-confirm", "dialog-save", "dialog-open", "dialog-save", "fs-all", "shell-open", "devtools" ] }
 serde = { version = "1.0", features = ["derive"] }
 serde_json = "1.0"
-trajoptlib = { git = "https://github.com/SleipnirGroup/TrajoptLib.git", rev = "20bb8c2ef016ceedab0dc5c0271ad9b69048039a" }
-
+trajoptlib = { git = "https://github.com/SleipnirGroup/TrajoptLib.git", rev = "c9f8140e92dff07b2edb9c7034fea4d18dc46c9a" }
 
 [features]
 # this feature is used for production builds or when `devPath` points to the filesystem

src-tauri/src/main.rs

@@ -54,7 +54,6 @@ struct ChoreoSegmentScope {
 #[derive(serde::Serialize, serde::Deserialize)]
 #[serde(untagged)]
 enum ChoreoConstraintScope {
-
   Segment([usize;2]),
   Waypoint([usize;1])
 }
@@ -73,6 +72,25 @@ enum Constraints {
 // Also add the constraint type here
 //define_enum_macro!(BoundsZeroVelocity, WptVelocityDirection, WptZeroVelocity);
 
+#[derive(Debug, serde::Serialize, serde::Deserialize)]
+#[allow(non_snake_case)]
+struct CircleObstacle {
+  x: f64, y: f64, radius: f64
+}
+
+#[derive(Debug, serde::Serialize, serde::Deserialize)]
+#[allow(non_snake_case)]
+struct PolygonObstacle {
+  x: Vec<f64>, y: Vec<f64>, radius: f64
+}
+
+#[derive(Debug, serde::Serialize, serde::Deserialize)]
+#[allow(non_snake_case)]
+enum Obstacle {
+  Circle(CircleObstacle),
+  Polygon(PolygonObstacle)
+}
+
 fn fix_scope(idx: usize, removed_idxs: &Vec<usize>) -> usize {
   let mut to_subtract: usize = 0;
   for removed in removed_idxs {
@@ -89,8 +107,15 @@ async fn cancel() {
   builder.cancel_all();
 }
 
+#[allow(non_snake_case)]
 #[tauri::command]
-async fn generate_trajectory(path: Vec<ChoreoWaypoint>, config: ChoreoRobotConfig, constraints: Vec<Constraints>) -> Result<HolonomicTrajectory, String> {
+async fn generate_trajectory(
+    path: Vec<ChoreoWaypoint>, 
+    config: ChoreoRobotConfig, 
+    constraints: Vec<Constraints>, 
+    circleObstacles: Vec<CircleObstacle>,
+    polygonObstacles: Vec<PolygonObstacle>
+  ) -> Result<HolonomicTrajectory, String> {
 
     let mut path_builder = SwervePathBuilder::new();
     let mut wpt_cnt : usize = 0;
@@ -239,8 +264,16 @@ async fn generate_trajectory(path: Vec<ChoreoWaypoint>, config: ChoreoRobotConfi
           }
         ]
       };
-    //path_builder.set_bumpers(config.bumperLength, config.bumperWidth);
-    path_builder.sgmt_circle_obstacle(0, path.len()-1, 3.0, 3.0, 1.0);
+
+    path_builder.set_bumpers(config.bumperLength, config.bumperWidth);
+
+    for o in circleObstacles {
+      path_builder.sgmt_circle_obstacle(0, wpt_cnt - 1, o.x, o.y, o.radius);
+    }
+
+    for o in polygonObstacles {
+      path_builder.sgmt_polygon_obstacle(0, wpt_cnt - 1, o.x, o.y, o.radius);
+    }
     path_builder.set_drivetrain(&drivetrain);
     path_builder.generate()
 }

src/components/config/CircularObstacleConfigPanel.tsx

@@ -0,0 +1,61 @@
+import { observer } from "mobx-react";
+import React, { Component } from "react";
+import DocumentManagerContext from "../../document/DocumentManager";
+import styles from "./WaypointConfigPanel.module.css";
+import InputList from "../input/InputList";
+import Input from "../input/Input";
+import { ICircularObstacleStore } from "../../document/CircularObstacleStore";
+
+type Props = { obstacle: ICircularObstacleStore | null };
+
+type State = {};
+
+class CircularObstacleConfigPanel extends Component<Props, State> {
+  static contextType = DocumentManagerContext;
+  declare context: React.ContextType<typeof DocumentManagerContext>;
+  state = {};
+  render() {
+    let { obstacle } = this.props;
+    if (obstacle !== null) {
+      return (
+        <div className={styles.WaypointPanel}>
+          <InputList noCheckbox>
+            <Input
+              title="x"
+              suffix="m"
+              enabled={true}
+              setEnabled={(a) => null}
+              number={obstacle.x}
+              setNumber={(x) => obstacle!.setX(x)}
+              showCheckbox={false}
+              titleTooltip={"Obstacle Center X"}
+            />
+
+            <Input
+              title="y"
+              suffix="m"
+              enabled={true}
+              setEnabled={(a) => null}
+              number={obstacle.y}
+              setNumber={(y) => obstacle!.setY(y)}
+              showCheckbox={false}
+              titleTooltip={"Obstacle Center Y"}
+            />
+
+            <Input
+              title="r"
+              suffix="m"
+              enabled={true}
+              setEnabled={(a) => null}
+              number={obstacle.radius}
+              setNumber={(r) => obstacle!.setRadius(r)}
+              showCheckbox={false}
+              titleTooltip={"Obstacle Radius"}
+            />
+          </InputList>
+        </div>
+      );
+    }
+  }
+}
+export default observer(CircularObstacleConfigPanel);

src/components/field/Field.tsx

@@ -18,6 +18,8 @@ import "react-toastify/dist/ReactToastify.min.css";
 import { ToastContainer, toast } from "react-toastify";
 import { invoke } from "@tauri-apps/api";
 import { Close } from "@mui/icons-material";
+import { ICircularObstacleStore } from "../../document/CircularObstacleStore";
+import CircularObstacleConfigPanel from "../config/CircularObstacleConfigPanel";
 
 type Props = {};
 
@@ -66,6 +68,16 @@ export class Field extends Component<Props, State> {
               constraint={selectedSidebar as IConstraintStore}
             ></ConstraintsConfigPanel>
           )}
+        {selectedSidebar !== undefined &&
+          "radius" in selectedSidebar &&
+          activePath.obstacles.find(
+            (obstacle) =>
+              obstacle.uuid == (selectedSidebar as ICircularObstacleStore)!.uuid
+          ) && (
+            <CircularObstacleConfigPanel
+              obstacle={selectedSidebar as ICircularObstacleStore}
+            ></CircularObstacleConfigPanel>
+          )}
         {robotConfigOpen && (
           <div className={styles.WaypointPanel}>
             <RobotConfigPanel></RobotConfigPanel>

src/components/field/svg/FieldObstacles.tsx

@@ -0,0 +1,112 @@
+import React, { Component } from "react";
+import DocumentManagerContext from "../../../document/DocumentManager";
+import { observer } from "mobx-react";
+import { Circle } from "@mui/icons-material";
+import { Box } from "@mui/material";
+import * as d3 from "d3";
+import { ICircularObstacleStore } from "../../../document/CircularObstacleStore";
+
+type Props = { obstacle: ICircularObstacleStore; index: number };
+
+type State = {};
+const STROKE = 0.1;
+const DOT = 0.1;
+
+class FieldGrid extends Component<Props, State> {
+  static contextType = DocumentManagerContext;
+  context!: React.ContextType<typeof DocumentManagerContext>;
+  state = {};
+  rootRef: React.RefObject<SVGElement> = React.createRef<SVGElement>();
+
+  appendIndexID(id: string): string {
+    return `${id}${this.props.index}`;
+  }
+
+  dragPointTranslate(event: any) {
+    this.props.obstacle.setX(this.props.obstacle.x + event.dx);
+    this.props.obstacle.setY(this.props.obstacle.y + event.dy);
+  }
+
+  dragPointRadius(event: any) {
+    let dx = event.x - this.props.obstacle.x;
+    let dy = event.y - this.props.obstacle.y;
+    let r = Math.sqrt(dx * dx + dy * dy);
+
+    this.props.obstacle.setRadius(r);
+  }
+
+  componentDidMount(): void {
+    if (this.rootRef.current) {
+      var dragHandleDrag = d3
+        .drag<SVGCircleElement, undefined>()
+        .on("drag", (event) => this.dragPointTranslate(event))
+        .on("start", () => {
+          this.context.model.select(this.props.obstacle);
+          this.context.history.startGroup(() => {});
+        })
+        .on("end", (event) => this.context.history.stopGroup())
+        .container(this.rootRef.current);
+      d3.select<SVGCircleElement, undefined>(
+        `#oDragTarget${this.props.index}`
+      ).call(dragHandleDrag);
+      d3.select<SVGCircleElement, undefined>(
+        `#oCenterDragTarget${this.props.index}`
+      ).call(dragHandleDrag);
+
+      var radiusHandleDrag = d3
+        .drag<SVGCircleElement, undefined>()
+        .on("drag", (event) => this.dragPointRadius(event))
+        .on("start", () => {
+          this.context.model.select(this.props.obstacle);
+          this.context.history.startGroup(() => {});
+        })
+        .on("end", (event) => this.context.history.stopGroup())
+        .container(this.rootRef.current);
+      d3.select<SVGCircleElement, undefined>(
+        `#oRadiusDragTarget${this.props.index}`
+      ).call(radiusHandleDrag);
+    }
+  }
+
+  render() {
+    let o = this.props.obstacle;
+    return (
+      <g ref={this.rootRef}>
+        {/* Main Circle */}
+        <circle
+          cx={o.x}
+          cy={o.y}
+          r={o.radius - STROKE / 2}
+          fill={"red"}
+          fillOpacity={0.1}
+          onClick={() => this.context.model.select(o)}
+          id={this.appendIndexID("oDragTarget")}
+        ></circle>
+        {/* Center Dot */}
+        <circle
+          cx={o.x}
+          cy={o.y}
+          r={o.radius < DOT * 2 ? 0.0 : DOT}
+          fill={o.selected ? "var(--select-yellow)" : "red"}
+          fillOpacity={o.selected ? 1.0 : 0.8}
+          onClick={() => this.context.model.select(o)}
+          id={this.appendIndexID("oCenterDragTarget")}
+        ></circle>
+        {/* Radius Handle */}
+        <circle
+          cx={o.x}
+          cy={o.y}
+          r={o.radius - STROKE / 2}
+          fill={"transparent"}
+          pointerEvents={"visibleStroke"}
+          stroke={o.selected ? "var(--select-yellow)" : "red"}
+          strokeWidth={STROKE}
+          strokeOpacity={o.selected ? 1.0 : 0.8}
+          onClick={() => this.context.model.select(o)}
+          id={this.appendIndexID("oRadiusDragTarget")}
+        ></circle>
+      </g>
+    );
+  }
+}
+export default observer(FieldGrid);

src/components/field/svg/FieldOverlayRoot.tsx

@@ -11,6 +11,11 @@ import { NavbarLabels, ViewLayers } from "../../../document/UIStateStore";
 import FieldGeneratedLines from "./FieldGeneratedLines";
 import FieldAxisLines from "./FieldAxisLines";
 import FieldConstraintsAddLayer from "./FieldConstraintsAddLayer";
+import FieldObstacle from "./FieldObstacles";
+import { Box } from "@mui/material";
+import { Circle } from "@mui/icons-material";
+import { v4 as uuidv4 } from "uuid";
+import { CircularObstacleStore } from "../../../document/CircularObstacleStore";
 
 type Props = {};
 
@@ -130,11 +135,7 @@ class FieldOverlayRoot extends Component<Props, State> {
           )}
           {layers[ViewLayers.Grid] && <FieldGrid></FieldGrid>}
           <FieldAxisLines></FieldAxisLines>
-          {/* Line paths */}
-          {layers[ViewLayers.Waypoints] && <FieldPathLines></FieldPathLines>}
-          {layers[ViewLayers.Trajectory] && (
-            <FieldGeneratedLines></FieldGeneratedLines>
-          )}
+          {/* Obstacle and waypoint mouse capture*/}
           {layers[ViewLayers.Waypoints] &&
             this.context.model.uiState.isNavbarWaypointSelected() && (
               <circle
@@ -145,6 +146,30 @@ class FieldOverlayRoot extends Component<Props, State> {
                 onClick={(e) => this.createWaypoint(e)}
               ></circle>
             )}
+          {layers[ViewLayers.Obstacles] &&
+            this.context.model.uiState.isNavbarObstacleSelected() && (
+              <circle
+                cx={0}
+                cy={0}
+                r={10000}
+                style={{ fill: "transparent" }}
+                onClick={(e) => this.createObstacle(e)}
+              ></circle>
+            )}
+          {layers[ViewLayers.Obstacles] &&
+            this.context.model.document.pathlist.activePath.obstacles.map(
+              (obstacle, index) => (
+                <FieldObstacle
+                  obstacle={obstacle}
+                  index={index}
+                ></FieldObstacle>
+              )
+            )}
+          {/* Line paths */}
+          {layers[ViewLayers.Waypoints] && <FieldPathLines></FieldPathLines>}
+          {layers[ViewLayers.Trajectory] && (
+            <FieldGeneratedLines></FieldGeneratedLines>
+          )}
           {layers[ViewLayers.Waypoints] &&
             this.context.model.document.pathlist.activePath.waypoints.map(
               (point, index) => (
@@ -196,5 +221,26 @@ class FieldOverlayRoot extends Component<Props, State> {
       this.context.history.stopGroup();
     }
   }
+  createObstacle(e: React.MouseEvent<SVGCircleElement, MouseEvent>): void {
+    if (e.currentTarget === e.target) {
+      var coords = this.screenSpaceToFieldSpace(this.svgRef?.current, {
+        x: e.clientX,
+        y: e.clientY,
+      });
+      this.context.history.startGroup(() => {
+        var newObstacle =
+          this.context.model.document.pathlist.activePath.addObstacle(
+            CircularObstacleStore.create({
+              x: coords.x,
+              y: coords.y,
+              radius: 0.5,
+              uuid: uuidv4(),
+            })
+          );
+        // const selectedItem = this.context.model.uiState.selectedNavbarItem;
+      });
+      this.context.history.stopGroup();
+    }
+  }
 }
 export default observer(FieldOverlayRoot);