//	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 10bool 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. *********************************************************************/