Add launcher tool

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

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}
 )
 

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.
+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.

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

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,19 +45,38 @@ 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_vector)
+    gamepad.set_callback(gamepad_node.update)
     Thread(target=gamepad.run, daemon=True).start()
 
     rclpy.spin(gamepad_node)

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()

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)
 
 

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__":