After MARS.

arm_teleop/arm_teleop_node.py

@@ -27,6 +27,7 @@ class ArmTeleopNode(Node):
 
         # Main loop
         self.mainloop = self.create_timer(0.001, self.mainloop_callback) 
+        self.transloop = self.create_timer(0.01, self.translation_callback) 
 
         # Publisher
         self.franka_pose_publisher = self.create_publisher(Float64MultiArray, '/franka/pose_demand', 10)
@@ -34,12 +35,12 @@ class ArmTeleopNode(Node):
 
         # Subscriber
         self.franka_cartesian_pose_subscriber = self.create_subscription(Float64MultiArray, '/franka/pose', self.cart_pos_callback, 10)
-        self.target_translation_subscriber = self.create_subscription(Float64MultiArray, '/franka/translation_demand', self.translation_callback, 10)
+        # self.target_translation_subscriber = self.create_subscription(Float64MultiArray, '/franka/translation_demand', self.translation_callback, 10)
         self.target_orientation_subscriber = self.create_subscription(Float64MultiArray, '/franka/orientation_demand',self.orientation_callback, 10)
         self.hand_wrist_servo_position_subscriber = self.create_subscription(Float64MultiArray, '/hand_wrist/servo_positions',self.hand_wrist_servo_callback, 10)
 
         # Variables
-        self.translation_demand = None
+        self.translation_delta = [0.0, 0.0, 0.0]
         self.orientation_demand = None
         self.current_franka_pose = None
         self.current_hand_wrist_servo_pos = None
@@ -61,13 +62,22 @@ class ArmTeleopNode(Node):
         else:
             return False
         
-    def translation_callback(self, msg:Float64MultiArray):
-        self.translation_demand = msg
+    def translation_callback(self):
+        scale = 0.001
+
+        if abs(self.gp.axis_data["Ly"]) > 0.3:
+            self.translation_delta[2] += self.gp.axis_data["Ly"] * scale * -1
+
+        if abs(self.gp.axis_data["Lx"]) > 0.3:
+            self.translation_delta[1] += self.gp.axis_data["Lx"] * scale
+
         self.last_translation_update = cp(time.time())
 
+
     def orientation_callback(self, msg:Float64MultiArray):
         self.orientation_demand = msg
         self.last_orientation_update = cp(time.time())
+        # print("asdf")
 
     def cart_pos_callback(self, msg:Float64MultiArray):
         self.current_franka_pose = msg
@@ -78,29 +88,38 @@ class ArmTeleopNode(Node):
     def gamepad_callback(self, gamepad_raw_msg:Joy):
         self.gp.convert_joy_msg_to_dictionary(gamepad_raw_msg)
 
+    def is_trigger(self):
+        is_trigger = False
+        if abs(self.gp.axis_data["Lx"]) > 0.5 or abs(self.gp.axis_data["Ly"]) > 0.5:
+            is_trigger = True
+
+        return is_trigger
+    
     def mainloop_callback(self):
         if (self.current_franka_pose is None) or (self.current_hand_wrist_servo_pos is None):
             return
 
-        
-        if self.gp.button_data["L1"] == 1 and self.is_active == False:
+        if (self.gp.button_data["L1"] == 1 or self.is_trigger()) and self.is_active == False:
             self.initial_pose = list(cp(self.current_franka_pose.data))
             self.initial_pose_wrist = [self.current_hand_wrist_servo_pos.data[13], self.current_hand_wrist_servo_pos.data[14]]
             self.is_active = True
             self.waiting_timer = cp(time.time())
+            self.translation_delta = [0.0, 0.0, 0.0]
+            print("reset")
 
-        if self.gp.button_data["L1"] == 0:
+        if self.gp.button_data["L1"] == 0 and (self.is_trigger() == False):
             self.is_active = False
+            # print("zero")
 
         if self.is_active and (time.time() - self.waiting_timer > 0.5):
             target_pose = cp(self.initial_pose)
             target_wrist = cp(self.initial_pose_wrist)
             print_msg = "\n"
 
-            if (self.translation_demand is not None) and self.check_timer("translation"):
+            if (self.translation_delta is not None) and self.check_timer("translation"):
                 print_msg += "Translation  "
                 for i in range(3):
-                    target_pose[i] = cp(self.translation_demand[i])
+                    target_pose[i] += cp(self.translation_delta[i])
                 
             if (self.orientation_demand is not None) and self.check_timer("orientation"):
                 print_msg += "Orientation  "
@@ -135,7 +154,7 @@ class ArmTeleopNode(Node):
             # print([self.current_hand_wrist_servo_pos.data[13], self.current_hand_wrist_servo_pos.data[14]])
 
             # print(self.orientation_demand.data[7], self.orientation_demand.data[8])
-            print(left, right)
+            # print(left, right)
             print("")
             # print_msg += ("\n" + posestamp_message_as_str(target_pose))
             # self.get_logger().info(print_msg, throttle_duration_sec = 0.1)