From cf02790a6eb26f86f285b2b5bf2ee6fb530428ed Mon Sep 17 00:00:00 2001 From: DingoOz Date: Thu, 26 Dec 2024 16:13:21 +1000 Subject: [PATCH] input_devices(xbox_controller): High-speed deadman's switch added --- .../input_devices/xbox_controller.py | 58 +++++++++++++------ 1 file changed, 39 insertions(+), 19 deletions(-) diff --git a/software/ros_ws/src/input_devices/input_devices/xbox_controller.py b/software/ros_ws/src/input_devices/input_devices/xbox_controller.py index 857b746..bc480b3 100644 --- a/software/ros_ws/src/input_devices/input_devices/xbox_controller.py +++ b/software/ros_ws/src/input_devices/input_devices/xbox_controller.py @@ -11,8 +11,8 @@ class XboxController(Node): A ROS2 node that processes Xbox controller inputs and publishes velocity commands. This node subscribes to joystick inputs and converts them into velocity commands - for robot control. It includes safety features like a dead man's switch and - configurable scaling factors. + for robot control. It includes two dead man's switches for different speed modes + and configurable scaling factors. Publishers: input_devices/cmd_vel (geometry_msgs/Twist): Robot velocity commands @@ -21,8 +21,9 @@ class XboxController(Node): joy (sensor_msgs/Joy): Raw joystick inputs Parameters: - TRANSLATION_SCALE (float): Scaling factor for linear motion (default: 0.25) - ROTATION_SCALE (float): Scaling factor for angular motion (default: 0.50) + TRANSLATION_SCALE (float): Base scaling factor for linear motion (default: 0.25) + ROTATION_SCALE (float): Base scaling factor for angular motion (default: 0.50) + HIGH_SPEED_MULTIPLIER (float): Multiplier for high-speed mode (default: 2.0) DEADMAN_THRESHOLD (float): Threshold for dead man's switch activation (default: -0.95) """ @@ -33,12 +34,14 @@ def __init__(self): # Constants self.TRANSLATION_SCALE = 0.25 self.ROTATION_SCALE = 0.50 + self.HIGH_SPEED_MULTIPLIER = 2.0 self.DEADMAN_THRESHOLD = -0.95 # Xbox controller axis mappings self.LEFT_STICK_Y_AXIS = 1 self.RIGHT_STICK_X_AXIS = 2 - self.RIGHT_TRIGGER_AXIS = 5 + self.RIGHT_TRIGGER_AXIS = 5 # Regular speed deadman switch + self.LEFT_TRIGGER_AXIS = 6 # High speed deadman switch # Publishers and subscribers self.velocity_publisher = self.create_publisher( @@ -52,34 +55,43 @@ def process_joystick_input(self, joy_msg: Joy) -> None: """ Process incoming joystick messages and publish velocity commands. + The node supports two speed modes: + - Regular speed (RIGHT_TRIGGER): Normal operation + - High speed (LEFT_TRIGGER): Operates at 2x normal speed + Args: joy_msg (sensor_msgs/Joy): The incoming joystick message """ twist = Twist() - # Check if dead man's switch (right trigger) is engaged - is_deadman_engaged = ( + # Check deadman switches + is_regular_speed = ( joy_msg.axes[self.RIGHT_TRIGGER_AXIS] <= self.DEADMAN_THRESHOLD ) + is_high_speed = joy_msg.axes[self.LEFT_TRIGGER_AXIS] <= self.DEADMAN_THRESHOLD - if is_deadman_engaged: - # Process movement only when dead man's switch is engaged - twist.linear.x = ( - joy_msg.axes[self.LEFT_STICK_Y_AXIS] * self.TRANSLATION_SCALE - ) + if is_regular_speed or is_high_speed: + # Calculate base movement + linear_input = joy_msg.axes[self.LEFT_STICK_Y_AXIS] + rotation_input = joy_msg.axes[self.RIGHT_STICK_X_AXIS] + + # Apply appropriate scaling based on which deadman switch is engaged + speed_multiplier = self.HIGH_SPEED_MULTIPLIER if is_high_speed else 1.0 - rotation_input = joy_msg.axes[self.RIGHT_STICK_X_AXIS] * self.ROTATION_SCALE + twist.linear.x = linear_input * self.TRANSLATION_SCALE * speed_multiplier + + rotation_value = rotation_input * self.ROTATION_SCALE * speed_multiplier # Invert rotation direction when moving backwards twist.angular.z = ( - -1.0 * rotation_input if twist.linear.x < 0 else rotation_input + -1.0 * rotation_value if twist.linear.x < 0 else rotation_value ) - self._log_debug_info(joy_msg, twist, is_deadman_engaged) + self._log_debug_info(joy_msg, twist, is_regular_speed, is_high_speed) self.velocity_publisher.publish(twist) def _log_debug_info( - self, joy_msg: Joy, twist: Twist, is_deadman_engaged: bool + self, joy_msg: Joy, twist: Twist, is_regular_speed: bool, is_high_speed: bool ) -> None: """ Log debug information about controller state and outputs. @@ -87,7 +99,8 @@ def _log_debug_info( Args: joy_msg (sensor_msgs/Joy): The current joystick message twist (geometry_msgs/Twist): The calculated velocity command - is_deadman_engaged (bool): Current state of dead man's switch + is_regular_speed (bool): State of regular speed deadman switch + is_high_speed (bool): State of high speed deadman switch """ self.get_logger().debug("Xbox Controller Axes:") for idx, axis_value in enumerate(joy_msg.axes): @@ -96,9 +109,16 @@ def _log_debug_info( self.get_logger().debug("Velocity Output:") self.get_logger().debug(f" Linear X: {twist.linear.x:.2f} m/s") self.get_logger().debug(f" Angular Z: {twist.angular.z:.2f} rad/s") - self.get_logger().debug( - f' Dead Man\'s Switch: {"Engaged" if is_deadman_engaged else "Disengaged"}' + + speed_mode = ( + "High Speed" + if is_high_speed + else "Regular Speed" + if is_regular_speed + else "Disabled" ) + self.get_logger().debug(f" Speed Mode: {speed_mode}") + self.get_logger().debug("------------------------")