//-----------------------------------------------------------------------------
void CCBRobot::synchronize( double interval )
{
double timeNow;
double duration;
double sleepDuration = 0.0;
struct timeval tv;
gettimeofday( &tv, 0 );
timeNow = tv.tv_sec + tv.tv_usec * 1e-6;
duration = timeNow - mLastSynchronizeTime;
if ( duration < interval ) {
sleepDuration = interval - duration;
mSlowRunCount = (mSlowRunCount > 0 ) ? mSlowRunCount - 1 : 0;
usleep(( int )(( sleepDuration ) * 1e6 ) );
mLastLoopDuration = interval;
}
else {
mSlowRunCount = (mSlowRunCount < mSlowRunThreshold) ? mSlowRunCount + 1 :
mSlowRunCount;
if( mSlowRunCount >= mSlowRunThreshold ) {
mSlowRunCount = 0;
mFgRunningSlowly = true;
PRT_WARN1( "Control loop running consistently slowly (%f sec)\n", mLastLoopDuration );
}
PRT_MSG1( 8, "Control loop ran slowly (%f sec)\n", mLastLoopDuration );
// the first time we often get outragously large values
// which cause bad things to happen, so we limit this value
// to the interval [0,5]
mLastLoopDuration = limit(duration, 0.0, 5.0);
}
//gettimeofday( &tv, 0 );
//mLastSynchronizeTime = tv.tv_sec + tv.tv_usec * 1e-6;
mLastSynchronizeTime = timeNow + sleepDuration;
//printf("timeNow %f\n", timeNow);
}
//-----------------------------------------------------------------------------
CCharger::CCharger ( Stg::Model* mod ) : CDestination ( mod )
{
if ( ! mStgModel->GetPropertyFloat ( "chargerate", &mChargeRate, 0.0 ) )
PRT_WARN1 ( "chargerate not specified for sink %s\n",
mStgModel->Token() );
}
//-----------------------------------------------------------------------------
void CChatterboxCtrl::updateData ( float dt )
{
float distance;
mTime += dt;
mElapsedTime += dt;
mVoltageLpf = mVoltageLpf + ( dt / TAU_VOLTAGE_LPF )
* ( mPowerPack->getVoltage() - mVoltageLpf );
// update position estimate
estimateRobotPose();
//mDrivetrain->setEnabled(false);
if (mWheelDrop->isAnyTriggered() ) {
mState = FLYING;
}
switch (mState) {
case START:
if ( mDrivetrain->getOIMode() != CB_MODE_FULL ) {
mDrivetrain->setDefaultOIMode ( CB_MODE_FULL );
}
if (mTracker->isValid() ) {
mOdometry->setPose(mTracker->getPose() );
mEstRobotPose = mTracker->getPose();
}
if (mElapsedTime > 5.0 ) {
if (mPuckLoad == 0)
mState = GOTO_SOURCE;
else
mState = GOTO_SINK;
}
break;
case GOTO_SOURCE:
if (mFgStateChanged)
mWaypointId = 0;
if (mVoltageLpf < CHARGING_VOLTAGE) {
mState = GOTO_CHARGER;
}
//if ( driveTo(mCurrentTaskPtr->getSourcePose(), distance ) == COMPLETED ) {
if ( actionFollowWaypointVector(&(mCurrentTaskPtr->mSourceWaypointVector) ) == COMPLETED) {
mTravelTime += mElapsedTime;
mState = LOADING;
}
else if (mCurrentTaskPtr->getSourcePose().distance(mEstRobotPose) < SOURCE_PROXIMITY) {
mState = WAIT_AT_SOURCE;
}
break;
case WAIT_AT_SOURCE:
if ( (mElapsedTime > MIN_SOURCE_WAIT_TIME) &&
(drand48() < GIVE_UP_PROBABILITY) ) {
switchTask();
}
if ( driveTo(mCurrentTaskPtr->getSourcePose(), 0.2, distance ) == COMPLETED ) {
mTravelTime += mElapsedTime;
mState = LOADING;
}
break;
case GOTO_SINK:
if (mFgStateChanged)
mWaypointId = 0;
//if ( driveTo(mCurrentTaskPtr->getSourcePose(), distance ) == COMPLETED ) {
if ( actionFollowWaypointVector(&(mCurrentTaskPtr->mSinkWaypointVector) ) == COMPLETED) {
//if ( driveTo(mCurrentTaskPtr->getSinkPose(), distance ) == COMPLETED ) {
mTravelTime += mElapsedTime;
mState = UNLOADING;
}
break;
case GOTO_CHARGER:
if (actionGotoCharger() == COMPLETED)
mState = DOCKING;
break;
case DOCKING:
if (actionDocking() == COMPLETED)
break;
case UNDOCKING:
if (actionUndocking() == COMPLETED)
break;
case CHARGING:
if (actionCharging() == COMPLETED)
break;
case LOADING:
if (actionLoading() == COMPLETED)
mState = GOTO_SINK;
break;
case UNLOADING:
if (actionUnloading() == COMPLETED) {
mState = GOTO_SOURCE;
}
break;
case FLYING:
mDrivetrain->stop();
if (not mWheelDrop->isAnyTriggered() ) {
mState = START;
}
break;
default:
PRT_WARN1("Unknown FSM state %d ", mState);
mState = START;
break;
} // switch
if (mTracker->isValid() ) {
mLights->setLight(0, GREEN);
mLights->setLight(3, GREEN);
}
else {
mLights->setLight(0, RED);
mLights->setLight(3, RED);
}
mDataLogger->write( mTime );
if ( mPrevState != mState) {
mPrevState = mState;
mElapsedTime = 0;
mFgStateChanged = true;
rprintf("state %s, task %s\n", StateNames[mState].c_str(),
mCurrentTaskPtr->getName().c_str());
mPowerPack->print();
}
else {
mFgStateChanged = false;
}
}
//-----------------------------------------------------------------------------
extern "C" int Init ( Stg::Model* mod )
{
std::string logFileName = "fasr_analyst.log";
std::string value;
float probBroadcast = 1.0;
char* policy = new char[40];
static bool init = false;
Rapi::CLooseStageRobot* robot;
CTaskManager* taskManager;
CAnalyst* analyst;
ABaseRobotCtrl* robotCtrl;
static CCharger charger ( mod->GetWorld()->GetModel ( "charger" ) );
static CDestination depotDestination ( mod->GetWorld()->GetModel ( "depot" ) );
if ( not init ) {
init = true;
//mod->AddShutdownCallback ( ( Stg::stg_model_callback_t ) shutDownCallback, NULL );
printf ( "-----------------------------------\n" );
printf ( "FASR \n" );
printf ( " build %s %s \n", __DATE__, __TIME__ );
printf ( " compiled against RAPI version %s (%s) build %s\n", RAPI_VERSION(),
RAPI_GIT_VERSION(), RAPI_BUILD() );
printf ( "\n" );
//************************************
// init general stuff
ErrorInit ( 2, false );
initRandomNumberGenerator();
}
//************************************
// create robot and its controller
robot = new Rapi::CLooseStageRobot ( mod );
if ( ! mod->GetPropertyStr ( "policy", &policy, NULL ) ) {
PRT_WARN0 ( "No policy specified " );
exit ( -1 );
}
else {
if ( strcmp ( policy, "fixed" ) == 0 )
robotCtrl = new CFixedRatioPolicyRobotCtrl ( robot );
else if ( strcmp ( policy, "replan" ) == 0 )
robotCtrl = new CReplanPolicyRobotCtrl ( robot );
else if ( strcmp ( policy, "static" ) == 0 )
robotCtrl = new CStaticPolicyRobotCtrl ( robot );
else if ( strcmp ( policy, "loop" ) == 0 )
robotCtrl = new CLoopPolicyRobotCtrl ( robot );
else if ( strcmp ( policy, "epsilon" ) == 0 )
robotCtrl = new CEpsilonSamplerRobotCtrl ( robot );
else if ( strcmp ( policy, "wait" ) == 0 ) {
for ( unsigned int i=0; i< Stg::World::args.size(); i++ ) {
if ( Stg::World::args[i].compare ( 0, 3, "-pb" ) == 0 ) {
value = Stg::World::args[i].substr ( 4, Stg::World::args[i].size()-4 );
probBroadcast = atof ( value.c_str() );
}
}
printf ( "running with prob broadcast %f \n", probBroadcast );
robotCtrl = new CWaitProbPolicyRobotCtrl ( robot, probBroadcast );
}
else {
PRT_WARN1 ( "Unknown policy: %s", policy );
exit ( -1 );
}
}
//************************************
// determine name of log file
for ( unsigned int i=0; i< Stg::World::args.size(); i++ ) {
if ( Stg::World::args[i].compare ( 0, 3, "-lf" ) == 0 ) {
logFileName = Stg::World::args[i].substr ( 4, Stg::World::args[i].size()-4 );
}
}
//************************************
// get tasks from the task manager and hand them to the robot
taskManager = CTaskManager::getInstance (
mod->GetWorld()->GetModel ( "taskmanager" ) );
analyst = CAnalyst::getInstance ( mod->GetWorld()->GetModel ( "taskmanager" ), logFileName );
taskManager->registerRobot ( robotCtrl );
analyst->registerRobot ( robotCtrl );
robotCtrl->addCharger ( &charger );
robotCtrl->setDepot ( &depotDestination );
#ifdef GUI
gui = CFasrGui::getInstance ( 0, NULL );
gui->registerRobot ( robot );
#endif
return 0; // ok
}
