test_joint_state_check.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; 
00004 #roslib.load_manifest('planning_environment')
00005 roslib.load_manifest('slider_gui')
00006 import rospy
00007 import arm_navigation_msgs.srv
00008 from arm_navigation_msgs.srv import GetStateValidity,GetStateValidityRequest,SetPlanningSceneDiff,SetPlanningSceneDiffRequest
00009 from pr2_controllers_msgs.msg import JointTrajectoryControllerState
00010 from pr2_controllers_msgs.msg import JointControllerState
00011 from sensor_msgs.msg import JointState 
00012 
00013 def currentJointState():
00014     '''
00015     Utility to find current state of all joints.
00016     Returns /joint_states message instance
00017     '''
00018     try:
00019         # Args: Topic, topic_type, timeout=None:
00020         
00021         jointState = rospy.wait_for_message("/joint_states",
00022                                             JointState, 
00023                                             5); #sec
00024     except rospy.ROSException:
00025         # Timeout:
00026         return None;
00027     except rospy.ROSInterruptException:
00028         # Shutdown interrupt waiting:
00029         return None;
00030     return jointState
00031   
00032   
00033 # ------------------------------------------- Initialization ---------------------------------  
00034 
00035 rospy.init_node('test_collision_check')
00036 
00037 # One-time initialization of the environment server:
00038 
00039 SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff"
00040 rospy.wait_for_service(SET_PLANNING_SCENE_DIFF_NAME);
00041 set_planning_scene_server = rospy.ServiceProxy(SET_PLANNING_SCENE_DIFF_NAME, SetPlanningSceneDiff)
00042 # Just send an empty message: 
00043 res = set_planning_scene_server.call(SetPlanningSceneDiffRequest())
00044 
00045 # One-time initialization of the state validity server: 
00046 rospy.wait_for_service('/planning_scene_validity_server/get_state_validity');
00047 state_validity_server = rospy.ServiceProxy('/planning_scene_validity_server/get_state_validity', GetStateValidity)
00048 
00049 # Set up a request message for state checks:
00050 state_req = GetStateValidityRequest()
00051 state_req.check_collisions = True
00052 state_req.robot_state.joint_state.header.stamp = rospy.Time.now()
00053 
00054 # -------------------------------------------- Usage ----------------------------------------
00055 
00056 # Get the current joint state, which is presumably collision free:
00057 currState = currentJointState();
00058 print str(currState)
00059 
00060 # Set the joint name and joint value arrays in the state check request message
00061 # just for the right shoulder joints. The joint service uses the current joint
00062 # states for any unfilled joint fields in the request message:
00063 
00064 for name in currState.name:
00065     state_req.robot_state.joint_state.name.append(name)
00066     state_req.robot_state.joint_state.position.append(currState.position[currState.name.index(name)])
00067 
00068 #state_req.robot_state.joint_state.name = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint']
00069 #state_req.robot_state.joint_state.position = [currState.position[currState.name.index('r_shoulder_pan_joint')],
00070 #                                              currState.position[currState.name.index('r_shoulder_lift_joint')]]
00071 # Ask for the joint state check:
00072 print str(state_req)
00073 res = state_validity_server.call(state_req)
00074 # Assuming the current state of the right shoulder is OK,
00075 # we should get an OK back:
00076 if res.error_code.val != res.error_code.SUCCESS:
00077     print "False positive: Incorrectly reports collision when collision-free: " + str(res.error_code.val)
00078 else:
00079     print "Correctly reports collision-free scenario."
00080 
00081 # Check state when all right-arm joints are at zero:
00082 state_req.robot_state.joint_state.name = ['r_shoulder_pan_joint', 'l_shoulder_pan_joint']
00083 
00084 state_req.robot_state.joint_state.position = [-1.0, 1.0]
00085 #print str(state_req)
00086 
00087 res = state_validity_server.call(state_req)
00088 if (res.error_code.val == res.error_code.SUCCESS):
00089   print "False negative: Reports collision-free with collision: " + str(res.error_code.val) 
00090 elif (res.error_code.val < 1):
00091   print "Properly reports collision."
00092 
00093 print "Done testing joint state check."
00094 
00095 
00096 


slider_gui
Author(s): Dirk Thomas
autogenerated on Wed Aug 26 2015 15:37:50