From cd0e8626890683ca0ff1ac77bbe32809a9196d31 Mon Sep 17 00:00:00 2001 From: El Jawad Alaa Date: Thu, 12 Sep 2024 12:48:25 +0200 Subject: [PATCH] Extend mapping to index of an array (#76) --- joy_teleop/config/joy_teleop_example.yaml | 24 +++++ joy_teleop/joy_teleop/joy_teleop.py | 30 +++++- joy_teleop/test/test_array_indexing.py | 112 +++++++++++++++++++++ joy_teleop/test/test_get_interface_type.py | 6 +- 4 files changed, 164 insertions(+), 8 deletions(-) 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 5263dad..f8c2adc 100644 --- a/joy_teleop/config/joy_teleop_example.yaml +++ b/joy_teleop/config/joy_teleop_example.yaml @@ -82,6 +82,30 @@ joy_teleop: - 4 - 2 + array3: + 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: 4 + axis: 2 + scale: 1 + offset: 0 + torso_up: type: action interface_type: teleop_tools_msgs/action/Increment diff --git a/joy_teleop/joy_teleop/joy_teleop.py b/joy_teleop/joy_teleop/joy_teleop.py index 98f6908..6c15330 100644 --- a/joy_teleop/joy_teleop/joy_teleop.py +++ b/joy_teleop/joy_teleop/joy_teleop.py @@ -32,6 +32,7 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +import array import importlib import typing @@ -62,14 +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 +146,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 +238,22 @@ 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) + 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) # If there is a stamp field, fill it with now(). if hasattr(msg, 'header'): diff --git a/joy_teleop/test/test_array_indexing.py b/joy_teleop/test/test_array_indexing.py new file mode 100644 index 0000000..061af35 --- /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. + +from joy_teleop_testing_common import generate_joy_test_description, TestJoyTeleop +import pytest +import rclpy +from std_msgs.msg import UInt8MultiArray + + +@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() 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():