Merge pull request #1130 from Naoki-Hiraoka/PR-custom_limb

[controller_config_converter.py] enable to read custom limb name from .yaml file
start-jsk · Apr 16, 2023 · 621d9ca · 621d9ca
2 parents e4c41c9 + c4aec1b
commit 621d9ca
Showing 1 changed file with 14 additions and 23 deletions.
diff --git a/hrpsys_ros_bridge/scripts/controller_config_converter.py b/hrpsys_ros_bridge/scripts/controller_config_converter.py
@@ -32,31 +32,22 @@
 
     lst = yaml.load(open(inputfile).read())
 
-    limb_candidates = ('larm', 'rarm', 'lleg', 'rleg', 'torso', 'head') ## candidates of limb names
-
-    invalid_yaml = True
-    for l in lst.keys():
-        if l in limb_candidates:
-            invalid_yaml = False
-            break
-
-    if invalid_yaml:
-        of.close()
-        exit(0)
-
     print >> of, 'controller_configuration:'
 
-    for limb in limb_candidates:
-        #    endcoords_name = limb+'-end-coords'
-        #    endcoords_info = lst[endcoords_name]
-        if limb in lst.keys():
-            jlst = [j.keys()[0] for j in lst[limb]]
-            if len(jlst) > 0:
-                print >> of, '  - group_name: ' + limb
-                print >> of, '    controller_name: /' + limb + '_controller'
-                print >> of, '    joint_list:'
-                for j in jlst:
-                    print >> of, '      - ' + j
+    for limb in lst.keys():
+        if limb == 'sensors':
+            continue
+        if limb.endswith('-coords') or limb.endswith('-vector'):
+            continue
+        if limb == 'links' or limb == 'replace_xmls':
+            continue
+        jlst = [j.keys()[0] for j in lst[limb] if isinstance(j, dict) and isinstance(j.values()[0], str)]
+        if len(jlst) > 0:
+            print >> of, '  - group_name: ' + limb
+            print >> of, '    controller_name: /' + limb + '_controller'
+            print >> of, '    joint_list:'
+            for j in jlst:
+                print >> of, '      - ' + j
 
     of.close()
     exit(0)