#include "Shared/RobotInfo.h"
#ifdef TGT_HAS_GRIPPER

#include "Behaviors/StateMachine.h"

$nodeclass GraspTest1 : VisualRoutinesStateNode {

  $nodeclass FindCyl : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
    mapreq.addObjectColor(cylinderDataType, "red");
  }


  $nodeclass GetIt : GrasperNode(GrasperRequest::grasp) : doStart {
    NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(worldShS));
    if ( !cyls.empty() )
      graspreq.object = cyls[0];
    else {
      cout << "No valid cylinders in view!" << endl;
      cancelThisRequest();
    }
  }

  $setupmachine{
    FindCyl =C=> GetIt =C=> SpeechNode("done")
  }

}

REGISTER_BEHAVIOR(GraspTest1);

#endif