include missing packags

.github/workflows/python-publish.yml

@@ -1,8 +1,11 @@
 name: Publish Python Package
 
 on:
-  release:
-    types: [published]
+  # release:
+  #   types: [published]
+  push:
+    branches:
+      - main
 
 jobs:
   pypi-publish:

setup.py

@@ -1,33 +1,43 @@
-from setuptools import setup, find_packages
-
+import re
 from pathlib import Path
+from setuptools import setup
+
+
 this_directory = Path(__file__).parent
 long_description = (this_directory / "README.md").read_text()
 
+# replace relative links with absolute links
+long_description = re.sub(
+    r"\]\((?!http)([^)]+)\)",
+    r"](https://github.com/SpesRobotics/teleop/raw/main/\1)",
+    long_description,
+)
+
+
 setup(
-    name='teleop',
-    version='0.0.2',
-    packages=find_packages(),
+    name="teleop",
+    version="0.0.3",
+    packages=["teleop", "teleop.basic", "teleop.ros2"],
     long_description=long_description,
-    long_description_content_type='text/markdown',
-    description='Turns your phone into a robot arm teleoperation device by leveraging the WebXR API',
+    long_description_content_type="text/markdown",
+    description="Turns your phone into a robot arm teleoperation device by leveraging the WebXR API",
     install_requires=[
-        'Flask',
-        'numpy',
-        'transforms3d',
-        'werkzeug',
-        'pytest',
-        'requests',
+        "Flask",
+        "numpy",
+        "transforms3d",
+        "werkzeug",
+        "pytest",
+        "requests",
     ],
     package_data={
-        'teleop': ['cert.pem', 'key.pem', 'index.html'],
+        "teleop": ["cert.pem", "key.pem", "index.html"],
     },
-    license='Apache 2.0',
-    author='Spes Robotics',
-    author_email='contact@spes.ai',
+    license="Apache 2.0",
+    author="Spes Robotics",
+    author_email="contact@spes.ai",
     project_urls={
-        'Documentation': 'https://github.com/SpesRobotics/teleop',
-        'Source': 'https://github.com/SpesRobotics/teleop',
-        'Tracker': 'https://github.com/SpesRobotics/teleop/issues',
+        "Documentation": "https://github.com/SpesRobotics/teleop",
+        "Source": "https://github.com/SpesRobotics/teleop",
+        "Tracker": "https://github.com/SpesRobotics/teleop/issues",
     },
 )

teleop/__init__.py

@@ -44,6 +44,7 @@ class Teleop:
         port (int, optional): The port number. Defaults to 4443.
         ssl_context (ssl.SSLContext, optional): The SSL context for secure communication. Defaults to None.
     """
+
     def __init__(self, host="0.0.0.0", port=4443, ssl_context=None):
         self.__server = None
         self.__host = host
@@ -182,12 +183,16 @@ def run(self) -> None:
         Runs the teleop server. This method is blocking.
         """
         # self.__app.run(host=self.__host, port=self.__port, ssl_context=self.__ssl_context, use_reloader=False, debug=True)
-        self.__server = ThreadedWSGIServer(app=self.__app, host=self.__host, port=self.__port, ssl_context=self.__ssl_context)
+        self.__server = ThreadedWSGIServer(
+            app=self.__app,
+            host=self.__host,
+            port=self.__port,
+            ssl_context=self.__ssl_context,
+        )
         self.__server.serve_forever()
 
     def stop(self) -> None:
         """
         Stops the teleop server.
         """
         self.__server.shutdown()
-

teleop/ros2/__main__.py

@@ -6,7 +6,7 @@
     import rclpy
     import geometry_msgs.msg
     from geometry_msgs.msg import PoseStamped, TransformStamped
-    from tf2_ros import TransformBroadcaster 
+    from tf2_ros import TransformBroadcaster
 except ImportError:
     raise ImportError(
         "ROS2 is not sourced. Please source ROS2 before running this script."
@@ -44,27 +44,25 @@ def main():
     rclpy.init()
 
     parser = argparse.ArgumentParser()
-    parser.add_argument('--host', type=str, default='0.0.0.0', help='Host address')
-    parser.add_argument('--port', type=int, default=4443, help='Port number')
-    parser.add_argument('--topic', type=str, default='target_frame', help='Topic for pose publishing')
+    parser.add_argument("--host", type=str, default="0.0.0.0", help="Host address")
+    parser.add_argument("--port", type=int, default=4443, help="Port number")
+    parser.add_argument(
+        "--topic", type=str, default="target_frame", help="Topic for pose publishing"
+    )
 
     args = parser.parse_args()
 
     teleop = Teleop(host=args.host, port=args.port)
     current_robot_pose_message = None
     pose_initiated = False
-    node = rclpy.create_node('ros2_teleop')
-    pose_publisher = node.create_publisher(
-        PoseStamped, args.topic, 1
-    )
+    node = rclpy.create_node("ros2_teleop")
+    pose_publisher = node.create_publisher(PoseStamped, args.topic, 1)
     broadcaster = TransformBroadcaster(node)
 
-
     def ros_current_pose_callback(msg):
         nonlocal current_robot_pose_message
         current_robot_pose_message = msg
 
-
     def teleop_pose_callback(pose, params):
         nonlocal current_robot_pose_message
         nonlocal teleop
@@ -74,8 +72,8 @@ def teleop_pose_callback(pose, params):
 
         tf_message = TransformStamped()
         tf_message.header.stamp = node.get_clock().now().to_msg()
-        tf_message.header.frame_id = 'base_link'
-        tf_message.child_frame_id = 'teleop_target'
+        tf_message.header.frame_id = "base_link"
+        tf_message.child_frame_id = "teleop_target"
         tf_message.transform.translation.x = pose[0, 3]
         tf_message.transform.translation.y = pose[1, 3]
         tf_message.transform.translation.z = pose[2, 3]
@@ -93,16 +91,16 @@ def teleop_pose_callback(pose, params):
 
         if current_robot_pose_message is None:
             return
-        
-        if not params['move'] and not pose_initiated:
+
+        if not params["move"] and not pose_initiated:
             current_robot_pose = ros2numpy(current_robot_pose_message.pose)
             teleop.set_pose(current_robot_pose)
             pose_initiated = True
             return
 
         message = PoseStamped()
         message.header.stamp = node.get_clock().now().to_msg()
-        message.header.frame_id = 'link_base'
+        message.header.frame_id = "link_base"
         message.pose.position.x = pose[0, 3]
         message.pose.position.y = pose[1, 3]
         message.pose.position.z = pose[2, 3]
@@ -113,19 +111,14 @@ def teleop_pose_callback(pose, params):
         message.pose.orientation.z = quat[3]
         pose_publisher.publish(message)
 
-
     current_pose_subscriber = node.create_subscription(
-        PoseStamped,
-        '/current_pose',
-        ros_current_pose_callback,
-        1
+        PoseStamped, "/current_pose", ros_current_pose_callback, 1
     )
 
-    print(f'Server start on the adress https://{args.host}:{args.port}')
+    print(f"Server start on the adress https://{args.host}:{args.port}")
     teleop.subscribe(teleop_pose_callback)
     teleop.run()
 
 
 if __name__ == "__main__":
     main()
-

tests/test_pose_compounding.py

@@ -1,27 +1,20 @@
 import unittest
 import threading
 import requests
+import time
 from teleop import Teleop
 
 
 def get_message():
     return {
-            "move": False,
-            "position": {
-                "x": 0.0,
-                "y": 0.0,
-                "z": 0.0
-            },
-            "orientation": {
-                "w": 1.0,
-                "x": 0.0,
-                "y": 0.0,
-                "z": 0.0
-            },
-            "reference_frame": "base"
-        } 
-
-BASE_URL = 'https://localhost:4443'
+        "move": False,
+        "position": {"x": 0.0, "y": 0.0, "z": 0.0},
+        "orientation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0},
+        "reference_frame": "base",
+    }
+
+
+BASE_URL = "https://localhost:4443"
 
 
 class TestPoseCompounding(unittest.TestCase):
@@ -44,22 +37,44 @@ def setUpClass(cls):
 
     def test_response(self):
         payload = get_message()
-        response = requests.post(f'{BASE_URL}/pose', json=payload, verify=False, timeout=5)
+        response = requests.post(
+            f"{BASE_URL}/pose", json=payload, verify=False, timeout=5
+        )
 
         self.assertEqual(response.status_code, 200)
         self.assertEqual(response.json(), {"status": "ok"})
 
     def test_single_position_update(self):
         payload = get_message()
+
+        # The first message with `move==True` is used as a reference
         payload["move"] = True
-        response = requests.post(f'{BASE_URL}/pose', json=payload, verify=False, timeout=5)
+        response = requests.post(
+            f"{BASE_URL}/pose", json=payload, verify=False, timeout=5
+        )
+        self.assertEqual(response.status_code, 200)
+        self.assertIsNotNone(self.__last_pose)
+        self.assertIsNotNone(self.__last_message)
+
+        # Move the phone up by 5cm (Y-axis)
+        payload["move"] = True
+        payload["position"]["y"] = 0.05
+        response = requests.post(
+            f"{BASE_URL}/pose", json=payload, verify=False, timeout=5
+        )
+        self.assertEqual(response.status_code, 200)
+
+        # In total the result should be 5cm on the Z-axis because of the RUB -> FLU conversion
+        self.assertEqual(self.__last_pose[2, 3], 0.05)
+
+        # Move the phone up by another 5cm (Y-axis)
         payload["move"] = True
         payload["position"]["y"] = 0.1
-        response = requests.post(f'{BASE_URL}/pose', json=payload, verify=False, timeout=5)
+        response = requests.post(
+            f"{BASE_URL}/pose", json=payload, verify=False, timeout=5
+        )
         self.assertEqual(response.status_code, 200)
 
-        self.assertIsNotNone(self.__last_pose)
-        self.assertIsNotNone(self.__last_message)
         self.assertEqual(self.__last_pose[2, 3], 0.1)
 
     @classmethod