From 31c4445697c2340f85b1e14f76f5b9a3047b1ad5 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Mon, 16 Sep 2024 08:43:15 +0000 Subject: [PATCH] Move gamepad changes to other branch. Signed-off-by: Jelmer de Wolde --- python_nodes/gamepad_node.py | 26 +------------------------- rcdt_utilities/virtual_gamepad.py | 20 +++++++------------- 2 files changed, 8 insertions(+), 38 deletions(-) diff --git a/python_nodes/gamepad_node.py b/python_nodes/gamepad_node.py index 03799dc..d5e8731 100755 --- a/python_nodes/gamepad_node.py +++ b/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,30 +40,11 @@ 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.""" @@ -76,7 +52,7 @@ def main(args: str = None) -> None: 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) diff --git a/rcdt_utilities/virtual_gamepad.py b/rcdt_utilities/virtual_gamepad.py index 2d7d0aa..ec8f88e 100644 --- a/rcdt_utilities/virtual_gamepad.py +++ b/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__":