print_current_allowed_collision_matrix.py

Go to the documentation of this file.

#! /usr/bin/env python

import roslib; roslib.load_manifest('pr2_arm_navigation_actions')
import rospy
import arm_navigation_msgs.srv
import sys


allowed_collision_service_name = "get_current_allowed_collision_matrix"
default_prefix = "/environment_server_right_arm"

if __name__ == '__main__':
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.
        rospy.init_node('print_current_allowed_collision_matrix')

        prefix = default_prefix

        if len(sys.argv) > 1:
            prefix = sys.argv[1]
            
        full_name = prefix+'/'+allowed_collision_service_name

        rospy.wait_for_service(full_name)

        get_allow_service = rospy.ServiceProxy(full_name,arm_navigation_msgs.srv.GetAllowedCollisionMatrix)

        get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
        
        get_res = get_allow_service(get_mat)
        
        if not get_res:
            print "something wrong with get current allowed collision"
        else:
            print "get current allowed collision successful"
        
            num = len(get_res.matrix.link_names)

            print '{0:41}'.format(''),
            for x in range(0,num):
                print '({0:2})'.format(x),
            print ''

            for x in range(0,num):
                print '({0:2}) {1:35}'.format(x,get_res.matrix.link_names[x]),
                for y in range(0, num):
                    print '{0:4}'.format(get_res.matrix.entries[x].enabled[y]),
                print ''
                                           
    except rospy.ROSInterruptException:
        print "program interrupted before completion"