diff --git a/gello.py b/gello.py index b1f65cc..406a211 100644 --- a/gello.py +++ b/gello.py @@ -9,6 +9,11 @@ import time from main import config, HOST, PORT import threading +joystick_max = -0.9583490120438358 +joystick_min = -1.8495918498053934 +gripper_max = 0.93 +gripper_min = 0.0 + class RobotControllerNode(Node): def __init__(self): @@ -20,7 +25,9 @@ class RobotControllerNode(Node): self.js_sub = self.create_subscription( JointState, "/joint_states", self.js_callback, 10 ) - self.gello_sub = self.create_subscription(JointState, "/gello/joint_states", self.gello_callback, 10) + self.gello_sub = self.create_subscription( + JointState, "/gello/joint_states", self.gello_callback, 10 + ) self.last_js = JointState() self.last_gello = JointState() @@ -41,6 +48,21 @@ class RobotControllerNode(Node): def gello_callback(self, msg: JointState): self.last_gello = msg + def map_range( + self, + value, + in_min=joystick_min, + in_max=joystick_max, + out_min=gripper_min, + out_max=gripper_max, + ): + """Преобразует значение из одного диапазона в другой.""" + # Защита от деления на ноль + if in_max == in_min: + raise ValueError("Невалидный входной диапазон: in_min и in_max равны") + # Линейное преобразование + return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min + def send_gripper_command( self, target_pose: float, tolerance: float = 0.01, timeout: float = 3.0 ) -> bool: @@ -96,13 +118,11 @@ class RobotControllerNode(Node): self.waiting_for_gripper = True threading.Thread( target=self.handle_gripper_step, - args=(self.last_gello.position[-1], 10.0, 0.1), + args=(self.map_range(self.last_gello.position[-1]), 10.0, 0.1), daemon=True, ).start() self.sequence_step += 1 - - def main(args=None): rclpy.init(args=args)