massagerobot增加用户调整

Massage/MassageControl/MassageRobot_nova5.py

@@ -1336,6 +1336,52 @@ class MassageRobot:
         result_points.append(copy.deepcopy(center_point))
         return result_points
     
+    def user_adjust(self,pose_increment = None, force = None, temperature = None, speed = None, direction = None ,gear = None, shake = None,press = None,frequency = None):
+        self.logger.log_info("用户调整")
+        if force is not None:
+            self.adjust_wrench = np.array(force)
+            self.adjust_wrench_envent.set()
+        if temperature is not None:
+            if self.thermotherapy is not None:
+                self.thermotherapy.set_working_status(heat_level=temperature)
+            elif self.stone is not None:
+                self.stone.set_working_status(heat_level=temperature)
+            elif self.ion is not None:
+                self.ion.set_working_status(goal_state=temperature)
+            elif self.heat is not None:
+                self.heat.set_working_status(heat_level=temperature)
+        if speed is not None:
+            if self.stone is not None:
+                self.stone.set_working_status(speed_level=speed)
+        if direction is not None:
+            if self.stone is not None:
+                self.stone.set_working_status(rotate_direction=direction)
+        if gear is not None:
+            if self.thermotherapy is not None:
+                self.thermotherapy.set_working_status(cur_level=gear)
+        if shake is not None:
+            if self.thermotherapy is not None:
+                self.thermotherapy.set_working_status(vib_level=shake)
+        if press is not None:
+            if self.shockwave is not None:
+                self.shockwave.p_set(level=press)
+        if frequency is not None:
+            if self.shockwave is not None:
+                self.shockwave.f_set(level=frequency)
+
+    def user_interrupt(self):
+        self.logger.log_info("用户打断")
+        self.interrupt_event.set()
+
+    def user_pause(self):
+        self.logger.log_info("用户暂停")
+        self.pause_envent.set()
+
+    def user_skip(self):
+        self.logger.log_info("用户跳过")
+        self.skip_envent.set()
+    
+    
     def traj_generate(self, t: Union[int, float, List[float]], pose = None, wrench = None, interpolation: Literal["linear", "cubic","circle","cloud_point"] = 'linear',**kwargs):
         """
         轨迹生成