#!/usr/bin/env python ''' # File Name : report_anlges_node.py # Author : Joey Song # Version : V1.0 # Date : 6 Jan, 2016 # Modified Date : 6 Jan, 2016 # Description : This documents is for uarm ROS Library and ROS package # Copyright(C) 2016 uArm Team. All right reserved. ''' # All libraries needed to import # Import system library import rospy import sys import time from std_msgs.msg import Int32 # raise error def raiseError(): print 'ERROR: Input Incorrect' print 'ERROR: 3 options for print angles' print 'ERROR: 1: Input nothing after report_anlges_node.py (example: rosrun uarm report_angles_node.py)' print 'ERROR: 2: Input how many times to print (example: rosrun uarm report_angles_node.py 12)' print 'ERROR: 3: Input times and time_sec between each times (example: rosrun uarm report_angles_node.py 12 2)' # main exection function 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 if __name__ == '__main__': try: execute() pass except: print '==========================================' print 'ERROR: Exectuion error. Input numbers only' raiseError() pass