Extend mapping to index of an array (#76)

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

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'):

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()

joy_teleop/test/test_get_interface_type.py

@@ -32,14 +32,14 @@
 # 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
 
 import pytest
 
 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():