Merge pull request #82 from Hiroaki-Masuzawa/pr_fix_gen_settings

設定ファイル生成スクリプトの整理

Author: YoheiKakiuchi

python/generate_cnoid.py

@@ -1,38 +1,29 @@
 #!/bin/python3
 import argparse
-import numpy
-import os
 import sys
 
-from distutils.util import strtobool
-
-try:
-    import cnoid.Body
-    import cnoid.Util
-except ImportError:
-    import sys
-    import shutil
-    choreonoid_bin_path = shutil.which('choreonoid')
-    if choreonoid_bin_path is None:
-        print('Error: choreonoid is not found.', file=sys.stderr)
-        sys.exit(1)
-    choreonoid_bin_dir_path = os.path.dirname(choreonoid_bin_path)
-    choreonoid_share_path = os.path.join(choreonoid_bin_dir_path, '../share')
-    chorenoid_ver = [dirname[dirname.find('choreonoid-')+len('choreonoid-'):] for dirname in os.listdir(choreonoid_share_path) if dirname.find('choreonoid-') != -1]
-    if len(chorenoid_ver) > 0:
-        chorenoid_ver = chorenoid_ver[0]
-    else :
-        chorenoid_ver = None
-    choreonoid_python_path = os.path.join(choreonoid_bin_dir_path, '../lib/choreonoid-{}/python'.format(chorenoid_ver))
-    print(choreonoid_python_path)
-    if choreonoid_python_path is None or not os.path.exists(choreonoid_python_path):
-        print('Error: choreonoid_python_path not found.', file=sys.stderr)
-        sys.exit(1)
-    sys.path.append(choreonoid_python_path)
-    import cnoid.Body
-    import cnoid.Util
+from irsl_choreonoid.robot_util import RobotModelWrapped as RobotModel
 
+def print_config(joint_names, output=None):
+    """
+    Args:
+        output (optional) : output file-stream. If None, sys.stdout is used
+    """
+    if output is None:
+        output = sys.stdout
+    text = f"""\
+# This file is configuration for irsl_dynamixel_hardware_shm
 
+dynamixel_hardware_shm:
+  port_name: /dev/ttyUSB0
+  baud_rate: 1000000
+  joint:
+"""
+    print(text, file=output)
+    for jointname in joint_names:
+        print("    #{}:".format(jointname), file=output)
+        print("    - { ID: XX, DynamixelSettings: { Return_Delay_Time: 0, Operating_Mode: 3 } }", file=output)
+        
 
 if __name__=='__main__':
     parser = argparse.ArgumentParser(
@@ -44,36 +35,7 @@
 
     args = parser.parse_args()
     fname = args.bodyfile
-    if not os.path.isfile(str(fname)):
-        print("File is not exist.", file=sys.stderr)
-        print("Please check file : {}".format(fname), file=sys.stderr)
-        exit(1)
-    rbody = cnoid.Body.BodyLoader().load(str(fname))
-    if rbody is None:
-        print("File is broken.", file=sys.stderr)
-        print("Please check file : {}".format(fname), file=sys.stderr)
-        exit(1)
-    
-    rbody.updateLinkTree()
-    rbody.initializePosition()
-    rbody.calcForwardKinematics()
+    robot = RobotModel.loadModel(fname)
+    joint_names = robot.jointNames
 
-    num_joint = rbody.getNumJoints()
-    joint_list = []
-    for idx in range(num_joint):
-        joint = rbody.getJoint(idx)
-        joint_list.append(joint)
-    print("# This file is configlation for irsl_dynamixel_hardware_shm")
-    print("")
-    print("_default_joint: &default_joint")
-    print("  DynamixelSettings:")
-    print("    { Return_Delay_Time: 0, Operating_Mode: 3 }")
-    print("dynamixel_hardware_shm:")
-    print("  port_name: /dev/ttyUSB0")
-    print("  baud_rate: 1000000")
-    print("  joint:")
-
-    for jointname in [j.jointName for j in joint_list]:
-        print("    #{}:".format(jointname))
-        print("    - { ID: XX, <<: *default_joint}")
-        
+    print_config(joint_names)

python/generate_controller_config.py

@@ -1,38 +1,18 @@
 #!/bin/python3
 import argparse
-import numpy
-import os
 import sys
 
-from distutils.util import strtobool
-
-try:
-    import cnoid.Body
-    import cnoid.Util
-except ImportError:
-    import sys
-    import shutil
-    choreonoid_bin_path = shutil.which('choreonoid')
-    if choreonoid_bin_path is None:
-        print('Error: choreonoid is not found.', file=sys.stderr)
-        sys.exit(1)
-    choreonoid_bin_dir_path = os.path.dirname(choreonoid_bin_path)
-    choreonoid_share_path = os.path.join(choreonoid_bin_dir_path, '../share')
-    chorenoid_ver = [dirname[dirname.find('choreonoid-')+len('choreonoid-'):] for dirname in os.listdir(choreonoid_share_path) if dirname.find('choreonoid-') != -1]
-    if len(chorenoid_ver) > 0:
-        chorenoid_ver = chorenoid_ver[0]
-    else :
-        chorenoid_ver = None
-    choreonoid_python_path = os.path.join(choreonoid_bin_dir_path, '../lib/choreonoid-{}/python'.format(chorenoid_ver))
-    print(choreonoid_python_path)
-    if choreonoid_python_path is None or not os.path.exists(choreonoid_python_path):
-        print('Error: choreonoid_python_path not found.', file=sys.stderr)
-        sys.exit(1)
-    sys.path.append(choreonoid_python_path)
-    import cnoid.Body
-    import cnoid.Util
-
+from irsl_choreonoid.robot_util import RobotModelWrapped as RobotModel
 
+def print_config(joint_names, output=None):
+    """
+    Args:
+        output (optional) : output file-stream. If None, sys.stdout is used
+    """
+    if output is None:
+        output = sys.stdout
+    for jointname in joint_names:
+        print(f'- "{jointname}"', file=output)
 
 if __name__=='__main__':
     parser = argparse.ArgumentParser(
@@ -44,25 +24,7 @@
 
     args = parser.parse_args()
     fname = args.bodyfile
-    if not os.path.isfile(str(fname)):
-        print("File is not exist.", file=sys.stderr)
-        print("Please check file : {}".format(fname), file=sys.stderr)
-        exit(1)
-    rbody = cnoid.Body.BodyLoader().load(str(fname))
-    if rbody is None:
-        print("File is broken.", file=sys.stderr)
-        print("Please check file : {}".format(fname), file=sys.stderr)
-        exit(1)
-    
-    rbody.updateLinkTree()
-    rbody.initializePosition()
-    rbody.calcForwardKinematics()
 
-    num_joint = rbody.getNumJoints()
-    joint_list = []
-    for idx in range(num_joint):
-        joint = rbody.getJoint(idx)
-        joint_list.append(joint)
-    jointnames = [j.jointName for j in joint_list]
-    for jointname in jointnames:
-        print('- "{}"'.format(jointname))
+    robot = RobotModel.loadModel(fname)
+    joint_names = robot.jointNames
+    print_config(joint_names)