diff --git a/joy_teleop/config/joy_teleop_example.yaml b/joy_teleop/config/joy_teleop_example.yaml index a02910a..bd1074b 100644 --- a/joy_teleop/config/joy_teleop_example.yaml +++ b/joy_teleop/config/joy_teleop_example.yaml @@ -3,9 +3,10 @@ joy_teleop: walk: type: topic - interface_type: geometry_msgs/msg/Twist + interface_type: geometry_msgs/msg/TwistStamped topic_name: cmd_vel deadman_buttons: [4] + toggle_buttons: [0] axis_mappings: linear-x: axis: 1 @@ -20,6 +21,8 @@ joy_teleop: linear-z: button: 2 scale: 3.0 + header-frame_id: + value: 'my_tf_frame' force_push: type: topic diff --git a/joy_teleop/joy_teleop/joy_teleop.py b/joy_teleop/joy_teleop/joy_teleop.py index a003264..54fcfc4 100644 --- a/joy_teleop/joy_teleop/joy_teleop.py +++ b/joy_teleop/joy_teleop/joy_teleop.py @@ -75,17 +75,22 @@ def set_member(msg: typing.Any, member: str, value: typing.Any) -> None: 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, toggle_buttons: str) -> None: self.buttons: typing.List[str] = [] + self.toggle_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] + if toggle_buttons in config: + self.toggle_buttons = config[toggle_buttons] - if len(self.buttons) == 0 and len(self.axes) == 0: + if len(self.buttons) == 0 and len(self.axes) == 0 and len(self.toggle_buttons) == 0: raise JoyTeleopException("No buttons or axes configured for command '{}'".format(name)) + self.prev_joy_state = sensor_msgs.msg.Joy() + # Used to short-circuit the run command if there aren't enough buttons in the message. self.min_button = 0 if len(self.buttons) > 0: @@ -127,12 +132,24 @@ def update_active_from_buttons_and_axes(self, joy_state: sensor_msgs.msg.Joy) -> 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', 'toggle_buttons') self.name = name self.topic_type = get_interface_type(config['interface_type'], 'msg') + self.toggle_buttons = [] + if 'toggle_buttons' in config: + self.toggle_buttons = config['toggle_buttons'] + self.toggle_enabled = True + # Need 2 rising or falling edges for a change in toggle state (button pressed and released) + self.toggle_press_count = 0 + + # For this control mode, self.buttons are deadman_buttons + self.have_deadman = False + if len(self.buttons) > 0: + self.have_deadman = True + # A 'message_value' is a fixed message that is sent in response to an activation. It is # mutually exclusive with an 'axis_mapping'. self.msg_value = None @@ -154,16 +171,18 @@ def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) self.axis_mappings = config['axis_mappings'] # Now check that the mappings have all of the required configuration. for mapping, values in self.axis_mappings.items(): - if 'axis' not in values and 'button' not in values: - raise JoyTeleopException("Axis mapping for '{}' must have an axis or button" - .format(name)) - if 'offset' not in values: - raise JoyTeleopException("Axis mapping for '{}' must have an offset" + if 'axis' not in values and 'button' not in values and 'value' not in values: + raise JoyTeleopException("Axis mapping for '{}' must have an axis, button, or value" .format(name)) - if 'scale' not in values: - raise JoyTeleopException("Axis mapping for '{}' must have a scale" - .format(name)) + if 'axis' in values: + if 'offset' not in values: + raise JoyTeleopException("Axis mapping for '{}' must have an offset" + .format(name)) + + if 'scale' not in values: + raise JoyTeleopException("Axis mapping for '{}' must have a scale" + .format(name)) if self.msg_value is None and not self.axis_mappings: raise JoyTeleopException("No 'message_value' or 'axis_mappings' " @@ -192,11 +211,26 @@ def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None: last_active = self.active self.update_active_from_buttons_and_axes(joy_state) - if not self.active: - return + # This is where the return due to deadman switch happens + if self.have_deadman: + if not self.active: + return if self.msg_value is not None and last_active == self.active: return + # Check toggle status for this command + if len(self.prev_joy_state.buttons) > 0: + for toggle_button in self.toggle_buttons: + if joy_state.buttons[int(toggle_button)] != self.prev_joy_state.buttons[toggle_button]: + # Need 2 rising or falling edges for a change in toggle state (button pressed and released) + self.toggle_press_count = self.toggle_press_count + 1 + if self.toggle_press_count == 2: + self.toggle_enabled = not self.toggle_enabled + self.toggle_press_count = 0 + self.prev_joy_state = joy_state + if not self.toggle_enabled: + return + if self.msg_value is not None: # This is the case for a static message. msg = self.msg_value @@ -223,6 +257,9 @@ def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None: 'but #{} was referenced in config.'.format( len(joy_state.buttons), values['button'])) val = 0.0 + elif 'value' in values: + # Pass on the value as its Python-implicit type + val = values.get('value') else: node.get_logger().error( 'No Supported axis_mappings type found in: {}'.format(mapping)) @@ -240,7 +277,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', 'toggle_buttons') self.name = name @@ -289,7 +326,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', 'toggle_buttons') self.name = name