?? globalmap.h
字號(hào):
#pragma once
#include <vector>
#include <fstream>
#include "../particleFlt/sir_pf_share.h"
#include "../Global.h"
#include "../Localization.h"
//#include <afxwin.h>
/********************************************************************
created: 2008/07/11
created: 11:7:2008 9:20
filename: c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\GlobalMap.h
file path: c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2
file base: GlobalMap
file ext: h
author: GaoYang
purpose: 全局地圖類(lèi)，包括讀入地圖，從地圖中查找值，地圖按照webots中仿真的坐標(biāo)系建立，地圖中央為原點(diǎn)，坐標(biāo)有正負(fù)，坐標(biāo)系方向則是向上為Y軸正向，與仿真中的Z軸反向
x軸與仿真中的webots重合
而地圖文件中是以左上角為原點(diǎn)，右、下方向?yàn)檎?*********************************************************************/
#define JUDGE_EDGE_POINT_IN_MAP//bool地圖中是否單獨(dú)考慮了障礙物的邊界點(diǎn)，若是，則定義它，則讀入bool地圖是，需注意，地圖中的邊界點(diǎn)，值不是0，而是2000
typedef std::vector<std::vector<float>> MAP_TYPE;
typedef std::vector<std::vector<bool>> BoolMapType;
#define ONE_PIXEL_LENGTH 50///<每一像素的長(zhǎng)度，單位豪米，應(yīng)與DrawMap中對(duì)應(yīng)
#define PRECISION 100///<地圖中存的距離值的有效精度,因?yàn)榇娴氖钦?所以以這種方法保證小數(shù)點(diǎn)后的精度.實(shí)際上單位為毫米的距離=地圖值/PRECISION*10，
#define NON_OBJECT_VAL 16777215///<無(wú)障礙物點(diǎn)的地圖值
#define EDGE_POINT_VAL_IN_BOOL_MAP 2000///<存在二值地圖的文件中，表示障礙物點(diǎn)的地圖值
class CGlobalMap
{
public:
CGlobalMap(void);
CGlobalMap(unsigned Height,unsigned Width);
public:
~CGlobalMap(void);
/**
*從已有文件中讀入二值地圖，1表示空白，0表示障礙物，本函數(shù)從默認(rèn)文件中讀
*
*/
bool ReadBoolMapFromFile(unsigned Height,unsigned Width);
#ifdef JUDGE_EDGE_POINT_IN_MAP
BoolMapType m_bvEdgePointMap;///<bool地圖，是邊界點(diǎn)則為真，不是則為假
/**
*@brief 是否邊界點(diǎn)的查詢(xún)函數(shù)，輸入世界坐標(biāo)系坐標(biāo)，但要求先轉(zhuǎn)化為像素為單位
*
*@parm XCoord 世界坐標(biāo)系X坐標(biāo)，但要求先轉(zhuǎn)化為像素為單位
*@parm YCoord 世界坐標(biāo)系Y坐標(biāo)，但要求先轉(zhuǎn)化為像素為單位
*@output 若是邊界點(diǎn)則返回真
*/
bool IsEdgePoint(float XCoord,float YCoord)
{
if (fabs(XCoord)>X_DisOfWordCord2MapCoord||
fabs(YCoord)>Y_DisOfWordCord2MapCoord)
{
return false;//產(chǎn)生的點(diǎn)超出整個(gè)地圖范圍時(shí)，返回不可能
}
float MemoryCoordX=X_DisOfWordCord2MapCoord+XCoord;//轉(zhuǎn)化為像素?cái)?shù)的圖像坐標(biāo)單位
float MemoryCoordY=Y_DisOfWordCord2MapCoord-YCoord;//轉(zhuǎn)化為像素?cái)?shù)的圖像坐標(biāo)單位
return m_bvEdgePointMap[Floor4_Ceil5(MemoryCoordY)][Floor4_Ceil5(MemoryCoordX)];
}
#endif
BoolMapType m_BoolGolbalMap;
MAP_TYPE m_mGolbalMap;///<全局地圖，由兩個(gè)vector嵌套而成，最外層的每一個(gè)元素為一行，即m_mGolbalMap[4][5]為下標(biāo)4行，5列的元素，下標(biāo)從0開(kāi)始數(shù)起
size_t X_DisOfWordCord2MapCoord;///<世界地圖原點(diǎn)相對(duì)圖像原點(diǎn)沿地圖的圖像坐標(biāo)系的X軸方向偏移
size_t Y_DisOfWordCord2MapCoord;///<世界地圖原點(diǎn)相對(duì)圖像原點(diǎn),沿地圖的圖像坐標(biāo)系的Y軸方向偏移
/**
*從已有文件中讀入地圖，本函數(shù)從默認(rèn)文件中讀,指定圖的寬度和高度
*
*/
bool ReadMapFromFile(unsigned Height,unsigned Width);
/**
*從已有地圖中，依據(jù)X,Y坐標(biāo)查詢(xún)地圖對(duì)應(yīng)點(diǎn)的誤差距離值（地圖中對(duì)應(yīng)點(diǎn)存的值）
*
*/
//float QueryGridValOnMap(float XCoord,float YCoord);
/**
*從已有地圖中，依據(jù)X,Y坐標(biāo)查詢(xún)地圖對(duì)應(yīng)點(diǎn)的誤差距離值（地圖中對(duì)應(yīng)點(diǎn)存的值）,當(dāng)所查詢(xún)點(diǎn)為一個(gè)靠近邊緣的點(diǎn)，則地圖中直接存了距離值取出即可
*
*
*/
float QueryAnFreeGridVal(float& XCoord,float& YCoord)
{
return QueryGridValOnMap(XCoord,YCoord);
}
/**
*部分額外的數(shù)學(xué)運(yùn)算，目前包括四舍五入運(yùn)算
*/
long Floor4_Ceil5(float& NumTobeOperate)
{
long IntPart=floor(NumTobeOperate);//取整數(shù)部分
if (NumTobeOperate-IntPart>=0.5)
{
return IntPart+1;
}
else
{
return IntPart;
}
}
/**
*@brief 在地圖中膨脹障礙物范圍，擴(kuò)充障礙物點(diǎn)，獲得新的二值地圖結(jié)果仍然存放在m_BoolGolbalMap
*
*@parm Height地圖的高度
*@parm Height地圖的寬度
*@parm ExpandWidthInPixel對(duì)障礙物點(diǎn)，需要向外擴(kuò)充多少個(gè)像素，注意單位是像素?cái)?shù)
*/
void ExpandObstacleOnboolMap(unsigned Height,unsigned Width,unsigned ExpandWidthInPixel);
/**
*@brief 從已有地圖中，依據(jù)X,Y坐標(biāo)查詢(xún)二值地圖對(duì)應(yīng)點(diǎn)的值（地圖中對(duì)應(yīng)點(diǎn)存的值）
*返回1表示空白，0表示障礙物,越界則返回0(false)
*/
bool QueryboolMap(float& XCoord,float& YCoord)
{
if (fabs(XCoord)>X_DisOfWordCord2MapCoord||
fabs(YCoord)>Y_DisOfWordCord2MapCoord)
{
return false;//產(chǎn)生的點(diǎn)超出整個(gè)地圖范圍時(shí)，返回不可能
}
float MemoryCoordX=X_DisOfWordCord2MapCoord+XCoord;//轉(zhuǎn)化為像素?cái)?shù)的圖像坐標(biāo)單位
float MemoryCoordY=Y_DisOfWordCord2MapCoord-YCoord;//轉(zhuǎn)化為像素?cái)?shù)的圖像坐標(biāo)單位
bool tmp=m_mGolbalMap[Floor4_Ceil5(MemoryCoordY)][Floor4_Ceil5(MemoryCoordX)];//測(cè)試用，應(yīng)去掉
return tmp;
}
bool IsAPossibleLocate(CParticle& LocTobeCheck)
{
return QueryboolMap(LocTobeCheck.XCoord,LocTobeCheck.YCoord);
}
typedef bool (CGlobalMap::*pCheckLocPossibleFun)(CParticle& ParticleTobeCheck);
bool (CGlobalMap::*m_pfCheckPossibleParticle)(CParticle& ParticleTobeCheck) ;///<指向IsAPossibleLocate函數(shù)的指針成員
/**
*@brief m_mGolbalMap地圖中存進(jìn)來(lái)的數(shù)據(jù)下標(biāo)是按照0開(kāi)始的，而輸入進(jìn)來(lái)的機(jī)器人世界坐標(biāo)系是原點(diǎn)在中心，有正負(fù)，因此本函數(shù)
*輸入世界坐標(biāo)系坐標(biāo)，返回查詢(xún)到的坐標(biāo)值，不經(jīng)任何運(yùn)算，僅進(jìn)行坐標(biāo)轉(zhuǎn)化，并返回查到的內(nèi)容,并未考慮輸入的坐標(biāo)越界的情況！！！
*
*注意，本函數(shù)假設(shè) m_mGolbalMap地圖中存進(jìn)來(lái)的數(shù)據(jù)下標(biāo)是按照0開(kāi)始的，原點(diǎn)或者說(shuō)坐標(biāo)系以左上角為原點(diǎn)，右、下方向?yàn)檎? *而假設(shè)輸入的絕對(duì)世界坐標(biāo)系則，以webots中仿真的坐標(biāo)系,地圖中央為原點(diǎn)，坐標(biāo)有正負(fù)，坐標(biāo)系方向則是向上為Y軸正向,
*X軸方向兩坐標(biāo)系相同，注意在地圖中是以像素為單位存的，而輸入的直角坐標(biāo)單位是毫米，一個(gè)像素=？毫米？？所以要進(jìn)行一次轉(zhuǎn)化
*
*@todo 萬(wàn)一地圖的大小是奇數(shù)，則進(jìn)行本函數(shù)轉(zhuǎn)化會(huì)有一個(gè)像素的誤差問(wèn)題！！
*/
float QueryGridValOnMap(float XCoord,float YCoord)
{
if (fabs(XCoord)>X_DisOfWordCord2MapCoord||
fabs(YCoord)>Y_DisOfWordCord2MapCoord)
{
return 3.4e8;//產(chǎn)生的點(diǎn)超出整個(gè)地圖范圍時(shí)，隨便返回的一個(gè)大的數(shù)，表示不可能
}
float MemoryCoordX=X_DisOfWordCord2MapCoord+XCoord;//轉(zhuǎn)化為像素?cái)?shù)的圖像坐標(biāo)單位
float MemoryCoordY=Y_DisOfWordCord2MapCoord-YCoord;//轉(zhuǎn)化為像素?cái)?shù)的圖像坐標(biāo)單位
int MapDisVal=m_mGolbalMap[Floor4_Ceil5(MemoryCoordY)][Floor4_Ceil5(MemoryCoordX)];
return TransferMapVal2RealDis(MapDisVal);///<將地圖存的距離轉(zhuǎn)化為實(shí)際單位為毫米的值
}
/**
*@brief地圖中存的距離值的是整型數(shù),當(dāng)初保證小數(shù)點(diǎn)后的精度.定義了一個(gè)PRECISION參數(shù)，所以實(shí)際上單位為毫米的實(shí)際距離=地圖值/PRECISION*10，
*
*@parm MapValue 地圖中存的距離值
*/
float TransferMapVal2RealDis(int MapValue)
{
return MapValue/PRECISION*10;
}
};
?? 快捷鍵說(shuō)明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -