Fix loading of ui components

bitbots_motion/bitbots_recordui_rqt/src/bitbots_recordui_rqt/animation_recording.py → bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/animation_recording.py

@@ -10,16 +10,10 @@
 import json
 import os
 import math
-
-import actionlib
-import humanoid_league_msgs
-import rosparam
-from rosgraph import MasterException
-import rospy
 from copy import deepcopy
 from socket import gethostname
 import rospkg
-from humanoid_league_msgs.msg import PlayAnimationAction
+from bitbots_msgs.action import PlayAnimation
 
 import subprocess
 
@@ -53,7 +47,7 @@ def __init__(self):
         self.steps = []
         self.redo_steps = []
         self.current_state = AnimationData()
-        self.anim_client = actionlib.SimpleActionClient('animation', PlayAnimationAction)
+        self.anim_client = actionlib.SimpleActionClient('animation', PlayAnimation)
         self.save_step('Initial step')
 
     def get_animation_state(self):

bitbots_motion/bitbots_recordui_rqt/src/bitbots_recordui_rqt/record_ui.py → bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py

@@ -1,10 +1,9 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 import rospkg
-import rospy
+import rclpy
 import time
 import math
-import inspect
 from copy import deepcopy
 
 from python_qt_binding.QtCore import Qt, QMetaType, QDataStream, QVariant, pyqtSignal
@@ -13,60 +12,42 @@
 from python_qt_binding.QtWidgets import QMainWindow, QTreeWidget, QTreeWidgetItem,QListWidgetItem, \
     QSlider, QGroupBox, QVBoxLayout, QLabel, QLineEdit, QListWidget, QAbstractItemView, QFileDialog, QDoubleSpinBox, QMessageBox, \
     QInputDialog, QShortcut
-from python_qt_binding.QtGui import QDoubleValidator, QKeySequence
+from python_qt_binding.QtGui import QKeySequence
 
 from bitbots_msgs.msg import JointCommand, JointTorque
 from sensor_msgs.msg import JointState
-from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 
-import os
+from rqt_gui.main import Main
 
-from .animation_recording import Recorder
+from rclpy.node import Node
 
+import os, sys
 
+from ament_index_python import get_package_share_directory
 
-class DragDropList(QListWidget):
-    ''' QListWidget with an event that is called when a drag and drop action was performed.'''
-    keyPressed = pyqtSignal()
-
-    def __init__(self, parent, ui):
-        super(DragDropList, self).__init__(parent)
-
-        self.ui = ui
-        self.setAcceptDrops(True)
-
-
-    def dropEvent(self, e):
-        super(DragDropList, self).dropEvent(e)
-        items = []
-        for i in range(0, self.count()):
-            items.append(self.item(i).text())
-        self.ui.change_frame_order(items)
-
-    def keyPressEvent(self, event):
-        if event.key() == Qt.Key_Delete:
-            super(DragDropList, self).keyPressEvent(event)
-            self.keyPressed.emit()
-        elif event.key() == Qt.Key_Up and self.currentRow()-1 >= 0:
-                self.setCurrentRow(self.currentRow()-1)
-        elif event.key() == Qt.Key_Down and self.currentRow()+1 < self.count():
-            self.setCurrentRow(self.currentRow()+1)
-
+from bitbots_animation_rqt.animation_recording import Recorder
+from bitbots_animation_rqt.utils import DragDropList
 
 
 
 class RecordUI(Plugin):
-    ''' class containing the UI part of the framework'''
+    '''
+    This class is the main class for the RecordUI. It is a plugin for the rqt framework and is used to record animations.
+    '''
     def __init__(self, context):
-        super(RecordUI, self).__init__(context)
-
+        super().__init__(context)
+        self._node: Node = context.node
 
+        # Initialize the window
         self._widget = QMainWindow()
-        rp = rospkg.RosPack()
-        ui_file = os.path.join(rp.get_path('bitbots_recordui_rqt'), 'resource', 'RecordUI.ui')
+
+        # Load XML ui definition
+        ui_file = os.path.join(
+            get_package_share_directory("bitbots_animation_rqt"), "resource", "RecordUI.ui"
+        )
         loadUi(ui_file, self._widget, {})
 
-        self._recorder = Recorder()
+        #self._recorder = Recorder()
         self._sliders = {}
         self._textFields = {}
         self._motorSwitched = {}
@@ -77,7 +58,7 @@ def __init__(self, context):
         self._currentPause = 0.0
         self._currentName = "new frame"
 
-        self._workingValues = {}                # this is the data about the frame that is displayed
+        self._workingValues = {}              # this is the data about the frame that is displayed
         self._workingDuration = 1.0
         self._workingPause = 0.0
         self._workingName = self._currentName
@@ -86,7 +67,9 @@ def __init__(self, context):
 
         self._saveDir = None
 
-        self._robot_anim_path = None
+        self._robot_anim_path = os.path.join(
+            get_package_share_directory("wolfgang_animations"), "animations"
+        )
 
         #save current frame when switching to other frames for reference
         #working frame
@@ -108,16 +91,18 @@ def __init__(self, context):
 
         self.setObjectName('Record Animation')
 
-        self._initial_joints = None
-
         self._widget.frameList = DragDropList(self._widget, self)
         self._widget.verticalLayout_2.insertWidget(0, self._widget.frameList)
         self._widget.frameList.setDragDropMode(QAbstractItemView.InternalMove)
 
-        self.state_sub = rospy.Subscriber("joint_states", JointState, self.state_update, queue_size=1, tcp_nodelay=True)
-        self._joint_pub = rospy.Publisher("record_motor_goals", JointCommand, queue_size=1)
-        self.effort_pub = rospy.Publisher("ros_control/set_torque_individual", JointTorque, queue_size=1)
+        # Create subscriptions
+        self.state_sub = self._node.create_subscription(JointState, "joint_states", self.state_update, 1)
 
+        # Create publishers
+        self._joint_pub = self._node.create_publisher(JointCommand, "record_motor_goals", 1)
+        self.effort_pub = self._node.create_publisher(JointTorque, "ros_control/set_torque_individual", 1)
+
+        # Create a dictionary to map joint names to ids # TODO this should not be hardcoded
         self.ids = {"HeadPan": 0,
                "HeadTilt": 1,
                "LShoulderPitch": 2,
@@ -141,37 +126,19 @@ def __init__(self, context):
 
         self._initial_joints = JointState()
 
-        self.update_time = rospy.Duration(0.1)
+
+        self.update_time = 0.1
 
         for k,v in self.ids.items():
-            rospy.loginfo(k)
+            self._node.get_logger().info(f"Adding {k} to initial_joints")
             self._initial_joints.name.append(k)
             self._initial_joints.position.append(0)
 
-        while not self._initial_joints:
-            if not rospy.is_shutdown():
-                time.rospy.sleep(0.5)
-                rospy.logwarn("wait")
-            else:
-                return
-
-        self.initialize()
-
-        context.add_widget(self._widget)
-        self._widget.statusBar.showMessage("Initialization complete.")
-
-    def initialize(self):
         for i in range(0, len(self._initial_joints.name)):
             self._currentGoals[self._initial_joints.name[i]] = self._initial_joints.position[i]
             self._workingValues[self._initial_joints.name[i]] = self._initial_joints.position[i]
             self._motorSwitched[self._initial_joints.name[i]] = True
 
-
-        host = os.environ['ROS_MASTER_URI'].split('/')[2].split(':')[0]
-        self._robot_anim_path = "bitbots@{}:~/wolfgang_ws/src/wolfgang_robot/wolfgang_animations/animations/motion/".format(host)
-
-        rospy.loginfo("Set animation path to: "+str(self._robot_anim_path+"record.json"))
-
         initTorque = {}
         for k, v in self._workingValues.items():
             initTorque[k] = 2
@@ -186,6 +153,9 @@ def initialize(self):
         self.update_frames()
         self.set_sliders_and_text_fields(manual=True)
 
+        context.add_widget(self._widget)
+        self._widget.statusBar.showMessage("Initialization complete.")
+
     def state_update(self, joint_states):
         '''
         Callback method for /joint_states. Updates the sliders to the actual values of the motors when the robot moves.
@@ -232,7 +202,7 @@ def motor_controller(self):
             layout.addWidget(self._sliders[k])
             layout.addWidget(self._textFields[k])
             group.setLayout(layout)
-            self._widget.motorControlLayout.addWidget(group, i / 5, i % 5)
+            self._widget.motorControlLayout.addWidget(group, i // 5, i % 5)
             i = i+1
 
     def action_connect(self):
@@ -665,7 +635,7 @@ def frame_select(self):
             self._selected_frame = None
 
 
-            for v in self._recorder.get_animation_state():
+            for v in []: # TODO self._recorder.get_animation_state():
                 if v["name"] == selected_frame_name:
                     self._selected_frame = v
                     break
@@ -911,7 +881,7 @@ def update_frames(self, keep=False):
         '''
         current_index = self._widget.frameList.currentIndex()
         current_mode = self._widget.treeModeSelector.currentIndex()
-        current_state = self._recorder.get_animation_state()
+        current_state = [] #TODO self._recorder.get_animation_state()
         while self._widget.frameList.takeItem(0):
             continue
 
@@ -942,3 +912,9 @@ def shutdown_plugin(self):
         '''Clean up by sending the HCM a signal that we are no longer recording'''
         self._joint_pub.publish(JointCommand())
         rospy.sleep(1)
+
+
+def main():
+    plugin = "bitbots_animation_rqt.record_ui.RecordUI"
+    main = Main(filename=plugin)
+    sys.exit(main.main(standalone=plugin))

bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/utils.py

@@ -0,0 +1,29 @@
+from python_qt_binding.QtCore import Qt, pyqtSignal
+from python_qt_binding.QtWidgets import QListWidget
+
+class DragDropList(QListWidget):
+    ''' QListWidget with an event that is called when a drag and drop action was performed.'''
+    keyPressed = pyqtSignal()
+
+    def __init__(self, parent, ui):
+        super(DragDropList, self).__init__(parent)
+
+        self.ui = ui
+        self.setAcceptDrops(True)
+
+
+    def dropEvent(self, e):
+        super(DragDropList, self).dropEvent(e)
+        items = []
+        for i in range(0, self.count()):
+            items.append(self.item(i).text())
+        self.ui.change_frame_order(items)
+
+    def keyPressEvent(self, event):
+        if event.key() == Qt.Key_Delete:
+            super(DragDropList, self).keyPressEvent(event)
+            self.keyPressed.emit()
+        elif event.key() == Qt.Key_Up and self.currentRow()-1 >= 0:
+                self.setCurrentRow(self.currentRow()-1)
+        elif event.key() == Qt.Key_Down and self.currentRow()+1 < self.count():
+            self.setCurrentRow(self.currentRow()+1)

bitbots_motion/bitbots_animation_rqt/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>bitbots_animation_rqt</name>
+  <version>2.0.0</version>
+  <description>
+    A Python GUI plugin to record joint space animations.
+  </description>
+
+  <maintainer email="[email protected]">Florian Vahl</maintainer>
+  <maintainer email="[email protected]">Hamburg Bit-Bots</maintainer>
+
+  <author email="[email protected]">Jasper Güldenstein</author>
+
+  <license>MIT</license>
+
+  <exec_depend version_gte="0.2.19">python_qt_binding</exec_depend>
+  <exec_depend>python3-rospkg</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>rqt_gui</exec_depend>
+  <exec_depend>rqt_gui_py</exec_depend>
+  <exec_depend>bitbots_hcm</exec_depend>
+
+  <export>
+    <architecture_independent/>
+    <rqt_gui plugin="${prefix}/plugin.xml"/>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
+

bitbots_motion/bitbots_recordui_rqt/plugin.xml → bitbots_motion/bitbots_animation_rqt/plugin.xml

@@ -1,17 +1,17 @@
 <library path="src">
-  <class name="RecordUI" type="bitbots_recordui_rqt.record_ui.RecordUI" base_class_type="rqt_gui_py::Plugin">
+  <class name="RecordUI" type="bitbots_animation_rqt.record_ui.RecordUI" base_class_type="rqt_gui_py::Plugin">
     <description>
-      TODO
+      A Python GUI plugin to record joint space animations.
     </description>
     <qtgui>
       <group>
         <label>RoboCup</label>
         <icon type="theme">folder</icon>
         <statustip>Plugins related to RoboCup.</statustip>
       </group>
-      <label>Animation record</label>
+      <label>Bit-Bots Animation</label>
       <icon type="theme">preferences-system-network</icon>
-      <statustip>TODO</statustip>
+      <statustip>A Python GUI plugin to record joint space animations.</statustip>
     </qtgui>
   </class>
 </library>

bitbots_motion/bitbots_animation_rqt/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/bitbots_animation_rqt
+[install]
+install_scripts=$base/lib/bitbots_animation_rqt

bitbots_motion/bitbots_animation_rqt/setup.py

@@ -0,0 +1,21 @@
+from setuptools import find_packages, setup
+
+package_name = "bitbots_animation_rqt"
+
+setup(
+    name=package_name,
+    packages=find_packages(),
+    data_files=[
+        ("share/" + package_name + "/resource", ["resource/RecordUI.ui"]),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name, ["plugin.xml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    entry_points={
+        "console_scripts": [
+            "animation_gui = " + package_name + ".record_ui:main",
+        ],
+    },
+)