Simultaneous deadman button

joy_teleop/config/joy_teleop_example.yaml

@@ -30,6 +30,7 @@ joy_teleop:
       interface_type: geometry_msgs/msg/Wrench
       topic_name: base_link_wrench
       deadman_buttons: [5, 7]
+      deadman_behavior: all
       axis_mappings:
         force-x:
           axis: 3

joy_teleop/joy_teleop/joy_teleop.py

@@ -75,16 +75,28 @@ 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, behavior: str, node: Node) -> None:
+        self.node: Node = node
         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.behavior: str = ''
+        if behavior in config:
+            self.behavior = config[behavior]
 
         if len(self.buttons) == 0 and len(self.axes) == 0:
-            raise JoyTeleopException("No buttons or axes configured for command '{}'".format(name))
+            raise JoyTeleopException(f"No buttons or axes configured for command '{name}'")
+
+        if self.behavior == '':
+            node.get_logger().warn(f'No button behavior was defined for "{name}".')
+            self. behavior = 'any'
+        if self.behavior not in ['any', 'all']:
+            node.get_logger().warn(f'Unknown button behavior "{self.behavior}" for "{name}".')
+            self. behavior = 'any'
+        node.get_logger().info(f'Button behavior set to "{self.behavior}" for "{name}".')
 
         # Used to short-circuit the run command if there aren't enough buttons in the message.
         self.min_button = 0
@@ -100,7 +112,7 @@ def __init__(self, name: str, config: typing.Dict[str, typing.Any],
         self.active = False
 
     def update_active_from_buttons_and_axes(self, joy_state: sensor_msgs.msg.Joy) -> None:
-        self.active = False
+        self.active = self.behavior == 'all'
 
         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):
@@ -109,7 +121,10 @@ def update_active_from_buttons_and_axes(self, joy_state: sensor_msgs.msg.Joy) ->
 
         for button in self.buttons:
             try:
-                self.active |= joy_state.buttons[button] == 1
+                if self.behavior == 'all':
+                    self.active &= joy_state.buttons[button] == 1
+                elif self.behavior == 'any':
+                    self.active |= joy_state.buttons[button] == 1
             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.
@@ -127,7 +142,7 @@ 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', 'deadman_behavior', node)
 
         self.name = name
 
@@ -245,7 +260,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', 'deadman_behavior', node)
 
         self.name = name
 
@@ -294,7 +309,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', 'deadman_behavior', node)
 
         self.name = name
 

joy_teleop/test/test_deadman_behavior.py

@@ -0,0 +1,139 @@
+# -*- coding: utf-8 -*-
+#
+# Copyright (c) 2020 Open Source Robotics Foundation
+# All rights reserved.
+#
+# Software License Agreement (BSD License 2.0)
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the copyright holder nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from geometry_msgs.msg import Twist
+from joy_teleop_testing_common import generate_joy_test_description, TestJoyTeleop
+
+import launch_testing
+import pytest
+import rclpy
+
+
+@pytest.mark.launch_test
+@launch_testing.parametrize('behavior', ['', 'all', 'any'])
+def generate_test_description(behavior):
+    parameters = {}
+    parameters['twist.type'] = 'topic'
+    parameters['twist.interface_type'] = 'geometry_msgs/msg/Twist'
+    parameters['twist.topic_name'] = '/cmd_vel'
+    parameters['twist.deadman_buttons'] = [2, 4]
+    parameters['twist.deadman_behavior'] = behavior
+    parameters['twist.axis_mappings.linear-x.axis'] = 1
+    parameters['twist.axis_mappings.linear-x.scale'] = 0.8
+    parameters['twist.axis_mappings.linear-x.offset'] = 0.0
+    parameters['twist.axis_mappings.angular-z.axis'] = 3
+    parameters['twist.axis_mappings.angular-z.scale'] = 0.5
+    parameters['twist.axis_mappings.angular-z.offset'] = 0.0
+
+    return generate_joy_test_description(parameters)
+
+
+class MyTestCase(TestJoyTeleop):
+
+    def setUp(self):
+        super().setUp()
+        self.twist: Twist = None
+        self.future = rclpy.task.Future()
+
+        qos = rclpy.qos.QoSProfile(history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST,
+                                   depth=1,
+                                   reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE,
+                                   durability=rclpy.qos.QoSDurabilityPolicy.VOLATILE)
+
+        self.twist_subscriber = self.node.create_subscription(
+            Twist,
+            '/cmd_vel',
+            self.receive_twist,
+            qos,
+        )
+
+    def tearDown(self):
+        self.node.destroy_subscription(self.twist_subscriber)
+        super().tearDown()
+
+    def receive_twist(self, msg):
+        self.twist = msg
+        self.future.set_result(True)
+
+    def test_all_deadman_button_pressed(self, behavior):
+        try:
+            # Here we simulate the button 2, 4 to be pressed
+            self.joy_msg.buttons = [0, 0, 1, 0, 1]
+            self.joy_msg.axes = [0.0, 1.0, 0.0, 1.0]
+
+            self.executor.spin_until_future_complete(self.future, timeout_sec=1)
+
+            self.joy_publisher.publish(self.joy_msg)
+            self.node.executor.spin_once(1)
+
+            # Check
+            self.assertTrue(self.future.done() and self.future.result(),
+                            'Timed out waiting for twist topic to complete')
+            self.assertEqual(self.twist.linear.x, 0.8)
+            self.assertEqual(self.twist.angular.z, 0.5)
+
+        finally:
+            # Cleanup
+            self.node.destroy_subscription(self.twist_subscriber)
+
+    def test_some_deadman_button_pressed(self, behavior):
+
+        try:
+            # This time only button 2 is pressed and not button 4
+            self.joy_msg.buttons = [0, 0, 1, 0, 0]
+            self.joy_msg.axes = [0.0, 1.0, 0.0, 1.0]
+
+            self.executor.spin_until_future_complete(self.future, timeout_sec=1)
+
+            self.publish_from_timer = False
+            self.joy_publisher.publish(self.joy_msg)
+            self.node.executor.spin_once(1)
+
+            # Check
+            if behavior in ['any', '']:
+                # the default behavior or 'any' is that if any of the deadman button is pressed
+                # the message will pass through
+                self.assertTrue(self.future.done() and self.future.result(),
+                                'Timed out waiting for twist topic to complete')
+                self.assertEqual(self.twist.linear.x, 0.8)
+                self.assertEqual(self.twist.angular.z, 0.5)
+            elif behavior == 'all':
+                # the 'all' behavior is that all deadman button must press simultaneously
+                # for the message to pass through
+                self.assertFalse(self.future.done() and self.future.result(),
+                                 'Received a message that instead of none')
+
+        finally:
+            # Cleanup
+            self.node.destroy_subscription(self.twist_subscriber)