From d9d676a770de0b473ba1d76cdf7862cecdf6deef Mon Sep 17 00:00:00 2001 From: Alaa Date: Mon, 2 May 2022 18:48:07 +0200 Subject: [PATCH 01/11] allow axis mapping to work on list --- joy_teleop/joy_teleop/joy_teleop.py | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/joy_teleop/joy_teleop/joy_teleop.py b/joy_teleop/joy_teleop/joy_teleop.py index cedbd82..0b97114 100644 --- a/joy_teleop/joy_teleop/joy_teleop.py +++ b/joy_teleop/joy_teleop/joy_teleop.py @@ -34,6 +34,7 @@ import importlib import typing +import array import rclpy from rclpy.action import ActionClient @@ -62,15 +63,15 @@ def get_interface_type(type_name: str, interface_type: str) -> typing.Any: return getattr(mod, message) -def set_member(msg: typing.Any, member: str, value: typing.Any) -> None: - ml = member.split('-') +def get_parent_member(msg: typing.Any, member: str) -> typing.Tuple[typing.Any, str]: + ml = member.strip('-').split('-') if len(ml) < 1: return target = msg for i in ml[:-1]: target = getattr(target, i) - setattr(target, ml[-1], value) + return target, ml[-1] class JoyTeleopCommand: @@ -144,7 +145,10 @@ def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) # config can't work. self.msg_value = self.topic_type() for target, param in msg_config.items(): - set_member(self.msg_value, target, param['value']) + res = get_parent_member(self.msg_value, target) + if res: + parent, attr_name = res + setattr(parent, attr_name, param['value']) # An 'axis_mapping' takes data from one part of the message and scales and offsets it to # publish if an activation happens. This is typically used to take joystick analog data @@ -233,7 +237,18 @@ def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None: 'No Supported axis_mappings type found in: {}'.format(mapping)) val = 0.0 - set_member(msg, mapping, val) + res = get_parent_member(msg, mapping) + if res: + parent, sub_field_name = res + if isinstance(getattr(parent, sub_field_name), (list, array.array)): + index_el = values.get('index', 0) + field_list = getattr(parent, sub_field_name) + while len(field_list) <= index_el: + # complete + field_list.append(0) + field_list[index_el] = val + else: + setattr(parent, sub_field_name, val) # If there is a stamp field, fill it with now(). if hasattr(msg, 'header'): From 783326f5a0b279494fd86056ecca5baf0d344f29 Mon Sep 17 00:00:00 2001 From: Alaa Date: Mon, 2 May 2022 19:19:35 +0200 Subject: [PATCH 02/11] fix linters and wrong import --- joy_teleop/joy_teleop/joy_teleop.py | 3 ++- joy_teleop/test/test_get_interface_type.py | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/joy_teleop/joy_teleop/joy_teleop.py b/joy_teleop/joy_teleop/joy_teleop.py index 0b97114..8d1f093 100644 --- a/joy_teleop/joy_teleop/joy_teleop.py +++ b/joy_teleop/joy_teleop/joy_teleop.py @@ -32,9 +32,9 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +import array import importlib import typing -import array import rclpy from rclpy.action import ActionClient @@ -73,6 +73,7 @@ def get_parent_member(msg: typing.Any, member: str) -> typing.Tuple[typing.Any, return target, ml[-1] + class JoyTeleopCommand: def __init__(self, name: str, config: typing.Dict[str, typing.Any], diff --git a/joy_teleop/test/test_get_interface_type.py b/joy_teleop/test/test_get_interface_type.py index 8aeb79e..51673b9 100644 --- a/joy_teleop/test/test_get_interface_type.py +++ b/joy_teleop/test/test_get_interface_type.py @@ -32,6 +32,7 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +import action_tutorials_interfaces.action from joy_teleop.joy_teleop import get_interface_type from joy_teleop.joy_teleop import JoyTeleopException @@ -39,7 +40,6 @@ import sensor_msgs.msg import std_srvs.srv -import test_msgs.action def test_message(): @@ -55,9 +55,9 @@ def test_service(): def test_action(): - interface_type = get_interface_type('test_msgs/action/Fibonacci', 'action') + interface_type = get_interface_type('action_tutorials_interfaces/action/Fibonacci', 'action') action = interface_type.Goal() - assert isinstance(action, test_msgs.action.Fibonacci.Goal) + assert isinstance(action, action_tutorials_interfaces.action.Fibonacci.Goal) def test_bad_message(): From 6cc0c2e3f7b389f1794018cc9047350772d2f771 Mon Sep 17 00:00:00 2001 From: Alaa Date: Tue, 3 May 2022 08:48:59 +0200 Subject: [PATCH 03/11] add an example of mapping --- joy_teleop/config/joy_teleop_example.yaml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/joy_teleop/config/joy_teleop_example.yaml b/joy_teleop/config/joy_teleop_example.yaml index 84f7e5b..480743d 100644 --- a/joy_teleop/config/joy_teleop_example.yaml +++ b/joy_teleop/config/joy_teleop_example.yaml @@ -82,6 +82,27 @@ joy_teleop: - 4 - 2 + array3: + type: topic + interface_type: std_msgs/msg/UInt8MultiArray + topic_name: array3 + axis_mappings: + data: + index: 0 + axis: 0 + scale: 1 + offset: 0 + data-: + index: 1 + axis: 1 + scale: 1 + offset: 0 + data--: + index: 2 + axis: 2 + scale: 1 + offset: 0 + torso_up: type: action interface_type: teleop_tools_msgs/action/Increment From cafdf7fbf238784a8306e8c66ff5b5d27539d71c Mon Sep 17 00:00:00 2001 From: Alaa Date: Tue, 3 May 2022 10:16:56 +0200 Subject: [PATCH 04/11] add unit test + example --- joy_teleop/config/joy_teleop_example.yaml | 5 +- joy_teleop/test/test_array_indexing.py | 112 ++++++++++++++++++++++ 2 files changed, 116 insertions(+), 1 deletion(-) create mode 100644 joy_teleop/test/test_array_indexing.py diff --git a/joy_teleop/config/joy_teleop_example.yaml b/joy_teleop/config/joy_teleop_example.yaml index 480743d..7bfd1d7 100644 --- a/joy_teleop/config/joy_teleop_example.yaml +++ b/joy_teleop/config/joy_teleop_example.yaml @@ -86,19 +86,22 @@ joy_teleop: type: topic interface_type: std_msgs/msg/UInt8MultiArray topic_name: array3 + deadman_buttons: [0] axis_mappings: data: index: 0 axis: 0 scale: 1 offset: 0 + # leading dash are going to be striped away, but this allows us to have the same + # field name (yaml doesn't allow duplicate keys) data-: index: 1 axis: 1 scale: 1 offset: 0 data--: - index: 2 + index: 4 axis: 2 scale: 1 offset: 0 diff --git a/joy_teleop/test/test_array_indexing.py b/joy_teleop/test/test_array_indexing.py new file mode 100644 index 0000000..f426654 --- /dev/null +++ b/joy_teleop/test/test_array_indexing.py @@ -0,0 +1,112 @@ +# -*- 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. + +import pytest +import rclpy +from std_msgs.msg import UInt8MultiArray + +from joy_teleop_testing_common import generate_joy_test_description, TestJoyTeleop + + +@pytest.mark.rostest +def generate_test_description(): + parameters = {} + parameters['array3.type'] = 'topic' + parameters['array3.interface_type'] = 'std_msgs/msg/UInt8MultiArray' + parameters['array3.topic_name'] = '/array3' + parameters['array3.deadman_buttons'] = [0] + + parameters['array3.axis_mappings.data.index'] = 0 + parameters['array3.axis_mappings.data.axis'] = 0 + parameters['array3.axis_mappings.data.scale'] = 1 + parameters['array3.axis_mappings.data.offset'] = 0 + + parameters['array3.axis_mappings.data-.index'] = 1 + parameters['array3.axis_mappings.data-.axis'] = 1 + parameters['array3.axis_mappings.data-.scale'] = 1 + parameters['array3.axis_mappings.data-.offset'] = 0 + + parameters['array3.axis_mappings.data--.index'] = 4 + parameters['array3.axis_mappings.data--.axis'] = 2 + parameters['array3.axis_mappings.data--.scale'] = 1 + parameters['array3.axis_mappings.data--.offset'] = 0 + + return generate_joy_test_description(parameters) + + +class ArrayIndexingMappingTestSuite(TestJoyTeleop): + def publish_message(self): + self.joy_publisher.publish(self.joy_msg) + + def test_array_mapping(self): + array: UInt8MultiArray = None + future = rclpy.task.Future() + + def receive_array(msg): + nonlocal array + nonlocal future + + array = msg + future.set_result(True) + + qos = rclpy.qos.QoSProfile(history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST, + depth=1, + reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE, + durability=rclpy.qos.QoSDurabilityPolicy.VOLATILE) + + array_subscriber = self.node.create_subscription( + UInt8MultiArray, + '/array3', + receive_array, + qos, + ) + + try: + self.joy_msg.buttons = [1] # deadman button pressed + self.joy_msg.axes = [1.0, 1.0, 1.0] + + self.executor.spin_until_future_complete(future, timeout_sec=10) + + # Check + self.assertTrue(future.done() and future.result(), + 'Timed out waiting for array topic to complete') + self.assertSequenceEqual(array.data, [1, 1, 0, 0, 1]) + + finally: + # Cleanup + self.node.destroy_subscription(array_subscriber) + + +if __name__ == '__main__': + pytest.main() From d01f60ff4b61b6689cfe24a8a4d66a25dbb9a9fd Mon Sep 17 00:00:00 2001 From: Alaa Date: Tue, 3 May 2022 10:17:14 +0200 Subject: [PATCH 05/11] handle list vs array.array --- joy_teleop/joy_teleop/joy_teleop.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/joy_teleop/joy_teleop/joy_teleop.py b/joy_teleop/joy_teleop/joy_teleop.py index 8d1f093..2a3f7f5 100644 --- a/joy_teleop/joy_teleop/joy_teleop.py +++ b/joy_teleop/joy_teleop/joy_teleop.py @@ -247,7 +247,11 @@ def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None: while len(field_list) <= index_el: # complete field_list.append(0) - field_list[index_el] = val + if isinstance(field_list, list): + field_list[index_el] = val + else: + # array.array: use first element which has correct type to cast + field_list[index_el] = type(field_list[0])(val) else: setattr(parent, sub_field_name, val) From 03cec698ca1d87ca77545cf6eb1b9403deb6bce2 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 5 Aug 2024 09:49:51 +0100 Subject: [PATCH 06/11] fix flake8 complaints --- joy_teleop/test/test_array_indexing.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joy_teleop/test/test_array_indexing.py b/joy_teleop/test/test_array_indexing.py index f426654..878f652 100644 --- a/joy_teleop/test/test_array_indexing.py +++ b/joy_teleop/test/test_array_indexing.py @@ -34,9 +34,9 @@ import pytest import rclpy -from std_msgs.msg import UInt8MultiArray from joy_teleop_testing_common import generate_joy_test_description, TestJoyTeleop +from std_msgs.msg import UInt8MultiArray @pytest.mark.rostest @@ -66,6 +66,7 @@ def generate_test_description(): class ArrayIndexingMappingTestSuite(TestJoyTeleop): + def publish_message(self): self.joy_publisher.publish(self.joy_msg) From 89592d64fb1f133e924dc49dd69d6bfef4a4d218 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 5 Aug 2024 10:00:27 +0100 Subject: [PATCH 07/11] ok flake8 you are drunk --- joy_teleop/test/test_array_indexing.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/joy_teleop/test/test_array_indexing.py b/joy_teleop/test/test_array_indexing.py index 878f652..b35e8eb 100644 --- a/joy_teleop/test/test_array_indexing.py +++ b/joy_teleop/test/test_array_indexing.py @@ -32,12 +32,12 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import pytest -import rclpy - from joy_teleop_testing_common import generate_joy_test_description, TestJoyTeleop from std_msgs.msg import UInt8MultiArray +import pytest +import rclpy + @pytest.mark.rostest def generate_test_description(): From 34fb590b60028edf0c4324fc7fa42f50d35e0b4a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 5 Aug 2024 10:03:52 +0100 Subject: [PATCH 08/11] pytest first --- joy_teleop/test/test_array_indexing.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joy_teleop/test/test_array_indexing.py b/joy_teleop/test/test_array_indexing.py index b35e8eb..4907350 100644 --- a/joy_teleop/test/test_array_indexing.py +++ b/joy_teleop/test/test_array_indexing.py @@ -32,10 +32,11 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +import pytest + from joy_teleop_testing_common import generate_joy_test_description, TestJoyTeleop from std_msgs.msg import UInt8MultiArray -import pytest import rclpy From 12339a5d8d184bdbd81552ea160cbcf61965af66 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 5 Aug 2024 10:08:14 +0100 Subject: [PATCH 09/11] wow funky ordering --- joy_teleop/test/test_array_indexing.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/joy_teleop/test/test_array_indexing.py b/joy_teleop/test/test_array_indexing.py index 4907350..f93174f 100644 --- a/joy_teleop/test/test_array_indexing.py +++ b/joy_teleop/test/test_array_indexing.py @@ -32,14 +32,13 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import pytest - from joy_teleop_testing_common import generate_joy_test_description, TestJoyTeleop from std_msgs.msg import UInt8MultiArray - +import pytest import rclpy + @pytest.mark.rostest def generate_test_description(): parameters = {} From d0829aa0065962aa824d7eb9d89d1727215f2b9a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 5 Aug 2024 10:12:01 +0100 Subject: [PATCH 10/11] that's about the last permutation --- joy_teleop/test/test_array_indexing.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joy_teleop/test/test_array_indexing.py b/joy_teleop/test/test_array_indexing.py index f93174f..d2ec29e 100644 --- a/joy_teleop/test/test_array_indexing.py +++ b/joy_teleop/test/test_array_indexing.py @@ -33,8 +33,8 @@ # POSSIBILITY OF SUCH DAMAGE. from joy_teleop_testing_common import generate_joy_test_description, TestJoyTeleop -from std_msgs.msg import UInt8MultiArray import pytest +from std_msgs.msg import UInt8MultiArray import rclpy From a00890bc984526c562da3b01842bd6d6765664f1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 5 Aug 2024 10:15:23 +0100 Subject: [PATCH 11/11] :shrug: --- joy_teleop/test/test_array_indexing.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/joy_teleop/test/test_array_indexing.py b/joy_teleop/test/test_array_indexing.py index d2ec29e..061af35 100644 --- a/joy_teleop/test/test_array_indexing.py +++ b/joy_teleop/test/test_array_indexing.py @@ -34,9 +34,8 @@ from joy_teleop_testing_common import generate_joy_test_description, TestJoyTeleop import pytest -from std_msgs.msg import UInt8MultiArray import rclpy - +from std_msgs.msg import UInt8MultiArray @pytest.mark.rostest