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