Correctly handle fixed length array conversion in services and actions (

#4)
locusrobotics · Feb 9, 2024 · 1243889 · 1243889
1 parent 3a35ce4
commit 1243889
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 10 deletions.
diff --git a/resource/interface_factories.cpp.em b/resource/interface_factories.cpp.em
@@ -350,7 +350,9 @@ void ServiceFactory<
 ) {
 @[      for field in service["fields"][type.lower()]]@
 @[        if field["array"]]@
+@[          if field["variable_length_array"]]@
   req@(to).@(field["ros" + to]["name"]).resize(req@(frm).@(field["ros" + frm]["name"]).size());
+@[          end if]@
   auto @(field["ros1"]["name"])1_it = req1.@(field["ros1"]["name"]).begin();
   auto @(field["ros2"]["name"])2_it = req2.@(field["ros2"]["name"]).begin();
   while (

diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py
@@ -74,6 +74,12 @@
 import rosmsg  # noqa
 
 
+# matches array variables in ROS messages like: double[24] or geometry_msgs/Pose[]
+array_pattern = re.compile(r'\[\d*\]$')
+# matches variable length array variables in ROS messages like: geometry_msgs/Twist[]
+variable_length_array_pattern = re.compile(r'\[\]$')
+
+
 def generate_cpp(output_path, template_dir):
     # Temporary directory is used to hold generated files, which are only
     # copied into place if they differ from previously generated files. This
@@ -849,16 +855,18 @@ def determine_common_services(
                             break
                 output[direction].append({
                     'basic': False if '/' in ros1_type else True,
-                    'array': True if '[]' in ros1_type else False,
+                    'array': array_pattern.search(ros1_type) is not None,
+                    'variable_length_array':
+                        variable_length_array_pattern.search(ros1_type) is not None,
                     'ros1': {
                         'name': ros1_name,
-                        'type': ros1_type.rstrip('[]'),
-                        'cpptype': ros1_type.rstrip('[]').replace('/', '::')
+                        'type': array_pattern.sub("", ros1_type),
+                        'cpptype': array_pattern.sub("", ros1_type).replace('/', '::')
                     },
                     'ros2': {
                         'name': ros2_name,
-                        'type': ros2_type.rstrip('[]'),
-                        'cpptype': ros2_type.rstrip('[]').replace('/', '::msg::')
+                        'type': array_pattern.sub("", ros2_type),
+                        'cpptype': array_pattern.sub("", ros2_type).replace('/', '::msg::')
                     }
                 })
         if match:
@@ -948,16 +956,18 @@ def determine_common_actions(
                             break
                 output[direction].append({
                     'basic': False if '/' in ros1_type else True,
-                    'array': True if '[]' in ros1_type else False,
+                    'array': array_pattern.search(ros1_type) is not None,
+                    'variable_length_array':
+                        variable_length_array_pattern.search(ros1_type) is not None,
                     'ros1': {
                         'name': ros1_name,
-                        'type': ros1_type.rstrip('[]'),
-                        'cpptype': ros1_type.rstrip('[]').replace('/', '::')
+                        'type': array_pattern.sub("", ros1_type),
+                        'cpptype': array_pattern.sub("", ros1_type).replace('/', '::')
                     },
                     'ros2': {
                         'name': ros2_name,
-                        'type': ros2_type.rstrip('[]'),
-                        'cpptype': ros2_type.rstrip('[]').replace('/', '::msg::')
+                        'type': array_pattern.sub("", ros2_type),
+                        'cpptype': array_pattern.sub("", ros2_type).replace('/', '::msg::')
                     }
                 })
         if match: