voice_teleop_advanced.py

Go to the documentation of this file.

#!/usr/bin/env

"""
voice_nav.py - Version 1.1 2013-12-20

Allows controlling a mobile base using simple speech commands.

Based on the voice_cmd_vel.py script by Michael Ferguson in
the pocketsphinx ROS package.

See http://www.ros.org/wiki/pocketsphinx
"""
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import String
from math import copysign

class VoiceNav:
     def __init__(self):
        #initialize the ROS node
        rospy.init_node('voice_teleop')
        rospy.on_shutdown(self.cleanup)
    
        # Set a number of parameters affecting the robot's speed
        self.max_speed = rospy.get_param("~max_speed", 0.4)
        self.max_angular_speed = rospy.get_param("~max_angular_speed", 1.5)
        self.speed = rospy.get_param("~start_speed", 0.1)
        self.angular_speed = rospy.get_param("~start_angular_speed", 0.5)
        self.linear_increment = rospy.get_param("~linear_increment", 0.05)
        self.angular_increment = rospy.get_param("~angular_increment", 0.4)
    
        # We don't have to run the script very fast
        self.rate = rospy.get_param("~rate", 5)
        r = rospy.Rate(self.rate)
    
        # A flag to determine whether or not voice control is paused
        self.paused = False
    
        # Initialize the Twist message we will publish.
        self.cmd_vel = Twist()
    
        # Publish the Twist message to the cmd_vel topic
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
    
        # Subscribe to the /recognizer/output topic to receive voice commands.
        rospy.Subscriber('/recognizer/output', String, self.speech_callback)
    
        # A mapping from keywords or phrases to commands
        #These are different kinds of commands you can give to the robot and you can delete/add any command you want.
        self.keywords_to_command = {'stop': ['stop', 'halt', 'abort', 'kill', 'panic', 'off', 'freeze', 'shut down', 'turn off', 'help', 'help me'],
                                    'slower': ['slow down', 'slower'],
                                    'faster': ['speed up', 'faster'],
                                    'forward': ['forward', 'ahead', 'straight'],
                                    'backward': ['back', 'backward', 'back up'],
                                    'rotate left': ['rotate left'],
                                    'rotate right': ['rotate right'],
                                    'turn left': ['turn left'],
                                    'turn right': ['turn right'],
                                    'quarter': ['quarter speed'],
                                    'half': ['half speed'],
                                    'full': ['full speed'],
                                    'pause': ['pause speech'],
                                    'continue': ['continue speech']}
    
        rospy.loginfo("Ready to receive voice commands")
    
        # We have to keep publishing the cmd_vel message if we want the robot to keep moving.
        while not rospy.is_shutdown():
            self.cmd_vel_pub.publish(self.cmd_vel)
            r.sleep()

def get_command(self, data):
    # Attempt to match the recognized word or phrase to the
    # keywords_to_command dictionary and return the appropriate
    # command
    for (command, keywords) in self.keywords_to_command.iteritems():
        for word in keywords:
            if data.find(word) > -1:
                return command

def speech_callback(self, msg):
    # Get the motion command from the recognized phrase
    command = self.get_command(msg.data)

    # Log the command to the screen
    rospy.loginfo("Command: " + str(command))

    # If the user has asked to pause/continue voice control,
    # set the flag accordingly
    if command == 'pause':
        self.paused = True
    elif command == 'continue':
        self.paused = False

    # If voice control is paused, simply return without
    # performing any action
    if self.paused:
        return

    # The list of if-then statements should be fairly
    # self-explanatory
    if command == 'forward':
        self.cmd_vel.linear.x = self.speed
        self.cmd_vel.angular.z = 0

    elif command == 'rotate left':
        self.cmd_vel.linear.x = 0
        self.cmd_vel.angular.z = self.angular_speed

    elif command == 'rotate right':
        self.cmd_vel.linear.x = 0
        self.cmd_vel.angular.z = -self.angular_speed

    elif command == 'turn left':
        if self.cmd_vel.linear.x != 0:
            self.cmd_vel.angular.z += self.angular_increment
        else:
            self.cmd_vel.angular.z = self.angular_speed

    elif command == 'turn right':
        if self.cmd_vel.linear.x != 0:
            self.cmd_vel.angular.z -= self.angular_increment
        else:
            self.cmd_vel.angular.z = -self.angular_speed

    elif command == 'backward':
        self.cmd_vel.linear.x = -self.speed
        self.cmd_vel.angular.z = 0

    elif command == 'stop':
        # Stop the robot!  Publish a Twist message consisting of all zeros.
        self.cmd_vel = Twist()

    elif command == 'faster':
        self.speed += self.linear_increment
        self.angular_speed += self.angular_increment
        if self.cmd_vel.linear.x != 0:
            self.cmd_vel.linear.x += copysign(self.linear_increment, self.cmd_vel.linear.x)
        if self.cmd_vel.angular.z != 0:
            self.cmd_vel.angular.z += copysign(self.angular_increment, self.cmd_vel.angular.z)

    elif command == 'slower':
        self.speed -= self.linear_increment
        self.angular_speed -= self.angular_increment
        if self.cmd_vel.linear.x != 0:
            self.cmd_vel.linear.x -= copysign(self.linear_increment, self.cmd_vel.linear.x)
        if self.cmd_vel.angular.z != 0:
            self.cmd_vel.angular.z -= copysign(self.angular_increment, self.cmd_vel.angular.z)

    elif command in ['quarter', 'half', 'full']:
        if command == 'quarter':
            self.speed = copysign(self.max_speed / 4, self.speed)

        elif command == 'half':
            self.speed = copysign(self.max_speed / 2, self.speed)

        elif command == 'full':
            self.speed = copysign(self.max_speed, self.speed)

        if self.cmd_vel.linear.x != 0:
            self.cmd_vel.linear.x = copysign(self.speed, self.cmd_vel.linear.x)

        if self.cmd_vel.angular.z != 0:
            self.cmd_vel.angular.z = copysign(self.angular_speed, self.cmd_vel.angular.z)

    else:
        return

    self.cmd_vel.linear.x = min(self.max_speed, max(-self.max_speed, self.cmd_vel.linear.x))
    self.cmd_vel.angular.z = min(self.max_angular_speed, max(-self.max_angular_speed, self.cmd_vel.angular.z))

def cleanup(self):
    # When shutting down be sure to stop the robot!
    twist = Twist()
    self.cmd_vel_pub.publish(twist)
    rospy.sleep(1)

if __name__=="__main__":
  try:
      VoiceNav()
      rospy.spin()
  except rospy.ROSInterruptException:
      rospy.loginfo("Voice navigation terminated.")