nav_speak.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import time
00005 
00006 from sound_play.libsoundplay import SoundClient
00007 from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseActionResult
00008 from actionlib_msgs.msg import GoalStatus
00009 
00010 
00011 def goal_status(status):
00012     return {GoalStatus.PENDING: 'PENDING',
00013             GoalStatus.ACTIVE: 'ACTIVE',
00014             GoalStatus.PREEMPTED: 'PREEMPTED',
00015             GoalStatus.SUCCEEDED: 'SUCCEEDED',
00016             GoalStatus.ABORTED: 'ABORTED',
00017             GoalStatus.REJECTED: 'REJECTED',
00018             GoalStatus.PREEMPTING: 'PREEMPTING',
00019             GoalStatus.RECALLING: 'RECALLING',
00020             GoalStatus.RECALLED: 'RECALLED',
00021             GoalStatus.LOST: 'LOST'}[status]
00022 
00023 class NavSpeak:
00024     def __init__(self):
00025         self.move_base_goal_sub = rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, self.move_base_goal_callback, queue_size = 1)
00026         self.move_base_result_sub = rospy.Subscriber("/move_base/result", MoveBaseActionResult, self.move_base_result_callback, queue_size = 1)
00027         self.sound = SoundClient()
00028 
00029     def move_base_goal_callback(self, msg):
00030         self.sound.play(2)
00031 
00032     def move_base_result_callback(self, msg):
00033         text = "{}: {}".format(goal_status(msg.status.status), msg.status.text)
00034         rospy.loginfo(text)
00035         if msg.status.status == GoalStatus.SUCCEEDED:
00036             self.sound.play(1)
00037         elif msg.status.status == GoalStatus.PREEMPTED:
00038             self.sound.play(2)
00039         elif msg.status.status == GoalStatus.ABORTED:
00040             self.sound.play(3)
00041         else:
00042             self.sound.play(4)
00043         time.sleep(1)
00044         self.sound.say(text)
00045 
00046 if __name__ == "__main__":
00047     global sound
00048     rospy.init_node("nav_speak")
00049     n = NavSpeak()
00050     rospy.spin()
00051 
00052 
00053 
00054 
00055 


jsk_fetch_startup
Author(s):
autogenerated on Sat Jul 1 2017 02:42:26