Python URDF.get_root方法代码示例

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import get_root [as 别名]
class CalibrationSetupHelper:
    def __init__(self, robot_description, base_link, arm_root_link, arm_tip_link, head_root_link, head_tip_link, arm_controller, head_controller, head_camera_frame, head_camera_joint, camera_namespace):
        self.robot_description = robot_description
        self.base_link = base_link
        self.arm_root_link = arm_root_link
        self.arm_tip_link = arm_tip_link
        self.head_root_link = head_root_link
        self.head_tip_link = head_tip_link
        self.arm_controller = arm_controller
        self.head_controller = head_controller
        self.head_camera_frame = head_camera_frame
        self.head_camera_joint = head_camera_joint
        self.camera_namespace = camera_namespace

        self.robot = URDF().parse(self.robot_description)
        self.robot_name = self.robot.name.lower()

        if not self.base_link:
            self.base_link = self.robot.get_root()
        if self.robot.link_map.has_key(self.base_link) == False:
            self.error_tip_link(self.base_link, '--base-link')
        if self.robot.link_map.has_key(self.arm_root_link) == False:
            self.error_tip_link(self.arm_root_link, '--arm-root-link')
        if self.robot.link_map.has_key(self.head_root_link) == False:
            self.error_tip_link(self.head_root_link, '--head-root-link')
        if self.robot.link_map.has_key(self.arm_tip_link) == False:
            self.error_tip_link(self.arm_tip_link, '--arm-tip-link')
        if self.robot.link_map.has_key(self.head_tip_link) == False:
            self.error_tip_link(self.head_tip_link, '--head-tip-link')
        if self.robot.link_map.has_key(self.head_camera_frame) == False:
            self.error_tip_link(self.head_camera_frame, '--head-camera-frame')
        if self.robot.joint_map.has_key(self.head_camera_joint) == False:
            self.error_joint(self.head_camera_joint, '--head-camera-joint')

        all_chain = []
        for limb, base_link, end_link in [('arm', self.arm_root_link, self.arm_tip_link), ('head', self.head_root_link, self.head_tip_link)]:
            all_chain.append(self.robot.get_chain(base_link, end_link)[1:])
        for c1,c2 in itertools.product(*all_chain):
            if c1 == c2 :
                rospy.logerr('arm/head chain share same joint ({}), this will cause failure'.format(c1))
                sys.exit()

        for limb, base_link, end_link in [('arm', self.arm_root_link, self.arm_tip_link), ('head', self.head_root_link, self.head_tip_link)]:
            joint_list = filter(lambda x: self.robot.joint_map.has_key(x) and self.robot.joint_map[x].type != 'fixed', [c for c in self.robot.get_chain(base_link, end_link)])
            exec('self.{0}_joint_list = {1}'.format(limb, joint_list)) in locals()

        rospy.loginfo('using following joint for {} chain'.format('arm'))
        rospy.loginfo('... {}'.format(self.arm_joint_list))
        rospy.loginfo('using following joint for {} chain'.format('head'))
        rospy.loginfo('... {}'.format(self.head_joint_list))

        # create robot_calibration directory
        self.dirname_top = self.robot_name+'_calibration'
        self.dirname_capture = self.robot_name+'_calibration/capture_data'
        self.dirname_estimate = self.robot_name+'_calibration/estimate_params'
        self.dirname_results = self.robot_name+'_calibration/view_results'

        try:
            os.makedirs(self.dirname_top)
            os.makedirs(self.dirname_capture)
            os.makedirs(self.dirname_estimate)
            os.makedirs(self.dirname_results)
        except Exception as e:
            rospy.logfatal(e)
            #sys.exit(-1)

        # setup capture_data
        self.write_all_pipelines(args.use_kinect)       # all_pipelines.launch
        self.write_all_viewers(args.use_kinect)         # all_viewers.launch
        self.write_capture_data()                       # capture_data.launch
        self.write_capture_exec()                       # capture_exec.launch
        self.write_hardware_config()                    # hardware_config
        self.write_make_samples()                       # make_samples.py
        # setup estimate_params
        self.write_estimation_config()                  # estimation_config.launch
        self.write_calibrate_sh()                       # calibrate_<robot>.sh
        self.write_estimate_params_config(args.use_kinect)             # free_arms.yaml  free_cameras.yaml  free_cb_locations.yaml  free_torso.yaml  system.yaml
        # setup view_results
        self.write_view_results()                       # scatter_config.yaml  view_scatter.sh


    def error_tip_link(self, link, option):
        rospy.logfatal('could not find valid link name ... {}'.format(link))
        rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.links)))
        f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+')
        print >>f, self.robot_description
        f.close()
        rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name))
        sys.exit(-1)

    def error_joint(self, joint, option):
        rospy.logfatal('could not find valid joint name ... {}'.format(joint))
        rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.joints)))
        f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+')
        print >>f, self.robot_description
        f.close()
        rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name))
        sys.exit(-1)

    def write_all_pipelines(self, use_kinect):
#.........这里部分代码省略.........