Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 import unittest
00040
00041 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
00042 from tf.transformations import quaternion_from_euler
00043 from moveit_commander import MoveGroupCommander, MoveItCommanderException, RobotCommander
00044 from moveit_msgs.msg import RobotTrajectory, PlanningScene, PlanningSceneComponents
00045 from moveit_msgs.srv import GetPlanningScene
00046 import rospy
00047
00048
00049 class TestMoveit(unittest.TestCase):
00050 _MOVEGROUP_MAIN = 'manipulator'
00051 _KINEMATICSOLVER_SAFE = 'RRTConnectkConfigDefault'
00052
00053 @classmethod
00054 def setUpClass(self):
00055 rospy.init_node('test_moveit_vs060')
00056 self.robot = RobotCommander()
00057 self._mvgroup = MoveGroupCommander(self._MOVEGROUP_MAIN)
00058
00059 self._mvgroup.set_planner_id(self._KINEMATICSOLVER_SAFE)
00060
00061 self._movegroups = sorted(['manipulator', 'manipulator_flange'])
00062 self._joints_movegroup_main = sorted(['j1', 'j2', 'j3', 'j4', 'j5', 'flange'])
00063
00064 @classmethod
00065 def tearDownClass(self):
00066 True
00067
00068 def _set_sample_pose(self):
00069 '''
00070 @return: Pose() with some values populated in.
00071 '''
00072 pose_target = Pose()
00073 pose_target.orientation.x = 0.00
00074 pose_target.orientation.y = 0.00
00075 pose_target.orientation.z = -0.20
00076 pose_target.orientation.w = 0.98
00077 pose_target.position.x = 0.18
00078 pose_target.position.y = 0.18
00079 pose_target.position.z = 0.94
00080 return pose_target
00081
00082 def _plan(self):
00083 '''
00084 Run `clear_pose_targets` at the beginning.
00085 @rtype: RobotTrajectory http://docs.ros.org/api/moveit_msgs/html/msg/RobotTrajectory.html
00086 '''
00087 self._mvgroup.clear_pose_targets()
00088
00089 pose_target = self._set_sample_pose()
00090 self._mvgroup.set_pose_target(pose_target)
00091 plan = self._mvgroup.plan()
00092 rospy.loginfo(' plan: '.format(plan))
00093 return plan
00094
00095 def test_list_movegroups(self):
00096 '''Check if the defined move groups are loaded.'''
00097 self.assertEqual(self._movegroups, sorted(self.robot.get_group_names()))
00098
00099 def test_list_activejoints(self):
00100 '''Check if the defined joints in a move group are loaded.'''
00101 self.assertEqual(self._joints_movegroup_main, sorted(self._mvgroup.get_active_joints()))
00102
00103 def test_plan(self):
00104 '''Evaluate plan (RobotTrajectory)'''
00105 plan = self._plan()
00106
00107
00108
00109 self.assertNotEqual(0, plan.joint_trajectory.points)
00110
00111 def test_planandexecute(self):
00112 '''
00113 Evaluate Plan and Execute works.
00114 Currently no value checking is done (, which is better to be implemented)
00115 '''
00116 self._plan()
00117
00118
00119 self.assertTrue(self._mvgroup.go())
00120
00121 def test_set_pose_target(self):
00122 '''
00123 Check for simple planning, originally testd in moved from denso_vs060_moveit_demo_test.py
00124 '''
00125 self._plan()
00126 p = [ 0.1, -0.35, 0.4]
00127 pose = PoseStamped(header = rospy.Header(stamp = rospy.Time.now(), frame_id = '/BASE'),
00128 pose = Pose(position = Point(*p),
00129 orientation = Quaternion(*quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))
00130 self._mvgroup.set_pose_target(pose)
00131 self.assertTrue(self._mvgroup.go() or self._mvgroup.go())
00132
00133 def test_planning_scene(self):
00134 '''
00135 Check if publish_simple_scene.py is working
00136 '''
00137 rospy.wait_for_service('/get_planning_scene', timeout=20);
00138 get_planning_scene = rospy.ServiceProxy("/get_planning_scene", GetPlanningScene)
00139 collision_objects = []
00140
00141 time_start = rospy.Time.now()
00142 while not collision_objects and (rospy.Time.now() - time_start).to_sec() < 10.0:
00143 world = get_planning_scene(PlanningSceneComponents(components=PlanningSceneComponents.WORLD_OBJECT_NAMES)).scene.world
00144 collision_objects = world.collision_objects
00145 rospy.loginfo("collision_objects = " + str(world.collision_objects))
00146 rospy.sleep(1)
00147 self.assertTrue(world.collision_objects != [], world)
00148
00149 if __name__ == '__main__':
00150 import rostest
00151 rostest.rosrun('vs060', 'test_moveit_vs060', TestMoveit)