CollisionChecker.py
Go to the documentation of this file.
00001 import roslib
00002 roslib.load_manifest('slider_gui')
00003 import rospy
00004 
00005 from arm_navigation_msgs.srv import GetStateValidity, GetStateValidityRequest, SetPlanningSceneDiff, SetPlanningSceneDiffRequest
00006 from pr2_controllers_msgs.msg import JointControllerState, JointTrajectoryControllerState
00007 from sensor_msgs.msg import JointState 
00008 
00009 class CollisionChecker:
00010 
00011     def __init__(self):
00012         self._latest_joint_state = None
00013 
00014         # initialization of the environment server
00015         SET_PLANNING_SCENE_DIFF_NAME = '/environment_server/set_planning_scene_diff'
00016         rospy.wait_for_service(SET_PLANNING_SCENE_DIFF_NAME)
00017         set_planning_scene_server = rospy.ServiceProxy(SET_PLANNING_SCENE_DIFF_NAME, SetPlanningSceneDiff)
00018         # just send an empty message
00019         res = set_planning_scene_server.call(SetPlanningSceneDiffRequest())
00020 
00021         # initialization of the state validity server
00022         rospy.wait_for_service('/planning_scene_validity_server/get_state_validity')
00023         self._state_validity_server = rospy.ServiceProxy('/planning_scene_validity_server/get_state_validity', GetStateValidity)
00024 
00025         rospy.Subscriber('/joint_states', JointState, self._receive_joint_states)
00026 
00027     def _receive_joint_states(self, joint_state_msg):
00028         self._latest_joint_state = joint_state_msg
00029 
00030     def is_current_state_in_collision(self):
00031         state_req = self._create_state_validity_request()
00032 
00033         # ask for the joint state check
00034         res = self._state_validity_server.call(state_req)
00035         return res.error_code.val != res.error_code.SUCCESS
00036 
00037     def is_in_collision(self, joint_values):
00038         state_req = self._create_state_validity_request()
00039         # set the joint name and joint value arrays in the state check request message
00040         for name, position in joint_values.items():
00041             if name in state_req.robot_state.joint_state.name:
00042                 index = state_req.robot_state.joint_state.name.index(name)
00043                 state_req.robot_state.joint_state.position[index] = position
00044             else:
00045                 state_req.robot_state.joint_state.name.append(name)
00046                 state_req.robot_state.joint_state.position.append(position)
00047         #print str(state_req.robot_state.joint_state.name)
00048         #print str(state_req.robot_state.joint_state.position)
00049 
00050         # ask for the joint state check
00051         res = self._state_validity_server.call(state_req)
00052         return res.error_code.val != res.error_code.SUCCESS
00053 
00054     def _create_state_validity_request(self):
00055         # set up a request message for state checks
00056         state_req = GetStateValidityRequest()
00057         state_req.check_collisions = True
00058         state_req.robot_state.joint_state.header.stamp = rospy.Time.now()
00059 
00060         # get the current joint state
00061         currState = self._current_joint_state()
00062         #print str(currState)
00063         if currState is None:
00064             return None
00065 
00066         # set the joint name and joint value arrays in the state check request message
00067         for name in currState.name:
00068             state_req.robot_state.joint_state.name.append(name)
00069             state_req.robot_state.joint_state.position.append(currState.position[currState.name.index(name)])
00070 
00071         #print str(state_req)
00072         return state_req
00073 
00074     def _current_joint_state(self):
00075         return self._latest_joint_state


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