Move gamepad changes to other branch.

Signed-off-by: Jelmer de Wolde <[email protected]>

python_nodes/gamepad_node.py

@@ -7,9 +7,7 @@
 from dataclasses import dataclass
 import rclpy
 from rclpy.node import Node
-from rclpy.action import ActionClient
 from geometry_msgs.msg import TwistStamped
-from franka_msgs.action import Move, Grasp
 from threading import Thread
 from rcdt_utilities.gamepad import Gamepad
 from rcdt_utilities.virtual_gamepad import VirtualGamepad
@@ -30,10 +28,7 @@ def __init__(self):
         self.topic = "/servo_node/delta_twist_cmds"
         self.frame_id = "fr3_link0"
         self.publisher = self.create_publisher(TwistStamped, self.topic, 10)
-        self.move_client = ActionClient(self, Move, "/fr3_gripper/move")
-        self.grasp_client = ActionClient(self, Grasp, "/fr3_gripper/grasp")
         self.vector = Vec3(0, 0, 0)
-        self.gripper_state = -1
         self.timer = self.create_timer(1 / 100.0, self.publish)
 
     def publish(self) -> None:
@@ -45,38 +40,19 @@ def publish(self) -> None:
         msg.twist.linear.z = float(self.vector.z)
         self.publisher.publish(msg)
 
-    def update(self, x: float, y: float, z: float, g: float = 0) -> None:
-        self.update_vector(x, y, z)
-        self.update_gripper(g)
-
     def update_vector(self, x: float, y: float, z: float) -> None:
         self.vector = Vec3(
             x * self.scale_factor, y * self.scale_factor, z * self.scale_factor
         )
 
-    def update_gripper(self, g: float) -> None:
-        if g == 1 and self.gripper_state != 1:
-            goal = Move.Goal()
-            goal.width = 0.04
-            self.move_client.wait_for_server()
-            if self.move_client.send_goal_async(goal):
-                self.gripper_state = 1
-        elif g == -1 and self.gripper_state != -1:
-            goal = Grasp.Goal()
-            goal.width = 0.0
-            goal.force = 100.0
-            self.grasp_client.wait_for_server()
-            if self.grasp_client.send_goal_async(goal):
-                self.gripper_state = -1
-
 
 def main(args: str = None) -> None:
     """Start the gamepadnode and the (virtual) gamepad in a separate thread."""
     rclpy.init(args=args)
     gamepad_node = GamepadNode()
     virtual = gamepad_node.get_parameter("virtual").get_parameter_value().bool_value
     gamepad = VirtualGamepad() if virtual else Gamepad()
-    gamepad.set_callback(gamepad_node.update)
+    gamepad.set_callback(gamepad_node.update_vector)
     Thread(target=gamepad.run, daemon=True).start()
 
     rclpy.spin(gamepad_node)

rcdt_utilities/virtual_gamepad.py

@@ -22,7 +22,6 @@ def __init__(self) -> None:
         self.x = 0
         self.y = 0
         self.z = 0
-        self.g = 0
 
     def set_callback(self, callback: Callable) -> None:
         self.callback = callback
@@ -51,25 +50,20 @@ def create_window(self) -> None:
         window = dpg.add_window(width=-1, height=-1)
         dpg.set_primary_window(window, True)
 
-        cols = 2
-        rows = 4
-        grid = dpg_grid.Grid(cols, rows, window)
+        rows = 2
+        cols = 3
+        grid = dpg_grid.Grid(rows, cols, window)
         grid.offsets = 8, 8, 8, 8
 
-        labels = ["X+", "Z+", "X-", "Z-", "Y+", "Y-", "G+", "G-"]
-        colors = {
-            "X": (200, 0, 0),
-            "Y": (0, 200, 0),
-            "Z": (0, 0, 200),
-            "G": (200, 200, 200),
-        }
+        labels = ["X+", "X-", "Y+", "Z+", "Z-", "Y-"]
+        colors = {"X": (200, 0, 0), "Y": (0, 200, 0), "Z": (0, 0, 200)}
         row, col = 0, 0
         for label in labels:
             uid = dpg.generate_uuid()
             button = dpg.add_button(parent=window, label=label, tag=uid)
             dpg.bind_item_handler_registry(uid, "handler")
             self.set_color(uid, colors[label[0]])
-            grid.push(button, col, row)
+            grid.push(button, row, col)
             col = col + 1 if col < cols - 1 else 0
             row = row + 1 if col == 0 else row
 
@@ -92,7 +86,7 @@ def button_callback(self, sender: int, app_data: int) -> None:
         value = 1 if label[1] == "+" else -1
         value = 0 if not active else value
         setattr(self, axis, value)
-        self.callback(self.x, self.y, self.z, self.g)
+        self.callback(self.x, self.y, self.z)
 
 
 if __name__ == "__main__":