diff --git a/joy_teleop/joy_teleop/joy_teleop.py b/joy_teleop/joy_teleop/joy_teleop.py index cedbd82..98f6908 100644 --- a/joy_teleop/joy_teleop/joy_teleop.py +++ b/joy_teleop/joy_teleop/joy_teleop.py @@ -100,7 +100,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 = 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): @@ -109,7 +109,7 @@ 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 + 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. @@ -117,7 +117,7 @@ def update_active_from_buttons_and_axes(self, joy_state: sensor_msgs.msg.Joy) -> for axis in self.axes: try: - self.active |= joy_state.axes[axis] == 1.0 + self.active &= joy_state.axes[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. diff --git a/joy_teleop/test/test_multi_button_command.py b/joy_teleop/test/test_multi_button_command.py new file mode 100644 index 0000000..bed9a50 --- /dev/null +++ b/joy_teleop/test/test_multi_button_command.py @@ -0,0 +1,109 @@ +# -*- coding: utf-8 -*- +# +# Copyright (c) 2023 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. + +import example_interfaces.srv +from joy_teleop_testing_common import generate_joy_test_description, TestJoyTeleop +import pytest +import rclpy + + +@pytest.mark.rostest +def generate_test_description(): + parameters = {} + parameters['addtwoints.type'] = 'service' + parameters['addtwoints.interface_type'] = 'example_interfaces/srv/AddTwoInts' + parameters['addtwoints.service_name'] = '/addtwoints' + parameters['addtwoints.buttons'] = [0, 4] + parameters['addtwoints.service_request.a'] = 6 + parameters['addtwoints.service_request.b'] = 1 + + return generate_joy_test_description(parameters) + + +class TestJoyTeleopServiceAddTwoInts(TestJoyTeleop): + + def publish_message(self): + self.joy_publisher.publish(self.joy_msg) + + def test_addtwoints_service(self): + service_result = None + future = rclpy.task.Future() + + def addtwoints(request, response): + nonlocal service_result + nonlocal future + + service_result = request.a + request.b + response.sum = service_result + future.set_result(True) + + return response + + srv = self.node.create_service(example_interfaces.srv.AddTwoInts, '/addtwoints', + addtwoints) + + try: + # Above we set the buttons to be used as '0' and '4', + # so here we set ONLY the '4' button active. + self.joy_msg.buttons = [0, 0, 0, 0, 1] + + self.executor.spin_until_future_complete(future, timeout_sec=10) + + # Check + self.assertFalse(future.done() and future.result(), + 'Completed addtwoints service call but timeout expected') + + # Above we set the buttons to be used as '0' and '4', + # so here we set ONLY the '0' button active. + self.joy_msg.buttons = [1, 0, 0, 0, 0] + + self.executor.spin_until_future_complete(future, timeout_sec=10) + + # Check + self.assertFalse(future.done() and future.result(), + 'Completed addtwoints service call but timeout expected') + + # Above we set the buttons to be used as '0' and '4', + # so here we set BOTH buttons active. + self.joy_msg.buttons = [1, 0, 0, 0, 1] + + self.executor.spin_until_future_complete(future, timeout_sec=10) + + # Check + self.assertTrue(future.done() and future.result(), + 'Timed out waiting for addtwoints service to complete') + self.assertEqual(service_result, 7) + finally: + # Cleanup + self.node.destroy_service(srv)