Add width specification to SE2 #3

au-rmr · Jul 6, 2022 · 72a6c7f · 72a6c7f
1 parent 25dfd01
commit 72a6c7f
Show file tree

Hide file tree

Showing 5 changed files with 63 additions and 20 deletions.
diff --git a/src/features/se2/SE2.tsx b/src/features/se2/SE2.tsx
@@ -1,20 +1,24 @@
 import * as log from "loglevel";
 import { useState, CSSProperties, useRef, useEffect } from "react";
 import { useAppDispatch, useAppSelector } from "../../app/hooks";
+import { Box, ButtonGroup, FormControl, FormControlLabel, FormLabel, Radio, RadioGroup, Stack } from "@mui/material";
+import { SE2Types } from "./se2.utils";
 import { setPose, selectPose } from "./se2Slice";
 
 import TargetAnchor from "./TargetAnchor";
 
 // from https://ieeexplore.ieee.org/document/9636008
 function SE2(
     {
-        width, height, scale = 1, interfaceType,
-        color = { default: "black", accent: "grey" }, style
+        width, height, scale = 1, interfaceType, widthInputType,
+        color = { default: "black", accent: "grey" }, style,
+        background
     }: {
         width: number,
         height: number,
         scale?: number,
         interfaceType: 'targetanchor',
+        widthInputType: 'none' | 'buttons' | 'drag'
         color?: {
             default?: string,
             accent?: string,
@@ -25,12 +29,14 @@ function SE2(
             }
         }
         style?: CSSProperties
+        background?: JSX.Element
     }) {
     const dispatch = useAppDispatch();
     const pose = useAppSelector(selectPose);
 
-    const [localPose, setLocalPose] = useState({ x: -1, y: -1, theta: 0 })
+    const [localPose, setLocalPose] = useState<SE2Types.Pose>({ x: -1, y: -1, theta: 0, width: 0 })
     const [mousePressed, setMousePressed] = useState(false);
+    const [poseWidth, setPoseWidth] = useState(40);
 
     const onMouseDown = (event) => {
         const { top, left } = event.target.getBoundingClientRect();
@@ -41,7 +47,8 @@ function SE2(
         setLocalPose({
             x: x,
             y: y,
-            theta: 0
+            theta: 0,
+            width: 0
         })
 
         setMousePressed(true)
@@ -50,14 +57,19 @@ function SE2(
     const onMouseMove = (event) => {
         if (mousePressed) {
             const { top, left } = event.target.getBoundingClientRect();
-    
+
             const x = event.clientX - left;
             const y = event.clientY - top;
 
+            if (widthInputType == 'drag') {
+                setPoseWidth(Math.sqrt(Math.pow(x - localPose.x, 2) + Math.pow(y - localPose.y, 2)));
+            }
+
             setLocalPose({
                 x: localPose.x,
                 y: localPose.y,
-                theta: -Math.atan2(y - localPose.y, x - localPose.x)
+                theta: -Math.atan2(y - localPose.y, x - localPose.x),
+                width: poseWidth
             })
         }
     }
@@ -78,12 +90,34 @@ function SE2(
         }
     }, [pose])
 
-    return <svg viewBox={`0 0 ${width/scale} ${height/scale}`} width={width} height={height} style={style}
-        onMouseDown={onMouseDown} onMouseMove={onMouseMove} onMouseUp={onMouseUp} onMouseLeave={onMouseLeave}>
-        {interfaceType == "targetanchor" &&
-            <TargetAnchor x={localPose.x/scale} y={localPose.y/scale} theta={ - localPose.theta * 180 / Math.PI} color={mousePressed ? color.accent : color.default} />
-        }
-    </svg>
+    return <Stack direction='column' spacing={2}>
+        <Box sx={{ position: "relative", width: width, height: height }}>
+            <svg viewBox={`0 0 ${width / scale} ${height / scale}`} width={width} height={height} style={style}
+                onMouseDown={onMouseDown} onMouseMove={onMouseMove} onMouseUp={onMouseUp} onMouseLeave={onMouseLeave}>
+                {interfaceType == "targetanchor" &&
+                    <TargetAnchor x={localPose.x / scale} y={localPose.y / scale} theta={- localPose.theta * 180 / Math.PI} color={mousePressed ? color.accent : color.default} width={poseWidth / scale} />
+                }
+            </svg>
+            {background}
+        </Box>
+        <Box>
+            <FormLabel id="demo-controlled-radio-buttons-group">Gripper Width</FormLabel>
+            <RadioGroup
+                aria-labelledby="demo-controlled-radio-buttons-group"
+                name="controlled-radio-buttons-group"
+                row
+                value={poseWidth}
+                onChange={(event: React.ChangeEvent<HTMLInputElement>) => {
+                    setPoseWidth(parseInt((event.target as HTMLInputElement).value))
+                }}
+            >
+                <FormControlLabel value={40} control={<Radio />} label="Small" />
+                <FormControlLabel value={100} control={<Radio />} label="Medium" />
+                <FormControlLabel value={200} control={<Radio />} label="Large" />
+            </RadioGroup>
+        </Box>
+    </Stack>
+
 
 }
 

diff --git a/src/features/se2/TargetAnchor.tsx b/src/features/se2/TargetAnchor.tsx
@@ -1,8 +1,8 @@
 import { SE2Types } from './se2.utils'
 
-function TargetAnchor({x, y, theta, color}: SE2Types.Pose & {color: string}) {
+function TargetAnchor({x, y, theta, width, color}: SE2Types.Pose & {color: string}) {
     return <g transform={`translate(${x} ${y}) rotate(${theta})`}>
-        <line x1={0} y1={0} x2={40} y2={0} stroke={color} strokeLinecap="round" strokeWidth={7}/>
+        <line x1={0} y1={0} x2={width} y2={0} stroke={color} strokeLinecap="round" strokeWidth={7}/>
         <circle cx={0} cy={0} r={7} fill={color}/>
     </g>
 }

diff --git a/src/features/se2/se2.utils.ts b/src/features/se2/se2.utils.ts
@@ -3,5 +3,6 @@ export namespace SE2Types {
         x: number,
         y: number,
         theta: number
+        width: number
     }
 }
diff --git a/src/features/se2/se2Slice.ts b/src/features/se2/se2Slice.ts
@@ -6,7 +6,8 @@ import { SE2Types } from "./se2.utils";
 const initialState: SE2Types.Pose = {
     x: -1,
     y: -1,
-    theta: 0
+    theta: 0,
+    width: 0
 }
 
 export const se2Slice = createSlice({
@@ -17,6 +18,7 @@ export const se2Slice = createSlice({
             state.x = action.payload.x;
             state.y = action.payload.y;
             state.theta = action.payload.theta;
+            state.width = action.payload.width;
         }
     }
 })

diff --git a/src/pages/index.tsx b/src/pages/index.tsx
@@ -37,7 +37,8 @@ const IndexPage: NextPage = () => {
     dispatch(generateHeuristicGrasp({
       x: se2.x / videoStreamSize.x,
       y: se2.y / videoStreamSize.y,
-      theta: se2.theta
+      theta: se2.theta,
+      width: se2.width
     }))
   };
 
@@ -50,10 +51,15 @@ const IndexPage: NextPage = () => {
       <Stack spacing={2} alignItems="center" mt={2}>
         {/* <Counter /> */}
         <Stack spacing={2} direction="row" alignItems="center">
-          <Box sx={{ position: "relative", width: videoStreamSize.x, height: videoStreamSize.y }}>
-            <SE2 width={videoStreamSize.x} height={videoStreamSize.y} interfaceType="targetanchor" style={{ border: 'solid', borderRadius: 4, position: "absolute", top: 0, left: 0, zIndex: 10 }} />
-            <ROSVideoDisplay style={{ borderRadius: 4, width: videoStreamSize.x, position: "absolute", top: 0, left: 0 }} topicName="/camera_wrist/color/image_raw/compressed" streamSizeCallback={handleStreamSize} />
-          </Box>
+          <SE2 width={videoStreamSize.x} height={videoStreamSize.y}
+            interfaceType="targetanchor" widthInputType="buttons"
+            style={{ border: 'solid', borderRadius: 4, position: "absolute", top: 0, left: 0, zIndex: 10 }}
+            background={<ROSVideoDisplay
+              topicName="/camera_wrist/color/image_raw/compressed"
+              streamSizeCallback={handleStreamSize}
+              style={{ borderRadius: 4, width: videoStreamSize.x, position: "absolute", top: 0, left: 0 }}
+            />}
+          />
           <Paper>
             <Stack spacing={2} sx={{ m: 2 }}>
               <LoadingButton