diff --git a/joy_teleop/config/joy_teleop_example.yaml b/joy_teleop/config/joy_teleop_example.yaml index f8c2adc..8b40805 100644 --- a/joy_teleop/config/joy_teleop_example.yaml +++ b/joy_teleop/config/joy_teleop_example.yaml @@ -6,6 +6,8 @@ joy_teleop: interface_type: geometry_msgs/msg/TwistStamped topic_name: cmd_vel deadman_buttons: [4] + #deadman_axes: [0] + #deadman_axes_negative: [1] axis_mappings: linear-x: axis: 1 @@ -30,6 +32,8 @@ joy_teleop: interface_type: geometry_msgs/msg/Wrench topic_name: base_link_wrench deadman_buttons: [5, 7] + #deadman_axes: [0] + #deadman_axes_negative: [1] axis_mappings: force-x: axis: 3 @@ -45,6 +49,8 @@ joy_teleop: interface_type: geometry_msgs/msg/Twist topic_name: cmd_vel deadman_buttons: [0, 2] + #deadman_axes: [0] + #deadman_axes_negative: [1] message_value: linear-x: value: 0.0 @@ -58,6 +64,8 @@ joy_teleop: interface_type: std_msgs/msg/String topic_name: chatter deadman_buttons: [2] + #deadman_axes: [0] + #deadman_axes_negative: [1] message_value: data: value: 'Hello' @@ -67,6 +75,8 @@ joy_teleop: interface_type: std_msgs/msg/UInt8MultiArray topic_name: bytes deadman_buttons: [5] + #deadman_axes: [0] + #deadman_axes_negative: [1] message_value: data: value: [1,3,3,7] @@ -76,6 +86,8 @@ joy_teleop: interface_type: std_msgs/msg/UInt8MultiArray topic_name: bytes deadman_buttons: [3] + #deadman_axes: [0] + #deadman_axes_negative: [1] message_value: data: value: @@ -87,6 +99,8 @@ joy_teleop: interface_type: std_msgs/msg/UInt8MultiArray topic_name: array3 deadman_buttons: [0] + #deadman_axes: [0] + #deadman_axes_negative: [1] axis_mappings: data: index: 0 @@ -113,6 +127,26 @@ joy_teleop: action_goal: increment_by: [0.05] buttons: [4] + # axes: [4] + # axes_negative: [5] + + torso_up_axes: + type: action + interface_type: teleop_tools_msgs/action/Increment + action_name: torso_controller/increment + action_goal: + increment_by: [0.05] + buttons: [4] + axes: [5] + + torso_down_axes_negative: + type: action + interface_type: teleop_tools_msgs/action/Increment + action_name: torso_controller/increment + action_goal: + increment_by: [-0.05] + buttons: [4] + axes_negative: [5] torso_down: type: action @@ -121,6 +155,8 @@ joy_teleop: action_goal: increment_by: [-0.05] buttons: [6] + # axes: [4] + # axes_negative: [5] fibonacci: type: action @@ -129,6 +165,8 @@ joy_teleop: action_goal: order: 5 buttons: [4, 5, 6, 7] + # axes: [4] + # axes_negative: [5] add_two_ints: type: service @@ -138,3 +176,5 @@ joy_teleop: a: 11 b: 31 buttons: [10] + # axes: [4] + # axes_negative: [5] \ No newline at end of file diff --git a/joy_teleop/joy_teleop/joy_teleop.py b/joy_teleop/joy_teleop/joy_teleop.py index 6c15330..1fa13c8 100644 --- a/joy_teleop/joy_teleop/joy_teleop.py +++ b/joy_teleop/joy_teleop/joy_teleop.py @@ -77,16 +77,23 @@ def get_parent_member(msg: typing.Any, member: str) -> typing.Tuple[typing.Any, class JoyTeleopCommand: def __init__(self, name: str, config: typing.Dict[str, typing.Any], - button_name: str, axes_name: str) -> None: + button_name: str, axes_name: str, axes_name_negative: str) -> None: + self.buttons: typing.List[str] = [] if button_name in config: self.buttons = config[button_name] self.axes: typing.List[str] = [] if axes_name in config: self.axes = config[axes_name] + self.axes_negative: typing.List[str] = [] + if axes_name_negative in config: + self.axes_negative = config[axes_name_negative] - if len(self.buttons) == 0 and len(self.axes) == 0: - raise JoyTeleopException("No buttons or axes configured for command '{}'".format(name)) + if len(self.buttons) == 0 and len(self.axes) == 0 and len(self.axes_negative) == 0: + raise JoyTeleopException( + "No buttons, axes or negative_axes configured for command '{}'" + .format(name) + ) # Used to short-circuit the run command if there aren't enough buttons in the message. self.min_button = 0 @@ -95,6 +102,9 @@ def __init__(self, name: str, config: typing.Dict[str, typing.Any], self.min_axis = 0 if len(self.axes) > 0: self.min_axis = int(min(self.axes)) + self.min_negative_axis = 0 + if len(self.axes_negative) > 0: + self.min_negative_axis = int(min(self.axes_negative)) # This can be used to "debounce" the message; if there are multiple presses of the buttons # or axes, the command may only activate on the first one until it toggles again. But this @@ -105,7 +115,8 @@ def update_active_from_buttons_and_axes(self, joy_state: sensor_msgs.msg.Joy) -> self.active = True if (self.min_button is not None and len(joy_state.buttons) <= self.min_button) and \ - (self.min_axis is not None and len(joy_state.axes) <= self.min_axis): + (self.min_axis is not None and len(joy_state.axes) <= self.min_axis) and \ + (self.min_negative_axis is not None and len(joy_state.axes) <= self.min_negative_axis): # Not enough buttons or axes, so it can't possibly be a message for this command. return @@ -125,11 +136,19 @@ def update_active_from_buttons_and_axes(self, joy_state: sensor_msgs.msg.Joy) -> # like (0, 10), but the length of the joystick buttons is only 1. Ignore these. pass + for negative_axis in self.axes_negative: + try: + self.active &= joy_state.axes[negative_axis] == -1.0 + except IndexError: + # An index error can occur if this command is configured for multiple buttons + # like (0, 10), but the length of the joystick buttons is only 1. Ignore these. + pass + class JoyTeleopTopicCommand(JoyTeleopCommand): def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) -> None: - super().__init__(name, config, 'deadman_buttons', 'deadman_axes') + super().__init__(name, config, 'deadman_buttons', 'deadman_axes', 'deadman_axes_negative') self.name = name @@ -265,7 +284,7 @@ def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None: class JoyTeleopServiceCommand(JoyTeleopCommand): def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) -> None: - super().__init__(name, config, 'buttons', 'axes') + super().__init__(name, config, 'buttons', 'axes', 'axes_negative') self.name = name @@ -314,7 +333,7 @@ def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None: class JoyTeleopActionCommand(JoyTeleopCommand): def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) -> None: - super().__init__(name, config, 'buttons', 'axes') + super().__init__(name, config, 'buttons', 'axes', 'axes_negative') self.name = name diff --git a/joy_teleop/test/test_parameter_failures.py b/joy_teleop/test/test_parameter_failures.py index d8fc38e..914c1fe 100644 --- a/joy_teleop/test/test_parameter_failures.py +++ b/joy_teleop/test/test_parameter_failures.py @@ -102,8 +102,8 @@ def test_no_buttons_or_axes(self): self.assertTrue(joy_teleop_process.wait_for_shutdown(timeout=10)) self.assertEqual(joy_teleop_process.exit_code, 1) - self.assertTrue('JoyTeleopException: No buttons or axes configured for command' - in joy_teleop_process.output) + self.assertTrue('JoyTeleopException: No buttons, axes or negative_axes ' + 'configured for command ' in joy_teleop_process.output) def test_teleop_no_message_value_or_axis_mappings(self): parameters = {}