robotskill.cpp

一个简单使用的控制器

//	if(d<= REACHED_DIST && turnAngle)
//		th=goal.th;



	// Deciding transvel and rotvel factor
	double transVelFactor, rotVelFactor;
	if (!_simulationMode) {
		//transVelFactor = 700.0;
		transVelFactor = 600.0;
		rotVelFactor = 200.0;
	}
	else {
		transVelFactor = 500.0;
		rotVelFactor = 100.0;
	}

	if (DEBUG_REACH_GOAL) {
		rlogDebug("REACH_GOAL: Robot x: %.2f z: %.2f th: %.2f \n", robot->getPosX(), robot->getPosZ(), robot->getPosTh());
		rlogDebug("REACH_GOAL: Goal x: %.2f z: %.2f  \n", goal.x, goal.z);
		rlogDebug("REACH_GOAL: Distance to goal: %.2f Angle to goal: %.2f\n", d, th);
	}

	done = (d <= REACHED_DIST);
/*	if(turnAngle){
		done =done &&  (fabs(AnglesNormAngle(robot->getPosTh()-goal.th)) <= 5.0);
        }*/
        tVel = 0;
	rVel=0;

	if(done)
	{
		if (DEBUG_REACH_GOAL) 
			rlogDebug("REACH_GOAL: Done.\n");
		robot->setVelocity(0,0);
		_rotCmdSpeed=0.0;
		return done;   
	}

	if (DEBUG_REACH_GOAL) 
		rlogDebug("REACH_GOAL: Not Done.\n"); 
       	
       /* if(d <= REACHED_DIST && turnAngle){
                 //transVel=0;
                 th=AnglesNormAngle(goal.th-robot->getPosTh());
        }*/

	// Controlling rotVel
	if (!_simulationMode) {
		double w_rot = fabs(StraightUp(fabs(th), 0, 45.0));
		double dir = 0;

		if(th != 0)
			dir = th / fabs(th);

		rVel = dir * w_rot * rotVelFactor;

		if (fabs(rVel) < 40 && fabs(rVel) >= 1)
			rVel=dir*40;
	}
	else {
		double w_rot = fabs(StraightUp(fabs(th), 0, 90.0));
		double dir = 0;

		if(th != 0)
			dir = th / fabs(th);

		rVel = dir * w_rot * rotVelFactor;
		if (fabs(rVel) < 40 && fabs(rVel) >= 1)
			rVel=dir*40;
        }

	// Controlling Tvel
	double w_r = fabs(StraightDown(fabs(th), 0, 20.0));
	double w_trans;
	if (!_simulationMode) 
		w_trans = StraightUp(fabs(d), 0.0, 1000.0);
	else
		w_trans = StraightUp(fabs(d), 0.0, 500.0);

	tVel = w_trans * w_r * transVelFactor; // controlling

	int transVel = (int)  tVel;
        int rotVel = (int) rVel;
	/*if(abs(transVel)<10 && rVel!=0 && fabs(rVel)<10)
                rotVel=(int)(rVel/rVel)*10;
        */
        _rotCmdSpeed=rotVel;
	if ( DEBUG_REACH_GOAL ) 
		rlogDebug("REACH_GOAL: Setting TransVel:  %d, RotVel: %d\n", transVel, rotVel);

	robot->setVelocity(transVel, rotVel);
	return done;
}


bool RobotSkill::robotInStall(int bumps)
{ 
	return false;  //FIXME
   
        struct timeval now=getCurrentTime();
	double timeDelta=TimevalDiffMS(&_lastInuTime,&now);

	if(fabs(timeDelta)<1000.0)
		return false;
	double InuSpeed=800.0*((robot->getPosTh()-_lastInu)/timeDelta);

	if(DEBUG_STALL){
		rlogDebug("_lastRotcmd %f inuspeed %f timeDelta %f\n",_lastRotCmdSpeed,InuSpeed,timeDelta);
		rlogDebug("_lastRotCmdSpeed -InuSpeed --> %f\n",fabs(_lastRotCmdSpeed -InuSpeed));
	}

	if(fabs(_lastRotCmdSpeed -InuSpeed)>= 80.0)
		_countStall++;
	else 
		_countStall-=2;

	if(fabs(_lastRotCmdSpeed -InuSpeed)>= 35.0)
		_countSmallStall++;
	else _countSmallStall-=30;
#define MAX_STALL_COUNT 6 
#define MAX_STALL_COUNT_SMALL 60 

	if(_countStall < 0) _countStall=0;
	if(_countSmallStall < 0) _countSmallStall=0;
	if(_countStall>MAX_STALL_COUNT ) _countStall=MAX_STALL_COUNT;
	if(_countSmallStall>MAX_STALL_COUNT_SMALL) _countSmallStall=MAX_STALL_COUNT_SMALL;

	_lastInu=robot->getPosTh();

	_lastInuTime=getCurrentTime();
	_lastRotCmdSpeed=_rotCmdSpeed;
	return _countStall+bumps*0.5+_countSmallStall*0.1>8;
}

int RobotSkill::getBestDir(Pos goal, VFHState* vfhState)
{ //Returns best direction based on VFH

	//Choose angle based on VFH  

	double th,k_targ= AnglesNormAngle(angleRobotToPoint(goal.x, goal.z));
	rescue_angle_pref_message msg;
	msg.target = (int)k_targ;
	int sec_th;
	int sectors=vfhState->getNumSectors();
	const int s_max = (int)round(0.9*sectors); //FIXME
	int* freeVal=vfhState->getFreeSpaceValues();
	bool found=false;
	float sectorSize=(360.0/(float)sectors);
	if(k_targ<0)
		k_targ+=360;

	int   goalSector=(int)(k_targ/sectorSize);

	int   bestDiff=sectors+1,bestSector=-1;
	int   free_threshold=0;
	int   valley_size=0,k_n=0,k_f=0;
	bool isValley=false;
	bool endValley=false;
	if(DEBUG_REACH_GOAL)
		rlogDebug("*******************VALLEYS START***********************\n");	
	for(int i= 0;i<sectors;i++){    	
		if(freeVal[i]<=free_threshold){
			valley_size++;
			if(!isValley){//Start Valley
				isValley=true; 
				k_n=i;
				if(DEBUG_REACH_GOAL)
					rlogDebug(" START: %d\n",k_n);
			}
			found=true;
			endValley=false;
		} 
		else{
			if(isValley){
				endValley=true;	

			} else 	endValley=false;
		}

		if(endValley ||(isValley && i==sectors-1)){
			// 		if(i==sectors-1)
			// 			i=sectors;

			isValley=false; 
			endValley=false;
			k_f=i-1;
			if(DEBUG_REACH_GOAL)
				rlogDebug(" END: %d: ",k_f);
			if(k_f-k_n>=s_max){ //wide valley
				sec_th=(2*k_n+s_max)/2;	
				if(DEBUG_REACH_GOAL) rlogDebug(" wide valley %d\n",k_f-k_n);
			}
			else {//Narrow valley
				sec_th=(k_n+k_f)/2;
				if(DEBUG_REACH_GOAL) rlogDebug("narrow valley %d\n",k_f-k_n);
			}

			if(goalSector>=k_n && goalSector<=k_f ){
				if(DEBUG_REACH_GOAL){
					rlogDebug("*****FOUND EXACT ANGLE!!!*********** %d < gs %d  < %d  \n",k_n,goalSector,k_f);
					rlogDebug("*******************VALLEY END***********************\n");
				}
				msg.choice=(int)AnglesNormAngle(k_targ);
				ComPublishToRobot(RESCUE_ANGLE_PREF_NAME, &msg);
				return (int)AnglesNormAngle(k_targ);
			}
			if( bestDiff>abs(goalSector-sec_th)){
				if(DEBUG_REACH_GOAL) rlogDebug("******NEW BEST SECTOR: %d -- goal sector %d -- new diff %d -- old diff %d -- old sec %d****************\n",sec_th,goalSector,abs(goalSector-sec_th), bestDiff, bestSector);
				bestSector=sec_th;
				bestDiff=abs(goalSector-sec_th);
				found=true;

			}
			valley_size=0;

		}

	}
	if(DEBUG_REACH_GOAL) rlogDebug("*******************VALLEY END***********************\n");



	if (!found){
		rlogError("ERROR: No free direction found: STUCK!!!\n");
		throw "stuck";
	}
	else{
		th=AnglesNormAngle(bestSector*sectorSize);
		rlogInfo(" Free sector %d at angle %f for sectorsize %f\n",bestSector,th,sectorSize);
		msg.choice=(int)th;
		ComPublishToRobot(RESCUE_ANGLE_PREF_NAME, &msg);
		return (int)th;
	}

}


bool RobotSkill::ActionBestAngleFreeFromStall(bool init, VFHState* vfhState)
{
	rlogNote("Meine Position ist: (%f,%f,%f)\n", 
			robot->getPosX(),
			robot->getPosZ(),
			robot->getPosTh());

	double distToReach = 150.0;
	if (init) {
		rlogDebug("ActionBestAngleFreeFromStall: Init\n");
		_beginPos.x = robot->getPosX();
		_beginPos.z = robot->getPosZ();
		_beginPos.th = robot->getPosTh();
	} 

	double angle;
	if (vfhState->getFrontClear()) {
		robot->setVelocity(100, 0);
		if (DEBUG_BEST_ANGLE_FREE_FROM_STALL)
			rlogDebug("ActionBestAngleFreeFromStall: GOING FORWARD!\n");
	} else if (vfhState->getRearClear()) {
		robot->setVelocity(-100, 0);
		if (DEBUG_BEST_ANGLE_FREE_FROM_STALL)
			rlogDebug("ActionBestAngleFreeFromStall: GOING BACK!\n");
	} else {
		angle = (double) vfhState->getAnglePreference();
		if (DEBUG_BEST_ANGLE_FREE_FROM_STALL)
			rlogDebug("ActionBestAngleFreeFromStall: GOING ANGLE %lf\n", angle);

		double fak = 0.1 + StraightUp(fabs(angle), 10,90);
		double rVel = 200 * fak;
		if(angle < 0)
			rVel = -rVel;

		robot->setVelocity(0, (int)rVel);
		if (DEBUG_BEST_ANGLE_FREE_FROM_STALL)
			rlogDebug("ActionBestAngleFreeFromStall: rVel: %lf\n", rVel);
	}

	double dist = hypot(robot->getPosX()-_beginPos.x, robot->getPosZ()-_beginPos.z);
	if (DEBUG_BEST_ANGLE_FREE_FROM_STALL)
		rlogDebug("ActionBestAngleFreeFromStall: Dist %lf, required dist %lf\n", dist, distToReach);
	if(dist >= distToReach*0.9){
		rlogDebug("ActionBestAngleFreeFromStall: Goal Reached\n");
		return true;
	}
	return false;
}

#define FFS_DELAY_FOR_RANDOM 5.0
#define FFS_RANDOM_PROB 0.5
#define FFS_TIMEOUT 10

bool RobotSkill::ActionRandomFreeFromStall(bool init, VFHState* vfhState) 
{
	(void)vfhState;
        (void)init;
	/*static struct timeval lastFreeFromStall = { 0, 0 };
	  bool choose_new_strategie = true;
	  static int timeout = 0;*/
	double tvel = 0.0, rvel = 0.0;

	/*if(init)
	  choose_new_strategie = true;

	  if(choose_new_strategie)
	  {
	//timeval curr = getCurrentTime();
	//if( TimevalDiff(&curr, &lastFreeFromStall) >
	//    FFS_DELAY_FOR_RANDOM && drand48() > FFS_RANDOM_PROB) //FIXME
	{
	*/    tvel = (drand48() > 0.5)? 900.0 : -900.0;
	rvel = (drand48() > 0.5)? 320.0 : -320.0;
	/*}
	  timeout = FFS_TIMEOUT;
	  lastFreeFromStall =  getCurrentTime();
	  choose_new_strategie = false;
	  }
	  */
	if (DEBUG_FREE_FROM_STALL)
		rlogDebug("FFS: FreeFromStall random action (tvel,rvel) = (%1.2f,%1.2f)\n",tvel,rvel);

	robot->setVelocity((int)tvel, (int)rvel);
	/*
	   timeout--;
	   if(timeout < 0)
	   choose_new_strategie = true;
	   */
	return false;
}


void RobotSkill::updateVictimRFID(bool ackState)
{
	_victimAck = ackState;
}


bool RobotSkill::VictimRFIDWaitRequired()
{
	return !_victimAck; 
}


void RobotSkill::setVictimWait() {
	_victimAck = false;
}


/*********************************************************************
 * (C) Copyright 2006 Albert Ludwigs University Freiburg
 *     Institute of Computer Science
 *
 * All rights reserved. Use of this software is permitted for
 * non-commercial research purposes, and it may be copied only
 * for that use. All copies must include this copyright message.
 * This software is made available AS IS, and neither the authors
 * nor the Albert Ludwigs University Freiburg make any warranty
 * about the software or its performance.
 *********************************************************************/