Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

<Impl/rqt mirror frame>: rqt cleanup #621

Merged
merged 5 commits into from
Nov 14, 2024
Merged

<Impl/rqt mirror frame>: rqt cleanup #621

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
bef7ca2
Implemented mirror buttons in rqt
lvaddi Nov 13, 2024
cbc0684
Changed textfields to QDoubleSpinBox to prevent unwanted inputs
lvaddi Nov 13, 2024
2a16634
Fixed mirror error which came up because of the Qdoublespinbox
lvaddi Nov 13, 2024
0804abb
Fixed error when deleting last frame
lvaddi Nov 13, 2024
ab98899
Code cleanup
lvaddi Nov 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
88 changes: 55 additions & 33 deletions bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,17 +2,18 @@
import math
import os
import sys
from typing import Literal

import rclpy
from ament_index_python import get_package_share_directory
from PyQt5.QtCore import Qt
from PyQt5.QtCore import QLocale, Qt
from PyQt5.QtGui import QKeySequence
from PyQt5.QtWidgets import (
QAbstractItemView,
QDoubleSpinBox,
QFileDialog,
QGroupBox,
QLabel,
QLineEdit,
QListWidgetItem,
QMainWindow,
QMessageBox,
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -200,8 +199,8 @@ def q_joint_state_update(self, joint_states: JointState) -> None:
# Check if the we are currently positioning the motor and want to store the value
if motor_active and motor_torqueless:
# Update textfield
self._motor_controller_text_fields[motor_name].setText(
str(round(math.degrees(joint_states.position[joint_states.name.index(motor_name)]), 2))
self._motor_controller_text_fields[motor_name].setValue(
round(math.degrees(joint_states.position[joint_states.name.index(motor_name)]), 2)
)
# Update working values
self._working_angles[motor_name] = joint_states.position[joint_states.name.index(motor_name)]
Expand All @@ -224,9 +223,12 @@ def create_motor_controller(self) -> None:
layout.addWidget(label)

# Add a textfield to display the exact value of the motor
textfield = QLineEdit()
textfield.setText("0.0")
textfield.textEdited.connect(self.textfield_update)
textfield = QDoubleSpinBox()
textfield.setLocale(QLocale("C"))
textfield.setMaximum(180.0)
textfield.setMinimum(-180.0)
textfield.setValue(0.0)
textfield.valueChanged.connect(self.textfield_update)
layout.addWidget(textfield)
self._motor_controller_text_fields[motor_name] = textfield

Expand Down Expand Up @@ -299,8 +301,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)
Expand Down Expand Up @@ -520,6 +522,10 @@ def delete(self):
self._node.get_logger().error(str(e))
return
assert self._recorder.get_keyframe(frame) is not None, "Selected frame not found in list of keyframes"
# Check if only one frame is remaining
if len(self._widget.frameList) == 1:
QMessageBox.warning(self._widget, "Warning", "Cannot delete last remaining frame")
return
self._recorder.delete(frame)
self._widget.statusBar.showMessage(f"Deleted frame {frame}")
self.update_frames()
Expand Down Expand Up @@ -580,11 +586,32 @@ 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")
# Get direction to mirror to
mirrored_source = {"R": "L", "L": "R"}[source]

# Go through all active motors
for motor_name, angle in self._working_angles.items():
# Set mirrored angles
if motor_name.startswith(source):
mirrored_motor_name = mirrored_source + motor_name[1:]
# Make -0.0 to 0.0
mirrored_angle = -angle if angle != 0 else 0.0
self._working_angles[mirrored_motor_name] = mirrored_angle

# Update the UI
for motor_name, angle in self._working_angles.items():
# Block signals
self._motor_controller_text_fields[motor_name].blockSignals(True)
# Set values
self._motor_controller_text_fields[motor_name].setValue(round(math.degrees(angle), 2))
# Enable signals again
self._motor_controller_text_fields[motor_name].blockSignals(False)

self._widget.statusBar.showMessage("Mirrored frame")

def invert_frame(self):
"""
Expand Down Expand Up @@ -613,7 +640,12 @@ def invert_frame(self):

# 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)))
# Block signals
self._motor_controller_text_fields[motor_name].blockSignals(True)
# Set values
self._motor_controller_text_fields[motor_name].setValue(round(math.degrees(angle), 2))
# Enable signals again
self._motor_controller_text_fields[motor_name].blockSignals(False)

self._widget.statusBar.showMessage("Inverted frame")

Expand Down Expand Up @@ -657,12 +689,13 @@ def react_to_frame_change(self):

# Update the motor angle controls (value and active state)
if active:
self._motor_controller_text_fields[motor_name].setText(
str(round(math.degrees(selected_frame["goals"][motor_name]), 2))
self._motor_controller_text_fields[motor_name].setValue(
round(math.degrees(selected_frame["goals"][motor_name]), 2)
)

self._working_angles[motor_name] = selected_frame["goals"][motor_name]
else:
self._motor_controller_text_fields[motor_name].setText("0.0")
self._motor_controller_text_fields[motor_name].setValue(0.0)

# Update the duration and pause
self._widget.spinBoxDuration.setValue(selected_frame["duration"])
Expand All @@ -675,23 +708,12 @@ def textfield_update(self):
If the textfield is updated, update working values
"""
for motor_name, text_field in self._motor_controller_text_fields.items():
try:
# Get the angle from the textfield
angle = float(text_field.text())
except ValueError:
# Display QMessageBox stating that the value is not a number
QMessageBox.warning(
self._widget,
"Warning",
f"Please enter a valid number.\n '{text_field.text()}' is not a valid number.",
)
continue
# Clip the angle to the maximum and minimum, we do this in degrees,
# because we do not want introduce rounding errors in the textfield
angle = round(max(-180.0, min(angle, 180.0)), 2)
# Get the angle from the textfield
angle = text_field.value()
angle = round(angle, 2)
# Set the angle in the textfield
if float(text_field.text()) != float(angle):
text_field.setText(str(angle))
if text_field.value() != angle:
text_field.setValue(angle)
# Set the angle in the working values if the motor is active
if self._motor_switcher_active_checkbox[motor_name].checkState(0) == Qt.CheckState.Checked:
self._working_angles[motor_name] = math.radians(angle)
Expand Down
Loading