C++ (Cpp) CPose2d Examples

Example #1

File: cbwallsensor.cpp Project: AutonomyLab/autolab-rapi

//-----------------------------------------------------------------------------
CCBWallSensor::CCBWallSensor(CCBDriver* driver, std::string devName)
 : ARangeFinder(devName)
{
  assert(driver);
  mCBDriver = driver;
  mNumSamples = 1;

  mRangeData = new tRangeData[mNumSamples];
  mRelativeBeamPose = new CPose2d[mNumSamples];

  // TODO: get a realistic value
  mFov = D2R( 20.0 );
  mRelativeBeamPose[0] = CPose2d(0.2, 0.2, D2R( -45.0) );
  mMinRange = 0.0;
  mMaxRange = 4.0;

  setEnabled ( true );
}

Example #2

File: localizer2d.cpp Project: magiclinux/autolab-rapi

//-----------------------------------------------------------------------------
void ALocalizer2d::setToZero()
{
  mPose = CPose2d( 0.0, 0.0, 0.0);
}

Example #3

File: chatterboxctrl.cpp Project: ashcharles/rapi-test

//-----------------------------------------------------------------------------
CChatterboxCtrl::CChatterboxCtrl ( ARobot* robot )
    : ARobotCtrl ( robot )
{
  CTask* task;
  ADrivetrain2dof* drivetrain;

  // Initialize robot
  mTime = 0.0;
  mState = START;
  mAvoidCount = 0;
  mElapsedTime = 0.0;
  mPuckLoad = 0;
  mFgPoseEstValid = true;
  mTrackerHeadingTime  = INFINITY;
  mTrackerPositionTime = INFINITY;
  mTravelTime = 0.0;
  mLoadingTime = 0.0;
  mUnloadingTime = 0.0;
  mStateName = StateNames[mState];

  // Get robot devices
  mRobot->findDevice ( mPowerPack, "CB:powerpack" );
  mRobot->findDevice ( drivetrain, "CB:drivetrain" );
  mRobot->findDevice ( mTextDisplay, "CB:textdisplay" );
  mRobot->findDevice ( mBumper, "CB:bumper" );
  mRobot->findDevice ( mButton, "CB:button" );
  mRobot->findDevice ( mLights, "CB:lights" );
  mRobot->findDevice ( mWheelDrop, "CB:wheeldrop" );
  mRobot->findDevice ( mLowSideDriver, "CB:lowsidedriver" );
  mRobot->findDevice ( mFrontFiducial, "CB:front_fiducial" );
  mRobot->findDevice ( mTopFiducial, "CB:top_fiducial" );
  mRobot->findDevice ( mPhoto, "CB:photosensor" );
  mRobot->findDevice ( mCliffSensor, "CB:cliff" );
  mRobot->findDevice ( mIr, "CB:ir" );
  mDrivetrain = ( CCBDrivetrain2dof* ) drivetrain;

  mTracker = new CAutolabTracker("Tracker", mRobot->getName(),
                                 "192.168.1.116", 6379);
  //mTracker = new CAutolabTracker("Tracker", "cb18",
  //                               "192.168.1.116", 6379);
  mRedisClient = CRedisClient::getInstance("192.168.1.116", 6379);

  mRobot->addDevice(mTracker);

  // Configure devices
  mVoltageLpf = mPowerPack->getVoltage();
  mLights->setBlink( DOT, true, 1.0 );
  mDrivetrain->setTranslationalAccelerationLimit( CLimit( -INFINITY, 0.5) );
  mDrivetrain->setRotationalAccelerationLimit( CLimit(-INFINITY, INFINITY) );
  mDrivetrain->setDefaultOIMode( CB_MODE_FULL );

  mOdometry = mDrivetrain->getOdometry();
  mDataLogger = CDataLogger::getInstance( "chatterbox.log", OVERWRITE );
  mDataLogger->setInterval( 0.1 );
  mOdometry->startLogging("");
  mTracker->startLogging("");
  mEstRobotPose.setName("EstRobotPose");
  mDataLogger->addVar(&mEstRobotPose, "EstRobotPose");
  mDrivetrain->setEnabled(true);

  //*********************************************************
  // Create Blue task
  task = new CTask("blue", BLUE, CPose2d(4.5, 2.5, 0.0),
                                 CPose2d(5.0, 9.2, 0.0) );

  task->mSourceWaypointVector.push_back(CPose2d(5.5, 5.0, 0.0));
  task->mSourceWaypointVector.push_back(CPose2d(5.5, 2.5, 0.0));
  task->mSourceWaypointVector.push_back(CPose2d(4.5, 2.5, 0.0)); // source


  task->mSinkWaypointVector.push_back(CPose2d(4.0, 5.0, 0.0));
  task->mSinkWaypointVector.push_back(CPose2d(4.0, 9.0, 0.0));
  task->mSinkWaypointVector.push_back(CPose2d(5.0, 9.2, 0.0));  // sink
  mTaskVector.push_back(task);

  //*********************************************************
  // Create Purple task
  task = new CTask( "purple", CRgbColor(255,0,255),
                                   CPose2d(1.2, 2.0, 0.0),
                                   CPose2d(2.0, 8.0, 0.0) );
  task->mSourceWaypointVector.push_back(CPose2d(3.0, 5.0, 0.0));
  task->mSourceWaypointVector.push_back(CPose2d(3.0, 2.0, 0.0));
  task->mSourceWaypointVector.push_back(CPose2d(1.2, 2.0, 0.0)); // source


  task->mSinkWaypointVector.push_back(CPose2d(1.0, 5.0, 0.0));
  task->mSinkWaypointVector.push_back(CPose2d(1.0, 8.0, 0.0));
  task->mSinkWaypointVector.push_back(CPose2d(2.0, 8.0, 0.0));  // sink
  mTaskVector.push_back(task);

  mCurrentTaskPtr = NULL;
  switchTask();

  rprintf("Power: ");  mPowerPack->print();
}

Example #4

File: localizer2d.cpp Project: magiclinux/autolab-rapi

//-----------------------------------------------------------------------------
ALocalizer2d::ALocalizer2d ( std::string devName )
    : ADevice ( devName )
{
	printf("localizer2d zhao\n");
  mCoordinateSystemOffset = CPose2d( 0.0, 0.0, 0.0 );
}