$search
00001 #! /usr/bin/env python 00002 00003 PKG = 'planning_environment' 00004 00005 import roslib; roslib.load_manifest(PKG) 00006 import rospy 00007 import arm_navigation_msgs.srv 00008 import sys 00009 import unittest 00010 import math 00011 00012 import sensor_msgs.msg 00013 import arm_navigation_msgs.msg 00014 from arm_navigation_msgs.msg import CollisionObject 00015 from arm_navigation_msgs.msg import AttachedCollisionObject 00016 from arm_navigation_msgs.msg import CollisionOperation, ArmNavigationErrorCodes, AllowedContactSpecification, RobotState, DisplayTrajectory, MotionPlanRequest, JointConstraint 00017 from arm_navigation_msgs.srv import GetMotionPlanRequest, GetMotionPlanResponse, GetMotionPlan 00018 from arm_navigation_msgs.msg import Shape 00019 from geometry_msgs.msg import Pose, PointStamped 00020 from arm_navigation_msgs.srv import GetRobotState, GetStateValidity, GetRobotStateRequest, GetStateValidityRequest 00021 from tf import TransformListener 00022 00023 default_prefix = "/environment_server" 00024 00025 class TestGetStateValidity(unittest.TestCase): 00026 00027 def setUp(self): 00028 00029 rospy.init_node('test_get_base_state_validity') 00030 00031 self.tf = TransformListener() 00032 00033 self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True) 00034 self.trajectory_pub = rospy.Publisher('display_path', DisplayTrajectory) 00035 00036 rospy.wait_for_service('/ompl_planning/plan_kinematic_path') 00037 self.motion_planner = rospy.ServiceProxy('/ompl_planning/plan_kinematic_path', GetMotionPlan) 00038 00039 obj1 = CollisionObject() 00040 00041 obj1.header.stamp = rospy.Time.now() 00042 obj1.header.frame_id = 'odom_combined' 00043 obj1.id = 'pole' 00044 obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD 00045 obj1.shapes = [Shape() for _ in range(1)] 00046 obj1.shapes[0].type = Shape.BOX 00047 obj1.shapes[0].dimensions = [float() for _ in range(3)] 00048 obj1.shapes[0].dimensions[0] = .1 00049 obj1.shapes[0].dimensions[1] = .1 00050 obj1.shapes[0].dimensions[2] = 1.0 00051 obj1.poses = [Pose() for _ in range(1)] 00052 obj1.poses[0].position.x = 4.55 00053 obj1.poses[0].position.y = -.5 00054 obj1.poses[0].position.z = .75 00055 obj1.poses[0].orientation.x = 0 00056 obj1.poses[0].orientation.y = 0 00057 obj1.poses[0].orientation.z = 0 00058 obj1.poses[0].orientation.w = 1 00059 00060 self.obj_pub.publish(obj1) 00061 00062 rospy.sleep(5.) 00063 00064 def test_get_base_state_motion_plan(self): 00065 00066 req = GetMotionPlanRequest() 00067 req.motion_plan_request.group_name = "right_arm" 00068 req.motion_plan_request.num_planning_attempts = 1 00069 req.motion_plan_request.allowed_planning_time = rospy.Duration(5.0) 00070 req.motion_plan_request.planner_id = "" 00071 00072 joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']] 00073 00074 req.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))] 00075 for i in range(len(joint_names)): 00076 req.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i] 00077 req.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0 00078 req.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08 00079 req.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08 00080 00081 req.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0 00082 req.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2 00083 req.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2 00084 00085 #should be in collision 00086 req.motion_plan_request.start_state.multi_dof_joint_state.joint_names.append("base_joint") 00087 req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append("odom_combined") 00088 req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append("base_footprint") 00089 pose = Pose() 00090 pose.position.x = 4.0 00091 pose.orientation.w = 1.0 00092 req.motion_plan_request.start_state.multi_dof_joint_state.poses.append(pose) 00093 00094 res = self.motion_planner.call(req) 00095 00096 print len(res.trajectory.joint_trajectory.joint_names) 00097 00098 disp = DisplayTrajectory() 00099 disp.model_id = "right_arm" 00100 disp.trajectory.joint_trajectory = res.trajectory.joint_trajectory 00101 disp.robot_state = res.robot_state 00102 self.trajectory_pub.publish(disp) 00103 00104 00105 if __name__ == '__main__': 00106 import rostest 00107 rostest.unitrun('test_get_state_validity', 'test_get_state_validity', TestGetStateValidity, sys.argv) 00108 00109