#!/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