// This code fragment finds dominoes that lie about 600 mm in front of the robot.
#include "Behaviors/StateMachine.h"
$nodeclass Dominoes {

  $nodeclass FindDominoes : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
    mapreq.addObjectColor(dominoDataType,"red");
    mapreq.addSecondColor(dominoDataType,"blue");
  }

  $nodeclass ReportDominoes : SpeechNode : doStart {
    NEW_SHAPEVEC(dominoes, DominoData, select_type<DominoData>(worldShS));
    SHAPEVEC_ITERATE(dominoes, DominoData, d) {
        textstream << "I see a domino with " << d->getLowValue() << " and " << d->getHighValue() << " dots!" << endl;
    } END_ITERATE
  }

  $setupmachine {
      FindDominoes =MAP=> ReportDominoes
  }
}

REGISTER_BEHAVIOR(Dominoes);