apfpathplan.cpp

路径规划源程序

//	double C,kk;
////	RobotWidth2=0.45;
	double anglespeed;
	double LineVBasic;//0.5
	LineVBasic=DecideCurrentLinearVParm(uSpeedLevel123_High);
	int DisBetweenTarA2SelfA=int(m_iTargetAngle-m_iSelfOrientation);
		
	anglespeed=DecideRealAngularV(nCornerSpeedCoefficient,bControlBall,DisBetweenTarA2SelfA);	
	if(m_iSelfOrientation==270)
	{
		LineVBasic=-LineVBasic;
	}
	VelocityMotionModel(nlineSpeedCoefficient*LineVBasic,anglespeed,m_dSpeedCalcuPar);

	m_dLineVPre=nlineSpeedCoefficient*LineVBasic;
}
float ApfPathPlan::DecideRealAngularV(float nCornerSpeedCoefficient,bool bControlBall,int DisBetweenTarA2SelfA)
{
	float anglespeed;
	if ((DisBetweenTarA2SelfA<0&&DisBetweenTarA2SelfA>-180)||DisBetweenTarA2SelfA>180)//目标处于自己当前朝向的角度的右边
	{
		anglespeed=nCornerSpeedCoefficient;	
	}
	else
	{
		anglespeed=-nCornerSpeedCoefficient;
	}
	if (!bControlBall)//不控球
	{
		if (anglespeed>m_dMaxAngleSpeedNoBall) 
		{
			anglespeed=m_dMaxAngleSpeedNoBall;
		}
	}
	else//控球
	{
		if (anglespeed>m_dMaxAngleSpeedWithBall) 
		{
			anglespeed=m_dMaxAngleSpeedWithBall;
		}
		else if (anglespeed<-m_dMaxAngleSpeedWithBall)
		{
			anglespeed=-m_dMaxAngleSpeedWithBall;
		}
	}
	return anglespeed;
}
void ApfPathPlan::SaveFoundCandidateAngle(int* pSaveCandidateArray,int& FoundCandAngleNum,int freeNum,int GapBeginAngle,int CurrentAngle,int AddPos_DecNeg)
{
	if (freeNum>MAX_SAFE_ANGLE)//空地的角跨度很大，自由路径的角度越大自由路径应该越宽，freeNumAdd应该越大
	{
		*(pSaveCandidateArray)=CurrentAngle-AddPos_DecNeg*3;//间隙的右边界处
		*(pSaveCandidateArray+1)=GapBeginAngle+AddPos_DecNeg*3;//间隙的左边界处	
		CheckAngleLimitedBeyond(*(pSaveCandidateArray),0);
		CheckAngleLimitedBeyond(*(pSaveCandidateArray+1),0);
		FoundCandAngleNum=2;
		//break;
	}
	else// if (freeNum>2)
	{
		*(pSaveCandidateArray)=CurrentAngle-freeNum/2;
		CheckAngleLimitedBeyond(*(pSaveCandidateArray),0);
		FoundCandAngleNum=1;
	}
}
inline void ApfPathPlan::OutputCandidateAngleFinded(int freeNum, int GapBeginAngle,int CurrentAngle,int AddPos_DecNeg)
{
	//int m_iNumOfTargetAngleAdd;	
	if (AddPos_DecNeg>0)//+方向输出
	{
		/*int BorderAnlgeTmp=GapBeginAngle-1;
		CheckAngleLimitedBeyond(BorderAnlgeTmp,0);*/
		SaveFoundCandidateAngle(TargetAngleAdd,m_iNumOfTargetAngleAdd,freeNum,GapBeginAngle,CurrentAngle,AddPos_DecNeg);
	}
	else
	{
		SaveFoundCandidateAngle(TargetAngleDec,m_iNumOfTargetAngleDec,freeNum,GapBeginAngle,CurrentAngle,AddPos_DecNeg);

	}
}
int ApfPathPlan::CountingABehindObject(int NumCount,int& AngIndOfCurr,int AddDecFlag)
{
	//考虑了出现于障碍物重叠的特殊情况，即前一个物体的边界角引导当前指针指向了当前这个跨０～３６０度分界线的障碍物所占区域的内部某一部分，
	//则根据当前角与当前障碍物目标边界间是否存在０～３６０度分界线，分别予以判断，因为此时的NumCount计算方式不同，目前看来还是可能有一点bug
//在于当障碍物很大时,ｖ１版本中的这部分有错误
	if (AddDecFlag>0)//+方向
	{
		if(AngIndOfCurr<360&&AngIndOfCurr>180)//防止出现极端情况，当加方向搜，当前角出现在第３、４象限，目标边界在第１、２象限此处前提是障碍物不会很大。
		{	
			NumCount+=(int)(360-AngIndOfCurr+m_aObstacleStruct[m_nFreRoadMap[AngIndOfCurr]-1].LBoundaryAngle)+1;//->加方向上累计扫描过的角度NumCountAdd=NumCountAdd+这次跨的跨度角，注意当该物体横跨360度线时
		}
		else//若当前搜索角在障碍物区域的靠左边界段，即>360度当前角、目标边界出现在第１、２象限．注意此处的跨度角计算公式为m_aObstacleStruct[m_nFreRoadMap[AngIndOfCurr]-1].LBoundaryAngle-AngIndOfCurr
		{
			NumCount+=(int)(m_aObstacleStruct[m_nFreRoadMap[AngIndOfCurr]-1].LBoundaryAngle-AngIndOfCurr)+1;//->加方向上累计扫描过的角度NumCountAdd=NumCountAdd+这次跨的跨度角，注意当该物体横跨360度线时
		}
	}
	else
	{
		if(AngIndOfCurr<=180&&AngIndOfCurr>0)//０～３６０座标系中是１８０，９０也可以，太小可能发生误判，只要当前角度是在第１、２象限目标边界在第３、４象限此处前提是障碍物不会很大。
		{
			NumCount+=(int)fabs(AngIndOfCurr+360-m_aObstacleStruct[m_nFreRoadMap[AngIndOfCurr]-1].RBoundaryAngle+1);//->加方向上累计扫描过的角度NumCountAdd=NumCountAdd+这次跨的跨度角，注意当该物体横跨360度线时
		}
		else//若当前搜索角在障碍物区域的靠右边界段，当前角、目标边界均出现在第３、４象限，则两者间不垮０～３６０度分界线
		{
			NumCount+=(int)fabs(m_aObstacleStruct[m_nFreRoadMap[AngIndOfCurr]-1].RBoundaryAngle-AngIndOfCurr+1);
		}
	}
	return NumCount;
	
}
bool ApfPathPlan::FindABlockedAng(int&freeNum,int&NumCount,
					 bool&FindGap,int& AngIndOfCurr,int&GapBeginAngle,int AddDecFlag)
{
	if (FindGap==true)//之前已发现了间隙
	{
		OutputCandidateAngleFinded(freeNum,GapBeginAngle,AngIndOfCurr,AddDecFlag);
		return true;
	}
	freeNum=0;//之前未发现了间隙该方向已得空白角度清０
	if (
		CheckObstacleIsBehind(m_aObstacleStruct[m_nFreRoadMap[AngIndOfCurr]-1].LBoundaryAngle
		,m_aObstacleStruct[m_nFreRoadMap[AngIndOfCurr]-1].angle,
		m_aObstacleStruct[m_nFreRoadMap[AngIndOfCurr]-1].RBoundaryAngle)
		)
	{//如果该障碍物是横跨360分界线的话,那么+方向碰到障碍物应该是朝该障碍物左边界去，同理可知-方向
		if (FindGap==true)
		{
			//	GapEndAngle=i;
			FindGap=false;
		}
	
		if (FindGap==true)
		{
			//		GapEndAngle=i;
			FindGap=false;
		} 
		NumCount=CountingABehindObject(NumCount, AngIndOfCurr,AddDecFlag);
		AngIndOfCurr=(int)m_aObstacleStruct[m_nFreRoadMap[AngIndOfCurr]-1].LBoundaryAngle+1;///<i是当前扫描到的角度，移位到新的位置		
	}
	else//障碍物在正常情况
	{
		NumCount+=(int)fabs(m_aObstacleStruct[m_nFreRoadMap[AngIndOfCurr]-1].RBoundaryAngle+1-AngIndOfCurr);//->加方向上累计扫描过的角度NumCountAdd=NumCountAdd+这次的跨度角
		AngIndOfCurr=(int)m_aObstacleStruct[m_nFreRoadMap[AngIndOfCurr]-1].RBoundaryAngle+1;///<AngIndOfCurr是当前扫描到的角度					
	}
	return false;
}
bool ApfPathPlan::CheckCurrentAngleInHalfPlane(int& freeNum,int& NumCount,
											   bool& FindGap,int& AngIndOfCurr,int& GapBeginAngle,int AddDecFlag)
{
	if ((int)m_nFreRoadMap[AngIndOfCurr]==0)//>该角度没有障碍物 
	{
		FindAFreeAngle(freeNum,NumCount,
			FindGap,AngIndOfCurr,GapBeginAngle,AddDecFlag);
	}
	else //>该角度有障碍物
	{
		if (FindABlockedAng(freeNum,NumCount,
			FindGap,AngIndOfCurr,GapBeginAngle,AddDecFlag))
		{
			return true;
		}
	}					
	if (freeNum>MAX_SAFE_ANGLE)//空地的角跨度，自由路径的角度越大自由路径应该越宽，freeNumAdd应该越大
	{
		OutputCandidateAngleFinded(freeNum,GapBeginAngle,AngIndOfCurr,AddDecFlag);
		return true;
	}
	return false;
}
void ApfPathPlan::SearchFreeAngleInHalfPlane(int& freeNum,int& NumCount,
											 bool& FindGap,int& AngIndOfCurr,int& GapBeginAngle,bool& PassBound,int AddDecFlag)
{
	while (NumCount<180)//>左右两边同时扫描,总的扫描过的角度接近360则停止扫描
	{
		if (PassBound==true) // j_PassBound,i_PassBound两个过界标志位只要出现过一次过界即置位，并保持下去
		{
			CheckAngleLimitedBeyond(AngIndOfCurr,AddDecFlag*2);///<减方向搜索检测,保证j落在-180~180,+方向过界报警
		}
		else
		{
			//robot_console_printf("......i_PassBound=%d",i_PassBound);
			PassBound=CheckAngleLimitedBeyond(AngIndOfCurr,AddDecFlag*2);
		}///<加方向搜索检测,保证i落在0~360,-方向过界报警
		///////////////////////////////////加方向///////////////////////////////////////
		if (CheckCurrentAngleInHalfPlane(freeNum,NumCount,FindGap,AngIndOfCurr,GapBeginAngle,AddDecFlag))
		{
			break;
		}				
	}
}
bool ApfPathPlan::FindTargetAngleInCandidate(int NumCountAdd,int NumCountDec,bool LeftForbidden,bool RightForbidden)
{
	if ((NumCountAdd>=180&&NumCountDec>=180)||(RightForbidden&&NumCountDec>=180)||(LeftForbidden&&NumCountAdd>=180))//没找到合适的目标角
	{
		return false;//返回错误
	}
	else if (RightForbidden) ///<右方禁止则左方,两方必有一方可行,即取角度增加的方向搜索结果
	{
		m_iTargetAngle=(int)SelectTargetAngleFromAddAndDec(true,false);
	}
	else if (LeftForbidden) ///<左方禁止则右方,即取角度减少的方向搜索结果
	{
		m_iTargetAngle=(int)SelectTargetAngleFromAddAndDec(false,true);
	}
	else///<两边都不禁止，
	{
		if (NumCountDec>=180) //-方向未扫描到自由路径
		{
			m_iTargetAngle=m_iTargetAngle=(int)SelectTargetAngleFromAddAndDec(true,false);
		}
		else if (NumCountAdd>=180) //+方向未扫描到自由路径
		{
			//m_iTargetAngle=(int)TargetAngleDec;
			m_iTargetAngle=m_iTargetAngle=(int)SelectTargetAngleFromAddAndDec(false,true);
		}
		else///<两边都扫描到自由路径，取离自己朝向最近的其一
		{
			m_iTargetAngle=(int)SelectTargetAngleFromAddAndDec(true,true);
		}
	}
}
bool ApfPathPlan::SearchFreeTargetAngle(bool LeftForbidden,bool RightForbidden)
{
	if ((int)m_nFreRoadMap[(int)m_aPathTarget.angle]==0)//这是目标本身正处于空白区则直冲球去的情况
	{
		m_iTargetAngle=m_aPathTarget.angle;
	}
	else///<并非特殊情况，目标角应该从左右角度中选一个
	{
		bool i_PassBound=false;//>i自加过程中已>180度边界的标志位
		bool j_PassBound=false;//>j自减过程中已<-180度边界的标志位
		int j=(int)m_aPathTarget.angle;//>i，j赋初值
		int i=(int)m_aPathTarget.angle+1;//>i，j赋初值
	
	/////////////////////////i,j越界处理/////////////////////////////////////////////////
		CheckAngleLimitedBeyond(j,1);///<保证j落在-180~180,过界报警
		CheckAngleLimitedBeyond(i,1);///<保证i落在-180~180,过界报警
		int freeNumAdd=0;//>freeNumAdd赋初值。是+方向至今跨过的空白角度
		int freeNumDec=0;//->freeNumDec是-方向至今跨过的空白角度
		int NumCountAdd=0;///<其中放的数是从起始角度-方向搜索到当前位置跨过的角度数
		int NumCountDec=0;///<其中放的数是从起始角度+方向搜索到当前位置跨过的角度数
		int GapBeginAngle=0;
		bool FindGap=false;
		//int GapWidthNum=0;

//		TRACE("CheckAngleLimitedBeyond(j*********=%d\n",j);
		if (!LeftForbidden) ///<如果允许左方搜索即+方向搜索的话
		{
			SearchFreeAngleInHalfPlane(freeNumAdd,NumCountAdd,FindGap,i,GapBeginAngle,i_PassBound,1);