public void set(FramePose framePose) { set(framePose, true); }
public void set(YoFramePose yoFramePose) { set(yoFramePose.getPosition().getFrameTuple(), yoFramePose.getOrientation().getFrameOrientation()); }
@Override public void doControl() { if (fiducialDetectorBehaviorService.getGoalHasBeenLocated()) { fiducialDetectorBehaviorService.getReportedGoalPoseWorldFrame(foundFiducialPose); foundFiducialYoFramePose.set(foundFiducialPose); foundFiducial.set(true); Point3d position = new Point3d(); foundFiducialPose.getPosition(position); sendTextToSpeechPacket("Target object located at " + position); } else { sendTextToSpeechPacket("Target object not located"); } if (headTrajectorySentTimer.totalElapsed() < 2.0) { return; } pitchHeadToFindFiducial(); // pitchHeadToCenterFiducial(); }