controller.py 文件源码

def printJointStates(self):
        try:
            # now = rospy.Time.now()
            # self.tf.waitForTransform(self.frame, 'r_gripper_tool_frame', now, rospy.Duration(10.0))
            self.tf.waitForTransform(self.frame, 'r_gripper_tool_frame', rospy.Time(), rospy.Duration(10.0))
            currentRightPos, currentRightOrient = self.tf.lookupTransform(self.frame, 'r_gripper_tool_frame', rospy.Time(0))

            msg = rospy.wait_for_message('/r_arm_controller/state', JointTrajectoryControllerState)
            currentRightJointPositions = msg.actual.positions
            print 'Right positions:', currentRightPos, currentRightOrient
            print 'Right joint positions:', currentRightJointPositions

            # now = rospy.Time.now()
            # self.tf.waitForTransform(self.frame, 'l_gripper_tool_frame', now, rospy.Duration(10.0))
            currentLeftPos, currentLeftOrient = self.tf.lookupTransform(self.frame, 'l_gripper_tool_frame', rospy.Time(0))

            msg = rospy.wait_for_message('/l_arm_controller/state', JointTrajectoryControllerState)
            currentLeftJointPositions = msg.actual.positions
            print 'Left positions:', currentLeftPos, currentLeftOrient
            print 'Left joint positions:', currentLeftJointPositions

            # print 'Right gripper state:', rospy.wait_for_message('/r_gripper_controller/state', JointControllerState)
        except tf.ExtrapolationException:
            print 'No transformation available! Failing to record this time step.'