?? apfpathplan.cpp
字號:
}
FindGap=false;
if (!RightForbidden) ///<如果允許右方搜索即-方向搜索的話
{
SearchFreeAngleInHalfPlane(freeNumDec,NumCountDec,FindGap,i,GapBeginAngle,j_PassBound,-1);
// robot_console_printf("*************************LeftnotForbid");
}
FindTargetAngleInCandidate(NumCountAdd,NumCountDec,LeftForbidden,RightForbidden);//選出最終的目標路徑角
}
CheckAngleLimitedBeyond(m_iTargetAngle,0);//限幅
m_iPreTargetAngle=m_iTargetAngle;///<保存目標路徑角
return true;
}
//bool ApfPathPlan::CheckAngleLimitedBeyond(double & Angle, int Add_Pos_Dec_Neg)
bool ApfPathPlan::CheckCurAngFindNexAng(int& AngTobeCheck,int& NearestObstacleIndex,
int& DisBetweenTarA2SelfA,int SearchInLeftOrRightPlaneFlag)
{
//__asm int 3;
if (m_nFreRoadMap[AngTobeCheck]==0)
{AngTobeCheck=AngTobeCheck+SearchInLeftOrRightPlaneFlag;}
else
{
if (m_dMinObstalceDis>m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].distance)//找到更小的距離
{
m_dMinObstalceDis=m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].distance;
NearestObstacleIndex=m_nFreRoadMap[AngTobeCheck];
// robot_console_printf("NearObstacleIndex=%d\n",m_nFreRoadMap[i]);
}
//////////////////////////////////////////////////////////////////////////
/* 檢查下一個角度,依據物體邊界的方法，目標極快速移動，
可能導致死循環，
僅在應用動態障礙物時出現,因此當應用于動態障礙物時改為挨個角度搜索*/
//////////////////////////////////////////////////////////////////////////
if (
CheckObstacleIsBehind(m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle,
m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].angle,
m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].RBoundaryAngle)
)//->檢查該障礙物是否處于后面,
{
if (SearchInLeftOrRightPlaneFlag>0)//+方向
{
//return true;
/*當搜索自己當前朝向到目標角間的障礙物最小距離時，若碰到一個在背后橫跨360分界線的障礙物時，
因目標角〉=自身朝向，所以不用再搜索了，它就是最后一個障礙物了，可以跳出.不對!!!*/
/* robot_console_printf("++AngTobeCheck=%d\n",AngTobeCheck);
robot_console_printf("OBjLBoundary=%f",m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle);
robot_console_printf(" OBjRBoundary=%f\n",m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].RBoundaryAngle);*/
AngTobeCheck=(int)m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle+1;///<AngTobeCheck指向下一個
}
else
{
/* robot_console_printf("--AngTobeCheck=%d\n",AngTobeCheck);
robot_console_printf("OBjLBoundary=%f",m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle);
robot_console_printf("OBjAngle=%f",m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].angle);
robot_console_printf(" OBjRBoundary=%f\n",m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].RBoundaryAngle);*/
AngTobeCheck=(int)m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].RBoundaryAngle-1;///<AngTobeCheck指向下一個
}
/*AngTobeCheck=(int)m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle+1;///<AngTobeCheck是當前掃描到的角度，移位到新的位置*/
}
else
{
if (SearchInLeftOrRightPlaneFlag>0)//+方向
{
AngTobeCheck=(int)m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].RBoundaryAngle+1;///<AngTobeCheck不可能過界
}
else
{
AngTobeCheck=(int)m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle-1;///<AngTobeCheck指向下一個
}
}
/*robot_console_printf("LeftOrRight=%d,AngTobeCheck=%d\n",SearchInLeftOrRightPlaneFlag,AngTobeCheck);
robot_console_printf("ObNum=%d",m_nFreRoadMap[AngTobeCheck]);*/
/*現改為挨個角度搜索*/
//if (SearchInLeftOrRightPlaneFlag>0)//+方向
//{
// AngTobeCheck++;///<AngTobeCheck指向下一個
//}
//else
//{
// AngTobeCheck--;///<AngTobeCheck指向下一個
//}
}
CheckAngleLimitedBeyond(AngTobeCheck,SearchInLeftOrRightPlaneFlag);
DisBetweenTarA2SelfA=int(m_iTargetAngle-AngTobeCheck);//不可取絕對值
//CheckAngleLimitedBeyond(AngTobeCheck,SearchInLeftOrRightPlaneFlag);
return false;
}
int ApfPathPlan::SearchNearestDis()
{
//考慮更改搜索方式，不再取巧，挨個角度搜索，放到掃描類實現
int NearestObstacleIndex=0;//搜到的最近障礙物標號，從1開始，為m_nFreRoadMap[i]
m_dMinObstalceDis=1000000;//最小距離初始值取1000;bushi目標距離
int i=m_iSelfOrientation,j=m_iSelfOrientation;
int DisBetweenTarA2SelfA=int(m_iTargetAngle-m_iSelfOrientation);//當前朝向與目標角間的差值
// robot_console_printf("DisBetweenTarA2SelfA=%d\n",DisBetweenTarA2SelfA);
if ((DisBetweenTarA2SelfA>=0&&DisBetweenTarA2SelfA<180)||DisBetweenTarA2SelfA<-180)//目標處于自己當前朝向的角度的左邊DisBetweenTarA2SelfA<-180是為當朝向反向準備的
{
//下面DisBetweenTarA2SelfA將暫存當前搜索角與目標角之差，與前面的內容無關
while ((DisBetweenTarA2SelfA>=0&&DisBetweenTarA2SelfA<180)||DisBetweenTarA2SelfA<-180) //相對目標角，當前角在右平面，
//這種循環方式的前提是，在目標角與當前朝向之間，無非常大的障礙物使得當前角直接跨越了右平面．
{
if (CheckCurAngFindNexAng(i,NearestObstacleIndex,DisBetweenTarA2SelfA,1))
{
break;
}
}
}
else//目標處于自己當前朝向的角度的右邊
{
while ((DisBetweenTarA2SelfA<0&&DisBetweenTarA2SelfA>-180)||DisBetweenTarA2SelfA>180)
{
if (CheckCurAngFindNexAng(i,NearestObstacleIndex,DisBetweenTarA2SelfA,-1))
{
break;
}
}
}
return NearestObstacleIndex;
}
/*********************************************************************
CheckObstacleIsBehind函數用來監測該障礙物是否位于身后而橫跨0~360度分界線
輸入為該障礙物的角度信息
************************************************************************/
// inline bool ApfPathPlan::CheckObstacleIsBehind(double dLBoundaryAngle,double dCenterAngle,double dRightBoundaryAngle)
// {
// //新添加的橫跨360度分界線的判斷標準，見GetObstacleList中的說明
// if ((dCenterAngle<dLBoundaryAngle||dCenterAngle>dRightBoundaryAngle)||
// (fabs(dRightBoundaryAngle-dLBoundaryAngle)>fabs(dRightBoundaryAngle))&&fabs(dCenterAngle-180)>110)
// {
// return true;
// }
// else
// {
// return false;
// }
// }
//inline int ApfPathPlan::GetShortDistanceBetween2Angle(int Angle1, int Angle2)
//{
// int DisOfTarA2SelfA=abs(Angle1-Angle2);
// if (DisOfTarA2SelfA>180)
// {
// DisOfTarA2SelfA=360-DisOfTarA2SelfA;
// }
// return DisOfTarA2SelfA;
//}
inline double ApfPathPlan::SelectTargetAngleFromAddAndDec(bool AngleInAdd,bool AngleInDec)
{
int AngleDisAdd2OriTmp,AngleDisAdd2PreTar,AngleDisAdd2Tar;
int AngleDisDec2OriTmp,AngleDisDec2PreTar,AngleDisDec2Tar;
double CostFunAdd,CostFunDec;
double CostFunMin=999999999,MinCostAngle;
if (AngleInAdd)
{
//robot_console_printf("m_iNumOfTargetAngleAdd=%d",CostFunAdd);
for (int i=0;i<m_iNumOfTargetAngleAdd;i++)
{
AngleDisAdd2OriTmp=GetShortDistanceBetween2Angle(TargetAngleAdd[i],m_iSelfOrientation);
AngleDisAdd2PreTar=GetShortDistanceBetween2Angle(TargetAngleAdd[i],m_iPreTargetAngle);
AngleDisAdd2Tar=GetShortDistanceBetween2Angle(TargetAngleAdd[i],m_aPathTarget.angle);
CostFunAdd=AngleDisAdd2OriTmp*m_dParCostFun_Orient2Angle+m_dParCostFun_Tar2Angle*AngleDisAdd2Tar+
m_dParCostFun_PreTarAngle2Angle*AngleDisAdd2PreTar;
/* robot_console_printf("\n1CostFunAdd=%f",CostFunAdd);
robot_console_printf("\n1TargetAngleAdd[i]=%d",TargetAngleAdd[i]);*/
if (CostFunMin>CostFunAdd)
{
CostFunMin=CostFunAdd;
MinCostAngle=TargetAngleAdd[i];
}
}
}
if (AngleInDec)
{
for (int i=0;i<m_iNumOfTargetAngleDec;i++)
{
AngleDisDec2OriTmp=GetShortDistanceBetween2Angle(TargetAngleDec[i],m_iSelfOrientation);
AngleDisDec2PreTar=GetShortDistanceBetween2Angle(TargetAngleDec[i],m_iPreTargetAngle);
AngleDisDec2Tar=GetShortDistanceBetween2Angle(TargetAngleDec[i],m_aPathTarget.angle);
CostFunDec=AngleDisDec2OriTmp*m_dParCostFun_Orient2Angle+m_dParCostFun_Tar2Angle*AngleDisDec2Tar+
m_dParCostFun_PreTarAngle2Angle*AngleDisDec2PreTar;
/* robot_console_printf("2CostFunDec=%f",CostFunDec);
robot_console_printf("2TargetAngleDec[i]=%d\n",TargetAngleDec[i]);*/
if (CostFunMin>CostFunDec)
{
CostFunMin=CostFunDec;
MinCostAngle=TargetAngleDec[i];
}
}
}
return MinCostAngle;
//robot_console_printf("CostFunAdd=%d",CostFunAdd);
}
//inline bool ApfPathPlan::ExpandObstacleRegion(ObjectPolePos *pObstacleList,int indexOfmaObstacleStruct)
//{
// ObjectPolePos* pObstacleTmp;//暫存指針
// pObstacleTmp=&m_aObstacleStruct[indexOfmaObstacleStruct];//為了不影響外面傳進來的障礙物 內容，在類里另存一份于數組中
// memcpy(pObstacleTmp,pObstacleList,m_iSizeOfObjectPolePos);
// double AngleAdd;//膨脹時的角度增量暫存
// if (pObstacleList->distance-m_dExpandRadius<0)//若將障礙物膨脹后已把自己包進障礙物，即已無法避開
// {//m_dExpandRadius為物體膨脹半徑
// AngleAdd=90;//2*asin(0.5*m_dExpandRadius/(pObstacleList->distance))*180*ONE_DEVIDE_PI;
// pObstacleTmp->distance=0.02;
//// robot_console_printf("Collising_pObstacleList->distance=%f\n",pObstacleTmp->distance);
// //return false;
// }
// else//正常情況下膨脹
// {
// AngleAdd=asin(m_dExpandRadius/(pObstacleList->distance))*180*ONE_DEVIDE_PI;
// pObstacleTmp->distance=pObstacleList->distance-m_dExpandRadius;
//// robot_console_printf("Normal_pObstacleList->distance=%f\n",pObstacleTmp->distance);
// }
//
// // robot_console_printf("AngleAdd=%f\n",AngleAdd);
// if (CheckObstacleIsBehind(pObstacleList->LBoundaryAngle,pObstacleList->angle,pObstacleList->RBoundaryAngle))
// {
// pObstacleTmp->LBoundaryAngle+=AngleAdd;
// pObstacleTmp->RBoundaryAngle-=AngleAdd;
// }
// else
// {
// pObstacleTmp->LBoundaryAngle-=AngleAdd;
// pObstacleTmp->RBoundaryAngle+=AngleAdd;
// }
// CheckAngleLimitedBeyond(pObstacleTmp->LBoundaryAngle,0);
// CheckAngleLimitedBeyond(pObstacleTmp->RBoundaryAngle,0);
///*萬一膨脹后出現左角度大于右角度的情況應該將他們交換*/
// if (pObstacleTmp->LBoundaryAngle > pObstacleTmp->RBoundaryAngle)
// {
// AngleAdd=pObstacleTmp->LBoundaryAngle;
// pObstacleTmp->LBoundaryAngle=pObstacleTmp->RBoundaryAngle;
// pObstacleTmp->RBoundaryAngle=AngleAdd;
// }
// return true;
//}
//////////////////////////////////////////////////////////////////////////
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -