Landmark detection

<< return to Python examples

These examples use ALLandMarkDetection.

Detect landmarks

Detected landmarks and print some information about them.

vision_landMarkDetection.py

# -*- 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."

Callback on landmark detection

React to a landmark detection by subscribing to the right event in ALMemory.

vision_onMarkDataChange.py

# -*- 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)

Landmark localization

Localize in robot space the detected landmark using transforms.

vision_localization.py

# -*- 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 (markData is None or 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")