diff --git a/joy_teleop/config/joy_teleop_example.yaml b/joy_teleop/config/joy_teleop_example.yaml index 84f7e5b..2ee55c1 100644 --- a/joy_teleop/config/joy_teleop_example.yaml +++ b/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 diff --git a/joy_teleop/joy_teleop/joy_teleop.py b/joy_teleop/joy_teleop/joy_teleop.py index cedbd82..74e70dc 100644 --- a/joy_teleop/joy_teleop/joy_teleop.py +++ b/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 diff --git a/joy_teleop/test/test_deadman_behavior.py b/joy_teleop/test/test_deadman_behavior.py new file mode 100644 index 0000000..20ff2a2 --- /dev/null +++ b/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)