test_alter_padding.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 
00011 import sensor_msgs.msg
00012 import mapping_msgs.msg
00013 from mapping_msgs.msg import CollisionObject
00014 from arm_navigation_msgs.msg import CollisionOperation, ArmNavigationErrorCodes, LinkPadding
00015 from arm_navigation_msgs.msg import Shape
00016 from geometry_msgs.msg import Pose, PointStamped
00017 from arm_navigation_msgs.srv import GetRobotState, GetStateValidity, GetRobotStateRequest, GetStateValidityRequest
00018 from tf import TransformListener
00019 
00020 default_prefix = "/environment_server"
00021 
00022 class TestAlterPadding(unittest.TestCase):
00023     
00024     def setUp(self):
00025 
00026         rospy.init_node('test_allowed_collision_near_start')
00027 
00028         self.tf = TransformListener()
00029         
00030         self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
00031       
00032         rospy.wait_for_service(default_prefix+'/get_robot_state')
00033         self.get_robot_state_server = rospy.ServiceProxy(default_prefix+'/get_robot_state', GetRobotState)
00034 
00035         rospy.wait_for_service(default_prefix+'/get_state_validity')
00036         self.get_state_validity_server = rospy.ServiceProxy(default_prefix+'/get_state_validity', GetStateValidity)
00037         
00038         obj1 = CollisionObject()
00039 
00040         obj1.header.stamp = rospy.Time.now()
00041         obj1.header.frame_id = "base_link"
00042         obj1.id = "obj1";
00043         obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00044         obj1.shapes = [Shape() for _ in range(1)]
00045         obj1.shapes[0].type = Shape.CYLINDER
00046         obj1.shapes[0].dimensions = [float() for _ in range(2)]
00047         obj1.shapes[0].dimensions[0] = .1
00048         obj1.shapes[0].dimensions[1] = 1.5
00049         obj1.poses = [Pose() for _ in range(1)]
00050         obj1.poses[0].position.x = .75
00051         obj1.poses[0].position.y = -.37
00052         obj1.poses[0].position.z = .75
00053         obj1.poses[0].orientation.x = 0
00054         obj1.poses[0].orientation.y = 0
00055         obj1.poses[0].orientation.z = 0
00056         obj1.poses[0].orientation.w = 1
00057 
00058         self.obj_pub.publish(obj1)
00059         
00060         rospy.sleep(5.)
00061 
00062     def test_alter_padding(self):
00063 
00064         req = GetRobotStateRequest()
00065 
00066         cur_state = self.get_robot_state_server.call(req)
00067 
00068         state_req = GetStateValidityRequest()
00069         state_req.robot_state = cur_state.robot_state
00070     
00071         state_req.check_collisions = True
00072 
00073         res = self.get_state_validity_server.call(state_req)
00074 
00075         #no big padding initially
00076         self.failIf(res.error_code.val != res.error_code.SUCCESS)
00077 
00078         padd = LinkPadding()
00079 
00080         padd.link_name = "r_end_effector"
00081         padd.padding = .03
00082 
00083         state_req.link_padding = []
00084         state_req.link_padding.append(padd)
00085 
00086         res = self.get_state_validity_server.call(state_req)
00087 
00088         #should be in collision
00089         self.failIf(res.error_code.val == res.error_code.SUCCESS)
00090         self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) 
00091 
00092         #should be some contacts
00093         self.failIf(len(res.contacts) == 0)
00094 
00095         #check that revert works
00096         state_req.link_padding = []
00097 
00098         res = self.get_state_validity_server.call(state_req)
00099 
00100         self.failIf(res.error_code.val != res.error_code.SUCCESS)
00101 
00102 if __name__ == '__main__':
00103     import rostest
00104     rostest.unitrun('test_alter_padding', 'test_alter_padding', TestAlterPadding)


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