mappath.cpp

基于vc 的环境下机器人自主避障的算法 图形处理 具有载物功能

//////////////////////////////////////////////////////////////////////
// MuRoS - Multi Robot Simulator
//
// Luiz Chaimowicz
// GRASP Lab. University of Pennsylvania
// VERLab - DCC - UFMG - Brasil
//
// MapPath.cpp: implementation of the CMapPath class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "simulator.h"
#include "MapPath.h"

#include "obstacleCircle.h"
#include "obstacleRect.h"
#include "obstaclePolygon.h"

#include <math.h>

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif


IMPLEMENT_SERIAL( CMapPath, CObject, 1 )

int CMapPath::m_sizeX = 200;
int CMapPath::m_sizeY = 200;
int CMapPath::m_resX = 5000 / m_sizeX;
int CMapPath::m_resY = 5000 / m_sizeY;
float CMapPath::m_cellDist = (float) m_resX;

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CMapPath::CMapPath()
{
	m_obstacles.RemoveAll();
	m_computeMap = FALSE;
	m_isComputed = FALSE;
}


CMapPath::CMapPath(BOOL computeMap)
{
	m_goalReal  = CPoint(-1,-1);
	m_originReal = CPoint(-1,-1);
	m_obstacles.RemoveAll();
	m_computeMap = computeMap;
	m_isComputed = FALSE;

	//Allocate memory for the map structure only if the map should be created
	if (m_computeMap)
		AllocateMaps();
}

// Copy constructor
CMapPath::CMapPath(const CMapPath *map, BOOL computeMap)
{
	int i;
	CObstacle *obst;

	m_goalReal = map->m_goalReal;
	m_originReal = map->m_originReal;
	m_computeMap = computeMap;
	m_isComputed = FALSE;

	//create obstacles according to their runtime type
	m_obstacles.RemoveAll();
	for(i=0; i<map->m_obstacles.GetSize(); i++){
		
		CRuntimeClass* pRuntimeClass = map->m_obstacles[i]->GetRuntimeClass();
		obst = (CObstacle *) pRuntimeClass->CreateObject();
		obst->SetObstacle(map->m_obstacles[i]);
		m_obstacles.Add(obst);
	}

	if (m_computeMap)
		AllocateMaps();
}

CMapPath::~CMapPath()
{
	if (m_isComputed)
		FreeMaps();
}


// Allocate memory for the maps and grassfire structures
void CMapPath::AllocateMaps()
{
	int i,j;

	m_freeSpace = (float **)malloc(m_sizeX*sizeof(float*));
	m_map = (float **)malloc(m_sizeX*sizeof(float*));

	for (i = 0; i < m_sizeX; i++) {
		m_freeSpace[i] = (float *)malloc(m_sizeY*sizeof(float));
		m_map[i] = (float *)malloc(m_sizeY*sizeof(float));
	}

	for (i=0;i<m_sizeX;i++)
		for (j=0;j<m_sizeY;j++){
			m_freeSpace[i][j] = 0.0;
			m_map[i][j] = 0.0;
		}

	m_line = (float **)malloc(20*sizeof(float*));
	for (i = 0; i < 20; i++) 
		m_line[i] = (float *)malloc(3*sizeof(float));

	m_lineSize = 0;
	for (i=0; i<3; i++)
		m_line[0][i] = -1;
}

// Deallocate memory for the maps and grassfire structures
void CMapPath::FreeMaps()
{
	for (int i = 0; i < m_sizeX; i++) {
		free(m_freeSpace[i]);
		free(m_map[i]);
	}
	free(m_freeSpace);
	free(m_map);

	for (i = 0; i < 20; i++) 
		free(m_line[i]);
	free(m_line);

	m_path.RemoveAll();
}

void CMapPath::Serialize(CArchive& ar)
{
	int i, j, aux;
	CObstacle* obst;
	CPoint p;
	
	CObject::Serialize(ar);

	if (ar.IsStoring()) {
		ar << m_goalReal;
		ar << m_originReal;
		ar << m_isComputed;
		ar << m_computeMap;

		if (m_computeMap) {
			if (m_isComputed) {
				for (i=0;i<m_sizeX;i++)
					for (j=0;j<m_sizeY;j++)
						ar << m_map[i][j];

				ar << m_oldDist;
				ar << m_currentLine;
				ar << m_lineSize;
				ar << m_setVelocity;
				ar >> m_maxValue;
				for (i=0; i<m_lineSize; i++){
					ar << m_line[i][0];
					ar << m_line[i][1];
					ar << m_line[i][2];
				}
				
				ar << m_path.GetSize();
				for(i=0; i<m_path.GetSize(); i++)
					ar << m_path[i];
			}
		}
		
		ar << m_obstacles.GetSize();
		for(i=0; i<m_obstacles.GetSize(); i++) {
			ar << m_obstacles[i];
		}
	}
	else {
		ar >> m_goalReal;
		ar >> m_originReal;
		ar >> m_isComputed;
		ar >> m_computeMap;

		if (m_computeMap) {
			AllocateMaps();
			if (m_isComputed) {
				for (i=0;i<m_sizeX;i++)
					for (j=0;j<m_sizeY;j++)
						ar >> m_map[i][j];

				ar >> m_oldDist;
				ar >> m_currentLine;
				ar >> m_lineSize;
				ar >> m_setVelocity;
				ar >> m_maxValue;
				for (i=0; i<m_lineSize; i++){
					ar >> m_line[i][0];
					ar >> m_line[i][1];
					ar >> m_line[i][2];
				}
				
				ar >> aux;
				for(i=0; i<aux; i++) {
					ar >> p;
					m_path.Add(p);
				}
			}
		}

		ar >> aux;
		for(i=0; i<aux; i++){
			ar >> obst;
			m_obstacles.Add(obst);
		}
	}
}


// Initialize strcutures with the obstacle positions for the grassfire algorithm
// The map structures are 200x200, while th ereal environment is 5000x5000. So its
// is necessary to convert between "real coordiantes" and "matrix coordinate". The
// same is valid in other methods, for example, ComputePath.
void CMapPath::InitFreeSpace()
{
	int i, j, k;
	int x0, y0, x1, y1;

	for (i=0;i<m_sizeX;i++)
		for (j=0;j<m_sizeY;j++){
			m_freeSpace[i][j] = 0.0;
	}

	for(k=0; k<m_obstacles.GetSize(); k++){
		if (m_obstacles[k]->m_visited){
			x0 = m_obstacles[k]->m_rect.TopLeft().x / m_resX;
			y0 = m_obstacles[k]->m_rect.TopLeft().y / m_resY;
			x1 = m_obstacles[k]->m_rect.BottomRight().x / m_resX;
			y1 = m_obstacles[k]->m_rect.BottomRight().y / m_resY;

			// safe region (marking two cells arround the obstacles as occupied)
			for (i=x0-2;i<=x1+2;i++)
				for (j=y0-2;j<=y1+2;j++)
					if ( (i>=0) && (i<m_sizeX) && (j>=0) && (j<m_sizeY) && (m_freeSpace[i][j] != -1) )
						m_freeSpace[i][j] = -2.0;

			// marking obstacle regions as occupied
			for (i=x0;i<=x1;i++)
				for (j=y0;j<=y1;j++)
					m_freeSpace[i][j] = -1.0;
		}
	}
}

// If the map should be computed, call the initializatio and grassfire algorithm
void CMapPath::ComputeMap()
{
	// If the flag is true and the goal is defined, compute map.
	if ( (m_computeMap) && (m_goalReal.x != -1) ) {
		InitFreeSpace();
		GrassFire(m_freeSpace, m_sizeX, m_sizeY, m_cellDist, m_map);
		m_isComputed = TRUE;
	}
}

// After computing th map, compute the best path connecting the robot to the goal
// using the values computed by the grassfire algorithm
void CMapPath::ComputePath()
{
	int i, j, k=0;

	// Line coordinates: ay + bx + c = 0
	float a = 0;
	float b = 0;
	float c = 0;

	// Origin and goal in matrix coordinates
	CPoint originMatrix;
	CPoint goalMatrix;

	// Path in matrix coordinates
	CArray<CPoint, CPoint> pathMatrix;

	// if the origin is not defined or the map is not computed we can't compute the path
	if ( (!m_isComputed) || (m_originReal.x == -1) ) 
		return;

	// converting real coordinates -> matrix coordinates
	originMatrix.x = m_originReal.x / m_resX;
	originMatrix.y = m_originReal.y / m_resY;
	CPoint p = originMatrix;
	goalMatrix.x = m_goalReal.x / m_resX;
	goalMatrix.y = m_goalReal.y / m_resY;

	// to try to avoid the origin point be inside a obstacle or safe region of a obstacle
	if ( m_map[originMatrix.x][originMatrix.y] < 0 ) {
		double aux = 5000;
		for(i=p.x-3; i<=p.x+3; i++)
			for(j=p.y-3; j<=p.y+3; j++)
				if (m_map[i][j] > 0 && m_map[i][j] < aux) {
					originMatrix = CPoint(i,j);
					aux = m_map[i][j];
				}
	}

	// computing path in matrix coordinates
	p = originMatrix;
	CPoint newp = p;

	pathMatrix.RemoveAll();
	pathMatrix.Add(newp);

	while (p != goalMatrix) {
		for(i=p.x-1; i<=p.x+1; i++)
			for(j=p.y-1; j<=p.y+1; j++)
				if ( (m_map[i][j] < m_map[newp.x][newp.y]) && (m_map[i][j] >= 0) ){
					newp.x = i;
					newp.y = j;
				}

		pathMatrix.Add(newp);
		p = newp;
		
		if (k++ > (m_sizeX * m_sizeY / 2)){
			MessageBox(NULL, "There is no possible path", "", MB_OK);
			return;
		}
	}


	// Converting the computed path to real coordinates
	m_path.RemoveAll();
	for(i=0; i < pathMatrix.GetSize(); i++)
		m_path.Add(CPoint(pathMatrix[i].x * m_resX + m_resX / 2, pathMatrix[i].y * m_resY + m_resY / 2));

	// computing the line segments for the path
	m_lineSize = 0;
	for(i=1; i<m_path.GetSize(); i++){
		if ( (m_path[i].x != m_path[i-1].x) ){
			a = -(m_path[i].y - m_path[i-1].y)/(float) (m_path[i].x - m_path[i-1].x);
			b = 1;
			c = -m_path[i].y - a * m_path[i].x;
		}
		else{
			a = -1;
			b = 0;
			c = (float) m_path[i].x;
		}

		if ( (m_lineSize == 0) || (a != m_line[m_lineSize-1][0]) || 
								  (b != m_line[m_lineSize-1][1]) || 
								  (c != m_line[m_lineSize-1][2]) ){
			m_line[m_lineSize][0] = a;
			m_line[m_lineSize][1] = b;
			m_line[m_lineSize][2] = c;
			m_lineSize++;
		}

	}
	// Initializing structure so the robot will follow a new path.
	m_currentLine = 0;
	m_setVelocity = TRUE;
	m_oldDist = 10000;
}

void CMapPath::GrassFire(float ** freespace, int freespaceX, int freespaceY, float distance, float ** navi)
{
	// Code from Ben, based on the matlab code from Joel. 
	// Left as it was for compatibility.
  
  /*
    Given a NxM matrix called FREESPACE, which has a 0 in entry (i,j) if 
    that cell is freespace and a -1 if that space is occupied by an obstacle.
    
    scalars gx and gy  which are integers from 1 to M or N respectively,
    indicating the goal cell
    
    And a scalar DISTANCE which indicates the size of the cell 
    (all cells are square). 
    
    this function returns and NxM matrix of positive real numbers and
    -1.0s called NAVI.  By computing the gradient of this matrix one
    can develop a feedback control law for solving the planning
    problem.
    
    algorithm from David Lee's thesis "the map-building and exploration strategies of a simple sonar equipped robot" 1996, Oxford
    
    full analysis in McKerrow 1991 Introduction to robotics
  */
  
	//some counter variables
	int i, j, p;
  
    //spacing between adjacent cells vert or horiz
	float d1 = distance;

	//the number of altered cells in the last iteration */
	int changed;
	//temporary variable */
	float temp;

	CPoint goalMatrix;
	int gx = m_goalReal.x / m_resX;
	int gy = m_goalReal.y / m_resY;

	/* diagonal spacing */
	float d2 = (float) 1.41 * d1;

	/* temporary array for navigation function */
	float ** tmpNavi = (float **)malloc((freespaceX+2)*sizeof(float *));
	for (i = 0; i < freespaceX+2; i++) {
		tmpNavi[i] = (float *)malloc((freespaceY+2)*sizeof(float *));
		for (j = 0; j < freespaceY+2; j++) 
			tmpNavi[i][j] = (float)-1;
	}
  
	/* initial value for the navigation function, */
	for (i = 0; i < freespaceX; i++)
		for (j = 0; j < freespaceY; j++)
			if (freespace[i][j] >= 0)
				navi[i][j] = freespace[i][j] + freespaceX*freespaceY*d1;
			else 
				navi[i][j] = freespace[i][j]; //(float)-1;
	/* set the goal position to zero */
	navi[gx][gy] = 0;

	/* now copy this into the tmpNavi array for processing */
	for (i = 0; i < freespaceX; i++) 
		for (j = 0; j < freespaceY; j++) 
			tmpNavi[i+1][j+1] = navi[i][j];
  
	/*algorithm terminates when changed ==0*/
	changed = 1;

	/* begin main loop*/
	while(changed != 0 ) {
		changed = 0;
		/* begin the forward scan */
		for (i = 0; i < freespaceX+1; i++) {
			for (j = 0; j < freespaceY+1; j++) {
				if (tmpNavi[i][j]>=0) {
					for (p = -1; p < 2; p++) {
						if (tmpNavi[i-1][j+p]>=0) {
							if (p==0)
								temp = tmpNavi[i-1][j+p] + d1; 
							else
								temp = tmpNavi[i-1][j+p] + d2; 
	      
							if (temp < tmpNavi[i][j] ) {
								tmpNavi[i][j] = temp;
								changed++;
							}
						}
					}
					if  (tmpNavi[i][j-1]>=0) {
						temp = tmpNavi[i][j-1] + d1;
						if (temp < tmpNavi[i][j]) {
							tmpNavi[i][j] = temp;
							changed++;
						}
					}
				}
			}
		}
      
		/* begin the backward scan */
		for (i = freespaceX+1; i > 0; i--) {
			for (j = freespaceY+1; j > 0; j--) {
				if (tmpNavi[i][j]>=0) {
					for (p =-1; p < 2; p++) { 
						if (tmpNavi[i+1][j+p]>=0) {
							if (p==0)
								temp = tmpNavi[i+1][j+p] + d1;
							else
								temp = tmpNavi[i+1][j+p] + d2; 
	      
							if (temp < tmpNavi[i][j]) {
								tmpNavi[i][j] = temp;
								changed++;
							}
						}
					}
					if  (tmpNavi[i][j+1]>=0) {
						temp = tmpNavi[i][j+1] + d1;
						if (temp < tmpNavi[i][j]) {
							tmpNavi[i][j] = temp;
							changed++;
						}
					}
				}
			}
		}
	}

	/* now copy back into navi */
	m_maxValue = 0; 
	for (i = 0; i < freespaceX; i++) 
		for (j = 0; j < freespaceY; j++){
			navi[i][j] = tmpNavi[i+1][j+1];
			m_maxValue = max(navi[i][j], m_maxValue);
	}

  
	/* free up tmpNavi */
	for (i = 0; i < freespaceX; i++)
		free(tmpNavi[i]);
	free(tmpNavi);

}

// Clean all the structures
void CMapPath::DeleteContents()
{
	int i,j;

	m_isComputed = FALSE;
	m_setVelocity = TRUE;
	m_currentLine = 0;
	m_goalReal  = CPoint(-1,-1);
	m_originReal = CPoint(-1,-1);

	m_obstacles.RemoveAll();
	m_path.RemoveAll();

	if (m_computeMap) {
		for (i=0;i<m_sizeX;i++)
			for (j=0;j<m_sizeY;j++){
				m_freeSpace[i][j] = 0.0;
				m_map[i][j] = 0.0;
			}

		m_lineSize = 0;
		for (i=0; i<3; i++)
			m_line[0][i] = -1;
	}

}