#!/usr/bin/env python # -*- coding: utf-8 -*- """ Created on Mon Feb 10 12:10:34 2020 @author: Maxi @edit: PatrickW """ import roslib import rospy import sys import time import std_msgs.msg as msg class Test: def __init__(self): """ setup ROS """ rospy.init_node('Tests') self.pub = rospy.Publisher('tests', msg.String, queue_size=10) self.CmdPub = rospy.Publisher('command', msg.String, queue_size=10) self.InterruptPub = rospy.Publisher('interrupt', msg.String, queue_size=10) self.RaisedSwitchPub = rospy.Publisher('raised_switch', msg.String, queue_size=10) self.ForwardSwitchPub = rospy.Publisher('forward_switch', msg.String, queue_size=10) self.StowedSwitchPub = rospy.Publisher('stowed_switch', msg.String, queue_size=10) self.RotatePub = rospy.Publisher('rotation', msg.String, queue_size=10) self.ElevatePub = rospy.Publisher('elevation', msg.String, queue_size=10) self.ServoPub = rospy.Publisher('servo', msg.String, queue_size=10) # while not rospy.is_shutdown(): # rospy.spin() def test_elevate(self): self.pub.publish(msg.String('Test 0')) rospy.sleep(1.0) self.StowedSwitchPub.publish(msg.String('LOW')) rospy.sleep(1.0) self.CmdPub.publish(msg.String('ELEVATE')) rospy.sleep(2.0) # seconds # self.RaisedSwitchPub.publish(msg.String('HIGH')) def test_retract(self): self.pub.publish(msg.String('Test 0')) # rospy.sleep(1.0) # self.RaisedSwitchPub.publish(msg.String('HIGH')) rospy.sleep(1.0) #self.CmdPub.publish(msg.String('SERVO 68')) #rospy.sleep(5.0) self.CmdPub.publish(msg.String('STOW')) #rospy.sleep(20.0) # seconds # self.StowedSwitchPub.publish(msg.String('LOW')) def test_tilt(self, tilt_state=""): self.pub.publish(msg.String('Test 0')) rospy.sleep(1.0) string = 'TILT' + str(tilt_state) self.CmdPub.publish(msg.String(string)) #self.CmdPub.publish(msg.String('TILT')) rospy.sleep(10.0) # seconds def test_sweep(self): self.pub.publish(msg.String('Test 0')) rospy.sleep(1.0) self.CmdPub.publish('ROTATE %s' % str(180)) rospy.sleep(45.0) self.CmdPub.publish('ROTATE %s' % str(-180)) rospy.sleep(45.0) self.CmdPub.publish('CENTER') rospy.sleep(30.0) def test_center(self): self.pub.publish(msg.String('Test 0')) rospy.sleep(1.0) # self.ForwardSwitchPub.publish('HIGH') # rospy.sleep(1.0) self.CmdPub.publish('ROTATE %s' % str(10)) rospy.sleep(15.0) self.CmdPub.publish('ROTATE %s' % str(-10)) rospy.sleep(15.0) self.CmdPub.publish('CENTER') rospy.sleep(30.0) # self.CmdPub.publish('ROTATE %s' % str(-80)) # rospy.sleep(10.0) # self.CmdPub.publish('CENTER') # rospy.sleep(20.0) # rospy.sleep(1.0) # self.ForwardSwitchPub.publish('HIGH') def test_rotate(self, angle): self.pub.publish(msg.String('Test 0')) rospy.sleep(1.0) self.CmdPub.publish('ROTATE %s' % str(angle)) # speed = 8 # grad pro sekunde # rospy.sleep(angle/speed) rospy.sleep(10) def test_elev_switch(self): self.pub.publish(msg.String('Test 0')) rospy.sleep(1.0) self.CmdPub.publish(msg.String('ELEVATE')) rospy.sleep(5.0) self.pub.publish(msg.String('Test 0')) rospy.sleep(1.0) self.CmdPub.publish(msg.String('STOW')) def test_servo(self, angle): self.pub.publish(msg.String('Test 0')) rospy.sleep(0.2) # self.CmdPub.publish(msg.String('SERVO 0')) # rospy.sleep(5.0) self.CmdPub.publish(msg.String('SERVO ' + str(angle))) # rospy.sleep(5.0) # self.CmdPub.publish(msg.String('SERVO 20')) # rospy.sleep(5.0) # self.CmdPub.publish(msg.String('SERVO 30')) # rospy.sleep(5.0) # self.CmdPub.publish(msg.String('SERVO 40')) # rospy.sleep(5.0) # self.CmdPub.publish(msg.String('SERVO 50')) # rospy.sleep(5.0) # self.CmdPub.publish(msg.String('SERVO 55')) # rospy.sleep(5.0) # self.CmdPub.publish(msg.String('SERVO 60')) # rospy.sleep(5.0) # self.CmdPub.publish(msg.String('SERVO 62')) # rospy.sleep(5.0) # self.CmdPub.publish(msg.String('SERVO 64')) # rospy.sleep(5.0) # self.CmdPub.publish(msg.String('SERVO 66')) # rospy.sleep(5.0) # self.CmdPub.publish(msg.String('SERVO 68')) # rospy.sleep(5.0) def test1(self): for t in range(40): my_msg = msg.String() my_msg.data = str(t) rospy.loginfo('sent %s' % my_msg.data) pub.publish(my_msg) if rospy.is_shutdown(): return rospy.Rate(0.8).sleep() def print_menu(): print("--- STEVE Console Interface ---") print 5*"-", "Choose Command", 5*"-" print "1. Raise from Storage" # Raise Mast (test_elevate) print "2. Initiate Azimuth" # Initiate Az (find forward switch) (test_sweep) print "3. Rotate Azimuth" print "4. Rotate Azimuth to Forward Position (180 deg)" print "5. Rotate Pitch Servo -SERVO NOT CONNECTED-" print "6. Move to Tilted Position" # Rotate Elevation to tilted print "7. Move to storage Azimuth (0 deg)" # Move to storage Az (test_center) print "8. Retract (Caution: Center Az first)" # test_retract return if __name__ == '__main__': t = Test() rospy.sleep(1.0) rate=rospy.Rate(10) print_limit = 8 number_of_commands = 0 print_menu() while not rospy.is_shutdown(): if number_of_commands>=print_limit: print_menu() number_of_commands = 0 try: choice = input("Command [1-8]: ") choice = int(choice) if choice == 1: print("Raising Mast...") t.test_elevate() print("Finished :)") elif choice == 2: choice = raw_input("Caution: Is Mast raised? [y/n] ") choice = str(choice) if choice == "y": print("Initiating: Finding Switch...") t.test_sweep() print("Finished :)") else: print("Cancelling...") elif choice == 3: set_angle_az = int(input("Enter set value [-359,359] -> [cw>0; ccw<0]: ")) if set_angle_az >-359 and set_angle_az < 359: print("Moving Base...") t.test_rotate(set_angle_az) print("Finished :)") else: print("Azimuth Set Value out of bounds!") elif choice == 4: choice = raw_input("Rotation cw/ccw? [cw: 1; ccw: 2]: ") choice = int(choice) if choice == 1: print("Moving to Forward Direction (cw)...") set_angle_az = 180 t.test_rotate(set_angle_az) print("Finished :)") elif choice == 2: print("Moving to Forward Direction (cw)...") set_angle_az = -180 t.test_rotate(set_angle_az) print("Finished :)") else: print("Cancelling...") elif choice== 5: set_angle_servo = input("Enter set value [0,68]: ") if set_angle_servo>68 or set_angle_servo<0: print("\n Error: Value out of bounds \n") else: print("Moving Servo...") t.test_servo(set_angle_servo) print("Finished :)") elif choice == 6: time = [0,18] tilt_choice = input("Choose tilt stage " + str(time) + ": ") tilt_choice = int(tilt_choice) if tilt_choice>=time[0] and tilt_choice<=time[1]: choice = raw_input("Caution: Is Mast raised? [y/n] ") choice = str(choice) if choice == "y": print("Moving to Tilted Elevation...") goal = "_" + str(tilt_choice) + "_" t.test_tilt(tilt_state = goal) t.CmdPub.publish(msg.String('STOP')) print("Finished :)") else: print("Cancelling...") else: print("Entry out of bounds") print("Cancelling...") elif choice == 7: print("Moving to storage Azimuth...") t.test_center() print("Finished :)") elif choice == 8: choice = raw_input("Caution: Is Mast centered && raised? [y/n] ") choice = str(choice) if choice == "y": print("Retracting Mast...") t.test_retract() print("Finished :)") else: print("Cancelling...") else: print("Unkown Command! Enter any key to try again..") except ValueError: print("ValueError:\tinvalid... try again..") except NameError as e: print(e) print("NameError:\tinvalid... try again..") except KeyboardInterrupt: print("KeyboardInterrupt") t.pub.publish(msg.String('STOP')) t.CmdPub.publish(msg.String('STOP')) sys.exit() except rospy.ROSInterruptException: pass except: print("invalid... try again..") finally: number_of_commands += 1 print("") #rospy.spin()