Go to the documentation of this file.00001
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
00015
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"