localmap.cpp

InnovLabSimu在vc++下实现

#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;
		}
	}
	
}