217 lines
7.0 KiB
Python
217 lines
7.0 KiB
Python
#!/usr/bin/env python
|
|
|
|
# -*- coding: utf-8 -*-
|
|
"""
|
|
Created on Mon Feb 10 12:10:34 2020
|
|
|
|
@author: Maxi
|
|
"""
|
|
|
|
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(10.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):
|
|
self.pub.publish(msg.String('Test 0'))
|
|
rospy.sleep(1.0)
|
|
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(5)
|
|
|
|
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()
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
t = Test()
|
|
rospy.sleep(2.0)
|
|
|
|
|
|
# First Run of tests.py should start with:
|
|
#t.test_elevate()
|
|
#t.test_sweep() # Search Forward Position
|
|
#t.test_center() # Move to Rearward Position
|
|
#t.test_retract()
|
|
|
|
#t.test_rotate(180)
|
|
# t.test_center()
|
|
# rospy.sleep(10.0)
|
|
#t.test_tilt()
|
|
# rospy.sleep(10.0)
|
|
# t.test_center()
|
|
# rospy.sleep(10.0)
|
|
# rospy.sleep(30.0)
|
|
# t.test_retract()
|
|
# rospy.sleep(30.0)
|
|
#t.test_elevate() # Aufrichten in die Vertikale
|
|
# rospy.sleep(10.0)
|
|
# t.test_sweep()
|
|
# rospy.sleep(30.0)
|
|
# t.test_center()
|
|
# rospy.sleep(30.0)
|
|
# t.test_retract()
|
|
# rospy.sleep(10.0)
|
|
# t.test_elevate()
|
|
# rospy.sleep(10.0)
|
|
# # rospy.sleep(30.0)
|
|
# t.test_tilt() # Tilt until Limit Switch pressed
|
|
# rospy.sleep(20.0)
|
|
# t.test_elevate()
|
|
# rospy.sleep(30.0)
|
|
#for angle in range(20, 30, 2):
|
|
## angle = 40 - angle
|
|
#t.test_servo(angle)
|
|
#rospy.sleep(0.6)
|
|
t.test_servo(20)
|
|
# rospy.sleep(1.0)
|
|
# t.test_rotate(-90)
|
|
# rospy.sleep(10.0).
|
|
# t.test_rotate(-270)
|
|
# rospy.sleep(10.0)
|
|
# t.test_rotate(-180)
|
|
# rospy.sleep(5.0)
|
|
# t.test_rotate(0)
|
|
except rospy.ROSInterruptException:
|
|
pass
|