test_get_base_state_validity.py
Go to the documentation of this file.
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 mapping_msgs.msg
00014 from mapping_msgs.msg import CollisionObject
00015 from mapping_msgs.msg import AttachedCollisionObject
00016 from arm_navigation_msgs.msg import CollisionOperation, ArmNavigationErrorCodes, AllowedContactSpecification, RobotState
00017 from arm_navigation_msgs.msg import Shape
00018 from geometry_msgs.msg import Pose, PointStamped
00019 from arm_navigation_msgs.srv import GetRobotState, GetStateValidity, GetRobotStateRequest, GetStateValidityRequest
00020 from tf import TransformListener
00021 
00022 default_prefix = "/environment_server"
00023 
00024 class TestGetStateValidity(unittest.TestCase):
00025     
00026     def setUp(self):
00027 
00028         rospy.init_node('test_get_base_state_validity')
00029 
00030         self.tf = TransformListener()
00031         
00032         self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
00033 
00034         rospy.wait_for_service(default_prefix+'/get_robot_state')
00035         self.get_robot_state_server = rospy.ServiceProxy(default_prefix+'/get_robot_state', GetRobotState)
00036         
00037         rospy.wait_for_service(default_prefix+'/get_state_validity')
00038         self.get_state_validity_server = rospy.ServiceProxy(default_prefix+'/get_state_validity', GetStateValidity)
00039         
00040         obj1 = CollisionObject()
00041 
00042         obj1.header.stamp = rospy.Time.now()
00043         obj1.header.frame_id = 'odom_combined'
00044         obj1.id = 'table'
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.0
00050         obj1.shapes[0].dimensions[1] = 1.0
00051         obj1.shapes[0].dimensions[2] = .1
00052         obj1.poses = [Pose() for _ in range(1)]
00053         obj1.poses[0].position.x = 4.25
00054         obj1.poses[0].position.y = 0
00055         obj1.poses[0].position.z = .75
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(5.)
00064 
00065     def test_get_base_state_validity(self):
00066 
00067         state_req = GetStateValidityRequest()
00068         state_req.check_collisions = True
00069 
00070         #empty state should be ok
00071         res = self.get_state_validity_server.call(state_req)
00072         self.failIf(res.error_code.val != res.error_code.SUCCESS)
00073 
00074         #should be in collision
00075         state_req.robot_state.multi_dof_joint_state.joint_names.append("base_joint")
00076         state_req.robot_state.multi_dof_joint_state.frame_ids.append("odom_combined")
00077         state_req.robot_state.multi_dof_joint_state.child_frame_ids.append("base_footprint")
00078         pose = Pose()
00079         pose.position.x = 4.0
00080         pose.orientation.w = 1.0
00081         state_req.robot_state.multi_dof_joint_state.poses.append(pose)
00082         res = self.get_state_validity_server.call(state_req)
00083         self.failIf(res.error_code.val == res.error_code.SUCCESS)
00084         self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) 
00085         
00086         #shouldn't be in collision as it doesn't have right parent_frame
00087         state_req.robot_state.multi_dof_joint_state.frame_ids[0] = ""
00088         res = self.get_state_validity_server.call(state_req)
00089         self.failIf(res.error_code.val != res.error_code.SUCCESS)
00090 
00091         #should be in collision now
00092         state_req.robot_state.joint_state.name.append("floating_trans_x");
00093         state_req.robot_state.joint_state.position.append(4.0);
00094         res = self.get_state_validity_server.call(state_req)
00095         self.failIf(res.error_code.val == res.error_code.SUCCESS)
00096         self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) 
00097 
00098 
00099         #should be in collision now
00100         #moving back, but should still be in collision
00101         state_req.robot_state.joint_state.position[0] = 3.3;
00102         res = self.get_state_validity_server.call(state_req)
00103         self.failIf(res.error_code.val == res.error_code.SUCCESS)
00104         self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) 
00105 
00106         #but at a different theta should be ok
00107         state_req.robot_state.joint_state.name.append("floating_rot_z");
00108         state_req.robot_state.joint_state.position.append(.7071);
00109         state_req.robot_state.joint_state.name.append("floating_rot_w");
00110         state_req.robot_state.joint_state.position.append(.7071);
00111         res = self.get_state_validity_server.call(state_req)
00112         self.failIf(res.error_code.val != res.error_code.SUCCESS)
00113 
00114 
00115 if __name__ == '__main__':
00116     import rostest
00117     rostest.unitrun('test_get_state_validity', 'test_get_state_validity', TestGetStateValidity, sys.argv)
00118 
00119  


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43