mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-17 13:01:12 +00:00
50 lines
1.4 KiB
Python
50 lines
1.4 KiB
Python
import time
|
|
import asyncio
|
|
from pydantic import BaseModel
|
|
|
|
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
|
|
|
|
|
|
class Point3D(BaseModel):
|
|
x: float
|
|
y: float
|
|
z: float
|
|
|
|
|
|
def d(a: Point3D, b: Point3D) -> float:
|
|
return ((a.x - b.x) ** 2 + (a.y - b.y) ** 2 + (a.z - b.z) ** 2) ** 0.5
|
|
|
|
|
|
class MockCNCAsync:
|
|
_ros_node: BaseROS2DeviceNode["MockCNCAsync"]
|
|
|
|
def __init__(self):
|
|
self._position: Point3D = Point3D(x=0.0, y=0.0, z=0.0)
|
|
self._status = "Idle"
|
|
|
|
def post_create(self, ros_node):
|
|
self._ros_node = ros_node
|
|
|
|
@property
|
|
def position(self) -> Point3D:
|
|
return self._position
|
|
|
|
async def get_position(self):
|
|
return self.position
|
|
|
|
@property
|
|
def status(self) -> str:
|
|
return self._status
|
|
|
|
async def set_position(self, position: Point3D, velocity: float = 10.0):
|
|
self._status = "Running"
|
|
current_pos = self.position
|
|
|
|
move_time = d(position, current_pos) / velocity
|
|
for i in range(20):
|
|
self._position.x = current_pos.x + (position.x - current_pos.x) / 20 * (i+1)
|
|
self._position.y = current_pos.y + (position.y - current_pos.y) / 20 * (i+1)
|
|
self._position.z = current_pos.z + (position.z - current_pos.z) / 20 * (i+1)
|
|
await self._ros_node.sleep(move_time / 20)
|
|
self._status = "Idle"
|