//-----------------------------------------------------------------------------
int CCBRobot::init()
{
if ( mFgInitialized ) {
PRT_WARN0( "Robot already initialized" );
return 1;
}
if ( mCBDriver->init() == 0 ) {
return 0; // failure
}
if ( mCBDriver->openPort( "/dev/ttyS2" ) == 0 ) {
ERROR0( "Failed to open create port: /dev/ttyS2" );
return 0; // failure
}
mFgInitialized = true;
return 1; // success
}
//-----------------------------------------------------------------------------
tActionResult CChatterboxCtrl::actionUnloading()
{
if (mElapsedTime > 2.0) {
mUnloadingTime = mElapsedTime;
return COMPLETED;
}
mDrivetrain->stop();
mLights->setLight( ALL_LIGHTS, BLACK );
if (mPuckLoad > 0) {
if (system("madplay r2d2.mp3") == -1)
PRT_WARN0("Sound error ");
mPuckLoad = 0;
}
return IN_PROGRESS;
}
//-----------------------------------------------------------------------------
int CCBRobot::findDevice( ABinarySensorArray* &device, std::string devName )
{
if ( not mFgInitialized ) {
PRT_WARN0( "Robot is not initialized, call init() first" );
device = NULL;
return 0; // error
}
if (( devName != CB_DEVICE_BUMPER ) &&
( devName != CB_DEVICE_VIRTUAL_WALL ) &&
( devName != CB_DEVICE_CLIFF ) &&
( devName != CB_DEVICE_OVERCURRENT ) &&
( devName != CB_DEVICE_BUTTON ) &&
( devName != CB_DEVICE_WHEEL_DROP ) ) {
ERROR1( "No such device: %s", devName.c_str() );
device = NULL;
return 0; // error
}
//************************************
// Bumper
if ( devName == CB_DEVICE_BUMPER ) {
// check if device already exists
if ( mCBBumper == NULL ) {
mCBBumper = new CCBBumper( mCBDriver, CB_DEVICE_BUMPER );
device = mCBBumper;
return mCBBumper->init();
}
// return already existing device
device = mCBBumper;
return 1;
}
//************************************
// Virtual wall
if ( devName == CB_DEVICE_VIRTUAL_WALL ) {
// check if device already exists
if ( mCBVirtualWall == NULL ) {
mCBVirtualWall = new CCBVirtualWallSensor( mCBDriver, CB_DEVICE_VIRTUAL_WALL );
device = mCBVirtualWall;
return mCBVirtualWall->init();
}
// return already existing device
device = mCBVirtualWall;
return 1;
}
//************************************
// Button
if ( devName == CB_DEVICE_BUTTON ) {
// check if device already exists
if ( mCBCreateButton == NULL ) {
mCBCreateButton = new CCBCreateButton( mCBDriver, CB_DEVICE_BUTTON );
device = mCBCreateButton;
return mCBCreateButton->init();
}
// return already existing device
device = mCBCreateButton;
return 1;
}
//************************************
// Wheel drop sensor
if ( devName == CB_DEVICE_WHEEL_DROP ) {
// check if device already exists
if ( mCBWheelDropSensor == NULL ) {
mCBWheelDropSensor =
new CCBWheelDropSensor( mCBDriver, CB_DEVICE_WHEEL_DROP );
device = mCBWheelDropSensor;
return mCBWheelDropSensor->init();
}
// return already existing device
device = mCBWheelDropSensor;
return 1;
}
//************************************
// Cliff sensor
if ( devName == CB_DEVICE_CLIFF ) {
// check if device already exists
if ( mCBCliffSensor == NULL ) {
mCBCliffSensor = new CCBCliffSensor( mCBDriver, CB_DEVICE_CLIFF );
device = mCBCliffSensor;
return mCBCliffSensor->init();
}
// return already existing device
device = mCBCliffSensor;
return 1;
}
//************************************
// Overcurrent sensor
if ( devName == CB_DEVICE_OVERCURRENT ) {
// check if device already exists
if ( mCBOverCurrentSensor == NULL ) {
mCBOverCurrentSensor = new CCBOverCurrentSensor( mCBDriver, CB_DEVICE_OVERCURRENT );
device = mCBOverCurrentSensor;
return mCBOverCurrentSensor->init();
}
// return already existing device
device = mCBOverCurrentSensor;
return 1;
}
return 0; // should never get here
}
//-----------------------------------------------------------------------------
int CCBRobot::findDevice( ARangeFinder* &device, std::string devName )
{
if ( not mFgInitialized ) {
PRT_WARN0( "Robot is not initialized, call init() first" );
device = NULL;
return 0; // error
}
if (( devName != CB_DEVICE_LASER ) &&
( devName != CB_DEVICE_WALL ) &&
( devName != CB_DEVICE_IR ) ) {
ERROR1( "No such device: %s", devName.c_str() );
device = NULL;
return 0; // error
}
//************************************
// Laser
if ( devName == CB_DEVICE_LASER ) {
// check if device already exists
if ( mCBLaser == NULL ) {
mCBLaser = new CCBLaser( mCBDriver, CB_DEVICE_LASER );
device = mCBLaser;
return mCBLaser->init();
}
// return already existing device
device = mCBLaser;
return 1; // success
}
//************************************
// IR sensor
if ( devName == CB_DEVICE_IR ) {
// check if device already exists
if ( mCBIrSensor == NULL ) {
mCBIrSensor = new CCBIrSensor( mCBDriver, CB_DEVICE_IR );
device = mCBIrSensor;
return mCBIrSensor->init();
}
// return already existing device
device = mCBIrSensor;
return 1; // success
}
//************************************
// Wall sensor
if ( devName == CB_DEVICE_WALL ) {
// check if device already exists
if ( mCBWallSensor == NULL ) {
mCBWallSensor = new CCBWallSensor( mCBDriver, CB_DEVICE_WALL );
device = mCBWallSensor;
return mCBWallSensor->init();
}
// return already existing device
device = mCBWallSensor;
return 1; // success
}
return 0; // should not be able to reach this, but silences compiler
}
//-----------------------------------------------------------------------------
void CCBRobot::run()
{
if ( not mFgInitialized ) {
PRT_WARN0( "Robot is not initialized, call init() first" );
return;
}
while ( mFgRunning ) {
// get data from ICreate
//printf("mLastLoopDuration %f \n",mLastLoopDuration);
if ( mCBDriver->readSensorData( mLastLoopDuration ) == 1 ) {
// update all devices
if ( mCBDrivetrain )
mCBDrivetrain->updateData( mUpdateInterval );
if ( mCBPowerPack )
mCBPowerPack->updateData( mLastLoopDuration );
if ( mCBLaser )
mCBLaser->updateData( mLastLoopDuration );
if ( mCBLights )
mCBLights->updateData( mLastLoopDuration );
if ( mCBSound )
mCBSound->updateData( mLastLoopDuration );
if ( mCBIrSensor )
mCBIrSensor->updateData( mLastLoopDuration );
if ( mCBTextDisplay )
mCBTextDisplay->updateData( mLastLoopDuration );
if ( mCBBumper )
mCBBumper->updateData( mLastLoopDuration );
if ( mCBWallSensor )
mCBWallSensor->updateData( mLastLoopDuration );
if ( mCBWheelDropSensor )
mCBWheelDropSensor->updateData( mLastLoopDuration );
if ( mCBCliffSensor )
mCBCliffSensor->updateData( mLastLoopDuration );
if ( mCBOverCurrentSensor )
mCBOverCurrentSensor->updateData( mLastLoopDuration );
if ( mCBFrontFiducial )
mCBFrontFiducial->updateData( mLastLoopDuration );
if ( mCBTopFiducial )
mCBTopFiducial->updateData( mLastLoopDuration );
if ( mCBPhotoSensor )
mCBPhotoSensor->updateData( mLastLoopDuration );
if ( mCBCreateButton )
mCBCreateButton->updateData( mLastLoopDuration );
if ( mCBVirtualWall )
mCBVirtualWall->updateData( mLastLoopDuration );
updateDevices();
// Low side drivers updateData() is empty, no need to call it here
//if ( mCBLowSideDriver )
// mCBLowSideDriver->updateData();
}
// update all registered controllers
updateControllers();
//******************************************************
// last step - keep everything in sync
synchronize( mUpdateInterval );
} // while
}
//-----------------------------------------------------------------------------
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
}
