Go to the documentation of this file.00001
00002
00003 PKG = 'move_arm'
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 actionlib
00011 import actionlib_msgs
00012 import math
00013
00014 import sensor_msgs.msg
00015 import mapping_msgs.msg
00016 from mapping_msgs.msg import CollisionObject
00017 from arm_navigation_msgs.msg import CollisionOperation
00018 from arm_navigation_msgs.msg import Shape
00019 from geometry_msgs.msg import Pose, PointStamped
00020 from move_arm_msgs.msg import MoveArmGoal, MoveArmAction
00021 from tf import TransformListener
00022 from arm_navigation_msgs.msg import JointConstraint
00023
00024 class TestMotionExecutionBuffer(unittest.TestCase):
00025
00026 def setUp(self):
00027
00028 self.tf = TransformListener()
00029
00030 self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
00031
00032 obj_pub = rospy.Publisher('collision_object',CollisionObject)
00033
00034 rospy.init_node('test_motion_execution_buffer')
00035
00036
00037 rospy.sleep(5.)
00038
00039 obj1 = CollisionObject()
00040
00041 obj1.header.stamp = rospy.Time.now()-rospy.Duration(.1)
00042 obj1.header.frame_id = "base_footprint"
00043 obj1.id = "obj2";
00044 obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00045 obj1.shapes = [Shape() for _ in range(4)]
00046 obj1.poses = [Pose() for _ in range(4)]
00047
00048 obj1.shapes[0].type = Shape.BOX
00049 obj1.shapes[0].dimensions = [float() for _ in range(3)]
00050 obj1.shapes[0].dimensions[0] = .5
00051 obj1.shapes[0].dimensions[1] = 1.0
00052 obj1.shapes[0].dimensions[2] = .2
00053 obj1.poses[0].position.x = .95
00054 obj1.poses[0].position.y = -.25
00055 obj1.poses[0].position.z = .62
00056 obj1.poses[0].orientation.x = 0
00057 obj1.poses[0].orientation.y = 0
00058 obj1.poses[0].orientation.z = 0
00059 obj1.poses[0].orientation.w = 1
00060
00061 obj1.shapes[1].type = Shape.BOX
00062 obj1.shapes[1].dimensions = [float() for _ in range(3)]
00063 obj1.shapes[1].dimensions[0] = .5
00064 obj1.shapes[1].dimensions[1] = 1.0
00065 obj1.shapes[1].dimensions[2] = .2
00066 obj1.poses[1].position.x = .95
00067 obj1.poses[1].position.y = -.25
00068 obj1.poses[1].position.z = .92
00069 obj1.poses[1].orientation.x = 0
00070 obj1.poses[1].orientation.y = 0
00071 obj1.poses[1].orientation.z = 0
00072 obj1.poses[1].orientation.w = 1
00073
00074 obj1.shapes[2].type = Shape.BOX
00075 obj1.shapes[2].dimensions = [float() for _ in range(3)]
00076 obj1.shapes[2].dimensions[0] = .2
00077 obj1.shapes[2].dimensions[1] = .1
00078 obj1.shapes[2].dimensions[2] = .2
00079 obj1.poses[2].position.x = .8
00080 obj1.poses[2].position.y = -.5
00081 obj1.poses[2].position.z = .78
00082 obj1.poses[2].orientation.x = 0
00083 obj1.poses[2].orientation.y = 0
00084 obj1.poses[2].orientation.z = 0
00085 obj1.poses[2].orientation.w = 1
00086
00087 obj1.shapes[3].type = Shape.BOX
00088 obj1.shapes[3].dimensions = [float() for _ in range(3)]
00089 obj1.shapes[3].dimensions[0] = .2
00090 obj1.shapes[3].dimensions[1] = .1
00091 obj1.shapes[3].dimensions[2] = .2
00092 obj1.poses[3].position.x = .8
00093 obj1.poses[3].position.y = .18
00094 obj1.poses[3].position.z = .78
00095 obj1.poses[3].orientation.x = 0
00096 obj1.poses[3].orientation.y = 0
00097 obj1.poses[3].orientation.z = 0
00098 obj1.poses[3].orientation.w = 1
00099 obj_pub.publish(obj1)
00100
00101 rospy.sleep(5.0)
00102
00103 def testMotionExecutionBuffer(self):
00104
00105 global padd_name
00106 global extra_buffer
00107
00108
00109 allow_padd = .05
00110
00111
00112 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']]
00113 goal = MoveArmGoal()
00114
00115 goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
00116
00117 goal.motion_plan_request.group_name = "right_arm"
00118 goal.motion_plan_request.num_planning_attempts = 1
00119 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
00120 goal.motion_plan_request.planner_id = ""
00121 goal.planner_service_name = "chomp_planner_longrange/plan_path"
00122 goal.disable_collision_monitoring = True;
00123
00124 r = rospy.Rate(10)
00125
00126 goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
00127 for z in range(10):
00128 for i in range(len(joint_names)):
00129 goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
00130 goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
00131 goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
00132 goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
00133
00134 if(z%2==0):
00135 goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -1.0
00136 goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
00137 goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
00138 else:
00139 goal.motion_plan_request.goal_constraints.joint_constraints[0].position = .22
00140 goal.motion_plan_request.goal_constraints.joint_constraints[1].position = .51
00141 goal.motion_plan_request.goal_constraints.joint_constraints[2].position = .05
00142 goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.41
00143 goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -.06
00144 goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -.1
00145 goal.motion_plan_request.goal_constraints.joint_constraints[6].position = 0.0
00146
00147 for x in range(3):
00148
00149 self.move_arm_action_client.send_goal(goal)
00150
00151 while True:
00152 cur_state = self.move_arm_action_client.get_state()
00153 if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
00154 cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
00155 break
00156 r.sleep()
00157
00158 end_state = self.move_arm_action_client.get_state()
00159
00160 if(end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break
00161
00162 final_state = self.move_arm_action_client.get_state()
00163
00164 self.assertEqual(final_state, actionlib_msgs.msg.GoalStatus.SUCCEEDED)
00165
00166 if __name__ == '__main__':
00167
00168 import rostest
00169 rostest.unitrun('test_motion_execution_buffer', 'test_motion_execution_buffer', TestMotionExecutionBuffer)
00170
00171
00172