Esempio n. 1
0
 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
Esempio n. 2
0
    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))
Esempio n. 3
0
    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 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)
Esempio n. 5
0
    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'
Esempio n. 6
0
    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()
Esempio n. 8
0
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 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)
Esempio n. 12
0
    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 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
Esempio n. 14
0
 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'
Esempio n. 15
0
    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'
Esempio n. 16
0
    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
Esempio n. 17
0
    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()
Esempio n. 19
0
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)
Esempio n. 20
0
    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)
Esempio n. 23
0
    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
Esempio n. 24
0
# 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
Esempio n. 25
0
	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
Esempio n. 27
0
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
Esempio n. 28
0
 def reset_odom(self):
     self.pub_base_reset.publish(Empty())
     self.pub_base_sound.publish(Sound(value=Sound.CLEANINGEND))
Esempio n. 29
0
    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'
Esempio n. 30
0
    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'