From c58fe54a67a4cb99711588310a69a6f3407522f8 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Mon, 16 Sep 2024 10:20:55 +0200 Subject: [PATCH] Add launcher tool Signed-off-by: Jelmer de Wolde --- CMakeLists.txt | 1 + README.md | 2 +- catalog-info.yaml | 12 +++ python_nodes/gamepad_node.py | 26 +++++- python_nodes/launcher.py | 145 ++++++++++++++++++++++++++++++ rcdt_utilities/launch_utils.py | 35 +++++++- rcdt_utilities/virtual_gamepad.py | 20 +++-- 7 files changed, 231 insertions(+), 10 deletions(-) create mode 100644 catalog-info.yaml create mode 100755 python_nodes/launcher.py diff --git a/CMakeLists.txt b/CMakeLists.txt index c956787..7befd2a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ ament_python_install_package(${PROJECT_NAME}) # Install Python executables install(PROGRAMS python_nodes/gamepad_node.py + python_nodes/launcher.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/README.md b/README.md index 95681b1..5feacf7 100644 --- a/README.md +++ b/README.md @@ -14,4 +14,4 @@ This project is licensed under the Apache License Version 2.0 - see [LICENSE](LI ## Contributing -Please read CODE_OF_CONDUCT, CONTRIBUTING, and PROJECT GOVERNANCE located in the overarching [RCDT robotics](https://github.com/alliander-opensource/rcdt_robotics) project repository for details on the process for submitting pull requests to us. \ No newline at end of file +Please read CODE_OF_CONDUCT, CONTRIBUTING, and PROJECT GOVERNANCE located in the overarching [RCDT robotics](https://github.com/alliander-opensource/rcdt_robotics) project repository for details on the process for submitting pull requests to us. diff --git a/catalog-info.yaml b/catalog-info.yaml new file mode 100644 index 0000000..45790e0 --- /dev/null +++ b/catalog-info.yaml @@ -0,0 +1,12 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + +apiVersion: backstage.io/v1alpha1 +kind: Component +metadata: + name: rcdt_utilities +spec: + owner: rcdt + type: infrastructure + lifecycle: experimental diff --git a/python_nodes/gamepad_node.py b/python_nodes/gamepad_node.py index d5e8731..03799dc 100755 --- a/python_nodes/gamepad_node.py +++ b/python_nodes/gamepad_node.py @@ -7,7 +7,9 @@ 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 @@ -28,7 +30,10 @@ 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: @@ -40,11 +45,30 @@ 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.""" @@ -52,7 +76,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_vector) + gamepad.set_callback(gamepad_node.update) Thread(target=gamepad.run, daemon=True).start() rclpy.spin(gamepad_node) diff --git a/python_nodes/launcher.py b/python_nodes/launcher.py new file mode 100755 index 0000000..80e8387 --- /dev/null +++ b/python_nodes/launcher.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + + +import psutil +from os import listdir +from importlib.util import spec_from_file_location, module_from_spec +import dearpygui.dearpygui as dpg +from subprocess import Popen, check_output +from threading import Thread +from time import sleep +from rcdt_utilities.launch_utils import LaunchArguments, get_package_path, get_file_path + + +class Launcher: + def __init__(self) -> None: + self.widgets = {} + self.ARGS = LaunchArguments() + self.create_window() + self.create_launch_file_selector() + self.create_buttons() + self.create_launch_options_selector() + self.create_active_nodes_panel() + self.run_gui() + + def create_window(self) -> None: + dpg.create_context() + dpg.create_viewport(title="Launcher", width=200, height=200) + self.window = dpg.add_window(width=-1, height=-1) + dpg.set_primary_window(self.window, True) + + def create_launch_file_selector(self) -> None: + self.package = "rcdt_franka" + launch_path = f"{get_package_path(self.package)}/launch" + launch_files = [f for f in listdir(launch_path) if f.endswith(".launch.py")] + launch_file = launch_files[0] + + group = dpg.add_group(parent=self.window, horizontal=True) + dpg.add_text(parent=group, default_value=f"{self.package}") + dpg.add_combo( + parent=group, + items=launch_files, + default_value=launch_file, + callback=lambda item: self.select_launch_file(dpg.get_value(item)), + ) + self.launch_file = launch_file + + def create_buttons(self) -> None: + dpg.add_spacer(parent=self.window, height=10) + group = dpg.add_group(parent=self.window, horizontal=True) + dpg.add_button(parent=group, label="Start", callback=self.start) + dpg.add_button(parent=group, label="Stop", callback=self.stop) + dpg.add_button(parent=group, label="Clear", callback=self.clear) + + def create_launch_options_selector(self) -> None: + dpg.add_spacer(parent=self.window, height=10) + dpg.add_text(parent=self.window, default_value="Launch options:") + uid = dpg.generate_uuid() + dpg.add_group(parent=self.window, tag=uid) + self.widgets["launch_options"] = uid + self.get_args_from_launch_file() + self.fill_launch_options_selector() + + def create_active_nodes_panel(self) -> None: + dpg.add_spacer(parent=self.window, height=10) + dpg.add_text(parent=self.window, default_value="Active nodes:") + dpg.add_text(parent=self.window, default_value="", tag="active_nodes") + Thread(target=self.show_active_nodes, daemon=True).start() + + def run_gui(self) -> None: + dpg.setup_dearpygui() + dpg.show_viewport() + dpg.start_dearpygui() + dpg.destroy_context() + + def fill_launch_options_selector(self) -> None: + uid = self.widgets["launch_options"] + dpg.delete_item(uid, children_only=True) + for name, value in self.ARGS.all_values().items(): + options = self.ARGS.get_options(name) + group = dpg.add_group(parent=uid, horizontal=True) + dpg.add_text(parent=group, default_value=name) + ( + dpg.add_radio_button( + parent=group, + label=name, + items=options, + default_value=value, + horizontal=True, + callback=lambda item: self.ARGS.set_value( + dpg.get_item_label(item), dpg.get_value(item) + ), + ), + ) + + def select_launch_file(self, launch_file: str) -> None: + self.launch_file = launch_file + self.get_args_from_launch_file() + self.fill_launch_options_selector() + + def get_args_from_launch_file(self) -> None: + self.package = "rcdt_franka" + file_path = get_file_path(self.package, ["launch"], self.launch_file) + spec = spec_from_file_location("launch_file", file_path) + module = module_from_spec(spec) + spec.loader.exec_module(module) + if hasattr(module, "ARGS"): + self.ARGS = module.ARGS + else: + self.ARGS = LaunchArguments() + + def launch_command(self) -> str: + command = f"ros2 launch {self.package} {self.launch_file}" + for name, value in self.ARGS.all_values().items(): + command += f" {name}:='{value }'" + return command + + def start(self) -> None: + self.process = Popen(self.launch_command(), shell=True) + + def stop(self) -> None: + if hasattr(self, "process"): + self.kill(self.process.pid) + + def kill(self, proc_pid: int) -> None: + process = psutil.Process(proc_pid) + for proc in process.children(recursive=True): + proc.kill() + process.kill() + + def clear(self) -> None: + Popen("clear", shell=True) + + def show_active_nodes(self) -> None: + while True: + nodes = check_output(["ros2", "node", "list"]).decode("utf-8") + dpg.set_value("active_nodes", nodes) + sleep(1) + + +if __name__ == "__main__": + Launcher() diff --git a/rcdt_utilities/launch_utils.py b/rcdt_utilities/launch_utils.py index 981d082..31cff9c 100644 --- a/rcdt_utilities/launch_utils.py +++ b/rcdt_utilities/launch_utils.py @@ -2,6 +2,7 @@ # # SPDX-License-Identifier: Apache-2.0 +import sys from typing import List import os import yaml @@ -10,8 +11,40 @@ from ament_index_python.packages import get_package_share_directory +class LaunchArguments: + def __init__(self) -> None: + self.values = {} + self.options = {} + + def add_value(self, name: str, value: str, options: List[str]) -> None: + self.set_value(name, value) + self.options[name] = options + + def set_value(self, name: str, value: str) -> None: + self.values[name] = value + + def get_value(self, name: str) -> str: + return self.values[name] + + def get_options(self, name: str) -> List[str]: + return self.options[name] + + def all_values(self) -> dict: + return self.values + + def update_from_sys(self) -> None: + for arg in sys.argv: + if ":=" in arg: + name, value = arg.split(":=") + self.set_value(name, value) + + +def get_package_path(package: str) -> str: + return get_package_share_directory(package) + + def get_file_path(package: str, folders: List[str], file: str) -> str: - package_path = get_package_share_directory(package) + package_path = get_package_path(package) return os.path.join(package_path, *folders, file) diff --git a/rcdt_utilities/virtual_gamepad.py b/rcdt_utilities/virtual_gamepad.py index ec8f88e..2d7d0aa 100644 --- a/rcdt_utilities/virtual_gamepad.py +++ b/rcdt_utilities/virtual_gamepad.py @@ -22,6 +22,7 @@ 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 @@ -50,20 +51,25 @@ def create_window(self) -> None: window = dpg.add_window(width=-1, height=-1) dpg.set_primary_window(window, True) - rows = 2 - cols = 3 - grid = dpg_grid.Grid(rows, cols, window) + cols = 2 + rows = 4 + grid = dpg_grid.Grid(cols, rows, window) grid.offsets = 8, 8, 8, 8 - labels = ["X+", "X-", "Y+", "Z+", "Z-", "Y-"] - colors = {"X": (200, 0, 0), "Y": (0, 200, 0), "Z": (0, 0, 200)} + 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), + } 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, row, col) + grid.push(button, col, row) col = col + 1 if col < cols - 1 else 0 row = row + 1 if col == 0 else row @@ -86,7 +92,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.callback(self.x, self.y, self.z, self.g) if __name__ == "__main__":