?? localmap.cpp
字號:
#include "LocalMap.h"
template <class type>
bool CheckAngleLimitedBeyond(type & Angle,int Add_Pos_Dec_Neg)
{
//=180或-180度均不超界
if (Add_Pos_Dec_Neg==1) //->當Add_Pos_Dec_Neg==1時限幅同時任一方向過界均報錯
{
if (Angle>360) //->i，j越界處理
{
Angle=Angle-360;
return true;//->返回過界標志
// AfxMessageBox("角度>180范圍");
}
else if (Angle<0)
{
Angle=360+Angle;
return true;//->返回過界標志
// AfxMessageBox("角度<-180范圍");
}
else
{return false;}
}
else if(Add_Pos_Dec_Neg==0)//->當Add_Pos_Dec_Neg==0時，表示不再區分從哪個方向過界，僅將輸入的角度限制在-180~180
{
if (Angle>360)//->i，j越界處理
{
Angle=Angle-360;
return true;//->返回過界標志
}//->錯誤報警
else if (Angle<0)
{
Angle=360+Angle;
return true;//->返回過界標志
}
else
{
return false;//->未過界
}
}
else if (Add_Pos_Dec_Neg>0)//->假如是檢測正方向是否過界
{
if (Angle>360) //->i，j越界處理
{
Angle=Angle-360;
return true;//->返回過界標志
}
else if (Angle<0)
{
// Angle=360+Angle;//AfxMessageBox("+方向搜索的角度<-180范圍");
// return true
}
else
{
return false;//->未過界
}
}
else//->Add_Pos_Dec_Neg<0假如是檢測-方向是否過界
{
if (Angle>360)//->i，j越界處理
{//AfxMessageBox("-方向搜索的角度>180范圍");
}//->錯誤報警
else if (Angle<0)
{
Angle=360+Angle;
return true;//->返回過界標志
}
else
{
return false;//->未過界
}
}
}
CLocalVisionMap::CLocalVisionMap(void)
{
m_dExpandRadius=SAFE_WIDTH*(0.5+0.1);//0.1部分為安全寬度富余0.5部分為機器人自身寬度的1半
}
CLocalVisionMap::~CLocalVisionMap(void)
{
}
bool CLocalVisionMap::ExpandObstacleRegion(ObjectPolePos *pObstacleTobeExpand)//,int indexOfmaObstacleStruct
{
// ObjectPolePos* pObstacleTmp;//暫存指針
m_aObstacleStruct.push_back(*pObstacleTobeExpand);//將障礙物另存一份
std::vector<PthPlSpa::ObjectPolePos>::iterator itTobeExpand=m_aObstacleStruct.end()-1;
//pObstacleTmp=&itTobeExpand;//為了不影響外面傳進來的障礙物 內容，在類里另存一份于數組中
double AngleAdd;//膨脹時的角度增量暫存
if (pObstacleTobeExpand->distance-m_dExpandRadius<0)//若將障礙物膨脹后已把自己包進障礙物，即已無法避開
{//m_dExpandRadius為物體膨脹半徑
AngleAdd=90;
itTobeExpand->distance=0.02;
}
else//正常情況下膨脹
{
AngleAdd=asin(m_dExpandRadius/(pObstacleTobeExpand->distance))*180*ONE_DEVIDE_PI;
itTobeExpand->distance=pObstacleTobeExpand->distance-m_dExpandRadius;
}
// robot_console_printf("AngleAdd=%f\n",AngleAdd);
if (CheckObstacleIsBehind(pObstacleTobeExpand->LBoundaryAngle,pObstacleTobeExpand->angle,pObstacleTobeExpand->RBoundaryAngle))
{
itTobeExpand->LBoundaryAngle+=AngleAdd;
itTobeExpand->RBoundaryAngle-=AngleAdd;
}
else
{
itTobeExpand->LBoundaryAngle-=AngleAdd;
itTobeExpand->RBoundaryAngle+=AngleAdd;
}
CheckAngleLimitedBeyond(itTobeExpand->LBoundaryAngle,0);
CheckAngleLimitedBeyond(itTobeExpand->RBoundaryAngle,0);
/*萬一膨脹后出現左角度大于右角度的情況應該將他們交換*/
if (itTobeExpand->LBoundaryAngle > itTobeExpand->RBoundaryAngle)
{
AngleAdd=itTobeExpand->LBoundaryAngle;
itTobeExpand->LBoundaryAngle=itTobeExpand->RBoundaryAngle;
itTobeExpand->RBoundaryAngle=AngleAdd;
}
return true;
}
/**
*判斷障礙物的有效性，不能過近過遠，以及角度的0~360度
*/
bool CLocalVisionMap::ConfirmObstacleList(PthPlSpa::ObjectPolePos* Obstacle,float TargetDis)
{
if (Obstacle->angle<0||Obstacle->angle>360)
{
return false;
}
if (Obstacle->distance)
{
if (TargetDis<Obstacle->distance)//0.2為余量，單位是毫米
{
return false;//該障礙物無效
}
if (Obstacle->distance>MAX_OBJ_DIS)//->距離太遠的不需要考慮避障
{
return false;
}
if (Obstacle->distance<MIN_OBJ_DIS)
{
return false;
}
return true;
}
return false;
}
bool CLocalVisionMap::AddObject2RoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int numOb,float TargetDis)
{
//robot_console_printf(" numOb_inAddObject2RoadMap=%d\n",numOb);
PretreatObstacle(pObstacleList,numOb,TargetDis);
std::vector<PthPlSpa::ObjectPolePos>::iterator pObstacleAferExpand=m_aObstacleStruct.end()-1;
// PthPlSpa::ObjectPolePos * pObstacleAferExpand;
//按障礙物面積從小到大的順序排列它與目標的夾角
if (CheckObstacleIsBehind(pObstacleAferExpand->LBoundaryAngle,pObstacleAferExpand->angle,pObstacleAferExpand->RBoundaryAngle))
//>障礙物在身后橫跨360度分界線
{
/*對橫跨在背后的障礙物，其信息寫入時
*要分兩批寫入,第一批:j從RBoundaryAngle加到360度
*第二批:j從LBoundaryAngle減到0度*/
// robot_console_printf("BehindObstacleNum=%d\n",numOb);
int j=0;
for (j=(int)pObstacleAferExpand->RBoundaryAngle;j<=360;j++)
//第一批寫入信息pObstacleAferExpand->RBoundaryAngle為大的角度也是0~180,
{
WriteObject2Map(pObstacleAferExpand,j,numOb+1);
}
for (j=(int)pObstacleAferExpand->LBoundaryAngle;j>=0;j--)//>障礙物在后面時左半邊的寫入。pObstacleAferExpand->LBoundaryAngle也是-180~180
//第二批寫入信息pObstacleAferExpand->LBoundaryAngle為大的角度也是-180~0,
{
WriteObject2Map(pObstacleAferExpand,j,numOb+1);
}
}
else//>障礙物并非橫跨后方左右時
{
for (int j=(int)pObstacleAferExpand->LBoundaryAngle;j<=pObstacleAferExpand->RBoundaryAngle;j++)
{
WriteObject2Map(pObstacleAferExpand,j,numOb+1);
}
}
return true;
}
void CLocalVisionMap::SetRoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum,float TargetDis)
{
memset(m_nFreRoadMap,0,sizeof(m_nFreRoadMap));
// memset(m_apObstaclePointerContainer,0,sizeof(m_apObstaclePointerContainer));
m_aObstacleStruct.clear();
int numOb;//障礙物的編號
numOb=0;//0表示沒障礙物
// ObjectPolePos * pObstacleAferExpand;
if (nObstacleMaxNum>11)//障礙物最大11個
{
return;
}
for (int i=0;i<nObstacleMaxNum;i++)
{
if (!pObstacleList)
{
break;
}
if (!ConfirmObstacleList(pObstacleList,TargetDis))//>若無效
{
pObstacleList=pObstacleList->next;//去掉無效的
continue;
}
//PretreatObstacle(pObstacleList,numOb,TargetDis);
AddObject2RoadMap(pObstacleList,numOb,TargetDis);
// robot_console_printf("oneObsOver=%d\n",numOb);
pObstacleList=pObstacleList->next;
numOb++;//>經確認的障礙物個數為numOb+1個
}
}
/************************************************************************/
/* CLocalMapFromLaser */
/************************************************************************/
CLocalMapFromLaser::CLocalMapFromLaser(void)
{
m_dExpandRadius=SAFE_WIDTH*(0.5+0.1);//0.1部分為安全寬度富余0.5部分為機器人自身寬度的1半
}
CLocalMapFromLaser::~CLocalMapFromLaser(void)
{
}
bool CLocalMapFromLaser::WriteLocalLaserMap(float* pLaserDataChunk,float TargetDis)
{
memset(LocalLaserMap,0,sizeof(LocalLaserMap));
for (unsigned i=0;i<NumOfData;i++)
{
float* pCurrentAngleVal=pLaserDataChunk+i;
if (ConfirmLaserData(pCurrentAngleVal,TargetDis))
{
ExpandObstacleCell(pCurrentAngleVal,i);//膨脹并寫入
//LocalLaserMap[i]=*(pLaserDataChunk);//寫入
}
}
return true;
}
bool CLocalMapFromLaser::ExpandObstacleCell(float* pLaserData,unsigned IndexInLocalMapStruct)
{
float AngleAdd;//膨脹時的角度增量暫存
if (*pLaserData-m_dExpandRadius<0)//若將障礙物膨脹后已把自己包進障礙物，即已無法避開
{//m_dExpandRadius為物體膨脹半徑
AngleAdd=90;
ExpandObstacleNeighborCell(AngleAdd,0.02,IndexInLocalMapStruct);
//LocalLaserMap[IndexInLocalMapStruct]=0.02;
}
else//正常情況下膨脹
{
AngleAdd=asin(m_dExpandRadius/(*pLaserData))*180*ONE_DEVIDE_PI;
ExpandObstacleNeighborCell(AngleAdd,*pLaserData-m_dExpandRadius,IndexInLocalMapStruct);
}
return true;
}
void CLocalMapFromLaser::ExpandObstacleNeighborCell(float AngleRegion,float Dis,unsigned IndexInLocalMapStruct)
{
unsigned InfluncedCellNum=ceil(AngleRegion*2);//本次擴充在LocalLaserMap中影響到的最大范圍
for(unsigned i=0;i<=InfluncedCellNum;i++)//+方向的擴充
{
if (!CheckExpandAble(Dis,i+IndexInLocalMapStruct))//若碰到不能擴充的則不再擴充
{
break;
}
}
for (unsigned i=1;i<=InfluncedCellNum;i++)//-方向的擴充
{
if (!CheckExpandAble(Dis,IndexInLocalMapStruct-i))//若碰到不能擴充的則不再擴充
{
break;
}
}
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -