Lab: The Grasper

From Tekkotsu Wiki

Jump to: navigation, search

Demo Code

Run this demo in the GraspableCylinder.mirage world. You will need to tilt the camera down so the robot can see the red cylinder.

#include "Behaviors/StateMachine.h"

$nodeclass GraspDemo1 : VisualRoutinesStateNode {

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

  $nodeclass GrabIt : GrasperNode(GrasperRequest::moveTo) : doStart {
    NEW_SHAPE(dest, PointData, new PointData(worldShS, Point(0, 500, 0, allocentric)));
    graspreq.targetLocation = dest;
    graspreq.object = find_if<CylinderData>(worldShS);
    if ( ! graspreq.object.isValid() )
      cancelThisRequest();
  }

  $setupmachine{
    startnode: FindCylinder =C=> grab

    grab: GrabIt
    grab =GRASP(noError)=> SpeechNode("Got it")
    grab =GRASP=> SpeechNode("Error during grasp")
    grab =F=> SpeechNode("Couldn't find a cylinder") =C=> startnode
  }
}

REGISTER_BEHAVIOR(GraspDemo1);