$search
00001 #! /usr/bin/env python 00002 00003 import roslib; roslib.load_manifest('pr2_arm_navigation_actions') 00004 import rospy 00005 import arm_navigation_msgs.srv 00006 import sys 00007 00008 00009 allowed_collision_service_name = "get_current_allowed_collision_matrix" 00010 default_prefix = "/environment_server_right_arm" 00011 00012 if __name__ == '__main__': 00013 try: 00014 # Initializes a rospy node so that the SimpleActionClient can 00015 # publish and subscribe over ROS. 00016 rospy.init_node('print_current_allowed_collision_matrix') 00017 00018 prefix = default_prefix 00019 00020 if len(sys.argv) > 1: 00021 prefix = sys.argv[1] 00022 00023 full_name = prefix+'/'+allowed_collision_service_name 00024 00025 rospy.wait_for_service(full_name) 00026 00027 get_allow_service = rospy.ServiceProxy(full_name,arm_navigation_msgs.srv.GetAllowedCollisionMatrix) 00028 00029 get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest() 00030 00031 get_res = get_allow_service(get_mat) 00032 00033 if not get_res: 00034 print "something wrong with get current allowed collision" 00035 else: 00036 print "get current allowed collision successful" 00037 00038 num = len(get_res.matrix.link_names) 00039 00040 print '{0:41}'.format(''), 00041 for x in range(0,num): 00042 print '({0:2})'.format(x), 00043 print '' 00044 00045 for x in range(0,num): 00046 print '({0:2}) {1:35}'.format(x,get_res.matrix.link_names[x]), 00047 for y in range(0, num): 00048 print '{0:4}'.format(get_res.matrix.entries[x].enabled[y]), 00049 print '' 00050 00051 except rospy.ROSInterruptException: 00052 print "program interrupted before completion"