testMotionExecution.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 PKG = 'move_arm'
00004 
00005 import roslib; roslib.load_manifest(PKG)
00006 import rospy
00007 import planning_environment_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 motion_planning_msgs.msg import CollisionOperation, MotionPlanRequest
00018 from move_arm_msgs.msg import MoveArmGoal, MoveArmAction
00019 from geometric_shapes_msgs.msg import Shape
00020 from geometry_msgs.msg import Pose, PointStamped
00021 from tf import TransformListener
00022 from motion_planning_msgs.msg import JointConstraint
00023 
00024 padd_name = "ompl_planning/robot_padd"
00025 extra_buffer = .1
00026 
00027 class TestMotionExecutionBuffer(unittest.TestCase):
00028 
00029     def setUp(self):
00030 
00031         rospy.init_node('test_motion_execution_buffer')
00032         
00033         self.tf = TransformListener()        
00034 
00035         self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
00036         
00037         self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
00038         self.move_arm_action_client.wait_for_server()
00039 
00040         obj1 = CollisionObject()
00041     
00042         obj1.header.stamp = rospy.Time.now()
00043         obj1.header.frame_id = "base_link"
00044         obj1.id = "obj1";
00045         obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00046         obj1.shapes = [Shape() for _ in range(1)]
00047         obj1.shapes[0].type = Shape.BOX
00048         obj1.shapes[0].dimensions = [float() for _ in range(3)]
00049         obj1.shapes[0].dimensions[0] = .1
00050         obj1.shapes[0].dimensions[1] = .1
00051         obj1.shapes[0].dimensions[2] = 2.0
00052         obj1.poses = [Pose() for _ in range(1)]
00053         obj1.poses[0].position.x = .9
00054         obj1.poses[0].position.y = -.5
00055         obj1.poses[0].position.z = 1.0
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         self.obj_pub.publish(obj1)
00062 
00063         rospy.sleep(1.0)
00064         
00065     def tearDown(self):
00066         obj1 = CollisionObject()
00067         obj1.header.stamp = rospy.Time.now()
00068         obj1.header.frame_id = "base_link"
00069         obj1.id = "all";
00070         obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
00071 
00072         #self.obj_pub.publish(obj1)
00073         
00074         rospy.sleep(2.0)
00075 
00076     def testMotionExecutionBuffer(self):
00077         
00078         global padd_name
00079         global extra_buffer
00080         
00081         #too much trouble to read for now
00082         allow_padd = .05#rospy.get_param(padd_name)
00083         
00084         joint_names = ['right_arm_0_joint',
00085                        'right_arm_1_joint',
00086                        'right_arm_2_joint',
00087                        'right_arm_3_joint',
00088                        'right_arm_4_joint',
00089                        'right_arm_5_joint',
00090                        'right_arm_6_joint']
00091         motion_plan_request = MotionPlanRequest()
00092 
00093         motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
00094 
00095         motion_plan_request.group_name = "right_arm"
00096         motion_plan_request.num_planning_attempts = 1
00097         motion_plan_request.allowed_planning_time = rospy.Duration(5.)
00098         motion_plan_request.planner_id = ""
00099         planner_service_name = "ompl_planning/plan_kinematic_path"
00100 
00101         motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
00102         for i in range(len(joint_names)):
00103             motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
00104             motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
00105             motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
00106             motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
00107 
00108         motion_plan_request.goal_constraints.joint_constraints[0].position = 1.5
00109         motion_plan_request.goal_constraints.joint_constraints[1].position = -1.8
00110         motion_plan_request.goal_constraints.joint_constraints[3].position = .5
00111 
00112         goal = MoveArmGoal()
00113         goal.planner_service_name = "ompl_planning/plan_kinematic_path"
00114         goal.motion_plan_request = motion_plan_request
00115 
00116         self.move_arm_action_client.send_goal(goal)
00117 
00118         while True:
00119              cur_state = self.move_arm_action_client.get_state()
00120              if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
00121                 cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
00122                  break 
00123 
00124         motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
00125         motion_plan_request.goal_constraints.joint_constraints[1].position = 0.0
00126         motion_plan_request.goal_constraints.joint_constraints[3].position = 0.0
00127 
00128         goal = MoveArmGoal()
00129         goal.planner_service_name = "ompl_planning/plan_kinematic_path"
00130         goal.motion_plan_request = motion_plan_request
00131 
00132         self.move_arm_action_client.send_goal(goal)
00133 
00134         while True:
00135              cur_state = self.move_arm_action_client.get_state()
00136              if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
00137                 cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
00138                  break 
00139 
00140 if __name__ == '__main__':
00141 
00142     import rostest
00143     rostest.unitrun('test_motion_execution_buffer', 'test_motion_execution_buffer', TestMotionExecutionBuffer)
00144 
00145 
00146     
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


rosie_arm_navigation_launch
Author(s): E. Gil Jones
autogenerated on Thu May 23 2013 14:11:54