robot.cpp

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

//////////////////////////////////////////////////////////////////////
// MuRoS - Multi Robot Simulator
//
// Luiz Chaimowicz
// GRASP Lab. University of Pennsylvania
// VERLab - DCC - UFMG - Brasil
//
// Robot.cpp : implementation file
//
/////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "simulator.h"
#include "Robot.h"

#include "RobotXR4000.h"
#include "RobotLabmate.h"
#include "ObstaclePolygon.h"
#include "ObstacleCircle.h"
#include "Obstaclerect.h"
#include "const.h"
#include "boxcircle.h"
#include "boxrect.h"
#include "boxpolygon.h"
#include  <math.h>

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

/////////////////////////////////////////////////////////////////////////////
// CRobot
IMPLEMENT_SERIAL( CRobot, CWnd, 1 )

// Empty constructor
CRobot::CRobot()
{
	m_localMap = new CMapPath(FALSE);
}

// Constructor
CRobot::CRobot(double x, double y, double theta, short status, short id, CString name)
{
	m_x = x;
	m_y = y;
	m_theta = theta;
	m_status = status;
	m_id = id;
	m_leader = m_id;
	m_name = name;

	m_radius = 0;

	m_beta = 0;
	m_r = 0;
	m_numRobots = 0;
	m_timer = 0;

	m_controlMsgCount = 0;
	
	m_sensorRange = 100;
	m_numObstacles = 0;

	m_localMap = new CMapPath(FALSE);
	m_localMap->m_originReal = CPoint(round(m_x), round(m_y));

	m_inContactBox = FALSE;
	m_path.RemoveAll();
}

CRobot::~CRobot()
{
}

//-----------------------------------------------
// Serialization
//-----------------------------------------------
void CRobot::Serialize(CArchive& ar)
{
	CObject::Serialize(ar);
	m_localMap->Serialize(ar);

	if (ar.IsStoring()) {
		ar << m_x;
		ar << m_y;
		ar << m_theta;
		ar << m_radius;
		ar << m_status;
		ar << m_id;
		ar << m_leader;
		ar << m_name;
		ar << m_beta;
		ar << m_r;
		ar << m_controlType;
		ar << m_sensorRange;
		ar << m_numObstacles;
		ar << m_numRobots;
		ar << m_controlMsgCount;
		ar << m_timer;
		ar << m_rect;
		ar << m_inContactBox;
		ar << m_distBox;

	}
	else {
		ar >> m_x;
		ar >> m_y;
		ar >> m_theta;
		ar >> m_radius;
		ar >> m_status;
		ar >> m_id;
		ar >> m_leader;
		ar >> m_name;
		ar >> m_beta;
		ar >> m_r;
		ar >> m_controlType;
		ar >> m_sensorRange;
		ar >> m_numObstacles;
		ar >> m_numRobots;
		ar >> m_controlMsgCount;
		ar >> m_timer;
		ar >> m_rect;
		ar >> m_inContactBox;
		ar >> m_distBox;

	}
}

//--------------------------------------
// Virtual Functions, redefined in the subclasses
//--------------------------------------
void CRobot::Update(CArray<CRobot*, CRobot*> *robots, double simTime, double dt, CBox* box, CMapPath *mapPath)
{
	Sensor(mapPath, box);
	DetectBox(box);
}

void CRobot::Draw(CDC *pDC)
{
}

void CRobot::Output(double simTime)
{
}

void CRobot::ForceFromObstacles(CObstacle *obst, double &fx, double &fy)
{
}

void CRobot::ForceFromBoxes(CBox *box, double &fx, double &fy)
{
}

void CRobot::ForceOnBoxes(CBox *box,double &fx, double &fy, double &ftheta)
{
}

void CRobot::ForceFromRobots(CRobot *robot, double &fx, double &fy)
{
}

CString CRobot::GetInfo()
{
	return "";
}

//---------------------------------------------------
// Generic Robot methods
//---------------------------------------------------

// Update local map with the obstacles that are inside the sensor range
void CRobot::Sensor(CMapPath *globalMap, CBox *box)
{
	short j;
	short detect;

	m_numObstacles = 0;

	for(j=0; j<globalMap->m_obstacles.GetSize(); j++){
		// The function returns:
		// 0  - out of range
		// 1  - detected
		// 2  - detected for the first time
		// -1 - detected erased for the first time
		detect = DetectObstacles(m_localMap->m_obstacles[j], globalMap->m_obstacles[j]->m_erased);
		
		if (detect > 0)
			m_numObstacles++;
		
		// detected for the first time (normal or erased): recompute map
		if ( (detect == 2) || (detect == -1) ) {
			m_localMap->m_originReal = CPoint(round(m_x), round(m_y));
			m_localMap->ComputeMap();
			m_localMap->ComputePath();
			if (m_localMap->m_isComputed)
				m_localView.Invalidate();
		}
	}
}


// This function detects if an obstacle is inside the sensor range computing the
// distance and angle to the obstacle and updating its information in the local
// list of obstacles. For now, the distance works fine for a 'circle' robots
// but need to be redefined for the labmate
//
// The function returns:
// 0  - Obstacle out of range
// 1  - Obstacle detected
// 2  - Obstacle detected for the first time
// -1 - Obstacle detected erased for the first time
short CRobot::DetectObstacles(CObstacle *obst, BOOL globalErased)
{

	short detect;
	BOOL inRange;
	BOOL intersect;
	CPoint p1, p2;
	double dist;

	// obst->m_dist is alway positive when inRange. -1 when out of range
	// when in contact, obst->m_dist is the distance inside the obstacle
	obst->m_dist = -1;
	obst->m_inContact = FALSE;
	inRange = FALSE;
	detect = 0;



	// For each type of obstacle, test if the obstacle is in range and compute the 
	// distance and angle for the obstacle
	if (obst->IsKindOf( RUNTIME_CLASS(CObstacleCircle) ) ) {
		DistanceCircleCircle(obst->m_center.x, obst->m_center.y, m_x, m_y, obst->m_radius, 0, dist, obst->m_angle);
		
		// if there obstacle is in sensor range
		if (dist < m_sensorRange) {
			inRange = TRUE;
			if (!globalErased){
				obst->m_dist =  dist - m_radius;
				obst->m_inContact = (obst->m_dist < 0);
				obst->m_dist = fabs(dist);
			}
		}
	}
	else if (obst->IsKindOf(RUNTIME_CLASS(CObstacleRect) ) ) {
		
		// if the obstacle is in sensor range
		intersect = ((CObstacleRect *) obst)->m_polygon->IntersectCircle(CPoint(round(m_x), round(m_y)), (short) m_sensorRange, p1, p2);
		DistanceCircleCircle(obst->m_center.x, obst->m_center.y, m_x, m_y, 0, 0, dist, obst->m_angle);
		if (intersect || (dist < m_sensorRange) ) {
			inRange = TRUE;		
			if (!globalErased) {
				CPoint temp;
				DistanceCircleRect(m_x, m_y, m_radius, obst->m_rect, obst->m_dist, obst->m_angle, obst->m_inContact, temp);
			}
		}
	}
	else if (obst->IsKindOf(RUNTIME_CLASS(CObstaclePolygon) ) ) {
		
		// if the obstacle is in sensor range (detecting intersection between obstacle and sensor circle or distance between the centers)
		intersect = ((CObstaclePolygon *) obst)->m_polygon->IntersectCircle(CPoint(round(m_x), round(m_y)), (short) m_sensorRange, p1, p2);
		DistanceCircleCircle(obst->m_center.x, obst->m_center.y, m_x, m_y, 0, 0, dist, obst->m_angle);
		if (intersect || (dist < m_sensorRange) ) {
			inRange = TRUE;
			if (!globalErased){
				// computing distance in case of contact
				double px, py;
				short n = ((CObstaclePolygon *) obst)->m_polygon->IntersectCircle(CPoint(round(m_x), round(m_y)), (short) m_radius, p1, p2);
				if (n > 0){
					obst->m_inContact = TRUE;
					if (n == 1) {
						px = p1.x;
						py = p1.y;
					}
					else {
						px = (p1.x + p2.x)/(double)2.0;
						py = (p1.y + p2.y)/(double)2.0;
					}

					obst->m_angle = atan2(py - m_y, px - m_x);
					if (n <= 2) {
						obst->m_dist  = m_radius - sqrt( (py - m_y) * (py - m_y) + (px - m_x) * (px - m_x) );
					}
					else {
						obst->m_dist  = sqrt( (py - m_y) * (py - m_y) + (px - m_x) * (px - m_x) );
					}
					if (obst->m_dist < 0)
						obst->m_dist = 0.01;
				}
			}
		}
	}

	// If the obstacle is inside the range, update its informaton
	if (inRange) {
		// if the obstacle is not erased
		if (!globalErased){
			detect = 1;
			obst->m_inRange = TRUE;
			if (!obst->m_visited) {
				detect = 2;
				obst->m_erased = FALSE;
				obst->m_visited = TRUE;
			}
		}
		else {
			// Mark the obstacle as erased;
			if (!obst->m_erased) {
				detect = -1;
				obst->m_erased = TRUE;
				obst->m_inRange = FALSE;
				obst->m_visited = FALSE;
			}
		}
	}
	else {
		// Mark the obstacle as out of range
		obst->m_inRange = FALSE;
	}

	return detect;
}


// Detect if the robot is in contact with the box
// This works for "circle" robots and is redifined in the Labmate
void CRobot::DetectBox(CBox *box)
{
	m_inContactBox = FALSE;
	m_distBox = -1;

	if (!box->m_exist)
		return;

	if (box->IsKindOf(RUNTIME_CLASS(CBoxCircle) )) {
		DistanceCircleCircle(box->m_x, box->m_y, m_x, m_y, box->m_radius, m_radius, m_distBox, m_angleBox);
		
		if (m_distBox < 0){
			m_distBox =  fabs(m_distBox);
			m_inContactBox = TRUE;
		}
	}
	else {
		CPolygon *poly;
		if (box->IsKindOf(RUNTIME_CLASS(CBoxRect) ) )
			poly = ( (CBoxRect *) box)->m_polygon;
		else if (box->IsKindOf(RUNTIME_CLASS(CBoxPolygon) ) )
			poly = ( (CBoxPolygon *) box)->m_polygon;

		// computing distance in case of contact
		double px, py;
		CPoint p1, p2;
		short n = poly->IntersectCircle(CPoint(round(m_x), round(m_y)), (short) m_radius, p1, p2);
		if (n > 0){
			m_inContactBox = TRUE;
			if (n == 1) {
				px = p1.x;
				py = p1.y;
			}
			else {
				// getting the middle point between the 2 contact points
				px = (p1.x + p2.x)/(double)2.0;
				py = (p1.y + p2.y)/(double)2.0;
			}

			m_angleBox = atan2(py - m_y, px - m_x);
			m_distBox  = m_radius - sqrt( (py - m_y) * (py - m_y) + (px - m_x) * (px - m_x) );
			m_contactPoint = CPoint(round(px), round(py));
			if (m_distBox < 0)
				m_distBox = 0.01;
		}
		else {
			//DistanceCircleCircle(box->m_x, box->m_y, m_x, m_y, 0, m_radius, m_distBox, m_angleBox);
			poly->ShortDistance(m_x, m_y, m_distBox, m_angleBox);
		}
	}
}


void CRobot::SetOrientation(double angle)
{
	m_theta = angle;
}


void CRobot::OpenOutput()
{
	m_out = fopen(m_name + ".out", "w");
}

void CRobot::CloseOutput()
{
	fclose(m_out);
}

void CRobot::DeleteContents() 
{
	if (m_localMap->m_isComputed) 
		m_localView.DestroyWindow();
	
	delete(m_localMap);
	DestroyWindow();
}

//-------------------------------------------------------
// Data and Control messages
//-------------------------------------------------------

void CRobot::SendControlMsg(CControlMsg *msg)
{
	GetParent()->SendMessage(WM_CONTROLMSG, (WPARAM) msg);
}

void CRobot::PostControlMsg(CControlMsg *msg)
{
	GetParent()->PostMessage(WM_CONTROLMSG, (WPARAM) msg);
}

void CRobot::SendDataMsg(CDataMsg *msg)
{
	GetParent()->SendMessage(WM_DATAMSG, (WPARAM) msg);
}

/////////////////////////////////////////////////////////////////////////////
// CRobot message handlers

BEGIN_MESSAGE_MAP(CRobot, CWnd)
	ON_MESSAGE(WM_CONTROLMSG, OnControlMsg)
	ON_MESSAGE(WM_DATAMSG, OnDataMsg)
	//{{AFX_MSG_MAP(CRobot)
		// NOTE - the ClassWizard will add and remove mapping macros here.
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()


// Handle that treats the control messages received by the robots
// It can be redefined in the subclasses
LRESULT CRobot::OnControlMsg(WPARAM wParam, LPARAM lParam)
{
	CControlMsg *controlMsg, *newControlMsg;
	
	controlMsg = (CControlMsg *)wParam;

	switch (controlMsg->m_code){

	case NEWROBOT:
		if (m_numRobots <= controlMsg->m_from)
			m_numRobots = (short) controlMsg->m_from+1;
		break;

	case DELETEROBOT:
		m_numRobots = (short) controlMsg->m_extra;
	break;

	case BOXLOCKED:
		m_controlMsgCount++;
		if (m_controlMsgCount == m_numRobots){
			m_status = TRANSPORT;
			m_controlMsgCount = 0;
		}
		break;

	case LOSTBOX:
		m_controlMsgCount++;
		if (m_controlMsgCount == m_numRobots){
			m_status = DOCK;
			m_controlMsgCount = 0;
		}
		break;

	case TIMEOUT:
		m_controlMsgCount = 0;
		if (m_status == WAIT) {
			m_status = TRANSPORT;
			m_timer = 0;
		}
		else {
			m_status = STOPPED;

			newControlMsg = new CControlMsg;
			newControlMsg->m_code = LOSTBOX;
			newControlMsg->m_from = m_id;
			newControlMsg->m_to = 0;

			PostControlMsg(newControlMsg);
		}

		break;

	}

	return 0L;
}

// Handle that treats the data messages received by the robots
// It is redefined in the subclasses
LRESULT CRobot::OnDataMsg(WPARAM wParam, LPARAM lParam)
{
	return 0L;
}