ROS & NAO: Speech Recognition, Marker tracking(ar_pose), Follow Commands

posture.py

# -*- coding: utf-8 -*-
# Author : Sirres Raphael
# Date: 16.12.2014
# Description: Basic Posture Class
from naoqi import ALProxy
class NaoPosture:
	def __init__(self, IP, PORT=9559):
		self.postureProxy = ALProxy("ALRobotPosture", IP, PORT)
		self.speed=0.7

	def stand(self):
		self.postureProxy.goToPosture("Stand", self.speed)

	def sit(self):
		self.postureProxy.goToPosture("Sit", self.speed)

	def sit_relax(self):
		postureProxy.goToPosture("SitRelax", self.speed)

	def print_list_of_postures(self):
		print postureProxy.getPostureList()

if __name__ == '__main__':
	pos=NaoPosture("10.151.0.65")
	pos.sit()

sensors.py

# -*- encoding: UTF-8 -*-
# Before running this command please check your PYTHONPATH is set correctly to the folder of your pynaoqi sdk.
from naoqi import ALProxy 
import time
from math import atan2
import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Vector3
from ar_pose_msgs.msg import ARMarker
from nao_msgs.msg import JointAnglesWithSpeed
from std_msgs.msg import String



# Set the IP address of your NAO.
ip = "10.151.1.47"

# Connect to ALSonar module.
sonarProxy = ALProxy("ALSonar", ip, 9559)

# Subscribe to sonars, this will launch sonars (at hardware level) and start data acquisition.
sonarProxy.subscribe("myApplication")

#Now you can retrieve sonar data from ALMemory.
memoryProxy = ALProxy("ALMemory", ip, 9559)


motionProxy = ALProxy("ALMotion", ip, 9559)


#while 1:
	#print("Left %s" % (memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")))
	#print("Right %s" % (memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")))
	#time.sleep(1)

# Get sonar left first echo (distance in meters to the first obstacle).
#memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")

# Same thing for right.
#memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")

# Please read Sonar ALMemory keys section if you want to know the other values you can get.

#global MarkerSubscriber
FollowMarker=True
onMyWay=True
closeDistance = 0.8
stopDistance = 0.4
deviationStep = 0




MARKER_TOPIC="ar_pose_marker"
JOINT_TOPIC="joint_angles"
WALK_TOPIC="cmd_vel"
NAO_CONTROLLER_TOPIC="V_R"


head_pub=rospy.Publisher(JOINT_TOPIC, JointAnglesWithSpeed, queue_size=10)
body_pub=rospy.Publisher(WALK_TOPIC, Twist, queue_size=10)

def voiceCallback(data):
	p=data.data
	global FollowMarker

	if(p == "stop"):
		print "stop the NAO"
		msg=Twist(linear=Vector3(x=0.0, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0))
		body_pub.publish(msg)
		FollowMarker=False
	elif(p == "follow"):
		print "let the NAO follow"
		FollowMarker=True


def callback(data):
	p=data.pose.pose.position
	global onMyWay
	global closeDistance
	global stopDistance
	global deviationStep
	global FollowMarker
	
	yaw,pitch=-p.x, p.y
	#yaw,pitch=atan2(-p.x, p.z), atan2(p.y, p.z)
	#print("X:%s Y:%s Z:%s Yaw: %s Pitch: %s" % (p.x, p.y, p.z, yaw, pitch) )

	msg=JointAnglesWithSpeed(joint_names=("HeadYaw","HeadPitch"), joint_angles=(yaw,pitch), speed=0.45)
	head_pub.publish(msg)

	if not FollowMarker: return None
	#print "Follow in callback"

	# Body Motion
	# Linear vector m/s and Angular vector rad/s
	# Define maximum angular velocity
	max_angular=0.5
	# Observed that the yaw is between ~ -3.1 and 3.1
	# The aim her is to get the ratio (percentage) of the center of the marker wrt. position of the NAO.
	# So, the nearer the marker is detected on the borders of the NAO image, the faster the body will adapt his angular velocity 
	ratio=min(yaw,3.1)/3.1

	# If yaw is positive the NAO goes left otherwise right
	# To reflect this behavior, we check the sign of the head's yaw position and assign the same sign to the direction
	direction=max_angular*ratio if yaw > 0 else -1*max_angular*ratio

	#print("X:%s Y:%s Z:%s Yaw: %s Pitch: %s Ratio: %s" % (p.x, p.y, p.z, yaw, pitch, ratio) )


	left = memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
	right = memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
	#print("Left %s" % left)
	#print("Right %s" % right)
	#time.sleep(0.1)

	# No obstacles in the way
	if(deviationStep == 0):
		if(left < stopDistance and right < stopDistance):
			print("0: STOP!")
			onMyWay = False
			deviationStep = 1				
			#MarkerSubscriber.unregister()
			# At the moment the NAO is moving straight regardless it detects the marker or not.
			msg=Twist(linear=Vector3(x=0.0, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0))
			body_pub.publish(msg)
		elif(left > stopDistance and right > stopDistance):
			print("0: Forward!")
			#msg=Twist(linear=Vector3(x=0.9, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=direction))
			msg=Twist(linear=Vector3(x=1.0, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0))
			body_pub.publish(msg)
		elif(left < closeDistance and right > stopDistance):
			print("0: Left problem")
			msg=Twist(linear=Vector3(x=0.5, y=-1.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0))
			body_pub.publish(msg)
		elif(left > stopDistance and right < closeDistance):
			print("0: Right problem")
			msg=Twist(linear=Vector3(x=0.5, y=1.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0))
			body_pub.publish(msg)
		else:
			print("0: Forward!")
			FollowMarker=True
			#print("0: Weird")
	# Obstacle right in front of NAO
	elif(deviationStep == 1):
		while(right < stopDistance):
			left = memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
			right = memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
			msg=Twist(linear=Vector3(x=0.0, y=1.0, z=0.2), angular=Vector3(x=0.0, y=0.0, z=0.0))
			#print msg
			#print("Left %s" % left)
			#print("Right %s" % right)
			body_pub.publish(msg)
			print("1: Go left")
		deviationStep = 2

	# NAO clear of obstacle; pass obstacle
	elif(deviationStep == 2):
		while(left > stopDistance and right < closeDistance):
			left = memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
			right = memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
			msg=Twist(linear=Vector3(x=1.0, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0))
			#print("Left %s" % left)
			#print("Right %s" % right)
			body_pub.publish(msg)
			print("2: Move forward")
		deviationStep = 3

	# Obstacle passed; back to normal
	elif(deviationStep == 3):
		if(left > stopDistance and right > stopDistance):
			print("3: Back to normal")
			deviationStep = 0
			FollowMarker=True
			#MarkerSubscriber = rospy.Subscriber(MARKER_TOPIC, ARMarker, callback)
		#while(left > stopDistance and right > stopDistance):
		#	left = memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
		#	right = memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
		#	msg=Twist(linear=Vector3(x=1.0, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0))
		#	#print("Left %s" % left)
		#	#print("Right %s" % right)
		#	body_pub.publish(msg)
			
		

		onMyWay = True
		

def main():

	rospy.init_node('marker', anonymous=True)

	MarkerSubscriber = rospy.Subscriber(MARKER_TOPIC, ARMarker, callback)
	VoiceSubscriber = rospy.Subscriber(NAO_CONTROLLER_TOPIC, String, voiceCallback)
	
	motionProxy.setStiffnesses('Body', 1.0)

	rospy.spin()
	
	

if __name__ == '__main__':
	main()

speech.html

<!DOCTYPE html>
<html>
   <head>
      <meta charset="UTF-8">
      <meta name="viewport" content="width=device-width, initial-scale=1.0"/>
      <meta name="author" content="Aurelio De Rosa">
      <title>Web Speech API Demo by Aurelio De Rosa</title>
      <style>
         *
         {
            -webkit-box-sizing: border-box;
            -moz-box-sizing: border-box;
            box-sizing: border-box;
         }
         body
         {
            max-width: 500px;
            margin: 2em auto;
            padding: 0 0.5em;
            font-size: 20px;
         }
         h1
         {
            text-align: center;
         }
         .buttons-wrapper
         {
            text-align: center;
         }
         .api-support
         {
            display: block;
         }
         .hidden
         {
            display: none;
         }
         
         #transcription,
         #log
         {
            display: block;
            width: 100%;
            height: 5em;
            overflow-y: scroll;
            border: 1px solid #333333;
            line-height: 1.3em;
         }
         .button-demo
         {
            padding: 0.5em;
            display: inline-block;
            margin: 1em auto;
         }
         .author
         {
            display: block;
            margin-top: 1em;
         }
      </style>
      <script src="https://code.jquery.com/jquery-1.11.1.min.js"></script>
   </head>
   <body>
      <a href="http://www.sitepoint.com/introducing-web-speech-api/">Go back to the article</a>

      <h1>Web Speech API</h1>

      <span id="ws-unsupported" class="api-support hidden">API not supported</span>

      <h2>Transcription</h2>
      <textarea id="transcription" readonly="readonly"></textarea>

      <span>Results:</span>
      <label><input type="radio" name="recognition-type" value="final" checked="checked" /> Final only</label>
      <label><input type="radio" name="recognition-type" value="interim" /> Interim</label>

      <h3>Log</h3>
      <div id="log"></div>

      <div class="buttons-wrapper">
         <button id="button-play-ws" class="button-demo">Play demo</button>
         <button id="button-stop-ws" class="button-demo">Stop demo</button>
         <button id="clear-all" class="button-demo">Clear all</button>
      </div>

      <small class="author">
         Demo created by <a href="http://www.audero.it">Aurelio De Rosa</a>
         (<a href="https://twitter.com/AurelioDeRosa">@AurelioDeRosa</a>).<br />
         This demo is part of the <a href="https://github.com/AurelioDeRosa/HTML5-API-demos">HTML5 API demos repository</a>.
      </small>

      <script>
         // Test browser support
         window.SpeechRecognition = window.SpeechRecognition        ||
                                    window.webkitSpeechRecognition  ||
                                    null;
         if (window.SpeechRecognition === null) {
            document.getElementById('ws-unsupported').classList.remove('hidden');
            document.getElementById('button-play-ws').setAttribute('disabled', 'disabled');
            document.getElementById('button-stop-ws').setAttribute('disabled', 'disabled');
         } else {
            var recognizer = new window.SpeechRecognition();
            var transcription = document.getElementById('transcription');
            var log = document.getElementById('log');
            // Recogniser doesn't stop listening even if the user pauses
            recognizer.continuous = true;




            

            wordList = ["follow", "stop", "sit", "down", "stand", "up"];
            var checkKeyWord = function(speech){
               var contains = false;
               $.each(wordList, function(index, keyword){
                  if (speech.indexOf(keyword) > -1) { contains=true; return false; }
               });
               return contains;
            }

            
            // Start recognising
            recognizer.onresult = function(event) {
               transcription.textContent = '';
               for (var i = event.resultIndex; i < event.results.length; i++) {
                  if (event.results[i].isFinal) {
                     transcription.textContent = event.results[i][0].transcript + ' (Confidence: ' + event.results[i][0].confidence + ')';
                     if (checkKeyWord(transcription.textContent))
                        $.post("http://localhost:8080/", {"text": event.results[i][0].transcript});
                  } else {
                     transcription.textContent += event.results[i][0].transcript;
                  }
               }
            };








            // Listen for errors
            recognizer.onerror = function(event) {
               log.innerHTML = 'Recognition error: ' + event.message + '<br />' + log.innerHTML;
            };
            document.getElementById('button-play-ws').addEventListener('click', function() {
               // Set if we need interim results
               recognizer.interimResults = document.querySelector('input[name="recognition-type"][value="interim"]').checked;
               try{
                  recognizer.start();
                  log.innerHTML = 'Recognition started' + '<br />' + log.innerHTML;
               } catch(ex) {
                  log.innerHTML = 'Recognition error: ' + ex.message + '<br />' + log.innerHTML;
               }
            });
            document.getElementById('button-stop-ws').addEventListener('click', function() {
               recognizer.stop();
               log.innerHTML = 'Recognition stopped' + '<br />' + log.innerHTML;
            });
            document.getElementById('clear-all').addEventListener('click', function() {
               transcription.textContent = '';
               log.textContent = '';
            });
         }
      </script>
   </body>
</html>

speech.py

# -*- coding: utf-8 -*-
# Author : Sirres Raphael
# Date: 16.12.2014
# Description: Launch Server which serves a web page that performs the text recognition with the Web Speech API (Google Chrome).
#			   Recognized text is send to the server which on the other hand fowards the command to the NAO.
#				
# How to use:    0. (Install pip: easy_install pip)  Dont need this if pip is already installed  
#				 0. (Install bottle: pip install bottle)  
#				 1. Adapt the IP address in the main function. 
#				 2. Run the following command: python speech.py
# 			  	 3. Next, browse with Google Chrome (version > 25) to the following internet address: http://localhost:8080
# 			  	 4. Press the Play demo button and allow Google Chrome to use the Web Speech API
# 				 5. Say one of the following commands: "sit down", "stand up", "stop", "follow"
	

import os
from bottle import route, run, template, static_file, request, post

from posture import NaoPosture

import rospy
from std_msgs.msg import String


pos,nao_controller=None,None
root_folder = os.path.dirname(os.path.realpath(__file__))

STAND_UP = "stand up"
SIT_DOWN = "sit down"
FOLLOW = "follow"
STOP = "stop"


# Topic for un/subscribe the ar_marker form the NAO Controller Node
# Sidenote: The NAO Controller is in charge of tracking the marker
NAO_CONTROLLER_TOPIC="V_R"


def stand_up():
	print "Stand Up"
	pos.stand()

def sit_down():
	print "Sit down"
	pos.sit()

# We send the string "follow"/"stop" to the NAO_Controller node which (un)subscribe from/to the ar_pose node
def follow():
	print "follow"
	nao_controller.publish(FOLLOW)	

def stop():
	print "stop"
	nao_controller.publish(STOP)
	

mapping = {
	STAND_UP : stand_up,
	SIT_DOWN : sit_down,
	STOP : stop,
	FOLLOW : follow
}

# Relax the requirements of detecting a command.
# For instance, it is enough (in this case) to just hear "sit" or "stand" to perform the action
def normalize_string(keyword):
	keyword = keyword.strip()
	res = keyword
	if keyword in STAND_UP: res = STAND_UP
	if keyword in SIT_DOWN: res = SIT_DOWN
	if FOLLOW in keyword: res = FOLLOW
	if STOP in keyword: res = STOP
	return res

# This route receives the string recognized by the Web speech api
# and calls the corresponding function 
@post("/")
def speech():
	speech = request.forms.get("text")	
	norm_speech = normalize_string(speech)
	
	if norm_speech in mapping:
		mapping[norm_speech]()

# Serve static assets
@route("/")
@route('<filename>')
def server_static(filename="speech.html"):
    return static_file(filename, root=root_folder)


def main():
	#Adapt IP address
	global pos
	pos=NaoPosture("10.151.1.47")

	#Publisher sends the commands "stop" and "follow" in order to subscribe/unsubscribe from the ar_pose node
	#This way the robot should either ignore or follow the marker.
	global nao_controller
	nao_controller = rospy.Publisher(NAO_CONTROLLER_TOPIC, String, queue_size=10)
	rospy.init_node('Voice_Recognition_Node', anonymous=True)
	
	run(host='localhost', port=8080, reloader=False)

if __name__ == '__main__':
	main()