304 lines
11 KiB
Python
304 lines
11 KiB
Python
#!/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() |