def depth_callback(self, depth_msg):
""" Handle depth callbacks. """
global PREVIOUS
sound = Sound()
d = 0
# Convert the depth message to a numpy array
depth = self.cv_bridge.imgmsg_to_cv2(depth_msg)
# YOUR CODE HERE.
# HELPER METHODS ARE GOOD.)
#print (np_array.shape) #480 x 640
slice = depth[:,320]
if PREVIOUS == None:
PREVIOUS = slice
else:
d = self.calcNorm(slice)
self.updateAverage(d)
if AVERAGE != None:
if d/AVERAGE > 2:
sound.value = 0
self.sound_pub.publish(sound)
def listen(self, msg):
if self.blackboard.target_id > -1:
return TaskStatus.SUCCESS
rospy.loginfo("Listening: " + msg.data)
now = int(round(time.time()))
if msg.data.find(self.blackboard.wake_word) > -1:
rospy.loginfo("Commanded!")
self.blackboard.command_time = now
self.blackboard.pub_beep_.publish(Sound(Sound.ON))
return TaskStatus.RUNNING
elif now - self.blackboard.command_time < 5:
if msg.data.find("resistor") > -1:
self.blackboard.target_id = 1
self.blackboard.pub_beep_.publish(Sound(Sound.OFF))
return TaskStatus.SUCCESS
elif msg.data.find("home") > -1:
self.blackboard.target_id = 0
self.blackboard.pub_beep_.publish(Sound(Sound.OFF))
return TaskStatus.SUCCESS
elif msg.data.find("trash") > -1:
self.blackboard.target_id = 2
self.blackboard.pub_beep_.publish(Sound(Sound.OFF))
return TaskStatus.SUCCESS
elif msg.data.find("water") > -1:
self.blackboard.target_id = 3
self.blackboard.pub_beep_.publish(Sound(Sound.OFF))
return TaskStatus.SUCCESS
else:
return TaskStatus.FAILURE
else:
return TaskStatus.FAILURE
def main():
global bumper
global direction
print("Initialisation")
rospy.init_node('scenario')
rospy.sleep(0.5)
rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, bumper)
pub1 = rospy.Publisher("/mobile_base/commands/velocity", Twist)
pub2 = rospy.Publisher("/robotsound", SoundRequest)
pub3 = rospy.Publisher("/mobile_base/commands/sound", Sound)
cmd = Twist()
sound = SoundRequest()
bip = Sound()
sound.arg = "/opt/ros/indigo/share/sound_play/sounds/R2D2a.wav"
sound.sound = -2
sound.command = 1
bip.value=4
while not rospy.is_shutdown():
duree= 1+random.random()*5
tempsActuel = rospy.get_time()
stop = rospy.get_time() + duree
while (rospy.get_time() < stop):
if bumper==0:
cmd.linear.x = 0
cmd.angular.z = 0
pub2.publish(sound)
rospy.sleep(0.5)
pub1.publish(cmd)
rospy.sleep(5)
cmd.linear.x = -0.2
cmd.angular.z = -2
cmd.angular.z = -2
pub1.publish(cmd)
rospy.sleep(0.5)
bumper == 3
elif bumper==1:
cmd.linear.x = -0.2
pub1.publish(cmd)
pub3.publish(bip)
rospy.sleep(0.5)
bumper == 3
elif bumper==2:
cmd.linear.x = 0
cmd.angular.z = 2
pub1.publish(cmd)
rospy.sleep(0.5)
bumper== 3
else:
cmd.linear.x = 0.2
cmd.angular.z = 0
pub1.publish(cmd)
if bumper>2:
bumper = math.floor(random.random()*10)
print("The End")
rospy.spin()
def main():
global bumper
global direction
print("Initialisation")
rospy.init_node('scenario')
rospy.sleep(0.5)
rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, bumper)
pub1 = rospy.Publisher("/mobile_base/commands/velocity", Twist)
pub2 = rospy.Publisher("/robotsound", SoundRequest)
pub3 = rospy.Publisher("/mobile_base/commands/sound", Sound)
cmd = Twist()
sound = SoundRequest()
bip = Sound()
sound.arg = "/opt/ros/indigo/share/sound_play/sounds/R2D2a.wav"
sound.sound = -2
sound.command = 1
bip.value = 4
while not rospy.is_shutdown():
duree = 1 + random.random() * 5
tempsActuel = rospy.get_time()
stop = rospy.get_time() + duree
while (rospy.get_time() < stop):
if bumper == 0:
cmd.linear.x = 0
cmd.angular.z = 0
pub2.publish(sound)
rospy.sleep(0.5)
pub1.publish(cmd)
rospy.sleep(5)
cmd.linear.x = -0.2
cmd.angular.z = -2
cmd.angular.z = -2
pub1.publish(cmd)
rospy.sleep(0.5)
bumper == 3
elif bumper == 1:
cmd.linear.x = -0.2
pub1.publish(cmd)
pub3.publish(bip)
rospy.sleep(0.5)
bumper == 3
elif bumper == 2:
cmd.linear.x = 0
cmd.angular.z = 2
pub1.publish(cmd)
rospy.sleep(0.5)
bumper == 3
else:
cmd.linear.x = 0.2
cmd.angular.z = 0
pub1.publish(cmd)
if bumper > 2:
bumper = math.floor(random.random() * 10)
print("The End")
rospy.spin()
def run(self):
while(True and not rospy.is_shutdown()):
beep = Sound()
if(self.intruder):
rospy.loginfo( "Intruder Alert!")
beep.value = 0
self.sound_publish.publish(beep)
self.intruder = False
def sendSounds():
rospy.init_node('sound_sender', anonymous=True)
pub = rospy.Publisher('/mobile_base/commands/sound', Sound, queue_size=10)
while pub.get_num_connections() == 0:
pass
s = Sound()
for x in range(0, 7):
s.value = x
pub.publish(s)
rospy.sleep(1.5)
def main():
global bumper
print("Lancement navigation aleatoire")
rospy.init_node('navigation_aleatoire')
rospy.sleep(0.5)
rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, bumper)
pub = rospy.Publisher("/mobile_base/commands/velocity", Twist)
pub2 = rospy.Publisher("/robotsound", SoundRequest)
pub3 = rospy.Publisher("/mobile_base/commands/sound", Sound)
cmd = Twist()
sound = SoundRequest()
sonnerie = Sound()
sound.arg = "/opt/ros/indigo/share/sound_play/sounds/R2D2a.wav"
sound.sound = -2
sound.command = 1
sonnerie.value= 4
while not rospy.is_shutdown():
duree= 1+random.random()*5
tempsActuel = rospy.get_time()
stop = rospy.get_time() + duree
while (rospy.get_time() < stop):
if bumper==0:
cmd.linear.x = 0
cmd.angular.z = -2
pub.publish(cmd)
rospy.sleep(0.5)
bumper=3
elif bumper==1:
cmd.linear.x = -0.2
cmd.angular.z = 0
pub.publish(cmd)
pub2.publish(sound)
rospy.sleep(0.5)
bumper=3
elif bumper==2:
cmd.linear.x = 0
cmd.angular.z = 2
pub.publish(cmd)
rospy.sleep(0.5)
bumper=3
else:
cmd.linear.x = 0.2
cmd.angular.z = 0
pub.publish(cmd)
if bumper>2:
bumper = math.floor(random.random()*10)
print(sound)
print("fin navigation autonome")
rospy.spin() #boucle infinie tant qu'on quitte pas proprement
def main():
global bumper
print("Lancement navigation aleatoire")
rospy.init_node('navigation_aleatoire')
rospy.sleep(0.5)
rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, bumper)
pub = rospy.Publisher("/mobile_base/commands/velocity", Twist)
pub2 = rospy.Publisher("/robotsound", SoundRequest)
pub3 = rospy.Publisher("/mobile_base/commands/sound", Sound)
cmd = Twist()
sound = SoundRequest()
sonnerie = Sound()
sound.arg = "/opt/ros/indigo/share/sound_play/sounds/R2D2a.wav"
sound.sound = -2
sound.command = 1
sonnerie.value = 4
while not rospy.is_shutdown():
duree = 1 + random.random() * 5
tempsActuel = rospy.get_time()
stop = rospy.get_time() + duree
while (rospy.get_time() < stop):
if bumper == 0:
cmd.linear.x = 0
cmd.angular.z = -2
pub.publish(cmd)
rospy.sleep(0.5)
bumper = 3
elif bumper == 1:
cmd.linear.x = -0.2
cmd.angular.z = 0
pub.publish(cmd)
pub2.publish(sound)
rospy.sleep(0.5)
bumper = 3
elif bumper == 2:
cmd.linear.x = 0
cmd.angular.z = 2
pub.publish(cmd)
rospy.sleep(0.5)
bumper = 3
else:
cmd.linear.x = 0.2
cmd.angular.z = 0
pub.publish(cmd)
if bumper > 2:
bumper = math.floor(random.random() * 10)
print(sound)
print("fin navigation autonome")
rospy.spin() #boucle infinie tant qu'on quitte pas proprement
def execute(self, userdata):
rospy.loginfo('Executing state WAIT')
global prevState
global sound_pub
if prevState == "Forward":
sound = Sound()
sound.value = 6
sound_pub.publish(sound)
rospy.spin()
else:
rospy.sleep(5)
prevState = 'Wait'
return 'start'
def reactToInstruction(instruction):
global sound_pub
if instruction.command == 0:
HPR(True)
return True
elif instruction.command == 10:
HPR(False)
return True
elif instruction.command == 1:
motionAnalysis(True, 10)
return True
elif instruction.command == 2:
motionAnalysis(True, 12)
return True
elif instruction.command == 11:
motionAnalysis(False)
return True
elif instruction.command == 3:
rosVisual(True)
return True
elif instruction.command == 13:
rosVisual(False)
return True
elif instruction.command == 4:
command = "roslaunch kobuki_auto_docking activate.launch"
command = shlex.split(command)
subprocess.Popen(command)
sound_msg = Sound()
sound_msg.value = 0
sound_pub.publish(sound_msg)
return True
elif instruction.command == 5:
initial_pose()
time.sleep(2)
sound_msg = Sound()
sound_msg.value = 0
sound_pub.publish(sound_msg)
return True
elif instruction.command == -1:
startSound()
return True
elif instruction.command == -2:
endSound()
return True
elif instruction.command == -3:
errorSound()
return True
return False
def depth_callback(self, depth_msg):
""" Handle depth callbacks. """
# Convert the depth message to a numpy array
depth = self.cv_bridge.imgmsg_to_cv2(depth_msg)
x, y = depth.shape
if self.c is not None:
self.p = self.c
self.c = depth[:, x / 2] # extract central column
if self.p is not None and self.c is not None:
diff_arr = self.c - self.p
d = np.nansum(np.absolute(diff_arr))
self.avg = self.avg * self.alpha + d * (1 - self.alpha)
#rospy.loginfo(d/self.avg)
if d / self.avg > self.threshold:
pub = rospy.Publisher('/mobile_base/commands/sound',
Sound,
queue_size=10)
sound = Sound(0)
pub.publish(sound)
def execute(self, data):
global stop, cmd_vel_pub, callback_state, sound_pub
wait_time = rospy.Time.now() + rospy.Duration(1.4)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = 1.5
cmd_vel_pub.publish(self.twist)
wait_time = rospy.Time.now() + rospy.Duration(1)
callback_state = 1
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
callback_state = 0
object_count = numpy.argmax(object_counts["task1"]) + 1
for i in range(object_count):
sound_pub.publish(Sound(0))
wait_time_sound = rospy.Time.now() + rospy.Duration(0.5)
while rospy.Time.now()<wait_time_sound:
continue
rospy.loginfo("object count" + str(object_count))
wait_time = rospy.Time.now() + rospy.Duration(1.2)
while rospy.Time.now()<wait_time:
display_led(object_count)
self.twist.linear.x = 0
self.twist.angular.z = -1.5
cmd_vel_pub.publish(self.twist)
return 'go'
def play_sound(self, sound_type):
"""Plays a sound on the Turtlebot
The available sound sequences:
- 0 'turn on'
- 1 'turn off'
- 2 'recharge start'
- 3 'press button'
- 4 'error sound'
- 5 'start cleaning'
- 6 'cleaning end'
You can either pass the string or number above
"""
if not isinstance(sound_type, (int, str)):
self.say("!! Invalid sound type, must be an Integer or a String!")
return
if isinstance(sound_type, str):
try:
sound_type = self.sounds[sound_type]
except KeyError:
self.say("!! Invalid sound '{0}', must be one of: {1}".format(
sound_type, self.sounds.keys()))
return
self.__sound_pub.publish(Sound(sound_type))
def init():
global sound_pub, pub_start
rospy.init_node('radio_node_manager_robot')
rospy.Service('robot_instruction_receiver', InstructionWithAnswer, reactToInstruction)
sound_pub = rospy.Publisher('mobile_base/commands/sound', Sound, queue_size=1)
#Run here all the initial nodes
time.sleep(50)
initial_pose()
time.sleep(10)
sound_msg = Sound()
sound_msg.value = 6
sound_pub.publish(sound_msg)
while not rospy.is_shutdown():
rospy.spin()
def execute(self, userdata):
global START, SHAPE_MATCHED
if SHAPE_MATCHED:
self.sound_pub.publish(Sound(0))
return "success"
if not START:
return "exit"
def loop(self):
""" Make continuously sound after the first detection
"""
self.rate = rospy.Rate(10)
while not rospy.is_shutdown():
if self.send_sound:
self.sound_pub.publish(Sound(3))
self.toogle_led()
self.rate.sleep()
def explore():
global theta
global mapData
global pubgoal
global pubdisp
global pubsound
global xPosition
global yPosition
global width
global height
global done
global pubtest
global offsetX
global offsetY
myMap = list(mapData)
myMap = numpy.reshape(myMap, (height, width))
startAX = (int)((xPosition - offsetX - (.5 * resolution)) / resolution)
startAY = (int)((yPosition - offsetY - (.5 * resolution)) / resolution)
myStart = (startAX, startAY)
if(myMap[myStart[0]][myStart[1]] > 20):
radius = 4;
for j in range(0, 2*radius):
for k in range(0, 2*radius):
myMap[myStart[0]+j-radius][myStart[1]+k-radius] = 0
goal = find_frontiers(myStart, myMap, 20, 5, 1)
if(goal == False):
print "done"
done = True
snd = Sound()
snd.value = 6
pubsound.publish(snd)
else :
output = PoseStamped()
output.header.frame_id = 'map'
output.pose.position.x = (goal[0] * resolution) + offsetX + (.5 * resolution)
output.pose.position.y = (goal[1] * resolution) + offsetY + (.5 * resolution)
output.pose.orientation.w = 1
print "publishing goal"
pubgoal.publish(output)
def bumper_callback(self, data):
""" Emit a sound when a collision occured
* params:
- data: BumperEvent
"""
rospy.loginfo(rospy.get_caller_id() + "I heard %s %s", data.bumper,
data.state)
if data.state == 1:
s = Sound(3)
self.sound_pub.publish(s)
def execute(self, data):
global stop, cmd_vel_pub, callback_state, object_counts, shape_id_counts, chosen_shape, stop_count
shape_found = False
for i in range(0, 3):
# track the line
stop = False
while (not rospy.is_shutdown()) and not stop:
display_led(0) # clear LED
self.twist.linear.x = 0.1
self.twist.angular.z = -float(err) / 200
cmd_vel_pub.publish(self.twist)
print("see the red line")
# go a bit further
wait_time = rospy.Time.now() + rospy.Duration(3)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0.1
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
# dont check if we already found it
if shape_found:
continue
# check each one
wait_time = rospy.Time.now() + rospy.Duration(1.4)
while rospy.Time.now()<wait_time:
# turn
self.twist.linear.x = 0
self.twist.angular.z = 1.6
cmd_vel_pub.publish(self.twist)
wait_time = rospy.Time.now() + rospy.Duration(4)
callback_state = 3
while rospy.Time.now()<wait_time:
# stop
self.twist.linear.x = 0
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
callback_state = 0
current_shape = get_shape(numpy.argmax(shape_id_counts["task3"]))
rospy.loginfo(str(current_shape))
if (current_shape == chosen_shape) and not shape_found:
shape_found = True
wait_time_sound = rospy.Time.now() + rospy.Duration(0.5)
# while rospy.Time.now()<wait_time_sound:
sound_pub.publish(Sound(0))
# turn back to line
wait_time = rospy.Time.now() + rospy.Duration(1.4)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = -1.5
cmd_vel_pub.publish(self.twist)
stop = False
stop_count = -200
return 'go'
def execute(self, data):
global stop, cmd_vel_pub, stop_count, sound_pub
stop_count += 1
# go a bit further
wait_time = rospy.Time.now() + rospy.Duration(0.5)
while rospy.Time.now()<wait_time:
display_led(0)
self.twist.linear.x = 0.25
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
# determin which it is
print stop_count
if stop_count == 1:
wait_time = rospy.Time.now() + rospy.Duration(0.5)
while rospy.Time.now() < wait_time:
sound_pub.publish(Sound(0))
display_led(4)
return 'go'
elif stop_count == 2:
return 'go'
elif stop_count == 3:
print "task 4"
return 'task4'
elif stop_count >= 4:
print "task 3"
stop_count = 0
return 'go'
elif stop_count == 0:
return 'go'
'''
elif stop_count == 3:
return 'go'
elif stop_count == 5:
wait_time = rospy.Time.now() + rospy.Duration(0.5)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
return 'task3'
'''
# next line is the end line
# this is the second red in task 3 for some reason
# regular stop
wait_time = rospy.Time.now() + rospy.Duration(0.5)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
return 'go'
def countCallback(self, msg):
print("GOT COUNT", msg)
if msg.data == 1:
self.led1_pub.publish(Led(1))
elif msg.data == 2:
self.led2_pub.publish(Led(1))
elif msg.data == 3:
self.led1_pub.publish(Led(1))
self.led2_pub.publish(Led(1))
for _ in range(msg.data):
self.sound_pub.publish(Sound(0))
rospy.sleep(rospy.Duration(0.5))
self.done_count = True
def __init__(self):
""" Set up the Sentry node. """
rospy.init_node('sentry')
self.cv_bridge = CvBridge()
rospy.Subscriber('/camera/depth_registered/image',
Image,
self.depth_callback,
queue_size=1)
self.sound = rospy.Publisher('/mobile_base/commands/sound',
Sound,
queue_size=1)
self.prev_scan = []
self.average = 0.0
self.alpha = 0
self.beep = Sound()
rospy.spin()
def __init__(self):
""" Set up the Sentry node. """
rospy.init_node('sentry')
self.cv_bridge = CvBridge()
rospy.Subscriber('/camera/depth_registered/image',
Image,
self.depth_callback,
queue_size=1)
self.sound_pub = rospy.Publisher('/mobile_base/commands/sound',
Sound,
queue_size=1)
self.sound = Sound()
self.sound.value = 1
self.prev = None
self.avg = 0
self.th = 1.3
self.alpha = .9
rospy.spin()
def execute(self, userdata):
global START
print userdata.object_count
if START and not rospy.is_shutdown():
if userdata.object_count == 1:
self.led1_pub.publish(Led(1))
elif userdata.object_count == 2:
self.led2_pub.publish(Led(1))
elif userdata.object_count == 3:
self.led1_pub.publish(Led(1))
self.led2_pub.publish(Led(1))
for _ in range(userdata.object_count):
self.sound_pub.publish(Sound(0))
rospy.sleep(rospy.Duration(0.5))
return "success"
if not START:
return "exit"
def Play(self, sound):
# Plays a sound on the Turtlebot
# The available sound sequences:
# - 0 'turn on'
# - 1 'turn off'
# - 2 'recharge start'
# - 3 'press button'
# - 4 'error sound'
# - 5 'start cleaning'
# - 6 'cleaning end'
#You can either pass the string or number above
if not isinstance(sound, (int, str)):
self.say("!! Invalid sound type, must be an Integer or a String!")
return
if isinstance(sound, str):
try:
sound = self.__sounds[sound]
except KeyError:
print("!! Invalid sound")
return
self.__sound_pub.publish(Sound(sound))
def __init__(self):
""" Set up the Sentry node. """
rospy.init_node('sentry')
self.cv_bridge = CvBridge()
rospy.Subscriber('/camera/depth_registered/image',
Image,
self.depth_callback,
queue_size=1)
self.pub = rospy.Publisher('/mobile_base/commands/sound',
Sound,
queue_size=1)
self.sound = Sound()
self.p = np.array([])
self.first = True
self.average = 1
self.intrusion = False
while not rospy.is_shutdown():
if self.intrusion:
self.sound.value = 1
self.pub.publish(self.sound)
# FUN ZONE
# add sound when robot is running
import rospy
from kobuki_msgs.msg import Sound
sounds = [RUN]
texts = ["run, robot, run"]
rospy.init_node("test_sounds")
pub = rospy.Publisher('/mobile_base/commands/sound', Sound)
rate = rospy.Rate(0.5)
# Added below two line of code
# to wait until at least one subscriber is present or connected.
# Because rospy.Publisher will skip(or eat) first certain messages, if you publish just after rospy.init()
# Without this patch, first message - Sound.ON will never be published.
# I think this is a bug of rospy
# Younghun Ju
while not pub.get_num_connections():
rate.sleep()
msg = Sound()
while not rospy.is_shutdown():
for sound, text in zip(sounds, texts):
msg.value = sound
print text
pub.publish(msg)
rate.sleep()
break
import roslib; roslib.load_manifest('kobuki_testsuite')
import rospy
from kobuki_msgs.msg import Sound
sounds = [Sound.ON, Sound.OFF, Sound.RECHARGE, Sound.BUTTON, Sound.ERROR, Sound.CLEANINGSTART, Sound.CLEANINGEND]
texts = ["On", "Off", "Recharge", "Button", "Error", "CleaningStart", "CleaningEnd"]
rospy.init_node("test_sounds")
pub = rospy.Publisher('/mobile_base/commands/sound', Sound)
rate = rospy.Rate(0.5)
# Added below two line of code
# to wait until at least one subscriber is present or connected.
# Because rospy.Publisher will skip(or eat) first certain messages, if you publish just after rospy.init()
# Without this patch, first message - Sound.ON will never be published.
# I think this is a bug of rospy
# Younghun Ju
while not pub.get_num_connections():
rate.sleep()
msg = Sound()
while not rospy.is_shutdown():
for sound, text in zip(sounds, texts):
msg.value = sound
print text
pub.publish(msg)
rate.sleep()
break
def makeNoise(self): msg = Sound() msg.value = Sound.ON self.sound_publisher.publish(msg)
from batch_controller import execute, cancel
from rightTriangle import *
from struct import pack, unpack
from balloon_tracking_test import scan, get_blob_offset, rawBlobs
from timeit import default_timer
from math import isnan
pub_command = rospy.Publisher("/kobuki_command", Twist, queue_size=10) # publish command
pub_stop = rospy.Publisher("/emergency_stop", Empty, queue_size=10) # publish stop
pub_sound = rospy.Publisher("/mobile_base/commands/sound", Sound, queue_size=2)
kinect_angle = 0.0
depth_map = Image()
depth_available = False
ready = Sound()
ready.value = 0
##THOUGHTS:
# we probably need the pid to keep the balloon in the center of the screen
# We also might need to move quickly to the pid (sharpen the movement)
# get the angle of kinect
def angleCallback(data):
global kinect_angle
kinect_angle = data.data
def depthCallback(data):
global depth_map, depth_available
depth_map = data
def main():
global bumper
print("Lancement navigation aleatoire")
rospy.init_node('navigation_aleatoire')
rospy.sleep(0.5)
rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, bumper)
rospy.Subscriber("/facedetector/faces", Detection, image)
pub = rospy.Publisher("/mobile_base/commands/velocity", Twist)
# pub2 = rospy.Publisher("/robotsound", SoundRequest)
pub3 = rospy.Publisher("/mobile_base/commands/sound", Sound)
pub4 = rospy.Publisher("/mobile_base/commands/digital_output",
DigitalOutput)
cmd = Twist()
# sound = SoundRequest()
sonnerie = Sound()
# sound.arg = "/opt/ros/indigo/share/sound_play/sounds/R2D2a.wav"
# sound.sound = -2
# sound.command = 1
sonnerie.value = 6
digOut = DigitalOutput()
digOut.values = [False, False, False, False]
digOut.mask = [True, False, False, False]
flag = 0
rospy.sleep(1)
indiana_jones.main()
while not rospy.is_shutdown():
duree = 1 + random.random() * 5
tempsActuel = rospy.get_time()
stop = rospy.get_time() + duree
while (rospy.get_time() < stop):
if bumper == 0:
cmd.linear.x = 0
cmd.angular.z = -2
pub.publish(cmd)
rospy.sleep(0.5)
bumper = 3
elif bumper == 1:
cmd.linear.x = -0.2
cmd.angular.z = 0
digOut.values[0] = True
pub.publish(cmd)
vador2.main()
# pub2.publish(sound)
pub4.publish(digOut)
rospy.sleep(5)
while (NbGens > 0):
rospy.sleep(2.5)
flag = 1
digOut.values[0] = False
if flag == 1:
pub3.publish(sonnerie)
rospy.sleep(0.5)
flag = 0
pub4.publish(digOut)
rospy.sleep(0.5)
cmd.linear.x = -0.2
cmd.angular.z = 2
pub.publish(cmd)
rospy.sleep(0.5)
bumper = 3
elif bumper == 2:
cmd.linear.x = 0
cmd.angular.z = 2
pub.publish(cmd)
rospy.sleep(0.5)
bumper = 3
else:
cmd.linear.x = 0.2
cmd.angular.z = 0
pub.publish(cmd)
if bumper > 2:
bumper = math.floor(random.random() * 10)
if bumper == 1:
bumper = 3
print("fin navigation autonome")
rospy.spin() #boucle infinie tant qu'on quitte pas proprement
def execute(self, data):
global stop, cmd_vel_pub, callback_state, object_counts, shape_id_counts, chosen_shape, shape_found
for i in range(0, 2):
# track the line
stop = False
while (not rospy.is_shutdown()) and not stop:
self.twist.linear.x = 0.2
self.twist.angular.z = -float(err) / 200
cmd_vel_pub.publish(self.twist)
# check each one
wait_time = rospy.Time.now() + rospy.Duration(1.4)
while rospy.Time.now()<wait_time:
# turn
self.twist.linear.x = 0
self.twist.angular.z = 1.5
cmd_vel_pub.publish(self.twist)
wait_time = rospy.Time.now() + rospy.Duration(4)
callback_state = 3
while rospy.Time.now()<wait_time:
# stop
self.twist.linear.x = 0
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
callback_state = 0
current_shape = get_shape(numpy.argmax(shape_id_counts["task3"]))
rospy.loginfo(str(current_shape))
if (current_shape == chosen_shape) and not shape_found:
shape_found = True
wait_time_sound = rospy.Time.now() + rospy.Duration(0.5)
# while rospy.Time.now()<wait_time_sound:
sound_pub.publish(Sound(0))
wait_time = rospy.Time.now() + rospy.Duration(1.4)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = -1.5
cmd_vel_pub.publish(self.twist)
wait_time = rospy.Time.now() + rospy.Duration(1)
while rospy.Time.now()<wait_time:
# backup
self.twist.linear.x = -0.2
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
# check last one
wait_time = rospy.Time.now() + rospy.Duration(3.4)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0.2
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
wait_time = rospy.Time.now() + rospy.Duration(1.4)
while rospy.Time.now()<wait_time:
# turn
self.twist.linear.x = 0
self.twist.angular.z = 1.5
cmd_vel_pub.publish(self.twist)
wait_time = rospy.Time.now() + rospy.Duration(4)
callback_state = 3
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
callback_state = 0
current_shape = get_shape(numpy.argmax(shape_id_counts["task3"]))
rospy.loginfo(str(current_shape))
if (current_shape == chosen_shape) and not shape_found:
shape_found = True
wait_time_sound = rospy.Time.now() + rospy.Duration(0.5)
while rospy.Time.now()<wait_time_sound:
sound_pub.publish(Sound(Sound.BUTTON))
wait_time = rospy.Time.now() + rospy.Duration(1.4)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = -1.5
cmd_vel_pub.publish(self.twist)
stop = False
stop_count = -200
return 'go'
def startSound():
global sound_pub
sound_msg = Sound()
sound_msg.value = 0
sound_pub.publish(sound_msg)
def main():
global bumper
print("Lancement navigation aleatoire")
rospy.init_node('navigation_aleatoire')
rospy.sleep(0.5)
rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, bumper)
# rospy.Subscriber("/facedetector/faces",Detection , face)
# rospy.Subscriber("/camera/depth/image_rect",Image, image)
pub = rospy.Publisher("/mobile_base/commands/velocity", Twist)
# pub2 = rospy.Publisher("/robotsound", SoundRequest)
pub3 = rospy.Publisher("/mobile_base/commands/sound", Sound)
pub4 = rospy.Publisher("/mobile_base/commands/digital_output", DigitalOutput)
ic=image_converter()
cmd = Twist()
# sound = SoundRequest()
sonnerie = Sound()
# sound.arg = "/opt/ros/indigo/share/sound_play/sounds/R2D2a.wav"
# sound.sound = -2
# sound.command = 1
sonnerie.value= 6
digOut=DigitalOutput()
digOut.values= [False, False, False, False]
digOut.mask=[True, False, False, False]
flag=0
while not rospy.is_shutdown():
duree= 1+random.random()*5
tempsActuel = rospy.get_time()
stop = rospy.get_time() + duree
while (rospy.get_time() < stop):
if bumper==0 or bumper1 ==0 :
cmd.linear.x = 0
cmd.angular.z = -2
pub.publish(cmd)
rospy.sleep(0.5)
bumper=3
elif bumper==1 or bumper1==1:
cmd.linear.x = -0.2
cmd.angular.z = 0
digOut.values[0]=True
pub.publish(cmd)
# pub2.publish(sound)
pub4.publish(digOut)
rospy.sleep(5)
while (NbGens>0):
rospy.sleep(2.5)
flag=1
digOut.values[0]=False
if flag==1:
pub3.publish(sonnerie)
rospy.sleep(0.5)
flag=0
pub4.publish(digOut)
rospy.sleep(0.5)
cmd.linear.x = -0.2
cmd.angular.z = 2
pub.publish(cmd)
rospy.sleep(0.5)
bumper=3
elif bumper==2 or bumper1==2:
cmd.linear.x = 0
cmd.angular.z = 2
pub.publish(cmd)
rospy.sleep(0.5)
bumper=3
else:
cmd.linear.x = 0.2
cmd.angular.z = 0
pub.publish(cmd)
rospy.sleep(0.5)
# if bumper>2:
# bumper = math.floor(random.random()*10)
# if bumper == 1:
# bumper=3
print("fin navigation autonome")
rospy.spin() #boucle infinie tant qu'on quitte pas proprement
def endSound():
global sound_pub
sound_msg = Sound()
sound_msg.value = 1
sound_pub.publish(sound_msg)
def errorSound():
global sound_pub
sound_msg = Sound()
sound_msg.value = 4
sound_pub.publish(sound_msg)
def timerCallback(self, event):
if rospy.is_shutdown(): return
if self.done: return
self.elapsed_time = event.current_real.to_time() - self.init_time
if event.last_real == None:
event.last_real = rospy.Time.from_sec(0.0)
event.last_expected = rospy.Time.from_sec(0.0)
event.last_duration = 0.0
if self.state == 'WAIT_CONNECTION':
if self.sub_angle.get_num_connections() > 0 and\
self.sub_gyro.get_num_connections() > 0 and\
self.pub_velocity.get_num_connections() > 0:
if self.debug: print 'connection ready.'
self.state = 'ALIGNING'
if self.debug: print 'state:', self.state
return
else:
if self.debug: print 'not ready yet.'
return
if self.state == "ALIGNING":
if self.scan_angle is None:
return
scan_angle = self.scan_angle
if abs(scan_angle) > radians(1.0):
cmd_vel = Twist()
cmd_vel.angular.z = -0.33 * scan_angle / abs(scan_angle)
self.pub_velocity.publish(cmd_vel)
self.init_time = rospy.Time.now().to_time()
if self.debug:
print 'state:', self.state, ':', scan_angle, '-->', cmd_vel.angular.z
return
else:
cmd_vel = Twist()
cmd_vel.linear.x = 0.0 # in [m/s]
cmd_vel.angular.z = 0.0 # in [rad/s]
self.pub_velocity.publish(cmd_vel)
if self.elapsed_time > 5.0:
if self.debug: print 'alining ready.'
self.pub_sound.publish(Sound(value=Sound.RECHARGE))
self.state = 'WAIT_TRIGGER'
if self.debug: print 'state:', self.state
return
else:
if self.debug: print 'state:', self.state
return
if self.state == 'WAIT_TRIGGER':
if self.triggered or not self.use_button:
self.pub_sound.publish(Sound(value=Sound.CLEANINGEND))
self.state = 'CALC_ANGLE_PRERUN'
self.reset_angle = True
self.angle_count = 0
if self.debug: print 'state:', self.state
return
else:
return
if self.state == 'CALC_ANGLE_PRERUN':
if self.angle_count >= self.max_sample:
self.angle_prerun = self.angle_avg
if self.debug: print 'angle prerun: ', self.angle_prerun
self.pub_sound.publish(Sound(value=Sound.RECHARGE))
self.state = 'RUNNING'
self.init_time = rospy.Time.now().to_time()
if self.debug: print 'state:', self.state
self.init_imu = True
self.init_angle = 0.0
self.angle = 0.0
self.angle_rate = 0.0
self.diff_angle = 0.0
self.accumulated_angle = 0.0
return
else:
if self.debug:
print 'state:', self.state, ':', self.angle_count, self.scan_angle, self.angle_sum, self.angle_avg
return
if self.state == 'RUNNING':
if self.ending_condition():
self.state = 'STOP'
cmd_vel = Twist()
cmd_vel.linear.x = 0.0 # in [m/s]
cmd_vel.angular.z = 0.0 # in [rad/s]
self.pub_velocity.publish(cmd_vel)
self.running_time = self.elapsed_time
self.init_time = rospy.Time.now().to_time()
if self.debug: print 'state:', self.state
return
else:
cmd_vel = Twist()
cmd_vel.linear.x = self.command_vx # In [m/s]
cmd_vel.angular.z = self.command_wz # In [rad/s]
self.pub_velocity.publish(cmd_vel)
if self.debug:
print 'state:', self.state, ':', degrees(
self.accumulated_angle), self.test_angle
return
if self.state == 'STOP':
if self.elapsed_time > 5.0:
self.reset_angle = True
self.angle_count = 0
self.state = 'CALC_ANGLE_POSTRUN'
if self.debug: print 'state:', self.state
return
else:
if self.debug: print 'state:', self.state
return
if self.state == 'CALC_ANGLE_POSTRUN':
if self.angle_count >= self.max_sample:
self.angle_postrun = self.angle_avg
angle_gyro = self.accumulated_angle
sign = angle_gyro / abs(angle_gyro)
angle_laser = sign * radians(self.test_angle) + wrap_to_pi(
self.angle_postrun - self.angle_prerun)
rospy.loginfo('test_angle: {0} deg'.format(self.test_angle))
rospy.loginfo('test_command: {0} m/s {1} deg/s'.format(
self.command_vx, degrees(self.command_wz)))
rospy.loginfo('running_time: {0} s'.format(self.running_time))
rospy.loginfo('')
rospy.loginfo('angle_prerun: {0} deg'.format(
degrees(self.angle_prerun)))
rospy.loginfo('angle_postrun: {0} deg'.format(
degrees(self.angle_postrun)))
rospy.loginfo('angle_gyro: {0} deg'.format(
degrees(angle_gyro)))
rospy.loginfo('angle_laser: {0} deg'.format(
degrees(angle_laser)))
rospy.loginfo('')
rospy.loginfo(
'error: {0} deg/rev'.format(angle_gyro /
angle_laser * 360.0 -
360.0))
rospy.loginfo('Done')
self.pub_sound.publish(Sound(value=Sound.CLEANINGSTART))
self.state = 'DONE'
self.done = True
if self.debug: print 'state:', self.state
rospy.signal_shutdown('jobs_done')
time.sleep(5.0)
return
else:
if self.debug:
print 'state:', self.state, ':', self.angle_count, self.scan_angle, self.angle_sum, self.angle_avg
return
def execute(self, data):
global stop, cmd_vel_pub, err, line_lost, callback_state, object_counts, shape_id_counts, sound_pub, chosen_shape
print 'in task 2'
wait_time = rospy.Time.now() + rospy.Duration(1.5)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0.2
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
wait_time = rospy.Time.now() + rospy.Duration(1.4)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = 1.5
cmd_vel_pub.publish(self.twist)
# track the line
print 'tracking line'
while (not rospy.is_shutdown()) and (not line_lost):
self.twist.linear.x = 0.2
self.twist.angular.z = -float(err) / 200
cmd_vel_pub.publish(self.twist)
# reaches the end, stop for 2 second
print 'reaches the end'
callback_state = 2
wait_time = rospy.Time.now() + rospy.Duration(2)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
callback_state = 0
# turn back
wait_time = rospy.Time.now() + rospy.Duration(2)
object_count = numpy.argmax(object_counts["task2"]) + 1
while rospy.Time.now()<wait_time:
display_led(object_count)
self.twist.linear.x = 0
self.twist.angular.z = 1.5
cmd_vel_pub.publish(self.twist)
# track the line
for i in range(object_count):
# wait_time_sound = rospy.Time.now() + rospy.Duration(0.5)
# while rospy.Time.now()<wait_time_sound:
sound_pub.publish(Sound(0))
wait_time_sound = rospy.Time.now() + rospy.Duration(1)
while rospy.Time.now()<wait_time_sound:
continue
stop = False
while (not rospy.is_shutdown()) and not stop:
self.twist.linear.x = 0.2
self.twist.angular.z = -float(err) / 200
cmd_vel_pub.publish(self.twist)
# stops at red line, return to go state
wait_time = rospy.Time.now() + rospy.Duration(3)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0.2
self.twist.angular.z = 0
cmd_vel_pub.publish(self.twist)
wait_time = rospy.Time.now() + rospy.Duration(1)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = 1.5
cmd_vel_pub.publish(self.twist)
stop = False
chosen_shape = get_shape(numpy.argmax(shape_id_counts["task2"]))
rospy.loginfo(str(chosen_shape))
return 'go'
def reset_odom(self):
self.pub_base_reset.publish(Empty())
self.pub_base_sound.publish(Sound(value=Sound.CLEANINGEND))
