add map_range method

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,14 +118,12 @@ 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)