irex_demo.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
00003 from moveit_commander import MoveGroupCommander, conversions
00004 from tf.transformations import quaternion_from_euler
00005 import rospy
00006 import os
00007 from subprocess import check_call
00008 import roslib
00009 roslib.load_manifest("denso_pendant_publisher")
00010 roslib.load_manifest("actionlib_msgs")
00011 import denso_pendant_publisher.msg
00012 import std_msgs.msg
00013 import actionlib_msgs.msg
00014 rospy.init_node("test_vs060_moveit")
00015 
00016 g_runnable = False
00017 
00018 g_prev_status = None
00019 
00020 def pendantCB(msg):
00021     global g_runnable, g_prev_status
00022     if g_prev_status:
00023         if (not g_prev_status.button_cancel and msg.button_cancel) or (not g_prev_status.button_stop and msg.button_stop): # canceled or stopped
00024             g_runnable = False
00025             # here we should send cancel
00026             cancel = actionlib_msgs.msg.GoalID()
00027             cancel.id = ""
00028             cancel_pub.publish(cancel)
00029             rospy.loginfo("hello")
00030             running_pub.publish(std_msgs.msg.Bool(g_runnable))
00031         elif not g_prev_status.button_ok and msg.button_ok: # oked
00032             g_runnable = True
00033             running_pub.publish(std_msgs.msg.Bool(g_runnable))
00034     g_prev_status = msg
00035 
00036 sub = rospy.Subscriber("/denso_pendant_publisher/status", denso_pendant_publisher.msg.PendantStatus, pendantCB)
00037 arm = MoveGroupCommander("manipulator")
00038 running_pub = rospy.Publisher("/irex_demo_running", std_msgs.msg.Bool);
00039 #cancel_pub = rospy.Publisher("/arm_controller/follow_joint_trajectory/cancel", actionlib_msgs.msg.GoalID);
00040 cancel_pub = rospy.Publisher("/move_group/cancel", actionlib_msgs.msg.GoalID);
00041 
00042 
00043 SCENE_FILE = os.path.join(os.path.dirname(__file__), "..", "model", "irex_model.scene")
00044 LOAD_SCENE_PROG = os.path.join(os.path.dirname(__file__), "..", "bin", "publish_scene_from_text")
00045 
00046 def demo() :
00047     # load scene
00048     global g_runnable
00049     running_pub.publish(std_msgs.msg.Bool(g_runnable))
00050     check_call([LOAD_SCENE_PROG, SCENE_FILE])
00051     for p in [[ 0.35, -0.35, 0.4],
00052               [ 0.6,  0.0, 0.4],
00053               [ 0.35,  0.35, 0.4],
00054               [ 0.6,  0.0, 0.2],
00055               [ 0.4,  0.0, 0.8]]:
00056         running_pub.publish(std_msgs.msg.Bool(g_runnable))
00057         if g_runnable:
00058             print "set_pose_target(", p, ")"
00059             pose = PoseStamped(header = rospy.Header(stamp = rospy.Time.now(), frame_id = '/BASE'),
00060                                pose = Pose(position = Point(*p),
00061                                            orientation = Quaternion(*quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))
00062 
00063             arm.set_pose_target(pose)
00064             arm.go() or arm.go() or rospy.logerr("arm.go fails")
00065             rospy.sleep(1)
00066             if rospy.is_shutdown():
00067                 return
00068 
00069 if __name__ == "__main__":
00070     while not rospy.is_shutdown():
00071         demo()
00072 


vs060
Author(s): Ryohei Ueda
autogenerated on Wed Aug 26 2015 11:21:15