robotforage.cpp

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

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

#include "stdafx.h"
#include "simulator.h"
#include "RobotForage.h"
#include "const.h"
#include <math.h>

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

#include "myglobals.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
IMPLEMENT_SERIAL(CRobotForage, CRobotHolonomic, 1 )

// Initializing static variables
short CRobotForage::m_totalRescuedItems = 0;
CArray<CPoint, CPoint> CRobotForage::m_allItems;
CSize CRobotForage::m_area(0,0);

// empty constructor
CRobotForage::CRobotForage()
{
}

// Constructtor
CRobotForage::CRobotForage(double x, double y, double theta, short status, short id, CString name, short levelInfo) : CRobotHolonomic(x, y, theta, status, id, name)
{
	m_rescuedItems = 0;
	m_attached = -1;
	m_itemList.RemoveAll();

	m_levelInfo = levelInfo;
	m_trackPoint = CPoint(-1,-1);

}

CRobotForage::~CRobotForage()
{
	m_itemList.RemoveAll();
}

// Update the robot position. The forage function controls the random foraging, while the 
// track function divide the sarch area among the robots.
void CRobotForage::Update(CArray<CRobot*, CRobot*> *robots, double simTime, double dt, CBox* box, CMapPath *mapPath)
{
	CRobot::Update(robots, simTime, dt, box, mapPath);

	// Use differente strategies according to the initialization
	if (m_levelInfo <= 2)
		Forage(robots, dt, mapPath, box, simTime);
	else
		Track(robots, dt, mapPath, box, simTime);
		
	m_x += m_vx * dt;
	m_y += m_vy * dt;
}

// Draw the robot, changing the color acording to the robot state
void CRobotForage::Draw(CDC *pDC)
{
	CPoint line[2];

	pDC->SelectObject(&RED_PEN);

	if (m_status == TRANSPORT)
		pDC->SelectStockObject(BLACK_BRUSH);
	else if (m_status == WANDER)
		pDC->SelectStockObject(GRAY_BRUSH);
	else if (m_status == GETITEM)
		pDC->SelectObject(&RED);
	else if (m_status == GOPOSITION)
		pDC->SelectObject(&CYAN);
	
	pDC->Ellipse(CRect( round(m_x - m_radius), round(m_y - m_radius), round(m_x + m_radius), round(m_y + m_radius) ) );
}

void CRobotForage::Serialize(CArchive& ar)
{
	CPoint p;

	CRobotHolonomic::Serialize(ar);

	if (ar.IsStoring()) {
		m_itemList.Serialize(ar);
		ar << m_levelInfo;
		ar << m_rescuedItems;
		ar << m_attached;
	}
	else {
		m_itemList.Serialize(ar);
		ar >> m_levelInfo;
		ar >> m_rescuedItems;
		ar >> m_attached;
	}
}

// Forage. The robots move randomly through the environment searching for items to be retrieved.
// The robots can be in 3 differnt states: WANDER, GETITEM and TRASNPORT, and depending on the 
// initialization options, they can use or not communication and maintain a list of the items.
// The ontrol is made using potential functions.
void CRobotForage::Forage(CArray<CRobot*, CRobot*> *robots, double dt, CMapPath *map, CBox* box, double simTime)
{
	short i;

	double fx = 0;
	double fy = 0;

	double dist;
	double angle;

	CPoint p;

	// Repulsive forces from other robots
	for(i=0; i<robots->GetSize(); i++) 
		if (robots->GetAt(i)->m_id != m_id)
			ForceFromRobots(robots->GetAt(i), fx, fy);

	// Repulsive forces from obstacles
	for (i=0; i<m_localMap->m_obstacles.GetSize(); i++){
		ForceFromObstacles(m_localMap->m_obstacles[i], fx, fy);
	}

	// Atractive force from items
	for(i=0; i < GetNumItems(); i++){
		p = GetItem(i);
		if (p.x > 0) { // item is available
			dist = sqrt( ( p.x - m_x) * (p.x - m_x) + (p.y - m_y) * (p.y - m_y) );
			if (dist < m_sensorRange) {
				// If carryng a item and find other, put in the list for future visits (depending on the initial options)
				if ( ( (m_status == TRANSPORT) || (m_status == GETITEM) ) && (i != m_attached)){
					BOOL inList = InsertPoint(p);
					// If using communication and the item is not on the list, 
					// send a message to others with the item position
					if ( (m_levelInfo == 2) && (!inList) ){  
						CControlMsg *controlMsg;
						controlMsg = new CControlMsg;
						controlMsg->m_code = NEWPOINT;
						controlMsg->m_from = m_id;
						controlMsg->m_to = 0;
						controlMsg->m_x = p.x;
						controlMsg->m_y = p.y;
						SendControlMsg(controlMsg);			
					}
				}
				else if (m_status == WANDER){
					// Detect a item
					m_status = GETITEM;
					m_attached = i;
				}
			}
		}
	}

	if (m_status == GETITEM) {
		p = GetItem(m_attached);
		dist = sqrt( ( p.x - m_x) * (p.x - m_x) + (p.y - m_y) * (p.y - m_y) );
		angle = atan2( (p.y - m_y), (p.x - m_x) ) + PI;
		if (p.x > 0) {
			fx += Ka_rb * dist * cos(angle-PI);
			fy += Ka_rb * dist * sin(angle-PI);
			
			if (dist < m_radius){
				// Fisically get the point;
				m_status = TRANSPORT;
				m_localMap->m_goalReal = map->m_goalReal;
				SetItem(m_attached, CPoint(round(-m_x), round(-m_y)) );

				// If using communication, send a message to others telling that 
				// you are going to get this item
				if (m_levelInfo == 2) {
					CControlMsg *controlMsg;
					controlMsg = new CControlMsg;
					controlMsg->m_code = GOFORIT;
					controlMsg->m_from = m_id;
					controlMsg->m_to = 0;
					controlMsg->m_x = p.x;
					controlMsg->m_y = p.y;
					SendControlMsg(controlMsg);			
				}
			}
		}
		else
			m_status = WANDER;
	}


	// Get next item position in the list (random if the list is empty)
	if ( ( (m_status == WANDER) && CloseToPosition(m_localMap->m_goalReal,10) ) || (m_localMap->m_goalReal.x == -1) ){
		m_localMap->m_goalReal = GetNextPoint();
	}

	// Atractive force from goal
	if ( (m_status == TRANSPORT) || (m_status == WANDER) ) {
		dist = sqrt( (m_localMap->m_goalReal.x - m_x) * (m_localMap->m_goalReal.x - m_x) + (m_localMap->m_goalReal.y - m_y) * (m_localMap->m_goalReal.y - m_y) );
		angle = atan2( (m_localMap->m_goalReal.y - m_y), (m_localMap->m_goalReal.x - m_x) );
		
		fx += Ka_rg * dist * cos(angle);
		fy += Ka_rg * dist * sin(angle);

		if ( (m_attached != -1) && (m_status == TRANSPORT) ){
			SetItem(m_attached, CPoint(round(-m_x), round(-m_y) ) );
			if (dist < 20) {
				m_attached = -1;
				m_status = WANDER;
				m_localMap->m_goalReal = GetNextPoint();
				m_rescuedItems++;
				m_totalRescuedItems++;
			}
		}
	}

	// Damping
	fx += -Cr * m_vx;
	fy += -Cr * m_vy;

	// Computing velocities
	m_vx += fx * dt;
	m_vy += fy * dt;


	//Saturation (mantaining scale between vx and vy)
	double factor;
	if ( (m_vx > 200) || (m_vy > 200) )
		if (m_vx > m_vy) {
		factor = m_vx / 200;
			m_vx = m_vx / factor;
			m_vy = m_vy / factor;
		}
		else{
			factor = m_vy / 200;
			m_vy = m_vy / factor;
			m_vx = m_vx / factor;
		}
	if ( (m_vx < -200) || (m_vy < -200) )
		if (m_vx < m_vy) {
			factor = m_vx / -200;
			m_vx = m_vx / factor;
			m_vy = m_vy / factor;
		}
		else{
			factor = m_vy / -200;
			m_vy = m_vy / factor;
			m_vx = m_vx / factor;
		}
}

// Get an item position from the list, informing the others that this item will be taken
// If there is no item in the list, pik a ramdom position
CPoint CRobotForage::GetNextPoint()
{
	CPoint p;
	double dist, minDist;
	short pos;

	minDist = 100000;
	pos = 0;

	for(short i=0; i<m_itemList.GetSize(); i++){
		p = m_itemList[i];
		dist = sqrt( ( p.x - m_x) * (p.x - m_x) + (p.y - m_y) * (p.y - m_y) );
		if (dist < minDist) {
			minDist = dist;
			pos = i;
		}
	}

	if (m_itemList.GetSize() > 0){
		p = m_itemList.GetAt(pos);
		m_itemList.RemoveAt(pos);

		if (m_levelInfo == 2) {
			CControlMsg *controlMsg;
			controlMsg = new CControlMsg;
			controlMsg->m_code = GOFORIT;
			controlMsg->m_from = m_id;
			controlMsg->m_to = 0;
			controlMsg->m_x = p.x;
			controlMsg->m_y = p.y;
			SendControlMsg(controlMsg);			
		}
	}
	else{
		p.x = (rand() % GetArea().cx) + 1;
		p.y = (rand() % GetArea().cy) + 1;
	}
	return p;

}

// Insert a new point position in the list
BOOL CRobotForage::InsertPoint(CPoint p)