src/controllers/positionControllers/defaultWheelController.cpp

#include <controllers/positionControllers/defaultWheelController.h>

#include <controller/almControllers/sensorFusionComponent/associativeLearning/qLearning.h>

using namespace almendeSensorFusion;

using namespace srAlmende;

DefaultWheelController::DefaultWheelController(const std::string &name,
                srCore::RobotActorBase *robotActor): SubController(name, robotActor) {
        goalSpeedL = 0;                                         // Init goal speed to 0
        goalSpeedR = 0;
}

DefaultWheelController::~DefaultWheelController() {

}

CtrlMsg* DefaultWheelController::Act(CtrlMsg *message) {
        LOG_WARNING("Not yet implemented!");
        outgoingMsg.type = INVALID;
        return &outgoingMsg;
}

bool DefaultWheelController::decreaseSpeedSlowly() {
        // Decrease slowly
        LOG_WARNING("Not correctly implemented!");
        float lVel = 0; //todo getActuator("LeftScrew")->getVel(getDeltaSimTime());
        float rVel = 0; //todo getActuator("RightScrew")->getVel(getDeltaSimTime());
        float decreaseLevel = 1;
        if(lVel != 0 && rVel != 0)
        {
                if(lVel >= decreaseLevel) lVel -= decreaseLevel;
                else if(lVel <= -decreaseLevel) lVel += decreaseLevel;
                else if(lVel < decreaseLevel && lVel > -decreaseLevel) lVel = 0;

                if(rVel >= decreaseLevel) rVel -= decreaseLevel;
                else if(rVel <= -decreaseLevel) rVel += decreaseLevel;
                else if(rVel < decreaseLevel && rVel > -decreaseLevel) rVel = 0;

                //todo getActuator("LeftScrew")->setVel(lVel, getDeltaSimTime());
                //todo getActuator("RightScrew")->setVel(rVel, getDeltaSimTime());
                return false;
        }
        //      else if(resetRobotFlag && ((time(NULL) - resetTimer) < 3))
        //              return false;
        //      else if(resetRobotFlag)
        //      {
        //              resetRobot();
        //              return false;
        //      }
        //      else
        //              return true;
        return false;
}

void DefaultWheelController::wander() {
        goalSpeedL = QLearning::MAX_SPEED;
        goalSpeedR = goalSpeedL;
}

#ifdef MAKE_IT_WORK_IN_THIS_CONTEXT

/*
 * This function controls the robots to center and drive to a specified location
 * This location can be based on the estimated sound source location or the
 * location of the salient object in the image.
 * This function also calls the Qlearning function for weight updating
 * (Driving is for testing the saliency and sensor fusion modules only)
 */
void DefaultWheelController::driveToSalientLocation(float soundLocation, float imageLocation)
{
        // drive with speedL, speedR to object
        int speedL = QLearning::MAX_SPEED;
        int speedR = QLearning::MAX_SPEED;

        int classifyDistance  = 100;
        // Stop infront of an object
        unsigned long sensorLeft;
        getSensor("Front Left DistanceSensor")->evaluate(MAKE_PARAMETER_VALUE(&sensorLeft));

        unsigned long sensorRight;
        getSensor("Front Right DistanceSensor")->evaluate(MAKE_PARAMETER_VALUE(&sensorRight));

        //distanceSensor returns a 24bit value. we only need the highest 8bit
        sensorLeft = sensorLeft >> 16;
        sensorRight = sensorRight >> 16;
        bool objectIsGoal = true;

        // Convert the location from degrees to states
        int state = (float(imageLocation)+60.0)*(float(qLearn.NR_STATES)/120.0);
        int soundState = (float(soundLocation)+60.0)*(float(qLearn.NR_STATES)/120.0);

        // To overcome shaky behavior stay in the goal state with buffer of 5 degrees
        if((imageLocation > -5 && imageLocation < 5 ) ||
                        (imageLocation > -20 && imageLocation < 20  && cameraMotor.lastState == cameraMotor.goalState))
                state = cameraMotor.goalState;

        osg::Vec3 rot = getRobotRotation();

        if(imageLocation == DefaultWheelController::NO_LOCATION)
                fusionInput.image = NULL;

        // TODO: use the soundLocation when there is no salient image,
        // or take both values in consideration and stop turning when both values are aligned
        // If a lot of mismatches occur consider re-calibrating binaural-localization

        // something is close take whole picture
        if((sensorLeft < classifyDistance) && (sensorRight < classifyDistance))
        {
                fusionInput.imgRegionX = 0;
                fusionInput.imgRegionY = 0;
                fusionInput.imgRegionWidth  = fusionInput.imageWidth-1;
                fusionInput.imgRegionHeight = fusionInput.imageHeight-1;
        }
        int energyValue = -1;
        // possible states, maybe this needs to be self-organized associate the state with it
        // Both modalities are fixed on the same object
        if(soundLocation != NO_LOCATION && imageLocation != NO_LOCATION && state == soundState && useFusion)
        {
                energyValue = classifyObject(true);
        }
        // both modalities are fixed on something else
        else if(soundLocation != NO_LOCATION && imageLocation != NO_LOCATION)
        {
                // first check whether the image is of an interesting object
                float * tmpAudio = fusionInput.audio;
                fusionInput.audio = NULL;
                energyValue = classifyObject(true);
                fusionInput.audio = tmpAudio;
                if(energyValue != -1)
                {
                        if(AVFusion.gotKeypoints() == -1)
                        {
                                imageLocation = NO_LOCATION;
                                state = -3;
                                energyValue = 0;
                        }
                        else if(!useAttention)
                                state = AVFusion.gotKeypoints();
                }

                if(energyValue == 1 && robotEnergy == 1)
                        objectIsGoal = false;
                else if(energyValue == 2 && robotEnergy == 2)
                        objectIsGoal = false;
                else if(energyValue == 0 && robotEnergy == 1)
                        objectIsGoal = false;
                else if(energyValue == 0 && robotEnergy == 2)
                        objectIsGoal = false;
                else if(energyValue == -1)
                        objectIsGoal = false;

                if(!objectIsGoal)
                {
                        // then check whether the sound is of an interesting object
                        unsigned char** tmpImage =  fusionInput.image;
                        fusionInput.image = NULL;
                        int energyValue2 = classifyObject(true);
                        fusionInput.image = tmpImage;
                        objectIsGoal = true;
                        if(energyValue2 == 1 && robotEnergy == 1)
                                objectIsGoal = false;
                        else if(energyValue2 == 2 && robotEnergy == 2)
                                objectIsGoal = false;
                        else if(energyValue2 == 0 && robotEnergy == 1)
                                objectIsGoal = false;
                        else if(energyValue == 0 && robotEnergy == 2)
                                objectIsGoal = false;
                        else if(energyValue2 == -1)
                                objectIsGoal = false;

                        // or sound has found the goal use sound as state
                        // or sound has not found the goal but a unknown sound
                        // if the image and the sound are unknown, choose the one infront
                        if(objectIsGoal)
                        {
                                energyValue = energyValue2;
                                state = soundState;
                        }
                        else if(energyValue2 == -1 && energyValue != -1)
                        {
                                energyValue = energyValue2;
                                state = soundState;
                        }
                        else if(energyValue2 == -1 && energyValue == -1)
                        {
                                if(soundState == cameraMotor.goalState)
                                {
                                        energyValue = energyValue2;
                                        state = soundState;
                                }
                                else
                                {
                                        if(AVFusion.gotKeypoints() == -1 && (sensorLeft < classifyDistance) && (sensorRight < classifyDistance))
                                        {
                                                imageLocation = NO_LOCATION;
                                                state = -3;
                                                energyValue = 0;
                                        }
                                }
                        }
                        else
                        {
                                if(AVFusion.gotKeypoints() == -1 && (sensorLeft < classifyDistance) && (sensorRight < classifyDistance))
                                {
                                        imageLocation = NO_LOCATION;
                                        state = -3;
                                        energyValue = 0;
                                }
                        }
                }
        }
        else if(soundLocation != NO_LOCATION || imageLocation != NO_LOCATION)
        {
                energyValue = classifyObject(true);
                if(imageLocation == NO_LOCATION)
                        state = soundState;
                if((!useAttention || soundLocation == NO_LOCATION) && energyValue != -1)
                {
                        if(AVFusion.gotKeypoints() == -1 && (sensorLeft < classifyDistance) && (sensorRight < classifyDistance))
                        {
                                imageLocation = NO_LOCATION;
                                state = -3;
                                energyValue = 0;
                        }
                        else if(!useAttention)
                                state = AVFusion.gotKeypoints();
                }
        }

        if(soundLocation != NO_LOCATION || imageLocation != NO_LOCATION)
                if(robotEnergy == 1)
                        cout << "My energy is low: ";
                else
                        cout << "My energy is " << powerTime-(time(NULL)-energyTimer) <<  ": ";

        if(energyValue == 1 && robotEnergy == 1)
                objectIsGoal = false;
        else if(energyValue == 2 && robotEnergy == 2)
                objectIsGoal = false;
        else if(energyValue == 0 && robotEnergy == 1)
                objectIsGoal = false;
        else if(energyValue == 0 && robotEnergy == 2)
                objectIsGoal = false;
        else if(energyValue == 2 && robotEnergy == 1)
        {
                objectIsGoal = true;
                cout << "Energy source found" << endl;
        }
        else if(energyValue == 1 && robotEnergy == 2)
        {
                objectIsGoal = true;
                cout << "Empty device found" << endl;
        }
        else if(soundLocation != NO_LOCATION || imageLocation != NO_LOCATION)
        {
                objectIsGoal = true;
                cout << "Unknown device found" << endl;
        }
        else
                objectIsGoal = false;

        if(!objectIsGoal)
        {
                if(soundLocation != NO_LOCATION || imageLocation != NO_LOCATION)
                {
                        if(robotEnergy == 2)
                                std::cout << "Ignore object: doesn't need energy" << std::endl;
                        if(robotEnergy == 1)
                                std::cout << "Ignore object, has no energy" << std::endl;
                        soundLocation = NO_LOCATION;
                        imageLocation = NO_LOCATION;
                }
        }

        if(soundDrive)
        {
                state = soundState;
                imageLocation = soundLocation;
        }


        // if robot is on it's side then reset
        osg::Vec3 pos = getRobotPosition();
        if(rot[2] <= -90)
        {
                //      resetRobot();
                resetRobotFlag = true;
                goalSpeedL = 0;
                goalSpeedR = 0;
                return;
        }

        if(imageLocation != NO_LOCATION || soundLocation != NO_LOCATION)
        {
                std::cout << "state: " << state << std::endl;
                // If there is a salient object in the image find the associated
                // motor value with the location in the image
                if(false)
                {
                        // no time to learn or the moment
                        speedL = cameraMotor.IOStateMap[state].maxX;
                        speedR = cameraMotor.IOStateMap[state].maxY;
                }
                else
                {
                        float lVel = getActuator("LeftScrew")->getVel(getDeltaSimTime());
                        float rVel = getActuator("RightScrew")->getVel(getDeltaSimTime());
                        float accelValue = 0.4;
                        if(state==0)
                        {
                                lVel = -fabs(lVel);
                                rVel = -fabs(rVel);
                                lVel -= accelValue;
                                rVel += accelValue;
                                if (rVel >= 0)  rVel = -accelValue;
                                if (rVel < -QLearning::MAX_SPEED)   rVel =  -QLearning::MAX_SPEED;
                                if (lVel >= 0)  lVel = -0.1;
                                if (lVel < -QLearning::MAX_SPEED)   lVel =  -QLearning::MAX_SPEED;

                                rVel = -accelValue;
                                lVel = -1;
                        }
                        //rotate right
                        else
                        {
                                lVel = fabs(lVel);
                                rVel = fabs(rVel);
                                lVel += accelValue;
                                rVel -= accelValue;
                                if (rVel > QLearning::MAX_SPEED)        rVel = QLearning::MAX_SPEED;
                                if (rVel < 0)                                           rVel =  0;
                                if (lVel > QLearning::MAX_SPEED)        lVel = QLearning::MAX_SPEED;
                                if (lVel < 0)                                           lVel = 0;
                                rVel = 0;
                                lVel = 1;
                        }
                        speedL = lVel;
                        speedR = rVel;
                }

                // If the salient object is centered, drive to it
                if(state == cameraMotor.goalState)
                {
                        //printf("Driving to Object\n");
                        speedL = QLearning::MAX_SPEED;
                        speedR = QLearning::MAX_SPEED;
                }
                // Turn towards the salient object
                else
                {
                        //printf("Object Found\n");
                        if(false)
                        {
                                speedL -= QLearning::MAX_SPEED;
                                speedR -= QLearning::MAX_SPEED;
                        }
                }
                // Set the goal speed.
                // The goal velocity will be obtained by increasing the current
                // motor value on every tick
                goalSpeedL = speedL;
                goalSpeedR = speedR;
                trackTimer = time(NULL);
        }

        // stop if your in front of the object
        //cout << "left: " << sensorLeft << " right: " << sensorRight << endl;
        if ((sensorLeft < classifyDistance) && (sensorRight < classifyDistance) && state == cameraMotor.goalState &&  objectIsGoal)
        {
                goalSpeedL = 0;
                goalSpeedR = 0;

                // TODO: check if sound location works well enough to set a fixed state
                // maybe let this self-organize by associating state
                if(soundState != cameraMotor.goalState || soundLocation == NO_LOCATION)
                {
                        fusionInput.audio = NULL;
                        if(AVFusion.gotKeypoints() == -1)
                                return;
                }

                // takes a set to synchronise sound and energy levels
                if(time(NULL) - otherRobotEnergyTimer > 1)
                {
                        // test if the search result that put us in this goal state
                        // was right
                        if(energyValue == 1 && energyAtPos(false) == 2 && robotEnergy == 2)
                                experiment.setGaveNoEnergy();
                        if(energyValue == 2 && energyAtPos(false) == 1 && robotEnergy == 1)
                                experiment.setGotNoEnergy();
                        classifyObject(false);
                }
        }
}


void DefaultWheelController::obstacleAvoidance()
{
        float lVel = getActuator("LeftScrew")->getVel(getDeltaSimTime());
        float rVel = getActuator("RightScrew")->getVel(getDeltaSimTime());

        unsigned long sensorLeft;
        getSensor("Front Left DistanceSensor")->evaluate(MAKE_PARAMETER_VALUE(&sensorLeft));

        unsigned long sensorRight;
        getSensor("Front Right DistanceSensor")->evaluate(MAKE_PARAMETER_VALUE(&sensorRight));

        unsigned long sensorFrontRight;
        getSensor("Right Front DistanceSensor")->evaluate(MAKE_PARAMETER_VALUE(&sensorFrontRight));

        unsigned long sensorFrontLeft;
        getSensor("Left Front DistanceSensor")->evaluate(MAKE_PARAMETER_VALUE(&sensorFrontLeft));

        //distanceSensor returns a 24bit value. we only need the highest 8bit
        sensorLeft = sensorLeft >> 16;
        sensorRight = sensorRight >> 16;
        sensorFrontLeft = sensorFrontLeft >> 16;
        sensorFrontRight = sensorFrontRight >> 16;

        float accelValue = 0.2;
        float distanceValue = 100;
        float maxSpeed = 2;
        //go forward
        if ((sensorLeft > distanceValue) && (sensorRight > distanceValue) && sensorFrontLeft > 50  && sensorFrontRight > 50)
        {
                lVel = goalSpeedL;
                rVel = goalSpeedR;
        }
        //rotate left
        else if ((sensorLeft >= sensorRight) && sensorFrontLeft >= 95)
        {
                lVel = -fabs(lVel);
                rVel = -fabs(rVel);
                lVel -= accelValue;
                rVel += accelValue;
                if (rVel >= 0)  rVel = -accelValue;
                if (rVel < -maxSpeed)   rVel =  -maxSpeed;
                if (lVel >= 0)  lVel = -0.1;
                if (lVel < -maxSpeed)   lVel =  -maxSpeed;
        }
        //rotate right
        else if (sensorFrontRight >= 95)
        {
                lVel = fabs(lVel);
                rVel = fabs(rVel);
                lVel += accelValue;
                rVel -= accelValue;
                if (rVel > maxSpeed)    rVel = maxSpeed;
                if (rVel < 0)                                           rVel =  0;
                if (lVel > maxSpeed)    lVel = maxSpeed;
                if (lVel < 0)                                           lVel = 0;
        }
        else
        {
                srand( time(NULL) );
                cout << "random turn" << endl;
                if(rand()%2 == 0)
                {
                        lVel = -qLearn.MAX_SPEED;
                        rVel = -0.1;
                }
                else
                {
                        lVel = qLearn.MAX_SPEED;
                        rVel = 0;
                }
        }
        goalSpeedL = lVel;
        goalSpeedR = rVel;
}


/*
 * With a form of Q-learning motor values are learned to centre an object
 */
void DefaultWheelController::learnToDriveFunction(float soundLocation, float imageLocation)
{
        // drive with speedL, speedR to object
        int speedL = QLearning::MAX_SPEED;
        int speedR = QLearning::MAX_SPEED;

        // Convert the location from degrees to states
        int state = (float(imageLocation)+60.0)*(float(qLearn.NR_STATES)/120.0);



        osg::Vec3 rot = getRobotRotation();
        cout << " state: " << state << " ";

        // If the goal state was found before then reset the robot to learn different state
        if(cameraMotor.lastState == cameraMotor.goalState && state == cameraMotor.goalState)
        {
                cout << " Still at the goal state reset" << endl;
                //resetRobot();
                resetRobotFlag = true;
                goalSpeedL = 0;
                goalSpeedR = 0;
                return;
        }

        // If salient object is near the goal state, consider is as the goalstate
        // TODO: do not use hardcoded values, this is relative to the scale value of the image
        // print salient region and boundaries
        if(imageLocation > (cameraMotor.goalValue-5) && imageLocation < (cameraMotor.goalValue+5) )
                state = cameraMotor.goalState;

        // if the robot is on its side than don't mind the views
        if(rot[2] < -90)
                imageLocation = NO_LOCATION;

        // If there is an salient object in the image learn if the previous motor command
        // did put the salient object closer to the goal state.
        if(imageLocation != NO_LOCATION)
        {
                qLearn.learn(&cameraMotor, state, imageLocation, &speedL, &speedR);
                if(state == cameraMotor.goalState)
                {
                        goalSpeedL = 0;
                        goalSpeedR = 0;
                        cout << "Goal found, speed to 0 speedL:" << speedL << " speedR:" << speedR << endl;
                }
                else
                {
                        goalSpeedL = speedL-QLearning::MAX_SPEED;
                        goalSpeedR = speedR-QLearning::MAX_SPEED;
                        cout << "Use next speed values L:" << goalSpeedL << " R:" << goalSpeedR << endl;
                }
        }
        // Its the start or it has resulted in bad behavior decrease last value and reset position
        else
        {
                qLearn.learn(&cameraMotor, -((QLearning::NR_STATES/2)+1), imageLocation, &speedL, &speedR);
                cout << "no image location reset" << endl;
                //resetRobot();
                resetRobotFlag = true;
                goalSpeedL = 0;
                goalSpeedR = 0;
                return;
        }
}

void DetectRobotVisually::trackGoal()
{
        // if the time that the last object was found is less then wait time
        // then keep the current velocity
        int waitTime = 2;
        if((time(NULL) - trackTimer) < waitTime)
        {
                goalSpeedL = getActuator("LeftScrew")->getVel(getDeltaSimTime());
                goalSpeedR = getActuator("RightScrew")->getVel(getDeltaSimTime());
        }
        // turn to look for something
        //      if((time(NULL) - trackTimer) > 15 && (time(NULL) - trackTimer) < 17)
        //      {
        //              goalSpeedL = qLearn.MAX_SPEED;
        //              goalSpeedR = 0;
        //              cout << "Random Turn" << endl;
        //      }
}


#endif

---