diff --git a/ros2/README.md b/ros2/README.md index 1531551b..11302b22 100644 --- a/ros2/README.md +++ b/ros2/README.md @@ -90,27 +90,35 @@ colcon test ## Getting Started ### 1. **Run the GELLO Publisher** -#### Step 1: Determine your GELLO USB ID +#### Step 1: Determine the port IDs of your communication converters -To proceed, you need to know the USB ID of your GELLO device. This can be determined by running: +To proceed, you need to know the port IDs of your communication converters (U2D2 or OpenRB-150). +This can be determined by running: ```bash ls /dev/serial/by-id ``` Example output: +- U2D2: `usb-FTDI_USB__-__Serial_Converter_FT7WBG6` +- OpenRB-150: `usb-ROBOTIS_OpenRB-150_2B375CB3503059384C2E3120FF053624-if00` + +Use the ID to update the `com_port` parameter in your GELLO configuration file located in `/workspace/ros2/src/franka_gello_state_publisher/config/`. + +Rebuild the project to ensure the updated configuration is applied: ```bash -usb-FTDI_USB__-__Serial_Converter_FT7WBG6 +cd /workspace/ros2 +colcon build ``` -In this case, the `GELLO_USB_ID` would be `/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6`. +#### Step 2: Configure your GELLO -#### Step 2: Configure your GELLO - -If not done already, follow the instructions of the [`Create the GELLO configuration and determining joint ID's` section in the main README.md](../README.md#create-the-gello-configuration-and-determining-joint-ids). +If you are using the "Franka GELLO Duo" variant and assembled it carefully according to the assembly instructions, the default configuration file provided in `franka_gello_state_publisher/config/` should work out of the box and you can proceed with **Step 3**. -Use the output of the `gello_get_offset.py` script to update the `best_offsets` and `gripper_range_rad` in the `/workspace/ros2/src/franka_gello_state_publisher/config/gello_config.yaml` file. +If you have a different variant or still encounter configuration issues, please follow the instructions of the [`Create the GELLO configuration and determining joint ID's` section in the main README.md](../README.md#create-the-gello-configuration-and-determining-joint-ids). + +Use the output of the `gello_get_offset.py` script to update the `best_offsets` and `gripper_range_rad` in your GELLO configuration file located in `/workspace/ros2/src/franka_gello_state_publisher/config/`. Rebuild the project to ensure the updated configuration is applied: @@ -127,12 +135,12 @@ Create a configuration file in `src/franka_gello_state_publisher/config/` or mod ros2 launch franka_gello_state_publisher main.launch.py [config_file:=your_config.yaml] ``` -The `config_file` argument is **optional**. If not provided, it defaults to `example_fr3_config.yaml` in the `franka_gello_state_publisher/config/` directory. +The `config_file` argument is **optional**. If not provided, it defaults to `example_single.yaml` in the `franka_gello_state_publisher/config/` directory. **Configuration parameters:** -- `com_port`: the previously determined -- `namespace`: ROS 2 namespace (must match the robot and the gripper). +- `com_port`: the previously determined port ID of your communication converter +- `namespace`: ROS 2 namespace (must match the robot and the gripper) - `num_joints`: 7 for Franka FR3 - `joint_signs`: as used for calibration - `gripper`: true if Gello gripper state shall be used @@ -153,6 +161,10 @@ This is done by setting the following parameters in the configuration file. Each When the GELLO publisher is started, these parameters are used to configure the Dynamixel motors. The motors then operate in their internal "Current-based Position Control" mode with a current limit set to 600mA. Check the [Dynamixel documentation](https://emanual.robotis.com/docs/en/dxl/x/xl330-m288/) for more information on the control parameters. +> ⚠️ **Warning for OpenRB-150:** +> If using the OpenRB-150 instead of the U2D2 communication converter, do not enable torque until an external 5V power supply is connected to the board's power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque operation may damage your computer's USB port. +> Please refer to the [OpenRB-150 manual](https://emanual.robotis.com/docs/en/parts/controller/openrb-150/#when-running-5v-dynamixel-with-the-usb-power) for more information. + > 💡 **Hint:** > The example configuration files give a good starting point for these values: > - springs in joints 1, 2, 4 and gripper. diff --git a/ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml b/ros2/src/franka_gello_state_publisher/config/example_duo.yaml similarity index 54% rename from ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml rename to ros2/src/franka_gello_state_publisher/config/example_duo.yaml index 9019d408..461d33d9 100644 --- a/ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml +++ b/ros2/src/franka_gello_state_publisher/config/example_duo.yaml @@ -1,13 +1,20 @@ +# This is an example config file for two 7-DoF GELLO arms teleoperating two Franka FR3 robots. +# Check the README to learn how to adapt it to your setup. + LEFT: com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFSIO-if00-port0" namespace: "left" num_arm_joints: 7 joint_signs: [1, 1, 1, 1, 1, -1, 1] gripper: true - best_offsets: [3.142, 3.142, 3.142, 4.712, 3.142, 4.712, 1.571] + best_offsets: [3.142, 3.142, 3.142, 4.712, 3.142, 4.712, 1.571] # rad gripper_range_rad: [2.77856, 3.50931] # Optional Dynamixel control parameters for all arm joints (+ gripper): - dynamixel_torque_enable: [ 1, 1, 1, 1, 1, 1, 1, 1] # 1 on, 0 off + # Warning: If using the OpenRB-150 instead of the U2D2 communication converter, + # do not enable torque until an external 5V power supply is connected to the board's + # power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque + # operation may damage your computer's USB port. + dynamixel_torque_enable: [ 0, 0, 0, 0, 0, 0, 0, 0] # 1 on, 0 off dynamixel_goal_position: [ 0.0, 0.0, 0.0,-1.571, 0.0, 1.571, 0.0, 3.509] # rad dynamixel_kp_p: [ 30, 60, 0, 30, 0, 0, 0, 50] dynamixel_kp_i: [ 0, 0, 0, 0, 0, 0, 0, 0] @@ -19,10 +26,14 @@ RIGHT: num_arm_joints: 7 joint_signs: [1, 1, 1, 1, 1, -1, 1] gripper: true - best_offsets: [3.142, 3.142, -3.142, 4.712, 3.142, 4.712, 1.571] + best_offsets: [3.142, 3.142, 3.142, 4.712, 3.142, 4.712, 1.571] # rad gripper_range_rad: [2.77856, 3.50931] # Optional Dynamixel control parameters for all arm joints (+ gripper): - dynamixel_torque_enable: [ 1, 1, 1, 1, 1, 1, 1, 1] # 1 on, 0 off + # Warning: If using the OpenRB-150 instead of the U2D2 communication converter, + # do not enable torque until an external 5V power supply is connected to the board's + # power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque + # operation may damage your computer's USB port. + dynamixel_torque_enable: [ 0, 0, 0, 0, 0, 0, 0, 0] # 1 on, 0 off dynamixel_goal_position: [ 0.0, 0.0, 0.0,-1.571, 0.0, 1.571, 0.0, 3.509] # rad dynamixel_kp_p: [ 30, 60, 0, 30, 0, 0, 0, 50] dynamixel_kp_i: [ 0, 0, 0, 0, 0, 0, 0, 0] diff --git a/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml b/ros2/src/franka_gello_state_publisher/config/example_single.yaml similarity index 53% rename from ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml rename to ros2/src/franka_gello_state_publisher/config/example_single.yaml index 2b5e8abd..e0a2cd81 100644 --- a/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml +++ b/ros2/src/franka_gello_state_publisher/config/example_single.yaml @@ -1,13 +1,20 @@ -robot1: +# This is an example config file for a 7-DoF GELLO arm teleoperating a Franka FR3 robot. +# Check the README to learn how to adapt it to your setup. + +SINGLE: com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFR6C-if00-port0" namespace: "" num_arm_joints: 7 joint_signs: [1, 1, 1, 1, 1, -1, 1] gripper: true - best_offsets: [3.142, 3.142, -3.142, 4.712, 3.142, 4.712, 1.571] + best_offsets: [3.142, 3.142, -3.142, 4.712, 3.142, 4.712, 1.571] # rad gripper_range_rad: [2.77856, 3.50931] # Optional Dynamixel control parameters for all arm joints (+ gripper): - dynamixel_torque_enable: [ 1, 1, 1, 1, 1, 1, 1, 1] # 1 on, 0 off + # Warning: If using the OpenRB-150 instead of the U2D2 communication converter, + # do not enable torque until an external 5V power supply is connected to the board's + # power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque + # operation may damage your computer's USB port. + dynamixel_torque_enable: [ 0, 0, 0, 0, 0, 0, 0, 0] # 1 on, 0 off dynamixel_goal_position: [ 0.0, 0.0, 0.0,-1.571, 0.0, 1.571, 0.0, 3.509] # rad dynamixel_kp_p: [ 30, 60, 0, 30, 0, 0, 0, 50] dynamixel_kp_i: [ 0, 0, 0, 0, 0, 0, 0, 0] diff --git a/ros2/src/franka_gello_state_publisher/config/franka_gello_duo.yaml b/ros2/src/franka_gello_state_publisher/config/franka_gello_duo.yaml new file mode 100644 index 00000000..8e120ed6 --- /dev/null +++ b/ros2/src/franka_gello_state_publisher/config/franka_gello_duo.yaml @@ -0,0 +1,40 @@ +# This configuration works with the pre-assembled "Franka GELLO Duo" by Franka Robotics GmbH +# See https://franka.de/documents + +LEFT: + com_port: "usb-ROBOTIS_OpenRB-150_xxxxxxxx" # REPLACE WITH YOUR ID + namespace: "left" + num_arm_joints: 7 + joint_signs: [1, -1, 1, -1, 1, 1, 1] + gripper: true + best_offsets: [3.142, 3.142, 0.000, 0.000, 0.000, 1.571, 3.142] # rad + gripper_range_rad: [2.00, 3.22] + # Optional Dynamixel control parameters for all arm joints (+ gripper): + # Warning: If using the OpenRB-150 instead of the U2D2 communication converter, + # do not enable torque until an external 5V power supply is connected to the board's + # power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque + # operation may damage your computer's USB port. + dynamixel_torque_enable: [ 0, 0, 0, 0, 0, 0, 0, 0] # 1 on, 0 off + dynamixel_goal_position: [ 0.0, 0.0, 0.0,-1.571, 0.0, 1.571, 0.0, 3.509] # rad + dynamixel_kp_p: [ 30, 60, 0, 30, 0, 0, 0, 50] + dynamixel_kp_i: [ 0, 0, 0, 0, 0, 0, 0, 0] + dynamixel_kp_d: [ 250, 100, 80, 60, 30, 10, 5, 0] + +RIGHT: + com_port: "usb-ROBOTIS_OpenRB-150_xxxxxxxx" # REPLACE WITH YOUR ID + namespace: "right" + num_arm_joints: 7 + joint_signs: [1, -1, 1, -1, 1, 1, 1] + gripper: true + best_offsets: [3.142, 3.142, 6.283, 0.000, 6.283, 1.571, 3.142] # rad + gripper_range_rad: [2.00, 3.22] + # Optional Dynamixel control parameters for all arm joints (+ gripper): + # Warning: If using the OpenRB-150 instead of the U2D2 communication converter, + # do not enable torque until an external 5V power supply is connected to the board's + # power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque + # operation may damage your computer's USB port. + dynamixel_torque_enable: [ 0, 0, 0, 0, 0, 0, 0, 0] # 1 on, 0 off + dynamixel_goal_position: [ 0.0, 0.0, 0.0,-1.571, 0.0, 1.571, 0.0, 3.509] # rad + dynamixel_kp_p: [ 30, 60, 0, 30, 0, 0, 0, 50] + dynamixel_kp_i: [ 0, 0, 0, 0, 0, 0, 0, 0] + dynamixel_kp_d: [ 250, 100, 80, 60, 30, 10, 5, 0] \ No newline at end of file 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 2416bc94..97881d84 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 @@ -1,6 +1,9 @@ -# Dynamixel Motor Configuration +# Dynamixel Driver This directory contains the configuration-driven Dynamixel driver that supports multiple motor types through YAML configuration files. +The Dynamixel motors can be connected to the system via a communication converter: +- [U2D2](https://emanual.robotis.com/docs/en/parts/interface/u2d2/) +- [OpenRB-150](https://emanual.robotis.com/docs/en/parts/controller/openrb-150/) ## Configuration File Structure 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 58509525..eb1d2427 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 @@ -1,4 +1,5 @@ import time +from glob import glob from pathlib import Path from threading import Event, Lock, Thread from typing import Protocol, Sequence @@ -88,7 +89,15 @@ def read_value_by_name(self, name: str) -> Sequence[int]: """ ... - def close(self): + def start_joint_polling(self) -> None: + """Start the background polling thread.""" + ... + + def stop_joint_polling(self) -> None: + """Stop the background polling thread.""" + ... + + def close(self) -> None: """Close the driver.""" ... @@ -162,7 +171,13 @@ def read_value_by_name(self, name: str) -> Sequence[int]: def get_joints(self) -> np.ndarray: return self._pulses_to_rad(self._storage_map["goal_position"].copy()) - def close(self): + def start_joint_polling(self) -> None: + pass + + def stop_joint_polling(self) -> None: + pass + + def close(self) -> None: pass @@ -173,6 +188,7 @@ def __init__( port: str = "/dev/ttyUSB0", baudrate: int = 57600, motor_type: str = "xl330", + autostart_polling: bool = False, ): """ Initialize the DynamixelDriver class. @@ -187,6 +203,8 @@ def __init__( The baudrate for communication (default is 57600). motor_type : str, optional The type of motor to use, e.g., "xl330" (default is "xl330"). + autostart_polling : bool, optional + Whether to automatically start the joint polling thread (default is False). Raises ------ @@ -199,7 +217,7 @@ def __init__( self._ids = ids self._port = port self._baudrate = baudrate - self._joint_angles = None + self._buffered_joint_positions = None self._lock = Lock() # Load motor configuration @@ -239,10 +257,15 @@ def __init__( 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 + detected_ports = self._detect_com_ports() + if self._port in detected_ports: + msg = "Check that you have permissions to access it." + elif detected_ports: + ports_list = ", ".join(detected_ports) + msg = f"Did you specify the correct port? Detected ports: {ports_list}" + else: + msg = "Is the device connected? No supported devices were detected." + raise ConnectionError(f"Could not open port {self._port}. {msg}") from None # Verify connection by attempting to read the model number try: @@ -257,7 +280,11 @@ def __init__( self.write_value_by_name("torque_enable", [0] * len(self._ids)) self._stop_thread = Event() - self._start_reading_thread() + self._polling_thread = None + self._is_polling = False + + if autostart_polling: + self.start_joint_polling() def _set_group( self, @@ -285,7 +312,8 @@ def _set_group( raise RuntimeError(f"Failed to set {name} for Dynamixel with ID {dxl_id}") comm_result = groupSyncWriteHandler.txPacket() if comm_result != COMM_SUCCESS: - raise RuntimeError(f"Failed to syncwrite {name}") + result_string = self._packetHandler.getTxRxResult(comm_result) + raise RuntimeError(f"Failed to syncwrite {name}, {result_string}") groupSyncWriteHandler.clearParam() def write_value_by_name(self, name: str, values: Sequence[int | None]): @@ -302,7 +330,8 @@ def _read_group(self, name: str, groupSyncReadHandler, value_length: int) -> lis with self._lock: result = groupSyncReadHandler.txRxPacket() if result != COMM_SUCCESS: - raise RuntimeError(f"Failed to sync read {name}, comm result: {result}") + result_string = self._packetHandler.getTxRxResult(result) + raise RuntimeError(f"Failed to sync read {name}, {result_string}") values = [] for dxl_id in self._ids: if groupSyncReadHandler.isAvailable( @@ -324,36 +353,48 @@ def read_value_by_name(self, name: str) -> list[int]: value_length=self._ctrl_table[name]["len"], ) - def _start_reading_thread(self): - self._reading_thread = Thread(target=self._read_joint_angles_loop) - self._reading_thread.daemon = True - self._reading_thread.start() + def start_joint_polling(self) -> None: + if self._is_polling: + return + self._stop_thread.clear() + self._polling_thread = Thread(target=self._joint_polling_loop) + self._polling_thread.daemon = True + self._polling_thread.start() + self._is_polling = True + + def stop_joint_polling(self) -> None: + if not self._is_polling: + return + self._stop_thread.set() + if self._polling_thread is not None: + self._polling_thread.join() + self._is_polling = False + + def _joint_polling_loop(self): + # Continuously read joint positions and update the _buffered_joint_positions array - def _read_joint_angles_loop(self): - # Continuously read joint angles and update the joint_angles array while not self._stop_thread.is_set(): time.sleep(0.001) try: - _joint_angles = self._read_group( - "present_position", - self._groupSyncReadHandlers["present_position"], - self._ctrl_table["present_position"]["len"], + self._buffered_joint_positions = np.array( + self.read_value_by_name("present_position"), dtype=int ) - self._joint_angles = np.array(_joint_angles, dtype=int) except RuntimeError as e: - print(f"warning, comm failed: {e}") + print(f"Warning: {e}") continue def get_joints(self) -> np.ndarray: - # Return a copy of the joint_angles array to avoid race conditions - while self._joint_angles is None: - time.sleep(0.1) - _j = self._joint_angles.copy() - return self._pulses_to_rad(_j) - - def close(self): - self._stop_thread.set() - self._reading_thread.join() + if self._is_polling: + # If polling, return _buffered_joint_positions array, use copy to avoid race conditions + while self._buffered_joint_positions is None: + time.sleep(0.01) + return self._pulses_to_rad(self._buffered_joint_positions.copy()) + else: + joint_positions = np.array(self.read_value_by_name("present_position"), dtype=int) + return self._pulses_to_rad(joint_positions) + + def close(self) -> None: + self.stop_joint_polling() self._portHandler.closePort() def _pulses_to_rad(self, pulses) -> np.ndarray: @@ -363,3 +404,14 @@ def _pulses_to_rad(self, pulses) -> np.ndarray: def _rad_to_pulses(self, rad: float) -> int: """Convert radians to pulses using motor-specific configuration.""" return int(rad / (2 * np.pi) * self._pulses_per_revolution) + + def _detect_com_ports(self) -> list[str]: + """Detect available com_ports of supported communication converters.""" + SUPPORTED_CONVERTERS = [ + "usb-FTDI_USB__-__Serial_Converter", + "usb-ROBOTIS_OpenRB-150", + ] + matches = [] + for converter in SUPPORTED_CONVERTERS: + matches.extend(glob(f"/dev/serial/by-id/{converter}*")) + return matches diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py index 3fb6714a..18639be8 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py @@ -1,3 +1,4 @@ +import time import numpy as np from dataclasses import dataclass, field from typing import List, TypedDict, Iterator, Tuple @@ -73,7 +74,9 @@ class GelloHardware: def __init__( self, hardware_config: GelloHardwareParams, + logger, ) -> None: + self._logger = logger self._com_port = hardware_config["com_port"] self._gello_name = hardware_config["gello_name"] self._num_arm_joints = hardware_config["num_arm_joints"] @@ -100,6 +103,7 @@ def __init__( ) self._initialize_parameters() + self._driver.start_joint_polling() def _initialize_driver(self) -> None: """Initialize dynamixel driver with joint IDs and port.""" @@ -110,6 +114,18 @@ def _initialize_parameters(self) -> None: """Write all dynamixel configuration parameters to hardware.""" for param_name, param_value in self._dynamixel_control_config: self._driver.write_value_by_name(param_name, param_value) + if ( + param_name == "torque_enable" + and any(v == 1 for v in param_value) + and "OpenRB-150" in self._com_port + ): + self._logger.warning( + "Enabling torque... Please make sure you have connected an external power " + "supply to the OpenRB-150 board and that the jumper is set to 'VIN(DXL)'. " + "Using the USB connection as a power source for torque operation may cause " + "damage to your PC." + ) + time.sleep(0.1) # Dynamixels are not immediately ready after these parameter writes def update_dynamixel_control_parameter( self, param_name: str, param_value: list[float] | list[int] diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py index 174ca8b9..c5180689 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py @@ -12,6 +12,7 @@ class ParameterConfig: class GelloParameterConfig: """Configuration class for GELLO ROS2 parameters.""" + DEFAULT_COM_PORT = "/dev/ttyUSB0" DEFAULT_NUM_JOINTS = 7 DEFAULT_JOINT_SIGNS = [1] * DEFAULT_NUM_JOINTS DEFAULT_BEST_OFFSETS = [0.0] * DEFAULT_NUM_JOINTS @@ -19,7 +20,7 @@ class GelloParameterConfig: DEFAULT_CONTROL_GAINS = [0] * DEFAULT_NUM_JOINTS DEFAULT_GOAL_POSITION = [0.0] * DEFAULT_NUM_JOINTS - def __init__(self, default_com_port: str): + def __init__(self): self.hardware_params = [ ParameterConfig( ParameterDescriptor( @@ -28,7 +29,7 @@ def __init__(self, default_com_port: str): description="USB serial port path", read_only=True, ), - default_com_port, + self.DEFAULT_COM_PORT, ), ParameterConfig( ParameterDescriptor( @@ -37,7 +38,7 @@ def __init__(self, default_com_port: str): description="GELLO device identifier", read_only=True, ), - default_com_port, + self.DEFAULT_COM_PORT, ), ParameterConfig( ParameterDescriptor( 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 943f5513..a71d3474 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,6 +1,5 @@ import rclpy from rclpy.executors import ExternalShutdownException -from glob import glob from rclpy.node import Node from sensor_msgs.msg import JointState from std_msgs.msg import Float32 @@ -23,7 +22,7 @@ def __init__(self) -> None: hardware_params: GelloHardwareParams = self._setup_hardware_parameters() try: - self.gello_hardware = GelloHardware(hardware_params) + self.gello_hardware = GelloHardware(hardware_params, self.get_logger()) except ConnectionError as e: self.get_logger().error(f"Failed to initialize GELLO hardware: {e}") raise @@ -82,16 +81,6 @@ def destroy_node(self) -> None: self.gello_hardware.disable_torque() super().destroy_node() - def _determine_default_com_port(self) -> str: - """Auto-detect GELLO device COM port or return invalid placeholder.""" - matches = glob("/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter*") - if matches: - self.get_logger().info(f"Auto-detected com_ports: {matches}") - return matches[0] - else: - self.get_logger().warn("No com_ports detected. Please specify the com_port manually.") - return "INVALID_COM_PORT" - def _declare_ros2_param(self, param: ParameterConfig): """Declare ROS2 parameters.""" parameter_value = self.declare_parameter( @@ -102,8 +91,7 @@ def _declare_ros2_param(self, param: ParameterConfig): def _setup_hardware_parameters(self): """Declare and setup all hardware configuration parameters.""" - default_com_port = self._determine_default_com_port() - config = GelloParameterConfig(default_com_port) + config = GelloParameterConfig() hardware_params: GelloHardwareParams = {} for param in config: 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 4beffc15..513a5d3d 100755 --- a/ros2/src/franka_gello_state_publisher/launch/main.launch.py +++ b/ros2/src/franka_gello_state_publisher/launch/main.launch.py @@ -54,7 +54,7 @@ def generate_launch_description(): [ DeclareLaunchArgument( "config_file", - default_value="example_fr3_config.yaml", + default_value="example_single.yaml", description="Name of the gello configuration file to load", ), OpaqueFunction(function=generate_robot_nodes),