C++ ArACTS_1_2::getBlob方法代码示例

int main(void)
{
  ArACTSBlob blob;
  double xRel, yRel;
  int i;

  acts.openPort(NULL);
  acts.requestPacket();
  while(1)
  {
    if (acts.receiveBlobInfo())
      {
	//acts.receiveBlobInfo();
	for (i = 0; i < 8; i++)
	  if (acts.getNumBlobs(i) >= 1 && acts.getBlob(i, 1, &blob))
	    {
	      xRel = (double)(blob.getXCG() - WIDTH/2.0) / (double)WIDTH;
	      yRel = (double)(blob.getYCG() - HEIGHT/2.0) / (double)HEIGHT;
	      printf("Chan %d xRel %.4f yRel %.4f    ", i, xRel, yRel);
	      blob.log();
	    }
	acts.requestPacket();
	ArUtil::sleep(80);
      }
    ArUtil::sleep(5);
  }

}

ArActionDesired *DropOff::fire(ArActionDesired currentDesired)
{
  ArPose pose;
  ArACTSBlob blob;
  bool blobSeen = false;
  double xRel, yRel;
  double dist;


  myDesired.reset();
  if (myState == STATE_SUCCEEDED)
  {
    printf("DropOff: Succeeded\n");
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
  else if (myState == STATE_FAILED)
  {
    printf("DropOff: Failed\n");
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }

  if (myActs->getNumBlobs(myChannel) > 0 &&
      (blobSeen = myActs->getBlob(myChannel, 1, &blob)))
  {
    myLastSeen.setToNow();
  }

  // this if the stuff we want to do if we're not going to just drive forward
  // and home in on the color, ie the pickup-specific stuff
  if (myState == STATE_START_LOOKING)
  {
    mySentGripper.setToNow();
    myPointedDown = false;
    myState = STATE_LOOKING;
    myLastSeen.setToNow();
    myTried = false;
    myLastMoved.setToNow();
    myLastPose = myRobot->getPose();
    //myWaitingOnGripper = true;
    myWaitingOnLower = false;
    myLowered = false;
    myWaitingOnRaised = false;
    myStartRaised = false;
    printf("@@@@@ DropOff: Starting\n");
  }

  // we want to sit still until the lift is down or for a second and a half
  /*
  if (myWaitingOnGripper) 
  {
    if (mySentGripper.mSecSince() < 500 || 
	(myGripper->getGripState() != 1
	 && mySentGripper.mSecSince() < 4000))
    {
      myDesired.setVel(0);
      myDesired.setDeltaHeading(0);
      myLastMoved.setToNow();
      myLastPose = myRobot->getPose();
      return &myDesired;
    }
    else
    {
      myWaitingOnGripper = false;
    }
    myLastMoved.setToNow();
    myLastPose = myRobot->getPose();
  } 
  */
  //printf("sensors %d %d stall %d %d\n",!(myRobot->getDigIn() & ArUtil::BIT2),
  //!(myRobot->getDigIn() & ArUtil::BIT3),
  //myRobot->isLeftMotorStalled(), myRobot->isRightMotorStalled());
  if ((myRobot->isLeftBreakBeamTriggered() &&
       myRobot->isRightBreakBeamTriggered()) ||
       myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
  {
    if (!myWaitingOnLower && !myLowered && !myWaitingOnRaised)
    {
      myWaitingOnLower = true;
      printf("DropOff: Lowering gripper\n");
      mySentGripper.setToNow();
    }
  }
  if (myWaitingOnLower)
  {
    /// TODO
    if (mySentGripper.mSecSince() < 600 || 
	(!myGripper->isLiftMaxed() && mySentGripper.mSecSince() < 20000))
    {
      myGripper->liftDown();
      myDesired.setVel(0);
      myDesired.setDeltaHeading(0);
      myLastMoved.setToNow();
      myLastPose = myRobot->getPose();
      return &myDesired;
    }
    else
//.........这里部分代码省略.........

ArActionDesired *DriveTo::fire(ArActionDesired currentDesired)
{
  ArACTSBlob blob;
  double xRel, yRel;

  if (myState == STATE_SUCCEEDED || myState == STATE_FAILED)
  {
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }

  if (myState == STATE_START_LOOKING)
  {
    myState = STATE_LOOKING;
    myLastSeen.setToNow();
  }

  if (myActs->getNumBlobs(myChannel) == 0 || 
      !myActs->getBlob(myChannel, 1, &blob))
  {
    if (myLastSeen.mSecSince() > 1000)
    {
      printf("DriveTo:  Lost the blob, failed.\n");
      myState = STATE_FAILED;
      myDesired.setVel(0);
      myDesired.setDeltaHeading(0);
      return &myDesired;
    }
  }
  else
  {
    myLastSeen.setToNow();
  }

  xRel = (double)(blob.getXCG() - WIDTH/2.0) / (double)WIDTH;
  yRel = (double)(blob.getYCG() - HEIGHT/2.0) / (double)HEIGHT;
  
  //printf("xRel %.3f yRel %.3f\n", xRel, yRel);

  myDesired.reset();
  // this if the stuff we want to do if we're not going to just drive forward
  // and home in on the color, ie the pickup-specific stuff
  if (currentDesired.getMaxVelStrength() > 0 &&
      currentDesired.getMaxVel() < 125)
  {
    printf("DriveTo:  Close to a wall of some sort, succeeded.\n");
    myState = STATE_SUCCEEDED;
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
  
  if (ArMath::fabs(xRel) < .10)
  {
    //printf("Going straight ahead\n");
    myDesired.setDeltaHeading(0);
  }
  else
  {
    //printf("Turning %.2f\n", -xRel * 10);
    myDesired.setDeltaHeading(-xRel * 10);
  }
  myDesired.setVel(300);
  return &myDesired;
}

//.........这里部分代码省略.........
      printf("PickUp: Trying to pick up.\n");
      myTried = true;
      myTriedStart.setToNow();
    }
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
  // this means that the grippers are closed, but we don't have anything in 
  // them, ie that we failed to get the block
  else if (myTried && (myGripper->getGripState() == 2 || 
		       myTriedStart.mSecSince() > 5000))
  {
    myState = STATE_FAILED;
    myGripper->gripOpen();
    ArUtil::sleep(3);
    myGripper->liftUp();
    printf("PickUp: Grippers closed, didn't get a block, failed.\n");
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
     
  pose = myRobot->getPose();
  dist = myLastPose.findDistanceTo(pose);
  if (dist < 5 && myLastMoved.mSecSince() > 1500)
  {
    printf("PickUp: Failed, no movement in the last 1500 msec.\n");
    myState = STATE_FAILED;
    myGripper->gripOpen();
    myGripper->liftUp();
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
  else if (dist > 5)
  {
    myLastMoved.setToNow();
    myLastPose = myRobot->getPose();
  }

  if (myActs->getNumBlobs(myChannel) == 0 || 
      !(blobSeen = myActs->getBlob(myChannel, 1, &blob)))
  {
    if (((!myPointedDown && myLastSeen.mSecSince() > 1500) ||
	 (myPointedDown && myLastSeen.mSecSince() > 4000)) &&
	myGripper->getBreakBeamState() == 0)
    {
      printf("PickUp:  Lost the blob, failed, last saw it %ld msec ago.\n",
	     myLastSeen.mSecSince());
      myState = STATE_FAILED;
      myGripper->gripOpen();
      ArUtil::sleep(3);
      myGripper->liftUp();
      myDesired.setVel(0);
      myDesired.setDeltaHeading(0);
      return &myDesired;
    }
  } 
  else
    myLastSeen.setToNow();
  

  if (blobSeen)
  {
    xRel = (double)(blob.getXCG() - WIDTH/2.0) / (double)WIDTH;
    yRel = (double)(blob.getYCG() - HEIGHT/2.0) / (double)HEIGHT;
    //printf("xRel %.3f yRel %.3f:\n", xRel, yRel);
  } 
  else
  {
    //printf("No blob: ");
  }

  if (blobSeen && yRel < 0.2 && !myPointedDown)
  {
    printf("PickUp: Pointing the camera down!!!\n");
    mySony->panTilt(0, -ArSonyPTZ::MAX_TILT);
    myPointedDown = true;
  }
  
  
  if (!blobSeen || ArMath::fabs(xRel) < .10)
  {
    //printf("Going straight ahead\n");
    myDesired.setDeltaHeading(0);
  }
  else
  {
    //printf("Turning %.2f\n", -xRel * 10);
    if (ArMath::fabs(-xRel * 10) <= 10)
      myDesired.setDeltaHeading(-xRel * 10);
    else if (-xRel > 0)
      myDesired.setDeltaHeading(10);
    else
      myDesired.setDeltaHeading(-10);
  }
  myDesired.setVel(150);
  return &myDesired;
}