41 sys.modules[
"pyassimp"] = sys
46 from geometry_msgs.msg
import Pose, PoseStamped, Point, Quaternion
47 from tf.transformations
import quaternion_from_euler
48 from moveit_commander
import MoveGroupCommander, MoveItCommanderException, RobotCommander
49 from moveit_msgs.msg
import RobotTrajectory, PlanningScene, PlanningSceneComponents
50 from moveit_msgs.srv
import GetPlanningScene
54 from moveit_msgs.msg
import MoveGroupAction
57 _MOVEGROUP_MAIN =
'manipulator' 58 _KINEMATICSOLVER_SAFE =
'RRTConnectkConfigDefault' 62 rospy.init_node(
'test_moveit_vs060')
65 rospy.loginfo(
'wait for move_group')
66 client.wait_for_server();
67 rospy.loginfo(
'wait for move_group done')
73 self.
_movegroups = sorted([
'manipulator',
'manipulator_flange'])
82 @return: Pose() with some values populated in. 85 pose_target.orientation.x = 0.00
86 pose_target.orientation.y = 0.00
87 pose_target.orientation.z = -0.20
88 pose_target.orientation.w = 0.98
89 pose_target.position.x = 0.18
90 pose_target.position.y = 0.18
91 pose_target.position.z = 0.94
96 Run `clear_pose_targets` at the beginning. 97 @rtype: RobotTrajectory http://docs.ros.org/api/moveit_msgs/html/msg/RobotTrajectory.html 99 self._mvgroup.clear_pose_targets()
102 self._mvgroup.set_pose_target(pose_target)
103 plan = self._mvgroup.plan()
104 rospy.loginfo(
' plan: '.format(plan))
108 '''Check if the defined move groups are loaded.''' 109 self.assertEqual(self.
_movegroups, sorted(self.robot.get_group_names()))
112 '''Check if the defined joints in a move group are loaded.''' 116 '''Evaluate plan (RobotTrajectory)''' 121 self.assertNotEqual(0, plan.joint_trajectory.points)
125 Evaluate Plan and Execute works. 126 Currently no value checking is done (, which is better to be implemented) 132 ret = self._mvgroup.go()
140 Check for simple planning, originally testd in moved from denso_vs060_moveit_demo_test.py 143 p = [ 0.1, -0.35, 0.4]
144 pose = PoseStamped(header = rospy.Header(stamp = rospy.Time.now(), frame_id =
'/BASE'),
145 pose = Pose(position = Point(*p),
146 orientation = Quaternion(*quaternion_from_euler(1.57, 0, 1.57,
'sxyz'))))
148 self._mvgroup.set_pose_target(pose)
149 ret = self._mvgroup.go()
157 Check if publish_simple_scene.py is working 159 rospy.wait_for_service(
'/get_planning_scene', timeout=20);
160 get_planning_scene = rospy.ServiceProxy(
"/get_planning_scene", GetPlanningScene)
161 collision_objects = []
163 time_start = rospy.Time.now()
164 while not collision_objects
and (rospy.Time.now() - time_start).to_sec() < 10.0:
165 world = get_planning_scene(PlanningSceneComponents(components=PlanningSceneComponents.WORLD_OBJECT_NAMES)).scene.world
166 collision_objects = world.collision_objects
167 rospy.loginfo(
"collision_objects = " + str(world.collision_objects))
169 self.assertTrue(world.collision_objects != [], world)
171 if __name__ ==
'__main__':
173 rostest.rosrun(
'vs060',
'test_moveit_vs060', TestMoveit)
string _KINEMATICSOLVER_SAFE
def test_list_activejoints(self)
def test_set_pose_target(self)
def test_planandexecute(self)
def test_list_movegroups(self)
def _set_sample_pose(self)
def test_planning_scene(self)