pick_and_place.py 文件源码

def __init__(self):
        self.joint_status = UInt16MultiArray()
        self.gripper_status = Float32()

        self.is_exit = False
        self.run_real = False

        # init moveit commander
        moveit_commander.roscpp_initialize(sys.argv)
        # specify move group
        self.arm_group = moveit_commander.MoveGroupCommander("arm")
        self.gripper_group = moveit_commander.MoveGroupCommander("gripper")
        # init publisher
        self.arm_pub = rospy.Publisher('servo_write', UInt16MultiArray, queue_size=10)
        self.gripper_pub = rospy.Publisher('gripper_write', Float32, queue_size=10)
        # init ros node
        rospy.init_node('real_servo_driver', anonymous=True)
        # init robot and scene
        self.robot = moveit_commander.RobotCommander()
        self.scene = moveit_commander.PlanningSceneInterface()

        # set ros publisher rate, 10hz = 10 seconds for a circle
        self.rate = rospy.Rate(50)

        print "============ Waiting for RVIZ..."
        rospy.sleep(2)
        # move grasper to init position
        self.init_position()
        self.init_gripper()