/
scenario_5.py
148 lines (116 loc) · 3.49 KB
/
scenario_5.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
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
#!/usr/bin/env python
import rospy
import random
import math
from random import *
from geometry_msgs.msg import Twist
# sound module
from sound_publisher.msg import Tones
from sound_publisher.msg import TonesArray
from sound_publisher.msg import MusicalTones
from sound_publisher.msg import MusicalTonesArray
from sound_publisher.msg import SongTitle
from kobuki_msgs.msg import Sound
from turtlebot_scenario.msg import OrientationRequest
from kobuki_msgs.msg import BumperEvent
#face recognition
from facedetector.msg import Detection
# Global Variables
pub_motor = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=0)
pub_melody = rospy.Publisher('/mobile_base_commands/sound', Sound, queue_size = 0)
pub_Tones = rospy.Publisher('/sound/tones', TonesArray, queue_size = 1)
pub_MTones = rospy.Publisher('/sound/musical_note', MusicalTonesArray, queue_size =0)
pub_song = rospy.Publisher('sound/play_song', SongTitle, queue_size = 0)
pub_servo = rospy.Publisher('camera/orientation/request', OrientationRequest, queue_size = 0)
melody = Sound()
command = Twist()
song = SongTitle()
bumper = BumperEvent()
flag_detection = False
# easyly send command to the motor
def send_command_motor(linear = 0., angular = 0.):
global pub_motor, command
command.linear.x = linear
command.linear.y = 0
command.linear.z = 0
command.angular.x = 0
command.angular.y = 0
command.angular.z = angular
pub_motor.publish(command)
rospy.sleep(0.1)
# randomly choose a sign
def sign():
return choice([-1, 1])
# reaction to bumper hit
def callback_bumper(data):
global bumper
global pub_MTones
bumper = data
#if bumper.bumper == bumper.LEFT:
#pub_MTones.publish([MusicalTones('do', 8, 500)])
#elif bumper.bumper == bumper.RIGHT:
#pub_MTones.publish([MusicalTones('si', 8, 500)])
#rospy.sleep(0.5)
#reaction to a face recognition
def callback_face(face):
global pub_Tones, flag_detection
flag_detection = True
pub_Tones.publish([Tones(2000, 500)])
rospy.sleep(0.5)
def scenario():
global melody, pub_melody
global pub_Tones, pub_song, song
global pub_servo
global bumper
if bumper.bumper == bumper.LEFT:
send_command_motor(angular = -2.)
rospy.sleep(0.8)
bumper.bumper = 3
elif bumper.bumper == bumper.CENTER:
# Recule + levage camera
flag_detection = False
song.song = song.Star_Wars
pub_song.publish(song)
pub_servo.publish(True)
send_command_motor(linear = -0.2)
rospy.sleep(5)
# People still standing
while(flag_detection == True):
flag_detection = False
rospy.sleep(2.5)
# robot going away
pub_servo.publish(False)
send_command_motor(linear = -0.2, angular = sign() * 2)
rospy.sleep(1)
bumper.bumper = 3
elif bumper.bumper == bumper.RIGHT:
send_command_motor(angular = 2.)
rospy.sleep(0.8)
bumper.bumper = 3
else:
send_command_motor(linear = 0.2)
def main():
global bumper
global pub_song, song
rospy.sleep(2)
# topic subscribed
rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, callback_bumper)
rospy.Subscriber('/facedetector/faces', Detection, callback_face)
rospy.sleep(1)
song.song = song.Indiana_Jones
pub_song.publish(song)
bumper.bumper = 3
# moving loop
while not rospy.is_shutdown():
#randomm direction changing
duree = 3 + random()*5
time_end = rospy.get_time() + duree
while(rospy.get_time() < time_end):
scenario()
if bumper > 2:
print('aleatoire')
bumper.bumper = choice([bumper.LEFT, bumper.RIGHT, 3])
rospy.spin()
if __name__ == '__main__':
rospy.init_node('scenario', anonymous=True)
main()