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. """
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 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 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 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 execute(self, userdata):
sound = Sound()
sound.value = sound.ON
self.publisher.publish(sound)
return 'ok'
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
depth_available = True
def makeNoise(self): msg = Sound() msg.value = Sound.ON self.sound_publisher.publish(msg)
def main():
global localTurtleBotInputs, localSoundValue, localLinearSpeed, startUpTimer, localAngularSpeed, soundValueUpdateCounter, amcl_present
localTurtleBotInputs = turtlebotInputs()
localSoundValue = 0
localLinearSpeed = 0.0
localAngularSpeed = 0.0
soundValueUpdateCounter = 0
amcl_present = 0
startUpTimer = 0
# Name this node
nh = rospy.init_node('minimal_turtlebot_py')
# Set a rate for the loop
naptime = rospy.Rate(10) # 10hz
my_wheel_drop_subscription = rospy.Subscriber(
'mobile_base/events/wheel_drop', WheelDropEvent, wheelDropCallback)
my_bumper_subscription = rospy.Subscriber('mobile_base/events/bumper',
BumperEvent,
bumperMessageCallback)
my_cliff_subscription = rospy.Subscriber('mobile_base/events/cliff',
CliffEvent, cliffCallback)
my_imu_subscription = rospy.Subscriber('mobile_base/sensors/imu_data_raw',
Imu, imuCallback)
my_core_subscription = rospy.Subscriber('mobile_base/sensors/core',
SensorState, coreCallback)
my_odom_subscription = rospy.Subscriber('odom', Odometry, odomCallback)
my_amcl_subscription = rospy.Subscriber('amcl_pose',
PoseWithCovarianceStamped,
amclCallback)
my_pose2d_subscription = rospy.Subscriber('goal_pose2d', PoseStamped,
poseCallback)
scanSubscription = rospy.Subscriber('scan', LaserScan, scanCallback)
# Publish to the velocity topic
cmd_vel_pub_ = rospy.Publisher('mobile_base/commands/velocity',
Twist,
queue_size=10)
# Publish to the sound topic
my_publisher_object = rospy.Publisher('mobile_base/commands/sound',
Sound,
queue_size=10)
soundValue = Sound()
base_cmd = Twist()
# Main loop
while not rospy.is_shutdown():
localTurtleBotInputs.nanoSecs = rospy.Time.now().to_nsec()
localTurtleBotInputs, localSoundValue, localLinearSpeed, localAngularSpeed = turtlebot_controller(
localTurtleBotInputs, localSoundValue, localLinearSpeed,
localAngularSpeed)
soundValue.value = localSoundValue
base_cmd.linear.x = localLinearSpeed
base_cmd.angular.z = localAngularSpeed
if (startUpTimer < 1000):
startUpTimer = startUpTimer + 1
naptime.sleep()
cmd_vel_pub_.publish(base_cmd)
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 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
#!/usr/bin/env python
import rospy
from kobuki_msgs.msg import Sound
pub = rospy.Publisher('/mobile_base/commands/sound', Sound)
rospy.init_node('publish-sound')
while not rospy.is_shutdown():
msg = Sound()
rospy.loginfo(msg)
msg.value = 6
pub.publish(msg)
rospy.sleep(1.0)
def startSound():
global sound_pub
sound_msg = Sound()
sound_msg.value = 0
sound_pub.publish(sound_msg)
def play_sound():
if play_sound.publisher is None:
play_sound.publisher = rospy.Publisher('sound', Sound, queue_size=10)
sound = Sound()
sound.value = sound.ON
play_sound.publisher.publish(sound)
def play_sound(self, sound_id=0):
msg = Sound()
msg.value = max(0, min(sound_id, 6))
self.pub_sound.publish(msg)
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)
# 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 inner(*args, **kwargs):
sound = Sound()
sound.value = sound.ON
sound_pub.publish(sound)
fn(*args, **kwargs)
def __init__(self):
# declare the sounds to play at each way point
roslib.load_manifest("kobuki_testsuite")
rospy.init_node("test_kobuki_Sound")
pub = rospy.Publisher("/mobile_base/commands/sound",
Sound,
queue_size=1)
rate = rospy.Rate(10.0)
while not pub.get_num_connections():
rate.sleep()
msg = Sound()
# declare the coordinates of interest
self.x1 = 1.431
self.y1 = 2.828
self.x2 = 4.402
self.y2 = 0.528
self.x3 = 1.332
self.y3 = -3.171
self.x0 = -1.462
self.y0 = -0.748
self.goalReached = False
# initialize
choice = 0
pub = rospy.Publisher('/mobile_base/commands/sound',
Sound,
queue_size=1)
if (choice == 0): # go to next way point
msg.value = Sound.ON
pub.publish(msg)
self.goalReached = self.moveToGoal(self.x1, self.y1)
choice += 1
if (choice == 1): # go to the next way point
msg.value = Sound.ON
pub.publish(msg)
self.goalReached = self.moveToGoal(self.x2, self.y2)
choice += 1
if (choice == 2): # go to the next way point
msg.value = Sound.ON
pub.publish(msg)
self.goalReached = self.moveToGoal(self.x3, self.y3)
choice += 1
if (choice == 3): # go to the initial coordinates
msg.value = Sound.ON
pub.publish(msg)
self.goalReached = self.moveToGoal(self.x0, self.y0)
choice += 1
if (choice == 4): # stop the program when the path is completed
msg.value = Sound.CLEANINGEND
pub.publish(msg)
self.shutdown()
if (self.goalReached
): # display 'Congratulations' when the path is completed
rospy.loginfo("Congratulations!")
