From bef7ca2c07897aab8c828caf2a65d8c44df30d3a Mon Sep 17 00:00:00 2001
From: David Nguyen <nguyendavid2512@gmail.com>
Date: Wed, 13 Nov 2024 16:09:55 +0100
Subject: [PATCH] Implemented mirror buttons in rqt

---
 .../bitbots_animation_rqt/record_ui.py        | 27 ++++++++++++++-----
 1 file changed, 21 insertions(+), 6 deletions(-)

diff --git a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py
index 32a83bb96..97c221848 100755
--- a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py
+++ b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py
@@ -2,6 +2,7 @@
 import math
 import os
 import sys
+from typing import Literal
 
 import rclpy
 from ament_index_python import get_package_share_directory
@@ -84,8 +85,6 @@ def __init__(self, context):
         # Initialize the working values
         self._working_angles: dict[str, float] = {}
 
-        self.update_time = 0.1  # TODO what is this for?
-
         # QT directory for saving files
         self._save_directory = None
 
@@ -299,8 +298,8 @@ def connect_gui_callbacks(self) -> None:
         self._widget.actionAnimation_until_Frame.triggered.connect(self.play_until)
         self._widget.actionDuplicate_Frame.triggered.connect(self.duplicate)
         self._widget.actionDelete_Frame.triggered.connect(self.delete)
-        self._widget.actionLeft.triggered.connect(lambda: self.mirror_frame("L"))
-        self._widget.actionRight.triggered.connect(lambda: self.mirror_frame("R"))
+        self._widget.actionLeft.triggered.connect(lambda: self.mirror_frame("R"))
+        self._widget.actionRight.triggered.connect(lambda: self.mirror_frame("L"))
         self._widget.actionInvert.triggered.connect(self.invert_frame)
         self._widget.actionUndo.triggered.connect(self.undo)
         self._widget.actionRedo.triggered.connect(self.redo)
@@ -580,11 +579,27 @@ def redo(self):
         self._widget.statusBar.showMessage(status)
         self.update_frames()
 
-    def mirror_frame(self, direction):
+    def mirror_frame(self, source: Literal["L", "R"]) -> None:
         """
         Copies all motor values from one side of the robot to the other. Inverts values, if necessary
         """
-        raise NotImplementedError("This function is not implemented yet")
+        mirrored_source = {"R": "L", "L": "R"}[source]
+
+        # Go through all active motors
+        for motor_name, angle in self._working_angles.items():
+            # Check if the motor is on the right or left side and get the mirrored motor name
+            if motor_name.startswith(source):
+                mirrored_motor_name = mirrored_source + motor_name[1:]
+                if self._working_angles[motor_name] == 0.0:
+                    self._working_angles[mirrored_motor_name] = angle
+                else:
+                    self._working_angles[mirrored_motor_name] = -angle
+
+        # Update the UI
+        for motor_name, angle in self._working_angles.items():
+            self._motor_controller_text_fields[motor_name].setText(str(round(math.degrees(angle), 2)))
+
+        self._widget.statusBar.showMessage("Mirrored frame")
 
     def invert_frame(self):
         """