Fix/ros2 signal and error handling

ros2/.devcontainer/docker-compose.yml

@@ -4,6 +4,8 @@ services:
     privileged: true
     restart: no
     network_mode: host
+    init: true # Ensure signal forwarding to child processes
+    stop_signal: SIGINT # ros2 launch needs SIGINT to shutdown gracefully
     volumes:
       - ../../:/workspace
       - /dev/serial/by-id:/dev/serial/by-id

ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/README.md

@@ -48,13 +48,16 @@ operating_modes:
 
 ```python
 from gello.dynamixel.driver import DynamixelDriver
-
-driver = DynamixelDriver(
-    ids=[1, 2, 3], 
-    port="/dev/ttyUSB0", 
-    baudrate=57600, 
-    motor_type="xl330"
-)
+try:
+  driver = DynamixelDriver(
+      ids=[1, 2, 3], 
+      port="/dev/ttyUSB0", 
+      baudrate=57600, 
+      motor_type="xl330"
+  )
+except ConnectionError as e:
+  print(f"Error initializing DynamixelDriver: {e}")
+  return
 ```
 
 ### Fake Driver for Testing

ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py

@@ -2,6 +2,7 @@
 from pathlib import Path
 from threading import Event, Lock, Thread
 from typing import Protocol, Sequence
+from serial import SerialException
 
 import numpy as np
 import yaml
@@ -187,8 +188,17 @@ def __init__(
         motor_type : str, optional
             The type of motor to use, e.g., "xl330" (default is "xl330").
 
+        Raises
+        ------
+        ConnectionError
+            If the port cannot be opened or is already in use.
+        RuntimeError
+            If there is an error reading or writing to the servos.
+
         """
         self._ids = ids
+        self._port = port
+        self._baudrate = baudrate
         self._joint_angles = None
         self._lock = Lock()
 
@@ -199,7 +209,7 @@ def __init__(
         self._pulses_per_revolution = self._motor_config["pulses_per_revolution"]
 
         # Initialize the port handler, packet handler, and group sync read/write
-        self._portHandler = PortHandler(port)
+        self._portHandler = PortHandler(self._port)
         self._packetHandler = PacketHandler(2.0)
 
         # Create group sync read/write handlers for each control table entry
@@ -225,17 +235,26 @@ def __init__(
                 )
 
         # Open the port and set the baudrate
-        if not self._portHandler.openPort():
-            raise RuntimeError("Failed to open the port")
-
-        if not self._portHandler.setBaudRate(baudrate):
-            raise RuntimeError(f"Failed to change the baudrate, {baudrate}")
+        try:
+            self._portHandler.openPort()
+            self._portHandler.setBaudRate(self._baudrate)
+        except SerialException:
+            raise ConnectionError(
+                f"Could not open port {self._port}. Make sure you have specified the correct port, "
+                "that the device is connected and that you have proper permissions to access it."
+            ) from None
+
+        # Verify connection by attempting to read the model number
+        try:
+            self.read_value_by_name("model_number")
+        except (RuntimeError, SerialException):
+            raise ConnectionError(
+                f"Port {self._port} opened but could not read from motors. Make sure no other "
+                "process is using the same port, that all motors are wired correctly and get power."
+            ) from None
 
         # Disable torque for all Dynamixel servos
-        try:
-            self.write_value_by_name("torque_enable", [0] * len(self._ids))
-        except Exception as e:
-            print(f"port: {port}, {e}")
+        self.write_value_by_name("torque_enable", [0] * len(self._ids))
 
         self._stop_thread = Event()
         self._start_reading_thread()

ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py

@@ -1,7 +1,6 @@
 import rclpy
+from rclpy.executors import ExternalShutdownException
 from glob import glob
-from sys import exit
-from signal import signal, SIGINT, SIGTERM
 from rclpy.node import Node
 from sensor_msgs.msg import JointState
 from std_msgs.msg import Float32
@@ -23,10 +22,11 @@ def __init__(self) -> None:
 
         hardware_params: GelloHardwareParams = self._setup_hardware_parameters()
 
-        self.gello_hardware = GelloHardware(hardware_params)
-
-        signal(SIGINT, self._signal_handler)  # Handle Ctrl+C gracefully
-        signal(SIGTERM, self._signal_handler)  # Handle termination signals
+        try:
+            self.gello_hardware = GelloHardware(hardware_params)
+        except ConnectionError as e:
+            self.get_logger().error(f"Failed to initialize GELLO hardware: {e}")
+            raise
 
         self.arm_joint_publisher = self.create_publisher(JointState, "gello/joint_states", 10)
         self.gripper_joint_publisher = self.create_publisher(
@@ -38,6 +38,7 @@ def __init__(self) -> None:
             ParameterEvent, "/parameter_events", self.parameter_event_callback, 10
         )
 
+        self.get_logger().info("Publishing GELLO joint states.")
         self.timer = self.create_timer(1 / self.PUBLISHING_RATE, self.publish_joint_jog)
 
     def parameter_event_callback(self, event: ParameterEvent) -> None:
@@ -110,31 +111,24 @@ def _setup_hardware_parameters(self):
 
         return hardware_params
 
-    def _signal_handler(self, signum, frame):
-        """Handle system signals for graceful shutdown."""
-        if signum == SIGINT or signum == SIGTERM:
-            self.get_logger().info(f"Received signal {signum}, shutting down gracefully...")
-            self._safe_shutdown()
-            exit(0)
-        else:
-            self.get_logger().warn(f"Received unexpected signal {signum}")
-
-    def _safe_shutdown(self):
-        """Safely shutdown hardware."""
-        try:
-            self.get_logger().info("Disabling GELLO torque...")
-            self.gello_hardware.disable_torque()
-            self.get_logger().info("GELLO torque disabled successfully")
-        except Exception as e:
-            self.get_logger().error(f"Error disabling torque: {e}")
-
 
 def main(args=None):
     rclpy.init(args=args)
-    gello_publisher = GelloPublisher()
-    rclpy.spin(gello_publisher)
-    gello_publisher.destroy_node()
-    rclpy.shutdown()
+
+    try:
+        gello_publisher = GelloPublisher()
+    except ConnectionError:
+        rclpy.try_shutdown()
+        return
+
+    try:
+        rclpy.spin(gello_publisher)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    finally:
+        gello_publisher.gello_hardware.disable_torque()
+        gello_publisher.destroy_node()
+        rclpy.try_shutdown()
 
 
 if __name__ == "__main__":

ros2/src/franka_gello_state_publisher/launch/main.launch.py

@@ -1,7 +1,7 @@
 import os
 import yaml
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -29,6 +29,7 @@ def generate_robot_nodes(context):
                 name="gello_publisher",
                 namespace=namespace,
                 output="screen",
+                on_exit=Shutdown(),
                 parameters=[
                     {"com_port": "/dev/serial/by-id/" + config["com_port"]},
                     {"gello_name": item_name},