report_angles_node.py 文件源码

def execute():

    # define publisher and its topic 
    pub = rospy.Publisher('read_angles',Int32,queue_size = 10)
    rospy.init_node('report_angles_node',anonymous = True)
    rate = rospy.Rate(10)

    # report once       
    if len(sys.argv) == 1:
        pub.publish(1)
        time.sleep(0.15)
        print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(rospy.get_param('servo_1'),rospy.get_param('servo_2'),rospy.get_param('servo_3'),rospy.get_param('servo_4'))

    # report multiple time
    elif len(sys.argv) == 2:
        for i in range(0,int(sys.argv[1])):
            pub.publish(1)
            time.sleep(0.15)
            print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(rospy.get_param('servo_1'),rospy.get_param('servo_2'),rospy.get_param('servo_3'),rospy.get_param('servo_4'))

    elif len(sys.argv) == 3:
        for i in range(0,int(sys.argv[1])):
            pub.publish(1)
            time.sleep(0.15)
            print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(rospy.get_param('servo_1'),rospy.get_param('servo_2'),rospy.get_param('servo_3'),rospy.get_param('servo_4'))
            time.sleep(float(sys.argv[2])-0.15)

    else:
        raiseError()

    rate.sleep()

# main function