-
Notifications
You must be signed in to change notification settings - Fork 1
/
test_pub2.py
executable file
·60 lines (48 loc) · 1.59 KB
/
test_pub2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
#!/usr/bin/env python
# coding=utf-8
"""
test plan 算法库的测试程序
Copyright (c) 2017 Xu Zhihao (Howe). All rights reserved.
This program is free software; you can redistribute it and/or modify
This programm is tested on kuboki base turtlebot.
"""
import rospy
from std_msgs.msg import Bool
# speaker simulation
class tester1():
def __init__(self):
self.Connected = True
self.speak = False
self.count = 0
self.pub = rospy.Publisher('/speaker_done', Bool, queue_size=1)
rospy.Timer(rospy.Duration(0.1), self.pubCB)
rospy.Timer(rospy.Duration(0.1), self.speakCB)
rospy.Subscriber('/StopRun_run', Bool, self.stoprunCB)
rospy.spin()
def stoprunCB(self, data):
self.Connected = data.data
print 'recieve arrive: ', self.Connected
def pubCB(self, event):
if self.Connected:
if self.speak:
for i in range(2):
self.pub.publish(True)
self.Connected = False
self.speak = False
def speakCB(self,event):
if self.Connected:
if self.count > 50:
self.count = 0
self.speak = True
else:
self.speak = False
self.count += 1
print 'speaker sim:', self.count
if __name__=='__main__':
rospy.init_node('Plan_tester_pub2')
try:
rospy.loginfo( "initialization system")
tester1()
rospy.loginfo("process done and quit" )
except rospy.ROSInterruptException:
rospy.loginfo("node terminated.")