denso_launch: denso_vs060_moveit_demo

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 PKG = 'denso_launch'
00004 
00005 import unittest
00006 import rospy
00007 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
00008 from tf.transformations import quaternion_from_euler
00009 from moveit_commander import MoveGroupCommander, conversions
00010 
00011 class TestDensoMoveit(unittest.TestCase):
00012     arm = None
00013     def setUp(self):
00014         self.arm = MoveGroupCommander("manipulator")
00015 
00016     def test_moveit(self):
00017         p = [ 0.35, -0.35, 0.4]
00018         pose = PoseStamped(header = rospy.Header(stamp = rospy.Time.now(), frame_id = '/BASE'),
00019                            pose = Pose(position = Point(*p),
00020                                        orientation = Quaternion(*quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))
00021         self.arm.set_pose_target(pose)
00022         self.assertTrue(self.arm.go() or self.arm.go())
00023 
00024 if __name__ == '__main__':
00025     import rostest
00026     rospy.init_node("denso_vs060_moveit_demo_test")
00027     rostest.rosrun(PKG, 'denso_vs060_moveit_demo_test', TestDensoMoveit)
00028 
00029