Go to the documentation of this file.00001
00002
00003 import rospy
00004 import tf
00005 import geometry_msgs.msg
00006 import sys
00007
00008 from moveit_commander import MoveGroupCommander
00009 from tf.transformations import *
00010 import math
00011
00012 def kbhit():
00013 import select
00014 return sys.stdin in select.select([sys.stdin], [], [], 0)[0]
00015
00016 if __name__ == '__main__':
00017 rospy.init_node('ar_pose_commander', anonymous=True)
00018 group = MoveGroupCommander("right_arm")
00019
00020 base_frame_id = '/WAIST'
00021 ar_marker_id = '/ar_marker_4'
00022
00023 pub = rospy.Publisher('target_pose', geometry_msgs.msg.PoseStamped)
00024 listener = tf.TransformListener()
00025
00026 rate = rospy.Rate(10.0)
00027 pose_st_target = None
00028 while not rospy.is_shutdown() and not kbhit():
00029 try:
00030 now = rospy.Time(0)
00031 (trans,quat) = listener.lookupTransform(base_frame_id, ar_marker_id, now)
00032 quat = quaternion_multiply(quat, quaternion_about_axis(math.pi/2, (1,0,0)))
00033 quat = quaternion_multiply(quat, quaternion_about_axis(math.pi/2, (0,0,1)))
00034 pose_st_target = geometry_msgs.msg.PoseStamped()
00035 pose_st_target.pose.position.x = trans[0]
00036 pose_st_target.pose.position.y = trans[1]
00037 pose_st_target.pose.position.z = trans[2]
00038 pose_st_target.pose.orientation.x = quat[0]
00039 pose_st_target.pose.orientation.y = quat[1]
00040 pose_st_target.pose.orientation.z = quat[2]
00041 pose_st_target.pose.orientation.w = quat[3]
00042 pose_st_target.header.frame_id = base_frame_id
00043 pose_st_target.header.stamp = now
00044 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
00045 rospy.logwarn(e)
00046
00047 if pose_st_target:
00048 pub.publish(pose_st_target)
00049 rospy.loginfo(trans)
00050
00051 rate.sleep()
00052
00053 if raw_input() == 'q':
00054 sys.exit(1)
00055
00056
00057 pose_st_target.pose.position.z += 0.3
00058 rospy.loginfo("set target to {}".format(pose_st_target.pose))
00059 group.set_pose_target(pose_st_target.pose)
00060 plan = group.plan()
00061 rospy.loginfo("plan is {}".format(plan))
00062 ret = group.go()
00063 rospy.loginfo("executed ... {}".format(ret))
00064