From ec028919dc6d2bece0b020e0394f7f657996413b Mon Sep 17 00:00:00 2001
From: GLDuval <46896242+GLDuval@users.noreply.github.com>
Date: Mon, 6 Mar 2023 09:00:18 -0500
Subject: [PATCH] Arm presets config page (#163)

---
 src/renderer/components/StatusBar.tsx         |  16 ++-
 .../components/pages/Config/ConfigMenu.tsx    |   3 +
 .../components/pages/Config/ConfigPage.tsx    |   2 +
 .../pages/ArmPresetsConfig/ArmJointInput.tsx  |  53 +++++++
 .../pages/ArmPresetsConfig/ArmPreset.tsx      | 136 ++++++++++++++++++
 .../ArmPresetsConfig/ArmPresetsConfig.tsx     |  92 ++++++++++++
 src/renderer/hooks/useArmJointPositions.ts    |  27 ++++
 src/renderer/inputSystem/armModeActions.ts    |  29 ++--
 src/renderer/store/localStorage.ts            |   2 +
 src/renderer/store/modules/armPresets.ts      |  63 ++++++++
 src/renderer/store/store.ts                   |   2 +
 11 files changed, 412 insertions(+), 13 deletions(-)
 create mode 100644 src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmJointInput.tsx
 create mode 100644 src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmPreset.tsx
 create mode 100644 src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmPresetsConfig.tsx
 create mode 100644 src/renderer/hooks/useArmJointPositions.ts
 create mode 100644 src/renderer/store/modules/armPresets.ts

diff --git a/src/renderer/components/StatusBar.tsx b/src/renderer/components/StatusBar.tsx
index c84a4dd8..694fa776 100644
--- a/src/renderer/components/StatusBar.tsx
+++ b/src/renderer/components/StatusBar.tsx
@@ -17,6 +17,7 @@ import { BiWifi, BiWifi0, BiWifi1, BiWifi2, BiWifiOff } from 'react-icons/bi';
 import { useDispatch } from 'react-redux';
 import { toast } from 'react-toastify';
 import { ArmContext, armService } from '../state/arm';
+import { selectSelectedPreset } from '../store/modules/armPresets';
 
 export const StatusBar: FC = () => (
   <StyledStatusBarWrapper>
@@ -73,6 +74,8 @@ const ModeInfo = () => {
   const [arm] = useActor(armService);
   const [control] = useActor(controlService);
   const isReverse = useSelector(selectReverse);
+  const selectedArmPreset = useSelector(selectSelectedPreset);
+
   if (control.matches('flipper')) {
     return (
       <div>
@@ -87,11 +90,14 @@ const ModeInfo = () => {
     );
   } else if (control.matches('arm')) {
     return (
-      <div>
-        {arm.matches('joint')
-          ? 'JOINT ' + String((arm.context as ArmContext).jointValue + 1)
-          : 'CARTESIAN'}
-      </div>
+      <>
+        <div>
+          {arm.matches('joint')
+            ? 'JOINT ' + String((arm.context as ArmContext).jointValue + 1)
+            : 'CARTESIAN'}
+        </div>
+        <div>{selectedArmPreset.name.toUpperCase()}</div>
+      </>
     );
   } else {
     return <div />;
diff --git a/src/renderer/components/pages/Config/ConfigMenu.tsx b/src/renderer/components/pages/Config/ConfigMenu.tsx
index 8807f2f1..6d908722 100644
--- a/src/renderer/components/pages/Config/ConfigMenu.tsx
+++ b/src/renderer/components/pages/Config/ConfigMenu.tsx
@@ -16,6 +16,9 @@ export const ConfigMenu: FC = () => {
         <li>
           <StyledNavLink to="/config/graph">Graph</StyledNavLink>
         </li>
+        <li>
+          <StyledNavLink to="/config/armPresets">Arm Presets</StyledNavLink>
+        </li>
         <li>
           <StyledNavLink to="/config/gamepad">Gamepad</StyledNavLink>
         </li>
diff --git a/src/renderer/components/pages/Config/ConfigPage.tsx b/src/renderer/components/pages/Config/ConfigPage.tsx
index 9cd5e966..066ad728 100644
--- a/src/renderer/components/pages/Config/ConfigPage.tsx
+++ b/src/renderer/components/pages/Config/ConfigPage.tsx
@@ -7,6 +7,7 @@ import { GamepadConfig } from '@/renderer/components/pages/Config/pages/GamepadC
 import { GraphConfig } from '@/renderer/components/pages/Config/pages/GraphConfig/GraphConfig';
 import { styled } from '@/renderer/globalStyles/styled';
 import { LaunchConfig } from './pages/LaunchConfig/LaunchConfig';
+import ArmPresetsConfig from './pages/ArmPresetsConfig/ArmPresetsConfig';
 
 export const ConfigPage: FC = () => {
   return (
@@ -20,6 +21,7 @@ export const ConfigPage: FC = () => {
           <Route path="/general" element={<GeneralConfig />} />
           <Route path="/camera" element={<CameraConfig />} />
           <Route path="/graph" element={<GraphConfig />} />
+          <Route path="/armPresets" element={<ArmPresetsConfig />} />
           <Route path="/gamepad" element={<GamepadConfig />} />
           <Route path="/launch" element={<LaunchConfig />} />
         </Routes>
diff --git a/src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmJointInput.tsx b/src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmJointInput.tsx
new file mode 100644
index 00000000..601521e7
--- /dev/null
+++ b/src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmJointInput.tsx
@@ -0,0 +1,53 @@
+import { LabeledInput } from '@/renderer/components/common/LabeledInput';
+import React, { useCallback, useState } from 'react';
+
+interface ArmJointInputProps {
+  onChange: (event: React.ChangeEvent<HTMLInputElement>, id: number) => void;
+  value: number;
+  id: number;
+  min: number;
+  max: number;
+}
+
+const ArmJointInput = ({
+  onChange,
+  value,
+  id,
+  min,
+  max,
+}: ArmJointInputProps) => {
+  const [inputValue, setInputValue] = useState(value.toString());
+  const [error, setError] = useState(false);
+
+  const onInputChange = useCallback(
+    (event: React.ChangeEvent<HTMLInputElement>) => {
+      const jointValue = parseInt(event.target.value);
+      setInputValue(event.target.value);
+      if (jointValue >= min && jointValue <= max) {
+        onChange(event, id);
+        setError(false);
+      } else {
+        setError(true);
+      }
+    },
+    [id, max, min, onChange]
+  );
+
+  return (
+    <div>
+      <LabeledInput
+        label={`Joint ${id + 1}`}
+        value={inputValue}
+        type="number"
+        onChange={onInputChange}
+      />
+      {error && (
+        <div>
+          Value must be between {min} and {max}
+        </div>
+      )}
+    </div>
+  );
+};
+
+export default ArmJointInput;
diff --git a/src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmPreset.tsx b/src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmPreset.tsx
new file mode 100644
index 00000000..0c050c1a
--- /dev/null
+++ b/src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmPreset.tsx
@@ -0,0 +1,136 @@
+import React, { useCallback } from 'react';
+import {
+  ArmPreset,
+  armPresetsSlice,
+} from '@/renderer/store/modules/armPresets';
+import { styled } from '@/renderer/globalStyles/styled';
+import { LabeledInput } from '@/renderer/components/common/LabeledInput';
+import { useDispatch } from 'react-redux';
+import { FaTimes } from 'react-icons/fa';
+import ArmJointInput from './ArmJointInput';
+
+interface ArmPresetProps {
+  preset: ArmPreset;
+}
+
+//Temporary UI limit for each joint value.
+interface JointBoundaries {
+  min: number;
+  max: number;
+}
+
+const jointBoundaries: JointBoundaries[] = [
+  { min: 0, max: 360 }, //Joint 1
+  { min: 100, max: 260 }, //Joint 2
+  { min: 95, max: 260 }, //Joint 3
+  { min: 0, max: 360 }, //Joint 4
+  { min: 100, max: 250 }, //Joint 5
+  { min: 0, max: 360 }, //Joint 6
+];
+
+const Card = styled.div`
+  background-color: ${({ theme }) => theme.colors.darkerBackground};
+  border-radius: 4px;
+  padding: 16px;
+  margin: 8px;
+  display: flex;
+  min-width: 300px;
+  flex-direction: column;
+  justify-content: flex-start;
+  align-items: flex-start;
+  box-shadow: 0 0 10px 0 rgba(0, 0, 0, 0.5);
+  transition: box-shadow 0.2s ease-in-out;
+`;
+
+const HeaderRow = styled.div`
+  display: flex;
+  flex-direction: row;
+  justify-content: space-between;
+  align-items: center;
+  width: 100%;
+`;
+
+const DeleteButton = styled.a`
+  border: none;
+  border-radius: 4px;
+  padding: 4px 8px;
+  margin-top: 8px;
+  cursor: pointer;
+  transition: background-color 0.2s ease-in-out;
+  &:hover {
+    color: ${({ theme }) => theme.colors.danger};
+  }
+`;
+
+const ArmPreset = ({ preset }: ArmPresetProps) => {
+  const dispatch = useDispatch();
+
+  const onJointChange = useCallback(
+    (event: React.ChangeEvent<HTMLInputElement>, id: number) => {
+      const jointValue = parseInt(event.target.value);
+      // Update the preset in the store with the new joint value.
+      dispatch(
+        armPresetsSlice.actions.updatePreset({
+          ...preset,
+          positions: [
+            ...preset.positions.slice(0, id),
+            jointValue,
+            ...preset.positions.slice(id + 1),
+          ],
+        })
+      );
+    },
+    [dispatch, preset]
+  );
+
+  const onNameChange = useCallback(
+    (event: React.ChangeEvent<HTMLInputElement>) => {
+      // Update the preset in the store with the new name.
+      dispatch(
+        armPresetsSlice.actions.updatePreset({
+          ...preset,
+          name: event.target.value,
+        })
+      );
+    },
+    [dispatch, preset]
+  );
+
+  const onDelete = useCallback(
+    (id: string) => {
+      // Delete the preset from the store.
+      dispatch(armPresetsSlice.actions.removePreset(id));
+    },
+    [dispatch]
+  );
+
+  return (
+    <Card>
+      <HeaderRow>
+        <h3>{preset.name}</h3>
+        <DeleteButton onClick={() => onDelete(preset.id)}>
+          <FaTimes />
+        </DeleteButton>
+      </HeaderRow>
+      <LabeledInput
+        label="Preset Name"
+        value={preset.name}
+        type="text"
+        onChange={onNameChange}
+      />
+      {preset.positions.map((joint, index) => (
+        <div key={index}>
+          <ArmJointInput
+            value={joint}
+            id={index}
+            onChange={onJointChange}
+            min={jointBoundaries[index].min}
+            max={jointBoundaries[index].max}
+          />
+        </div>
+      ))}
+    </Card>
+  );
+};
+
+export default ArmPreset;
diff --git a/src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmPresetsConfig.tsx b/src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmPresetsConfig.tsx
new file mode 100644
index 00000000..624aceb5
--- /dev/null
+++ b/src/renderer/components/pages/Config/pages/ArmPresetsConfig/ArmPresetsConfig.tsx
@@ -0,0 +1,92 @@
+import { Button } from '@/renderer/components/common/Button';
+import { Select } from '@/renderer/components/common/Select';
+import { styled } from '@/renderer/globalStyles/styled';
+import useArmJointPositions from '@/renderer/hooks/useArmJointPositions';
+import {
+  armPresetsSlice,
+  selectAllPresets,
+  selectSelectedPreset,
+} from '@/renderer/store/modules/armPresets';
+import { round } from 'lodash';
+import { nanoid } from 'nanoid';
+import React, { useCallback } from 'react';
+import { FaPlus } from 'react-icons/fa';
+import { useDispatch, useSelector } from 'react-redux';
+import { SectionTitle } from '../../styles';
+import ArmPreset from './ArmPreset';
+
+const PresetsContainer = styled.div`
+  display: flex;
+  flex-direction: row;
+  justify-content: flex-start;
+  align-items: flex-start;
+  width: 100%;
+  flex-wrap: wrap;
+`;
+
+const ArmPresetsConfig = () => {
+  const armPresets = useSelector(selectAllPresets);
+  const selectedPreset = useSelector(selectSelectedPreset);
+  const dispatch = useDispatch();
+  const jointPositions = useArmJointPositions() ?? [];
+
+  const addPreset = useCallback(() => {
+    dispatch(
+      armPresetsSlice.actions.addPreset({
+        id: nanoid(),
+        name: 'New Preset',
+        positions: [180, 180, 180, 180, 180, 180],
+      })
+    );
+  }, [dispatch]);
+
+  const onPresetSelect = useCallback(
+    (e: React.ChangeEvent<HTMLSelectElement>) => {
+      dispatch(armPresetsSlice.actions.selectPreset(e.target.value));
+    },
+    [dispatch]
+  );
+
+  return (
+    armPresets &&
+    selectedPreset && (
+      <>
+        <SectionTitle>Selected Preset</SectionTitle>
+        <Select
+          options={armPresets.map((preset) => ({
+            key: preset.id,
+            content: preset.name,
+            value: preset.id,
+          }))}
+          value={selectedPreset.id}
+          onChange={onPresetSelect}
+        />
+        {jointPositions.length > 0 && (
+          <>
+            <SectionTitle>Current positions</SectionTitle>
+            <div>
+              {jointPositions.map((position, index) => (
+                <div key={index}>
+                  Joint {index + 1}: {round(position, 0)}
+                </div>
+              ))}
+            </div>
+          </>
+        )}
+
+        <SectionTitle>Presets</SectionTitle>
+        <Button onClick={addPreset}>
+          <FaPlus />
+          <span style={{ marginLeft: '0.2rem' }}>Add Preset</span>
+        </Button>
+        <PresetsContainer>
+          {armPresets.map((preset) => (
+            <ArmPreset key={preset.id} preset={preset} />
+          ))}
+        </PresetsContainer>
+      </>
+    )
+  );
+};
+
+export default ArmPresetsConfig;
diff --git a/src/renderer/hooks/useArmJointPositions.ts b/src/renderer/hooks/useArmJointPositions.ts
new file mode 100644
index 00000000..fa9a60b0
--- /dev/null
+++ b/src/renderer/hooks/useArmJointPositions.ts
@@ -0,0 +1,27 @@
+import { useCallback, useState } from 'react';
+import { TopicOptions } from '../utils/ros/roslib-ts-client/@types';
+import { useRosSubscribeNoData } from './useRosSubscribe';
+
+const jointPositionTopic: TopicOptions = {
+  name: 'ovis/arm/out/joint_position',
+  messageType: 'ovis_msgs/OvisJointPosition',
+};
+
+interface JointPositionMsg {
+  joint_positions: number[];
+}
+
+const useArmJointPositions = () => {
+  const [jointPositions, setJointPositions] = useState<number[]>();
+
+  useRosSubscribeNoData<JointPositionMsg>(
+    jointPositionTopic,
+    useCallback((message) => {
+      setJointPositions(message.joint_positions);
+    }, [])
+  );
+
+  return jointPositions;
+};
+
+export default useArmJointPositions;
diff --git a/src/renderer/inputSystem/armModeActions.ts b/src/renderer/inputSystem/armModeActions.ts
index 2af512e2..e39701ab 100644
--- a/src/renderer/inputSystem/armModeActions.ts
+++ b/src/renderer/inputSystem/armModeActions.ts
@@ -2,16 +2,25 @@ import { buttons as buttonMappings } from '@/renderer/inputSystem/mappings';
 import { rosClient } from '@/renderer/utils/ros/rosClient';
 import { Action } from '@/renderer/inputSystem/@types';
 import { TopicOptions } from '@/renderer/utils/ros/roslib-ts-client/@types';
-import { log } from '@/renderer/logger';
 import { ArmContext, armService } from '../state/arm';
 import { deadzone } from '../utils/gamepad';
 import { handleTpvControl } from './tpvControl';
+import { store } from '../store/store';
+import {
+  armPresetsSlice,
+  selectSelectedPreset,
+} from '../store/modules/armPresets';
 
 const jointGoalTopic: TopicOptions = {
   name: 'ovis/arm/in/joint_velocity_goal',
   messageType: 'ovis_msgs/OvisJointVelocity',
 };
 
+const jointPositionTopic: TopicOptions = {
+  name: 'ovis/arm/in/joint_position_goal',
+  messageType: 'ovis_msgs/OvisJointPosition',
+};
+
 export const armModeActions: Action[] = [
   {
     name: 'modeCartesian',
@@ -49,14 +58,18 @@ export const armModeActions: Action[] = [
   },
   {
     name: 'home',
-    bindings: [
-      { type: 'gamepadBtnDown', button: buttonMappings.Y },
-      // { type: 'keyboard', code: 'KeyT', onKeyDown: true },
-    ],
+    bindings: [{ type: 'gamepadBtnDown', button: buttonMappings.Y }],
+    perform: () => {
+      rosClient.publish(jointPositionTopic, {
+        joint_positions: selectSelectedPreset(store.getState()).positions,
+      });
+    },
+  },
+  {
+    name: 'changePreset',
+    bindings: [{ type: 'gamepadBtnDown', button: buttonMappings.RS }],
     perform: () => {
-      rosClient
-        .callService({ name: '/ovis/arm/in/home_joint_positions' })
-        .catch(log.error);
+      store.dispatch(armPresetsSlice.actions.nextPreset());
     },
   },
   {
diff --git a/src/renderer/store/localStorage.ts b/src/renderer/store/localStorage.ts
index dc095040..877768f3 100644
--- a/src/renderer/store/localStorage.ts
+++ b/src/renderer/store/localStorage.ts
@@ -5,6 +5,7 @@ import { initialState as rosState } from '@/renderer/store/modules/ros';
 import { initialState as inputState } from '@/renderer/store/modules/input';
 import { initialState as debugTabState } from '@/renderer/store/modules/debugTab';
 import { initialState as launchFilesState } from '@/renderer/store/modules/launchFiles';
+import { initialState as armPresetsState } from '@/renderer/store/modules/armPresets';
 import { log } from '@/renderer/logger';
 
 export const defaultState: GlobalState = {
@@ -13,6 +14,7 @@ export const defaultState: GlobalState = {
   input: inputState,
   debugTab: debugTabState,
   launchFiles: launchFilesState,
+  armPresets: armPresetsState,
 };
 
 // WARN
diff --git a/src/renderer/store/modules/armPresets.ts b/src/renderer/store/modules/armPresets.ts
new file mode 100644
index 00000000..bc6035a0
--- /dev/null
+++ b/src/renderer/store/modules/armPresets.ts
@@ -0,0 +1,63 @@
+import { createSlice, PayloadAction } from '@reduxjs/toolkit';
+import { nanoid } from 'nanoid';
+import { GlobalState } from '../store';
+
+export interface ArmPresetsState {
+  presets: ArmPreset[];
+  selectedPreset: ArmPreset;
+}
+
+export interface ArmPreset {
+  id: string;
+  name: string;
+  positions: number[];
+}
+
+const initialPreset: ArmPreset = {
+  id: nanoid(),
+  name: 'Right Angle',
+  positions: [180, 180, 180, 180, 180, 180],
+};
+
+export const initialState: ArmPresetsState = {
+  presets: [initialPreset],
+  selectedPreset: initialPreset,
+};
+
+export const armPresetsSlice = createSlice({
+  name: 'armPresets',
+  initialState,
+  reducers: {
+    addPreset: (state, { payload }: PayloadAction<ArmPreset>) => {
+      state.presets.push(payload);
+    },
+    removePreset: (state, { payload }: PayloadAction<string>) => {
+      state.presets = state.presets.filter((preset) => preset.id !== payload);
+    },
+    selectPreset: (state, { payload }: PayloadAction<string>) => {
+      state.selectedPreset =
+        state.presets.find((preset) => preset.id === payload) ?? initialPreset;
+    },
+    nextPreset: (state) => {
+      const index = state.presets.findIndex(
+        (preset) => preset.id === state.selectedPreset.id
+      );
+      state.selectedPreset =
+        state.presets[(index + 1) % state.presets.length] ?? initialPreset;
+    },
+    updatePreset: (state, { payload }: PayloadAction<ArmPreset>) => {
+      const index = state.presets.findIndex(
+        (preset) => preset.id === payload.id
+      );
+      if (index !== -1) {
+        state.presets[index] = payload;
+      }
+    },
+  },
+});
+
+export const selectAllPresets = (state: GlobalState) =>
+  state.armPresets.presets;
+
+export const selectSelectedPreset = (state: GlobalState) =>
+  state.armPresets.selectedPreset;
diff --git a/src/renderer/store/store.ts b/src/renderer/store/store.ts
index 2b89eab7..a9146456 100644
--- a/src/renderer/store/store.ts
+++ b/src/renderer/store/store.ts
@@ -10,6 +10,7 @@ import { debugTabSlice } from '@/renderer/store/modules/debugTab';
 import { configureStore, AnyAction, combineReducers } from '@reduxjs/toolkit';
 import { throttle } from 'lodash';
 import { launchFilesSlice } from './modules/launchFiles';
+import { armPresetsSlice } from './modules/armPresets';
 
 const appReducer = combineReducers({
   feed: feedSlice.reducer,
@@ -17,6 +18,7 @@ const appReducer = combineReducers({
   input: inputSlice.reducer,
   debugTab: debugTabSlice.reducer,
   launchFiles: launchFilesSlice.reducer,
+  armPresets: armPresetsSlice.reducer,
 });
 
 export type GlobalState = ReturnType<typeof appReducer>;