diff --git a/ros2/.devcontainer/docker-compose.yml b/ros2/.devcontainer/docker-compose.yml index beea6d00..27dd9e74 100644 --- a/ros2/.devcontainer/docker-compose.yml +++ b/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 diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/README.md b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/README.md index 40806908..2416bc94 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/README.md +++ b/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 diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py index 0c53b927..58509525 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py +++ b/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() diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py index 15793a76..943f5513 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py +++ b/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__": diff --git a/ros2/src/franka_gello_state_publisher/launch/main.launch.py b/ros2/src/franka_gello_state_publisher/launch/main.launch.py index b3c9d3a8..4beffc15 100755 --- a/ros2/src/franka_gello_state_publisher/launch/main.launch.py +++ b/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},