/home/fwph/code/wurde/modules/rFlexBase/rFlexServer.cpp

Go to the documentation of this file.

#include <CommsManager.H>
#include <RobotDrive.H>
#include <Egomotion.H>
#include <Power.H>
#include <RobotUtil.H>
#include <RangeFinder.H>
#include "RFlex.H"
#include <cmath>
#include <Logger.H>

using namespace WURDE;
using namespace std;

string parseRflexConfig(std::string config,std::string rflexName);

CommsManager myManager("TomRFlex");

void handler(int signum){
       myManager.setQuit();
}

int main(int argc, char *argv[]){

        myManager.setRealName("RFlexServer");
        myManager.setHelpString("\t-c: Specify a config file to use. Default is rflex-config.xml\n\n Note that the -n flag is used to select the configuration block to be used from the config file.\n\nThis module provides access to the RFlex controller on iRobot/RWI B21r and ATRV Jr. robots. It has not been tested with other iRobot/RWI robots.\n\n");
        myManager.parseOptions(argc,argv,"c:");
        Logger myLogger(myManager.getName());
        
        string port,confFile,robotType;
        bool unsafeBrake=false;
        unsigned char type=0;
        if(g_globalConfiguration.haveOption("c")){
                confFile=g_globalConfiguration.getOption("c");
                if(confFile[0]!='/'){
                        confFile=g_globalConfiguration.getConfigDirectory()+"/"+confFile;
                }
        }else{
                confFile=g_globalConfiguration.getConfigDirectory()+"/"+"rflex-config.xml";
        }
        
        g_logdebug << "Reading configuration for " << myManager.getName() << endl;
        port=parseRflexConfig(confFile,myManager.getName());

        if(g_globalConfiguration.getOption("rFlexUnsafeBrake")=="true"){
                unsafeBrake=true;
        }
        
        double trans=0, steer=0, tlimit=1.0, slimit=M_PI/2.0;
        
        if(g_globalConfiguration.haveOption("rFlexMaxTrans")){
                tlimit=strtod(g_globalConfiguration.getOption("rFlexMaxTrans").c_str(),NULL);
        }
        
        if(g_globalConfiguration.haveOption("rFlexMaxSteer")){
                slimit=strtod(g_globalConfiguration.getOption("rFlexMaxSteer").c_str(),NULL);
        }
        
        if(g_globalConfiguration.haveOption("rFlexType")){
                robotType=g_globalConfiguration.getOption("rFlexType");
                
        }else{
                robotType="B21r"; // default to B21; other possible value-- ATRVJr
        }
        
        RFlex rflex(port,robotType);
        
        rflex.setBrake();
        rflex.systemUpdate();
        rflex.motorSetVelocity(0,0);
        rflex.odometryOn();
        rflex.systemUpdate();
        
        RobotDrive myDrive(myManager.getName()+"Drive");
        Egomotion myEgomotion(myManager.getName()+"Motion");
        Power myPower(myManager.getName()+"Power");       
        RangeFinder mySonar(myManager.getName()+"Sonar");
        
        if(robotType=="B21r"){
                type=RFLEX_B21R;
        }else if(robotType=="ATRVJr"){
                type=RFLEX_ATRVJR;
        }
        
        Pose currentPose(0,0,0);
        bool brakestat=true;
        
        double baseX, baseY, baseT;
        //       char logbuf[1024];
        
        usleep(2000);
        
        baseX=rflex.x();
        baseY=rflex.y();
        baseT=rflex.heading();
        
        myManager.registerSupplier(&myLogger);
        myManager.registerSupplier(&myDrive);
        myManager.registerSupplier(&myEgomotion);
        myManager.registerSupplier(&myPower);
        if(type==RFLEX_ATRVJR){
                myManager.registerSupplier(&mySonar);
                rflex.sonarOn();
                g_logdebug << "Turning on sonar..." << endl;
                for(int i=0;i<rflex.getNumSonars();i++){
                        mySonar.data.ranges.push_back(0);
                }
                mySonar.info.uniform.setValueAndLock(false);
                mySonar.info.half_angle.setValueAndLock((7.5/180)*M_PI);
                mySonar.info.angle_min.setValueAndLock(0);
                
                vector<Pose3D> tmpPositions(rflex.getNumSonars());
                for(int i=0;i<rflex.getNumSonars();i++){
                        tmpPositions[i]=Pose3D(0,0,0.33);
                }
                
                mySonar.info.sensor_positions.setValueAndLock(tmpPositions);
                mySonar.publishInfo();
        }
        myManager.setSleep(0.02);
        
        /* these are more or less reasonable values. Not super
           safe, but we expect the obstacle avoider to pay more
           attention */
        myDrive.info.transLimit.setValue(tlimit);
        myDrive.info.angularLimit.setValue(slimit);
        
        /* we can set these values in Mage, but for now, we won't */
        myDrive.info.transAccel.setValue(-1);
        myDrive.info.angularAccel.setValue(-1);
        myDrive.info.transDecel.setValue(-1);
        myDrive.info.angularDecel.setValue(-1);
        
        myDrive.info.brake.setValue(true);
        
        /* finally, set the current velocities */
        myDrive.info.trans.setValue(0);
        myDrive.info.angular.setValue(0);
        
        myDrive.publishInfo();
        
        myEgomotion.data.location=currentPose;
        myEgomotion.data.transVelocity=0;
        myEgomotion.data.angularVelocity=0;
        myEgomotion.data.transAccel=0;
        myEgomotion.data.angularAccel=0;
        
        myEgomotion.publishData();
        //there's some question as to whether this interface is actualy useful
        // as is. we should probably have some sort of reasonable power percentage
        // measure, plus average drain rate, etc.
        myPower.data.volts=rflex.batteryVoltage();
        // guess values; we need to determine these all empirically like
        myPower.data.max_volts=25.0;
        myPower.data.min_volts=21.0;
        //this is arbitrary at present, but we can almost certainly determine it by the
        //voltage level-- the power level on the robot is at a distinct level when plugged in.
        myPower.data.ac_online=false;
        myPower.publishData();
        
        while(myManager.runUpdate()==STATE_RUN||
              myManager.getState()==STATE_IDLE){
                
                if(myManager.getState()==STATE_IDLE){
                        rflex.motorSetVelocity(0,0);
                        continue;
                }
                
                rflex.systemUpdate();
                
                if(myDrive.newRequest()){
                        //                   cout << "received request" << endl;
                        myDrive.getNextRequest();
                        tlimit=myDrive.requests.transLimit.getValue();
                        slimit=myDrive.requests.angularLimit.getValue();
                        trans=myDrive.requests.trans.getValue();
                        steer=myDrive.requests.angular.getValue();
                        
                        g_logdebug << "Received request: (trans,steer) " << trans << "," << steer << endl;
                        
                        if(myDrive.requests.brake.getValue()){
                                //                          cout << "Turning brake on.\n";
                                g_debug("Activating brake");
                                rflex.setBrake();
                                brakestat=true;
                        }else{
                                //                          cout << "Turning software brake off.\n";
                                g_debug("Turning off software brake. Hardware brake may still be active.");
                                //we don't really want to turn the brake off if it's 
                                // false, but we do want to turn the brake *on* if it's
                                // true.
                                
                                //UnCOMMENTING THIS LINE IS EXTREMELY DANGEROUS AND 
                                //NOT RECOMMENDED!
                                if(unsafeBrake){
                                        rflex.brakeOff();
                                }
                                brakestat=false;
                        }
                        
                        if(trans>tlimit)
                                trans=tlimit;
                        else if(trans<(-tlimit))
                                trans=(-tlimit);
                        
                        if(steer>slimit)
                                steer=slimit;
                        else if(steer<(-slimit))
                                steer=(-slimit);
                        
                }
                
                
                if(!brakestat){
                        rflex.motorSetVelocity(trans,steer);
                }else{
                        rflex.motorSetVelocity(0,0);
                }
                
                myDrive.info.trans.setValue(trans);
                myDrive.info.angular.setValue(steer);
                myDrive.info.transLimit.setValue(tlimit);
                myDrive.info.angularLimit.setValue(slimit);
                myDrive.info.brake.setValue(rflex.brakeOn());
                myDrive.publishInfo();
                
                currentPose.x(rflex.x());
                currentPose.y(rflex.y());
                currentPose.theta(rflex.heading());
                
                g_logdebug << "current pose " << rflex.x() << " " << rflex.y() << " " << rflex.heading() << endl;
                
                myEgomotion.data.location=currentPose;
                myEgomotion.data.transVelocity=rflex.translateVelocity();
                myEgomotion.data.angularVelocity=rflex.rotateVelocity();
                myEgomotion.data.transAccel=rflex.translateAcceleration();
                myEgomotion.data.angularAccel=rflex.rotateAcceleration();
                
                myEgomotion.publishData();
                
                if(type==RFLEX_ATRVJR){
                        for(int i=0;i<rflex.getNumSonars();i++){
                                mySonar.data.ranges[i]=rflex.getSonar(i);
                        }
                        
                        mySonar.publishData();
                }
                
                myPower.data.volts=rflex.batteryVoltage();
                myPower.publishData();
                
        }
        
        
        rflex.motorSetVelocity(0,0);
        
        if(type==RFLEX_ATRVJR){
                rflex.sonarOff();
        }
        
        myDrive.info.trans.setValue(0);
        myDrive.info.angular.setValue(0);
        myDrive.info.transLimit.setValue(tlimit);
        myDrive.info.angularLimit.setValue(slimit);
        myDrive.publishInfo();
        
        currentPose.x(0);
        currentPose.y(0);
        currentPose.theta(0);
        myEgomotion.data.location=currentPose;
        myEgomotion.data.transVelocity=0;
        myEgomotion.data.angularVelocity=0;
        myEgomotion.data.transAccel=0;
        myEgomotion.data.angularAccel=0;
        
        myEgomotion.publishData();
        g_debug("Shutting down rFlex cleanly.");
        myManager.runUpdate();
        
        myManager.cleanUp();    
        
        return 0;
}

---