//#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
src.rar （54个子文件） 
MrpSensorReading.cpp 8KB MrpLog.cpp 5KB MrpIPM3OldInterface.cpp 11KB MrpJoyHandler.cpp 10KB MrpCompositeMap.CPP 26KB MrpBasePacket.cpp 11KB MrpIPM3Interface.cpp 11KB MrpConfigArg.cpp 17KB MrpTcpConnection.cpp 6KB MrpRangeDevice.cpp 14KB MrpRobotPacketSender.cpp 7KB MrpIPM2Interface.cpp 8KB MrpDevice.cpp 229B MrpDataMan.cpp 1KB MrpRobotType.cpp 2KB MrpWorld.cpp 6KB MrpRobotPacketReceiver.cpp 9KB MrpRobot.cpp 34KB MrpSyncLoop.cpp 3KB MrpRobotParams.cpp 15KB MrpConn.cpp 980B MrpTransform.cpp 2KB MrpArg.cpp 6KB MrpRobotPacket.cpp 3KB MrpDeviceConnection.cpp 2KB MrpArgumentParser.cpp 14KB MrpCondition.cpp 4KB MrpThread_WIN.cpp 4KB MrpTopoMap.cpp 102B MrpASyncTask.cpp 778B MrpSonarPaketReciever.cpp 5KB MrpTaskFlags.cpp 303B MrpTCPInterface.cpp 16KB MrpConfig.cpp 35KB MrpSimpleConnector.cpp 16KB MrpWorldConfig.cpp 8KB VoiceParamInform.cpp 1KB MrpSerialConnection_WIN.cpp 10KB MrpRangeBuffer.cpp 14KB MrpSocket_WIN.cpp 13KB MrpSonarPacket.cpp 5KB MrpFunctorASynctask.Cpp 320B MrpUtil.cpp 20KB MrpSonarDevice.cpp 14KB MrpMutex_WIN.cpp 4KB MrpThread.cpp 3KB MrpArgumentBuilder.cpp 11KB MrpFileParser.cpp 10KB MrpSocket.cpp 5KB MrpCommTask.cpp 6KB MrpSyncTask.cpp 11KB MrpRangeDeviceThreaded.cpp 2KB MrpKeyHandler.cpp 6KB MrpGridMap.cpp 37KB资源评论
修梅2013-09-23看了，没有运行的执行文件
