localmap.h

InnovLabSimu在vc++下实现

#pragma once
/********************************************************************
	created:	2008/06/12
	created:	12:6:2008   9:44
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\LocalMap.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2
	file base:	LocalMap
	file ext:	h
	author:		GaoYang
	
	purpose:	局部路径规划中用到的局部地图，以及地图的写入、查找内容函数。
	            地图支持包括原来的图像处理格式和激光雷达扫描到的格式
*********************************************************************/
//#include "stdafx.h"
#include <vector>
//#include "stdlib.h"
#include "ObjectAngleCheck.h"
#include "ShareObject.h"
#include <math.h>

using namespace personal_ShareObject_pathPlanUsingSpace;
namespace PthPlSpa=personal_ShareObject_pathPlanUsingSpace;

class CLocalVisionMap:virtual public ObjectAngleCheck
{
public:
	CLocalVisionMap(void);
public:
	~CLocalVisionMap(void);
	
	/***********************图像处理格式的地图写入*********************************/
	//注意m_nFreRoadMap[i]中的内容为一些号，这些号代表了一个个物体，
	//其排序方式为一个接一个，并无空缺，比如1号物体被判无效了则m_nFreRoadMap[i]中的
	//1所代表的物体将不是1号物体可能是2或3物体但2并非代表2号物体，可能
	unsigned short m_nFreRoadMap[361];///<障碍物号及其分布[0,360]
	/**
	* @ brief  ExpandObstacleRegion函数用来膨胀扩充障碍物区域使不用另考虑机器人大小。
	*
	* 首先将需要扩充的障碍物信息另存一份到m_aObstacleStruct。然后再m_aObstacleStruct中扩充，
	* 将障碍物距离减少，而左右边界角增加。距离减少后竟然小于0，即由于某种误差机器人已经碰上了
	* 物体，则表示膨胀失败，不膨胀
	* @ parm pObstacleList是需要扩充的区域指针，
	* @ parm indexOfmaObstacleStruct是该指针应存在m_aObstacleStruct中位置的下标.
	*/
	bool ExpandObstacleRegion(PthPlSpa::ObjectPolePos *pObstacleTobeExpand);
	double m_dExpandRadius;//膨胀时用到的机器人的半径，即计算中的膨胀半径，在构造函数中赋值
	int m_iSizeOfObjectPolePos;///<PthPlSpa::ObjectPolePos的类型大小，在构造函数中初始化=sizeof(m_iSizeOfObjectPolePos)
	::std::vector<int>i;
	::std::vector<PthPlSpa::ObjectPolePos> m_aObstacleStruct;///<将经过确认的障碍物数组，经过膨胀处理后再将数据数据全存过来,每多一次存一个
	//unsigned short m_iObstacleNumAfterCheck;///<记录经过检查后确认的障碍物个数
	/**
	*判断障碍物的有效性，不能过近过远，以及角度的0~360度
	*/
	bool ConfirmObstacleList(PthPlSpa::ObjectPolePos* Obstacle,float TargetDis);
	/**
	* @ brief  SetRoadMap()函数用于将外部输入的障碍物信息反映到成员数组m_nFreRoadMap中
	*
	* @ parm pObstacleList障碍物链表
	* @ parm nObstacleMaxNum 最大障碍物数
	* @ parm TargetDis 目标的距离
	*/
	void SetRoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum,float TargetDis);

	/**
	*@brief m_apObstaclePointerContainer中存放所有经确认有效的障碍物，按先后顺序编号，1开始，本类中直接调用SetRoadMap
	*	函数获得，并将其写入地图，后续动态物体考虑时有可能其中的部分标号并未写入地图
	*
	*	m_apObstaclePointerContainer[15]中内容与对应m_nFreRoadMap中所用编号的说明：
	*	这个一维数组中存放着所有有效的障碍物的指针。
	*	按照输入的链表的顺序，有效的障碍物指针依次存入其中。
	*	m_apObstaclePointerContainer[0]中未存指针，其内容始终为0。
	*	m_apObstaclePointerContainer[1]中存的障碍物的编号即为1号，
	*            m_nFreRoadMap中若有单元存了1则表示在该对应角度方向上有1号障碍物。
	*            因此对应i度方向的障碍物为：m_apObstaclePointerContainer[m_nFreRoadMap[i]]所指向的障碍物。
	*            障碍物编号从1开始，按障碍物链表顺序，第一个经确认有效的障碍物为1号
	*	类似继续....
	*/
	//PthPlSpa::ObjectPolePos * m_apObstaclePointerContainer[10];///<该数组中存放着所有障碍物的指针，最大存15个
	/**
	*@brief 对障碍物预处理,包括检查有效性、膨胀、记录膨胀后的指针等，若发现无效障碍物则返回false
	*
	*/
	bool PretreatObstacle(PthPlSpa::ObjectPolePos *pObstacleList,short int numOb,float TargetDis)
	{
        if (ExpandObstacleRegion(pObstacleList))
		{
			//std::vector<PthPlSpa::ObjectPolePos>::iterator itExpanded=m_aObstacleStruct.end()-1;
		//	m_apObstaclePointerContainer[numOb+1]=&(*itExpanded);//>将有效障碍物的指针保存，同时该障碍物在m_apObstaclePointerContainer中的编号即numOb+1
		}
		else
		{
			/*std::vector<PthPlSpa::ObjectPolePos>::iterator itExpanded=m_aObstacleStruct.end()-1;
			m_apObstaclePointerContainer[numOb+1]=&(*itExpanded);*/
		}
		return true;
	}
	/**
	*@brief 从ApfPathPlan类SetRoadMap函数中分离出来的部分功能:将指定障碍物及其编号写入地图,并记录该障碍物指针
	*@parm pObstacleList 指定障碍物指针
	*@parm numOb 障碍物在m_nFreRoadMap中的编号-1
	*@parm TargetDis 目标的距离
	*/
	bool AddObject2RoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int numOb,float TargetDis);
	void WriteObject2Map(std::vector<PthPlSpa::ObjectPolePos>::iterator pObstacleAferExpand,unsigned IndexOfAngle,unsigned NumInstedObjInRoadMap)
	{
		if ((int)m_nFreRoadMap[IndexOfAngle]!=0)//已经有过障碍物在该角度赋过值
		{
			if (pObstacleAferExpand->distance<m_aObstacleStruct[m_nFreRoadMap[IndexOfAngle]-1].distance)//新的障碍物更近，需要更新的话
			{
				m_nFreRoadMap[IndexOfAngle]=NumInstedObjInRoadMap;//0对应m_nFreRoadMap[361]中的第0号数据（第1个），180度对应第360号（第361个）
			}//新的更远的话则不更新m_nFreRoadMap
		}
		else//第一次赋
		{		
			m_nFreRoadMap[IndexOfAngle]=NumInstedObjInRoadMap;//该角度方向的障碍物为numOb+1号				
		}
	}
};
// ***************************************************************
// CLocalMapFromLaser   version:  1.0   ·  date: 06/13/2008
//  -------------------------------------------------------------
//  
//  -------------------------------------------------------------
//  Copyright (C) 2008 - All Rights Reserved
// ***************************************************************
// 激光雷达地图类,包含了地图以及写入函数
// ***************************************************************
#define MAX_DETECT_RANGE 8000///<激光雷达最大探测距离,单位毫米
#define ONE_SCANNER_USED ///<使用了一个激光雷达,因此仅用0~180度的角度,分辨率为0.5度,因此
#define ANGLE_RESOLVING 0.5///<激光雷达角度精度单位度
#define NumOfData 361///<数据的个数
class CLocalMapFromLaser
{
public:
	CLocalMapFromLaser(void);
public:
	~CLocalMapFromLaser(void);
public:
	/**
	*@brief 接口函数，将外部扫描得到的数据经检验、膨胀后写入LocalLaserMap
	*若达到最大扫描距离的角度则认为该角度物物体
	*/
	bool WriteLocalLaserMap(float* pLaserDataChunk,float TargetDis);
protected:
	/**
	* @ brief  ExpandObstacleCell函数用来膨胀扩充障碍物区域使不用另考虑机器人大小并将结果写入到LocalLaserMap。
	*
	* 将每一方向经ConfirmLaserData验证的扫描值输入本函数,函数内依据它的距离扩充,包括领域,若领域内已有距离值且比自己小则
	*不填充进去.
	* @ parm pLaserData是输入的角度的物距值，
	* @ parm iIndexInLocalMapStruct是角度对应在LocalLaserMap中的位置即下标值.
	*/
	bool ExpandObstacleCell(float* pLaserData,unsigned IndexInLocalMapStruct);
	/**
	*@brief 在ExpandObstacleCell中调用的用来在该物体角度左右AngleRegion范围内扩充，以实现扩充障碍物
	*
	*@parm AngleRegion,该物体影响的角度范围，
	*@parm Dis 该物体的距离
	*@parm IndexInLocalMapStruct 该物体在LocalLaserMap中的位置即下标值
	*/
	void ExpandObstacleNeighborCell(float AngleRegion,float Dis,unsigned IndexInLocalMapStruct);
	/**
	*@brief 在ExpandObstacleNeighborCell中调用的用来在判断制定角度是否可以扩充,可以则扩充写入,否则返回false
	*
	*@parm Dis 物距
	*@parm CurrentIndex当前需要判断是否能扩充的位置,或者叫LocalLaserMap中的位置即下标值.
	*/
	bool CheckExpandAble(float Dis,unsigned CurrentIndex)
	{
		if (CurrentIndex<361&&CurrentIndex>0)//未超出雷达数据的界限
		{
			if (LocalLaserMap[CurrentIndex]<0.000001||
				LocalLaserMap[CurrentIndex]>Dis)//没有障碍物或者障碍物更远
			{
				LocalLaserMap[CurrentIndex]=Dis;
				return true;
			}
			else
			{
				return false;
			}
		}
		else
		{
			return false;
		}
	}
	bool ConfirmLaserData(float* LaserData,float TargetDis)
	{
		if (*(LaserData)>TargetDis||*(LaserData)>=MAX_DETECT_RANGE)
		{
			return false;
		}
		return true;
	}

protected:
	double m_dExpandRadius;//膨胀时用到的机器人的半径，即计算中的膨胀半径，在构造函数中赋值
	float LocalLaserMap[NumOfData];///<局部地图,源自1台激光扫描仪的数据为0~180度0~361个数分辨率为0.5度,其内部存储有每一角度上的距离值，没有物体的角度则其值为0,为与视觉格式一致,可考虑扩充0~359.5度,

private:
};