irex_demo.py

#!/usr/bin/env python                                                                                              
from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
from moveit_commander import MoveGroupCommander, conversions
from tf.transformations import quaternion_from_euler
import rospy
import os
from subprocess import check_call

rospy.init_node("test_vs060_moveit")
arm = MoveGroupCommander("manipulator")

SCENE_FILE = os.path.join(os.path.dirname(__file__), "..", "model", "irex_model.scene")
LOAD_SCENE_PROG = os.path.join(os.path.dirname(__file__), "..", "bin", "publish_scene_from_text")

def demo() :
    # load scene                                                                                                   
    check_call([LOAD_SCENE_PROG, SCENE_FILE])

    for p in [[ 0.35, -0.35, 0.4],
              [ 0.6,  0.0, 0.4],
    ,          [ 0.35,  0.35, 0.4],
              [ 0.6,  0.0, 0.2],
              [ 0.4,  0.0, 0.8]]:
        print "set_pose_target(", p, ")"
        pose = PoseStamped(header = rospy.Header(stamp = rospy.Time.now(), frame_id = '/BASE'),
                           pose = Pose(position = Point(*p),
                                       orientation = Quaternion(*quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))

        arm.set_pose_target(pose)
        arm.go() or arm.go() or rospy.logerr("arm.go fails")
        rospy.sleep(1)
        if rospy.is_shutdown():
            return

if __name__ == "__main__":
    while not rospy.is_shutdown():
        demo()