配置文件c++程序。打包文件

//#include "stdafx.h" #include "MrpGridMap.h" #include <memory.h> #include <fstream> #include <stdio.h> #include <stdlib.h> #include <assert.h> #include <math.h> using namespace std; double LaserGetAngleIncrement(int num_beams) { if (num_beams == 181 || num_beams == 180) return M_PI / 180.0; /* 1 degree = M_PI/180 */ else if (num_beams == 361 || num_beams == 360) return M_PI / 360.0; /* 0.5 degrees = M_PI/360 */ else if (num_beams == 401 || num_beams == 400) return M_PI / 720.0; /* 0.25 degrees = M_PI/720 */ else return M_PI / ((double) (num_beams-1)); } MrpPose LocalToWorld(const MrpPose localCoordinate, MrpPose localPos) { MrpPose newPos; newPos.m_X = (localCoordinate.m_X + localPos.m_X * cos(localCoordinate.getThRad()) - localPos.m_Y * sin(localCoordinate.getThRad())); newPos.m_Y = (localCoordinate.m_Y + localPos.m_X * sin(localCoordinate.getThRad()) + localPos.m_Y * cos(localCoordinate.getThRad())); newPos.setThRad(localCoordinate.getThRad() + localPos.getThRad()); newPos.setThRad(atan2(sin(newPos.getThRad()),cos(newPos.getThRad()))); return newPos; } MrpPose LocalToWorld(const MrpPose localCoordinate, double localRange, double localAngleRad) { MrpPose localPos; localPos.m_X = localRange*cos(localAngleRad); localPos.m_Y = localRange*sin(localAngleRad); localPos.setThRad(localAngleRad); return LocalToWorld(localCoordinate, localPos); } MRPEXPORT MrpGridMap::MrpGridMap(void):m_gridData(0),m_bInitialized(false) { m_leastExpandRange = 5; m_xsize = 2*m_leastExpandRange; m_ysize = 2*m_leastExpandRange; m_xmin = m_ymin = -m_leastExpandRange; m_xmax = m_ymax = m_leastExpandRange; m_resolution = 1.0f; //0.05 emptyProb1 = 0.35f; //0.30 blockProb1 = 0.60f; //0.70 emptyProb2 = 0.45f; blockProb2 = 0.35f; m_commentOrigin =0; m_commentDescription = 0; m_numPlaces = 0; m_MapFileName = 0; } MRPEXPORT MrpGridMap::MrpGridMap(const MrpGridMap& src) { m_leastExpandRange = src.m_leastExpandRange; m_xsize =src.m_xsize; m_ysize = src.m_ysize; m_xmin = src.m_xmin; m_xmax =src.m_xmax; m_ymin=src.m_ymin; m_ymax=src.m_ymax; m_resolution = src.m_resolution; emptyProb1 = src.emptyProb1; blockProb1 = src.blockProb1; emptyProb2 = src.emptyProb2; blockProb2 = src.blockProb2; m_commentOrigin =src.m_commentOrigin; m_commentDescription = src.m_commentDescription; m_numPlaces = src.m_numPlaces; m_MapFileName =src.m_MapFileName; m_bInitialized=src.m_bInitialized; m_gridData = (float**)calloc((int)m_xsize, sizeof(float*)); for (int i=0;i<(int)m_xsize;i++) { m_gridData[i] = (float*)calloc((int)m_ysize, sizeof(float)); for (int j=0;j<(int)m_ysize;j++) { m_gridData[i][j] = src.m_gridData[i][j]; } } } MRPEXPORT void MrpGridMap::Initialize(float mapWidth, float mapHeight, float mapResolution, int blockWidth) { m_gridData = 0; m_bInitialized = false; emptyProb1 = 0.35f; //0.30 blockProb1 = 0.60f; //0.70 emptyProb2 = 0.45f; blockProb2 = 0.35f; m_resolution = mapResolution; m_leastExpandRange = blockWidth; this->ReAllocateMap(mapWidth, mapHeight); } MRPEXPORT void MrpGridMap::UnInitialize() { if (m_gridData!=0) { for (int i=0;i<(int)m_xsize;i++) { free(m_gridData[i]); } free(m_gridData); } for(list<MrpMapPlace*>::iterator it = m_places.begin();it!=m_places.end();it++) { delete (*it); } m_objects.clear(); for(list<MrpMapObj*>::iterator it = m_objects.begin();it!=m_objects.end();it++) { delete (*it); } m_objects.clear(); m_bInitialized = false; } MRPEXPORT MrpGridMap::~MrpGridMap(void) { this->UnInitialize(); } MRPEXPORT void MrpGridMap::ReAllocateMap(float mapWidth, float mapHeight) { this->ReleaseGridMap(); m_xsize = mapWidth; m_ysize = mapHeight; m_xmin = -m_xsize/2.0; m_xmax = m_xsize/2.0; m_ymin = -m_ysize/2.0; m_ymax = m_ysize/2.0; m_gridData = (float**)calloc((int)m_xsize, sizeof(float*)); for (int i=0;i<(int)m_xsize;i++) { m_gridData[i] = (float*)calloc((int)m_ysize, sizeof(float)); for (int j=0;j<(int)m_ysize;j++) { m_gridData[i][j] = -2.0; } } m_bInitialized = true; } void MrpGridMap::ReleaseGridMap() { if (m_gridData != 0) { for (int i=0;i<(int)m_xsize;i++) { free(m_gridData[i]); } free(m_gridData); m_gridData = 0; } //m_bInitialized = false; } MRPEXPORT void MrpGridMap::CopyGridData(float** data, int xsize, int ysize) { m_gridData = (float**)calloc((int)xsize, sizeof(float*)); for (int i=0;i<(int)xsize;i++) { m_gridData[i] = (float*)calloc((int)ysize, sizeof(float)); for (int j=0;j<(int)ysize;j++) { m_gridData[i][j] = data[i][j]; } } } bool MrpGridMap::ExpandGridMap(ExpandDirection dir, int range) { if (!m_bInitialized) { return false; } float tmpXmin = m_xmin,tmpXmax=m_xmax,tmpYmin=m_ymin,tmpYmax=m_ymax; float tmpXsize=m_xsize,tmpYsize=m_ysize; if (range < m_leastExpandRange) { range = m_leastExpandRange; } if (dir == UP || dir == DOWN) { tmpYsize += range; if (dir == UP) { tmpYmax += range; } else { tmpYmin -= range; } } else { tmpXsize += range; if (dir == RIGHT) { tmpXmax += range; } else { tmpXmin -= range; } } float** tmpGridData = (float**)calloc((int)tmpXsize, sizeof(float*)); for (int i=0;i<(int)tmpXsize;i++) { tmpGridData[i] = (float*)calloc((int)tmpYsize, sizeof(float)); } if (dir == LEFT) { for (int i=0;i<(int)m_xsize;i++) { memcpy(*(tmpGridData+i+(int)range),*(m_gridData+i),m_ysize*sizeof(float)); } for (int i=0;i<range;i++) { for (int j=0;j<(int)m_ysize;j++) { tmpGridData[i][j] = -2.0; } } } else if (dir == RIGHT) { for (int i=0;i<(int)m_xsize;i++) { memcpy(*(tmpGridData+i),*(m_gridData+i),m_ysize*sizeof(float)); } for (int i=(int)m_xsize;i<(int)(m_xsize+range);i++) { for (int j=0;j<(int)m_ysize;j++) { tmpGridData[i][j] = -2.0; } } } else if(dir == UP) { for (int i=0;i<(int)m_xsize;i++) { memcpy(*(tmpGridData+i)+(int)range,*(m_gridData+i),m_ysize*sizeof(float)); } for (int i=0;i<(int)m_xsize;i++) { for (int j=0;j<range;j++) { tmpGridData[i][j] = -2.0; } } } else { for (int i=0;i<(int)m_xsize;i++) { memcpy(*(tmpGridData+i),*(m_gridData+i),m_ysize*sizeof(float)); } for (int i=0;i<(int)m_xsize;i++) { for (int j=(int)m_ysize;j<(int)(m_ysize+range);j++) { tmpGridData[i][j] = -2.0; } } } for (int i=0;i<(int)m_xsize;i++) { free(m_gridData[i]); } free(m_gridData); m_gridData = tmpGridData; m_xmin = tmpXmin; m_xmax = tmpXmax; m_ymin = tmpYmin; m_ymax = tmpYmax; m_xsize = tmpXsize; m_ysize = tmpYsize; return true; } int MrpGridMap::GetSpacePro(const float& x,const float& y, float& cellPro) const { int ix = (int)(x/m_resolution); int iy = (int)(y/m_resolution); if (ix>=m_xmin && ix<m_xmax && iy>m_ymin && iy<=m_ymax) { cellPro = m_gridData[ix-(int)m_xmin][(int)m_ymax-iy]; if (cellPro == -2) return 1; else if(cellPro >= 0.8) return 2; else return 3; } else { return 0; } } MRPEXPORT bool MrpGridMap::AdjustMapSizeWithPoints(MrpPose* points, int numOfPoints) { bool adjusted = false; float xmin=10000, ymin=10000, xmax=-10000, ymax=-10000; MrpPose tmpPoint; for (int i=0;i<numOfPoints;i++) { if (points[i].m_X<xmin) { xmin = points[i].m_X; } else if (points[i].m_X>xmax) { xmax = points[i].m_X; } if (points[i].m_Y<ymin) { ymin = points[i].m_Y; } else if (points[i].m_Y>ymax) { ymax = points[i].m_Y; } } tmpPoint.m_X = xmin; tmpPoint.m_Y = ymin; bool tmpBoolValue = AdjustMapSizeWithOnePoint(tmpPoint); if (!adjusted) { adj