These examples use ALLandMarkDetection.
Detected landmarks and print some information about them.
# -*- encoding: UTF-8 -*-
# This test demonstrates how to use the ALLandMarkDetection module.
# Note that you might not have this module depending on your distribution
#
# /!\ Please change the IP address with your robot IP.
#
# - We first instantiate a proxy to the ALLandMarkDetection module
# Note that this module should be loaded on the robot's naoqi.
# The module output its results in ALMemory in a variable
# called "LandmarkDetected"
# - We then read this ALMemory value and check whether we get
# interesting things.
import time
from naoqi import ALProxy
IP = "nao.local" # Replace here with your NaoQi's IP address.
PORT = 9559
# Create a proxy to ALLandMarkDetection
try:
landMarkProxy = ALProxy("ALLandMarkDetection", IP, PORT)
except Exception, e:
print "Error when creating landmark detection proxy:"
print str(e)
exit(1)
# Subscribe to the ALLandMarkDetection proxy
# This means that the module will write in ALMemory with
# the given period below
period = 500
landMarkProxy.subscribe("Test_LandMark", period, 0.0 )
# ALMemory variable where the ALLandMarkdetection modules
# outputs its results
memValue = "LandmarkDetected"
# Create a proxy to ALMemory
try:
memoryProxy = ALProxy("ALMemory", IP, PORT)
except Exception, e:
print "Error when creating memory proxy:"
print str(e)
exit(1)
# A simple loop that reads the memValue and checks whether landmarks are detected.
for i in range(0, 20):
time.sleep(0.5)
val = memoryProxy.getData(memValue)
print ""
print "*****"
print ""
# Check whether we got a valid output.
if(val and isinstance(val, list) and len(val) >= 2):
# We detected naomarks !
# For each mark, we can read its shape info and ID.
# First Field = TimeStamp.
timeStamp = val[0]
# Second Field = array of Mark_Info's.
markInfoArray = val[1]
try:
# Browse the markInfoArray to get info on each detected mark.
for markInfo in markInfoArray:
# First Field = Shape info.
markShapeInfo = markInfo[0]
# Second Field = Extra info (ie, mark ID).
markExtraInfo = markInfo[1]
print "mark ID: %d" % (markExtraInfo[0])
print " alpha %.3f - beta %.3f" % (markShapeInfo[1], markShapeInfo[2])
print " width %.3f - height %.3f" % (markShapeInfo[3], markShapeInfo[4])
except Exception, e:
print "Naomarks detected, but it seems getData is invalid. ALValue ="
print val
print "Error msg %s" % (str(e))
else:
print "No landmark detected"
# Unsubscribe the module.
landMarkProxy.unsubscribe("Test_LandMark")
print "Test terminated successfully."
React to a landmark detection by subscribing to the right event in ALMemory.
# -*- encoding: UTF-8 -*-
##############################################################################
# Name: vision_onMarkDataChange.py
#
# Summary: Test the ALLandMarkDetection module using a callback mechanism.
#
# Usage: vision_onMarkDataChange.py [Naoqi IP address, local IP address]
# - Naoqi IP: address Naoqi is listening to.
# - local IP: address where you launch the script and that Naoqi can
# use to call you back.
#
# Description:
# - Create a Python broker using the provided Naoqi and local addresses.
# - Start the LandMarkDetection extractor by subscribing to it.
# - Create an ALModule object (markHandler) with its call back function.
# - Call ALMemory's subscribeOnDataChange so that markHandler.onMarkChange()
# is called whenever the detection results change.
# - Wait for some time. Msg is printed if we detect naomarks.
# - Unsubscribe from LandMarkDetection.
#
##############################################################################
# Used in debug logs.
testName = "python: vision_onMarkDataChange: "
import sys
import time
# IP and PORT default values.
LOCAL_IP = "127.0.0.1" # Replace here with your computer IP address.
LOCAL_PORT = 9999
IP = "nao.local" # Replace here with your NaoQi's IP address.
PORT = 9559
# Read IP address from first argument if any.
if len( sys.argv ) > 1:
IP = sys.argv[1]
if len( sys.argv ) > 2:
LOCAL_IP = sys.argv[2]
########################################################
from naoqi import ALProxy, ALModule, ALBroker
##############################################################################
# Definition of our python module.
class MarkHandlerModule(ALModule):
""" The main point here is to declare a module with a call back function
that is called by ALMemory whenever the landmark's results change. """
# Call back function registered with subscribeOnDataChange that handles
# changes in LandMarkDetection results.
def onMarkChange(self, dataName, value, msg):
""" Call back method called when naomark detection updates its results. """
print str(value)
if (len(value) != 0):
print "We detected naomarks !"
##############################################################################
testFailed = 0
# Create a python broker on the local machine.
broker = ALBroker("pythonBroker", LOCAL_IP, LOCAL_PORT, IP, PORT)
# Creating a proxy to ALLandMarkDetection
try:
landMarkProxy = ALProxy("ALLandMarkDetection", IP, PORT)
except Exception,e:
print "Error when creating ALLandMarkDetection proxy:"
print str(e)
exit(1)
# Subscribe to the ALLandMarkDetection module.
# This means that the module will write its results in memValue with
# the given period below.
subscriptionPeriod = 500
print "%s : Subscribe to the ALLandMarkDetection proxy..." % (testName)
try:
landMarkProxy.subscribe("Test_LandMark", subscriptionPeriod , 1.0 )
print "%s : Subscribe to the ALLandMarkDetection proxy... OK" % (testName)
except Exception, e:
print "%s Error :" % (testName)
print str(e)
testFailed = 1
# ALMemory variable where the ALLandMarkdetection module outputs its results.
memValue = "LandmarkDetected"
try:
markHandlerName = "markHandler"
# Create our module object.
markHandler = MarkHandlerModule(markHandlerName)
# Have module.onMarkChange called back when detection results change.
memoryProxy = ALProxy("ALMemory")
memoryProxy.subscribeToEvent(memValue, markHandlerName, "onMarkChange")
# Let the NaoMark detection run for a little while. You can check the results using
# a browser connected on your Nao, then Advanced -> Memory -> type mark in the field
time.sleep(20)
memoryProxy.unsubscribeToEvent(memValue, markHandlerName)
# Shut the Python Broker down
broker.shutdown()
except Exception, e:
print "%s Error:" %(testName)
print str(e)
testFailed = 1
if (testFailed == 1):
print "%s : Failed" % (testName)
exit(1)
print "%s : Success" % (testName)
Localize in robot space the detected landmark using transforms.
# -*- encoding: UTF-8 -*-
from naoqi import ALProxy
import math
import almath
# Set here your robto's ip.
ip = "127.0.0.1"
# Set here the size of the landmark in meters.
landmarkTheoreticalSize = 0.06 #in meters
# Set here the current camera ("CameraTop" or "CameraBottom").
currentCamera = "CameraTop"
memoryProxy = ALProxy("ALMemory", ip, 9559)
landmarkProxy = ALProxy("ALLandMarkDetection", ip, 9559)
# Subscribe to LandmarkDetected event from ALLandMarkDetection proxy.
landmarkProxy.subscribe("landmarkTest")
# Wait for a mark to be detected.
markData = memoryProxy.getData("LandmarkDetected")
while (len(markData) == 0):
markData = memoryProxy.getData("LandmarkDetected")
# Retrieve landmark center position in radians.
wzCamera = markData[1][0][0][1]
wyCamera = markData[1][0][0][2]
# Retrieve landmark angular size in radians.
angularSize = markData[1][0][0][3]
# Compute distance to landmark.
distanceFromCameraToLandmark = landmarkTheoreticalSize / ( 2 * math.tan( angularSize / 2))
motionProxy = ALProxy("ALMotion", ip, 9559)
# Get current camera position in NAO space.
transform = motionProxy.getTransform(currentCamera, 2, True)
transformList = almath.vectorFloat(transform)
robotToCamera = almath.Transform(transformList)
# Compute the rotation to point towards the landmark.
cameraToLandmarkRotationTransform = almath.Transform_from3DRotation(0, wyCamera, wzCamera)
# Compute the translation to reach the landmark.
cameraToLandmarkTranslationTransform = almath.Transform(distanceFromCameraToLandmark, 0, 0)
# Combine all transformations to get the landmark position in NAO space.
robotToLandmark = robotToCamera * cameraToLandmarkRotationTransform *cameraToLandmarkTranslationTransform
print "x " + str(robotToLandmark.r1_c4) + " (in meters)"
print "y " + str(robotToLandmark.r2_c4) + " (in meters)"
print "z " + str(robotToLandmark.r3_c4) + " (in meters)"
landmarkProxy.unsubscribe("landmarkTest")