#include "Behaviors/StateMachine.h"

$nodeclass Amap {

	$nodeclass Look : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
		mapreq.setAprilTagFamily();
		mapreq.clearWorld = true;
	}

	$nodeclass Report : doStart {
		Shape<AprilTagData> tag1 = AprilTagData::findTag(worldShS, 1);
		//Tag Quaternion in camera reference frame
		if ( tag1.isValid() ) {
			AngTwoPi orientation = tag1->getQuaternion().ypr()[0];
			cout << "**********Printing the measurements of the tag in camera frame:" << endl;
			cout << "Tag " << tag1->getId() << " id=" << tag1->getTagID()
					 << " orientation=" << orientation*180/M_PI
					 << endl;
		}
	}	

  $setupmachine{
  	look: Look =C=> Report =TM=> look
  }

}

REGISTER_BEHAVIOR(Amap);