voice_nav.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004   voice_nav.py allows controlling a mobile base using simple speech commands.
00005   Based on the voice_cmd_vel.py script by Michael Ferguson in the pocketsphinx ROS package.
00006 """
00007 
00008 import roslib; roslib.load_manifest('pi_speech_tutorial')
00009 import rospy
00010 
00011 from geometry_msgs.msg import Twist
00012 from std_msgs.msg import String
00013 from math import copysign
00014 
00015 class voice_cmd_vel:
00016     def __init__(self):
00017         rospy.on_shutdown(self.cleanup)
00018         self.max_speed = rospy.get_param("~max_speed", 0.4)
00019         self.max_angular_speed = rospy.get_param("~max_angular_speed", 1.5)
00020         self.speed = rospy.get_param("~start_speed", 0.1)
00021         self.angular_speed = rospy.get_param("~start_angular_speed", 0.5)
00022         self.linear_increment = rospy.get_param("~linear_increment", 0.05)
00023         self.angular_increment = rospy.get_param("~angular_increment", 0.4)
00024         self.rate = rospy.get_param("~rate", 5)
00025         r = rospy.Rate(self.rate)
00026         self.paused = False
00027         
00028         # Initialize the Twist message we will publish.
00029         self.msg = Twist()
00030 
00031         # Publish the Twist message to the cmd_vel topic
00032         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
00033         
00034         # Subscribe to the /recognizer/output topic to receive voice commands.
00035         rospy.Subscriber('/recognizer/output', String, self.speechCb)
00036         
00037         # A mapping from keywords to commands.
00038         self.keywords_to_command = {'stop': ['stop', 'halt', 'abort', 'kill', 'panic', 'off', 'freeze', 'shut down', 'turn off', 'help', 'help me'],
00039                                     'slower': ['slow down', 'slower'],
00040                                     'faster': ['speed up', 'faster'],
00041                                     'forward': ['forward', 'ahead', 'straight'],
00042                                     'backward': ['back', 'backward', 'back up'],
00043                                     'rotate left': ['rotate left'],
00044                                     'rotate right': ['rotate right'],
00045                                     'turn left': ['turn left'],
00046                                     'turn right': ['turn right'],
00047                                     'quarter': ['quarter speed'],
00048                                     'half': ['half speed'],
00049                                     'full': ['full speed'],
00050                                     'pause': ['pause speech'],
00051                                     'continue': ['continue speech']}
00052         
00053         rospy.loginfo("Ready to receive voice commands")
00054         
00055         # We have to keep publishing the cmd_vel message if we want the robot to keep moving.
00056         while not rospy.is_shutdown():
00057             self.cmd_vel_pub.publish(self.msg)
00058             r.sleep()                       
00059             
00060     def get_command(self, data):
00061         for (command, keywords) in self.keywords_to_command.iteritems():
00062             for word in keywords:
00063                 if data.find(word) > -1:
00064                     return command
00065         
00066     def speechCb(self, msg):        
00067         command = self.get_command(msg.data)
00068         
00069         rospy.loginfo("Command: " + str(command))
00070         
00071         if command == 'pause':
00072             self.paused = True
00073         elif command == 'continue':
00074             self.paused = False
00075             
00076         if self.paused:
00077             return       
00078         
00079         if command == 'forward':    
00080             self.msg.linear.x = self.speed
00081             self.msg.angular.z = 0
00082             
00083         elif command == 'rotate left':
00084             self.msg.linear.x = 0
00085             self.msg.angular.z = self.angular_speed
00086                 
00087         elif command == 'rotate right':  
00088             self.msg.linear.x = 0      
00089             self.msg.angular.z = -self.angular_speed
00090             
00091         elif command == 'turn left':
00092             if self.msg.linear.x != 0:
00093                 self.msg.angular.z += self.angular_increment
00094             else:        
00095                 self.msg.angular.z = self.angular_speed
00096                 
00097         elif command == 'turn right':    
00098             if self.msg.linear.x != 0:
00099                 self.msg.angular.z -= self.angular_increment
00100             else:        
00101                 self.msg.angular.z = -self.angular_speed
00102                 
00103         elif command == 'backward':
00104             self.msg.linear.x = -self.speed
00105             self.msg.angular.z = 0
00106             
00107         elif command == 'stop': 
00108             # Stop the robot!  Publish a Twist message consisting of all zeros.         
00109             self.msg = Twist()
00110         
00111         elif command == 'faster':
00112             self.speed += self.linear_increment
00113             self.angular_speed += self.angular_increment
00114             if self.msg.linear.x != 0:
00115                 self.msg.linear.x += copysign(self.linear_increment, self.msg.linear.x)
00116             if self.msg.angular.z != 0:
00117                 self.msg.angular.z += copysign(self.angular_increment, self.msg.angular.z)
00118             
00119         elif command == 'slower':
00120             self.speed -= self.linear_increment
00121             self.angular_speed -= self.angular_increment
00122             if self.msg.linear.x != 0:
00123                 self.msg.linear.x -= copysign(self.linear_increment, self.msg.linear.x)
00124             if self.msg.angular.z != 0:
00125                 self.msg.angular.z -= copysign(self.angular_increment, self.msg.angular.z)
00126                 
00127         elif command in ['quarter', 'half', 'full']:
00128             if command == 'quarter':
00129                 self.speed = copysign(self.max_speed / 4, self.speed)
00130         
00131             elif command == 'half':
00132                 self.speed = copysign(self.max_speed / 2, self.speed)
00133             
00134             elif command == 'full':
00135                 self.speed = copysign(self.max_speed, self.speed)
00136             
00137             if self.msg.linear.x != 0:
00138                 self.msg.linear.x = copysign(self.speed, self.msg.linear.x)
00139 
00140             if self.msg.angular.z != 0:
00141                 self.msg.angular.z = copysign(self.angular_speed, self.msg.angular.z)
00142                 
00143         else:
00144             return
00145 
00146         self.msg.linear.x = min(self.max_speed, max(-self.max_speed, self.msg.linear.x))
00147         self.msg.angular.z = min(self.max_angular_speed, max(-self.max_angular_speed, self.msg.angular.z))
00148 
00149     def cleanup(self):
00150         # When shutting down be sure to stop the robot!  Publish a Twist message consisting of all zeros.
00151         twist = Twist()
00152         self.cmd_vel_pub.publish(twist)
00153 
00154 if __name__=="__main__":
00155     rospy.init_node('voice_nav')
00156     try:
00157         voice_cmd_vel()
00158     except:
00159         pass
00160 


pi_speech_tutorial
Author(s): patrick
autogenerated on Mon Jan 6 2014 11:33:51