From 234f18c27a630c19cacdde02fa57f18d0e8e2b10 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 8 Mar 2021 15:16:51 -0600 Subject: [PATCH 1/4] Allow a `value` type within an axis mapping. Useful for frame data. --- joy_teleop/config/joy_teleop_example.yaml | 4 +++- joy_teleop/joy_teleop/joy_teleop.py | 21 +++++++++++++-------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/joy_teleop/config/joy_teleop_example.yaml b/joy_teleop/config/joy_teleop_example.yaml index a02910a..0809765 100644 --- a/joy_teleop/config/joy_teleop_example.yaml +++ b/joy_teleop/config/joy_teleop_example.yaml @@ -3,7 +3,7 @@ joy_teleop: walk: type: topic - interface_type: geometry_msgs/msg/Twist + interface_type: geometry_msgs/msg/TwistStamped topic_name: cmd_vel deadman_buttons: [4] axis_mappings: @@ -20,6 +20,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..ce59044 100644 --- a/joy_teleop/joy_teleop/joy_teleop.py +++ b/joy_teleop/joy_teleop/joy_teleop.py @@ -154,16 +154,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' " @@ -223,6 +225,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)) From 6d97ecd391a4a25df2a807aa7fc306be389e8a8f Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 9 Mar 2021 10:21:08 -0600 Subject: [PATCH 2/4] Add a Toggle option to Topic-type commands --- joy_teleop/joy_teleop/joy_teleop.py | 47 ++++++++++++++++++++++++----- 1 file changed, 40 insertions(+), 7 deletions(-) diff --git a/joy_teleop/joy_teleop/joy_teleop.py b/joy_teleop/joy_teleop/joy_teleop.py index ce59044..b0c15e2 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 @@ -194,11 +211,27 @@ 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: + node.get_logger().info("RETURNING DUE TO TOGGLE") + return + if self.msg_value is not None: # This is the case for a static message. msg = self.msg_value @@ -245,7 +278,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 @@ -294,7 +327,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 From b8d6e1053b7d0dfc0044c28c4f3e67185411c2f4 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 9 Mar 2021 10:30:33 -0600 Subject: [PATCH 3/4] Add yaml example --- joy_teleop/config/joy_teleop_example.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/joy_teleop/config/joy_teleop_example.yaml b/joy_teleop/config/joy_teleop_example.yaml index 0809765..bd1074b 100644 --- a/joy_teleop/config/joy_teleop_example.yaml +++ b/joy_teleop/config/joy_teleop_example.yaml @@ -6,6 +6,7 @@ joy_teleop: interface_type: geometry_msgs/msg/TwistStamped topic_name: cmd_vel deadman_buttons: [4] + toggle_buttons: [0] axis_mappings: linear-x: axis: 1 From bc8c16dc4bea8cedc4056c9ae6dd11ff1497ae6d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 9 Mar 2021 10:58:57 -0600 Subject: [PATCH 4/4] Remove debug statement --- joy_teleop/joy_teleop/joy_teleop.py | 1 - 1 file changed, 1 deletion(-) diff --git a/joy_teleop/joy_teleop/joy_teleop.py b/joy_teleop/joy_teleop/joy_teleop.py index b0c15e2..54fcfc4 100644 --- a/joy_teleop/joy_teleop/joy_teleop.py +++ b/joy_teleop/joy_teleop/joy_teleop.py @@ -229,7 +229,6 @@ def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None: self.toggle_press_count = 0 self.prev_joy_state = joy_state if not self.toggle_enabled: - node.get_logger().info("RETURNING DUE TO TOGGLE") return if self.msg_value is not None: