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!")