diff --git a/.github/workflows/python-publish.yml b/.github/workflows/python-publish.yml index 7d7b611..cf15a62 100644 --- a/.github/workflows/python-publish.yml +++ b/.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: diff --git a/setup.py b/setup.py index 8bacb09..8dfe9bf 100644 --- a/setup.py +++ b/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", }, ) diff --git a/teleop/__init__.py b/teleop/__init__.py index 6cfd813..933a59d 100644 --- a/teleop/__init__.py +++ b/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,7 +183,12 @@ 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: @@ -190,4 +196,3 @@ def stop(self) -> None: Stops the teleop server. """ self.__server.shutdown() - diff --git a/teleop/ros2/__main__.py b/teleop/ros2/__main__.py index 5a21364..0d4daa6 100644 --- a/teleop/ros2/__main__.py +++ b/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,8 +91,8 @@ 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 @@ -102,7 +100,7 @@ def teleop_pose_callback(pose, params): 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() - diff --git a/tests/test_pose_compounding.py b/tests/test_pose_compounding.py index 6d8730f..7e5a529 100644 --- a/tests/test_pose_compounding.py +++ b/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