优化飞行手柄

robot_client/robots/realman/motors_bus.py

@@ -26,11 +26,6 @@ class RealmanMotorsBus(MotorsBus):
         calibration: dict[str, MotorCalibration] | None = None,
     ):
         super().__init__(port,motors, calibration)
-        self.rmarm = RoboticArm(rm_thread_mode_e.RM_DUAL_MODE_E)
-        if mock:
-            self.rmarm.rm_set_arm_run_mode(0)#仿真模式
-        else:
-            self.rmarm.rm_set_arm_run_mode(1)
         address  = port.split(':')
         self.ip = address[0]
         self.port = int(address[1])
@@ -38,6 +33,8 @@ class RealmanMotorsBus(MotorsBus):
         self.gripper_range = gripper_range
         self.init_joint_position = joint
         self.safe_disable_position = joint
+        self.mock = mock
+        self.rmarm = RoboticArm(rm_thread_mode_e.RM_DUAL_MODE_E)
         self.handle = None
         self.init_pose = None
 
@@ -58,8 +55,11 @@ class RealmanMotorsBus(MotorsBus):
         '''
         使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序
         '''
-
         self.handle = self.rmarm.rm_create_robot_arm(self.ip, self.port)
+        if self.mock:
+            self.rmarm.rm_set_arm_run_mode(0)#仿真模式
+        else:
+            self.rmarm.rm_set_arm_run_mode(1)
         enable_flag = False
         loop_flag = False
         # 设置超时时间（秒）
@@ -178,8 +178,8 @@ class RealmanMotorsBus(MotorsBus):
             values = list(actionData.values())
             for k,v in enumerate(self.init_pose):
                 self.init_pose[k]+=values[k]
-            print(f"init_pose:{self.init_pose[:-1]}")
-            self.rmarm.rm_movej_p(self.init_pose[:-1], 50, 0, 0, 0)
+            r = self.rmarm.rm_movej_p(self.init_pose[:-1], 50, 0, 0, 0)
+            print(f"r:{r},pose:{self.init_pose}")
             # self.init_pose[6]+=actionData['gripper']
             # self.rmarm.rm_set_gripper_position(int(self.init_pose[6]*1000), False, 0)