simulatordoc.cpp

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

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

#include "simulatorDoc.h"

#include "RobotXR4000.h"
#include "RobotLabmate.h"
#include "RobotScout.h"
#include "RobotForage.h"
#include "RobotHolonomic.h"
#include "RobotNonHolonomic.h"
#include "RandomGenerator.h"
#include "NormalGenerator.h"
#include "MapView.h"
#include "MapPath.h"
#include "const.h"
#include "DlgForageInit.h"
#include "Obstacle.h"
#include "ObstaclePolygon.h"
#include "ObstacleCircle.h"
#include "ObstacleRect.h"
#include "SimulatorView.h"
#include "MainFrm.h"
#include "myglobals.h"
#include <math.h>
#include <time.h>


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

/////////////////////////////////////////////////////////////////////////////
// CSimulatorDoc

IMPLEMENT_DYNCREATE(CSimulatorDoc, CDocument)

BEGIN_MESSAGE_MAP(CSimulatorDoc, CDocument)
	//{{AFX_MSG_MAP(CSimulatorDoc)
	ON_COMMAND(ID_SENSOR_RANGE, OnDrawRange)
	ON_UPDATE_COMMAND_UI(ID_SENSOR_RANGE, OnUpdateDrawRange)
	ON_COMMAND(ID_DRAW_ERASED, OnDrawErased)
	ON_UPDATE_COMMAND_UI(ID_DRAW_ERASED, OnUpdateDrawErased)
	ON_COMMAND(ID_APPLICATIONS_FORAGE, OnApplicationsForage)
	ON_COMMAND(ID_SELECT, OnSelect)
	ON_UPDATE_COMMAND_UI(ID_SELECT, OnUpdateSelect)
	ON_COMMAND(ID_CREATE_OBSTACLESCIRCLE, OnCreateObstacleCircle)
	ON_UPDATE_COMMAND_UI(ID_CREATE_OBSTACLESCIRCLE, OnUpdateCreateObstacleCircle)
	ON_COMMAND(ID_CREATE_OBSTACLESRECT, OnCreateObstacleRect)
	ON_UPDATE_COMMAND_UI(ID_CREATE_OBSTACLESRECT, OnUpdateCreateObstacleRect)
	ON_COMMAND(ID_CREATE_OBSTACLESPOLYGON5, OnCreateObstaclePolygon)
	ON_UPDATE_COMMAND_UI(ID_CREATE_OBSTACLESPOLYGON5, OnUpdateCreateObstaclePolygon)
	ON_COMMAND(ID_CREATEBOXPOLYGON, OnCreateBoxPolygon)
	ON_UPDATE_COMMAND_UI(ID_CREATEBOXPOLYGON, OnUpdateCreateBoxPolygon)
	ON_COMMAND(ID_CREATEBOXSQUARE, OnCreateBoxRect)
	ON_UPDATE_COMMAND_UI(ID_CREATEBOXSQUARE, OnUpdateCreateBoxRect)
	ON_COMMAND(ID_CREATELABMATE, OnCreateLabmate)
	ON_UPDATE_COMMAND_UI(ID_CREATELABMATE, OnUpdateCreateLabmate)
	ON_COMMAND(ID_CREATEBOXCIRCLE, OnCreateBoxCircle)
	ON_UPDATE_COMMAND_UI(ID_CREATEBOXCIRCLE, OnUpdateCreateBoxCircle)
	ON_COMMAND(ID_INIT_TIMER, OnInitTimer)
	ON_UPDATE_COMMAND_UI(ID_INIT_TIMER, OnUpdateInitTimer)
	ON_COMMAND(ID_FINISH_TIMER, OnFinishTimer)
	ON_UPDATE_COMMAND_UI(ID_FINISH_TIMER, OnUpdateFinishTimer)
	ON_COMMAND(ID_APPLICATIONS100, OnApplications100)
	ON_COMMAND(ID_DRAWPATH, OnDrawpath)
	ON_UPDATE_COMMAND_UI(ID_DRAWPATH, OnUpdateDrawpath)
	ON_COMMAND(ID_CREATEROBOTHOLONOMIC, OnCreaterobotholonomic)
	ON_UPDATE_COMMAND_UI(ID_CREATEROBOTHOLONOMIC, OnUpdateCreaterobotholonomic)
	ON_COMMAND(ID_CREATEROBOTNONHOLONOMIC, OnCreaterobotnonholonomic)
	ON_UPDATE_COMMAND_UI(ID_CREATEROBOTNONHOLONOMIC, OnUpdateCreaterobotnonholonomic)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CSimulatorDoc construction/destruction


// Constructor
CSimulatorDoc::CSimulatorDoc()
{
	m_globalMap = new CMapPath(FALSE);
	m_mapView.Create(IDD_MAPVIEW);
	m_mapView.SetWindowText("Map / Path");
	m_threadIsCreated = FALSE;
	m_box = new CBox();

}

CSimulatorDoc::~CSimulatorDoc()
{
	DeleteContents();
}

// Initialize doc and create the other trhead
void CSimulatorDoc::Initialize()
{
	m_selectedButton = SELECT;
	m_drawErased = FALSE;
	m_drawPath = FALSE;
	m_drawRange = TRUE;
	m_isRunning = FALSE;

	m_selectedObst = -1;
	m_selectedRobot = -1;


	// Creating the thread for updating robots
	if (!m_threadIsCreated) {
		m_updateThread = AfxBeginThread(UpdateThreadProc, this, THREAD_PRIORITY_NORMAL, 0, CREATE_SUSPENDED, NULL);
		m_threadIsCreated = TRUE;
	}
	else {
		m_updateThread->SuspendThread();
	}

}


/////////////////////////////////////////////////////////////////////////////
// CSimulatorDoc serialization

void CSimulatorDoc::Serialize(CArchive& ar)
{
	int i, numRobots;
	CPoint p;
	CRobot* robot;

	CDocument::Serialize(ar);
	
	m_globalMap->Serialize(ar);
	SerializeGlobals(ar);

	if (ar.IsStoring()) {
		ar << m_box;
		ar << m_selectedButton;
		ar << m_drawErased;
		ar << m_drawRange;
		ar << m_simTime;
		ar << m_selectedObst;
		ar << m_selectedRobot;

		ar << m_robots.GetSize();
		for(i=0; i<m_robots.GetSize(); i++){
			ar << m_robots[i];
		}

		CRobotForage::SerializeStatic(ar);
	}
	else {
		ar >> m_box;
		ar >> m_selectedButton;
		ar >> m_drawErased;
		ar >> m_drawRange;
		ar >> m_simTime;
		ar >> m_selectedObst;
		ar >> m_selectedRobot;

		ar >> numRobots;
		for(i=0; i<numRobots; i++){
			ar >> robot;
			m_robots.Add(robot);

			if (m_robots[i]->m_localMap->m_computeMap) {
				m_robots[i]->m_localView.Create(IDD_MAPVIEW);
				m_robots[i]->m_localView.SetWindowText(m_robots[i]->m_name);
			}

			m_robots[i]->Create(NULL, NULL, 0, CRect(1,1,2,2), m_view, m_robots[i]->m_id);
		}

		CRobotForage::SerializeStatic(ar);
	}
}

void CSimulatorDoc::SerializeGlobals(CArchive& ar)
{
	if (ar.IsStoring()) {
		ar << Kc_ro;
		ar << Cc_ro;
		ar << Kc_rr;
		ar << Cc_rr;
		ar << Kc_rb;
		ar << Cc_rb;
		ar << Kc_bo;
		ar << Cc_bo;

		ar << Ka_rb;
		ar << Ka_rg;
		ar << Kr_rr; 
		ar << Kr_ro;
		ar << boxAttraction;

		ar << Cr;
		ar << Cb;
		ar << Cbw;

		ar << intStep;
		ar << BoxMass;
	}
	else {
		ar >> Kc_ro;
		ar >> Cc_ro;
		ar >> Kc_rr;
		ar >> Cc_rr;
		ar >> Kc_rb;
		ar >> Cc_rb;
		ar >> Kc_bo;
		ar >> Cc_bo;

		ar >> Ka_rb;
		ar >> Ka_rg;
		ar >> Kr_rr; 
		ar >> Kr_ro;
		ar >> boxAttraction;

		ar >> Cr;
		ar >> Cb;
		ar >> Cbw;

		ar >> intStep;
		ar >> BoxMass;
	}
}

/////////////////////////////////////////////////////////////////////////////
// CSimulatorDoc diagnostics

#ifdef _DEBUG

void CSimulatorDoc::AssertValid() const
{
	CDocument::AssertValid();
}

void CSimulatorDoc::Dump(CDumpContext& dc) const
{
	CDocument::Dump(dc);
}
#endif //_DEBUG


// Add a new robot
void CSimulatorDoc::AddRobot(CRobot *robot)
{
//	CSingleLock sLock(&(m_mutex));
//	sLock.Lock();

	// Adding Robot
	int pos = m_robots.Add(robot);
	CString aux;
	aux.Format("Robot %3d", pos);
	m_robots[pos]->m_name = aux;
	
	// Creating the "window" so that the wobot will be able to receive windows messages
	m_robots[pos]->Create(NULL, NULL, 0, CRect(1,1,2,2), m_view, m_robots[pos]->m_id);

	// Initializing members
	if (m_robots[pos]->m_localMap->m_computeMap) {
		m_robots[pos]->m_localMap->m_originReal = CPoint(round(m_robots[pos]->m_x), round(m_robots[pos]->m_y));
		m_robots[pos]->m_localMap->ComputeMap();
		m_robots[pos]->m_localMap->ComputePath();

		m_robots[pos]->m_localView.Create(IDD_MAPVIEW);
		m_robots[pos]->m_localView.SetWindowText(m_robots[pos]->m_name);
	}

	if (m_globalMap->m_computeMap)
		m_globalMap->m_originReal = CPoint(round(m_robots[pos]->m_x), round(m_robots[pos]->m_y));

//	sLock.Unlock();

	// Sending message to the others informin that a new robots was created
	CControlMsg *controlMsg;
	controlMsg = new CControlMsg;
	controlMsg->m_code = NEWROBOT;
	controlMsg->m_from = m_robots[pos]->m_id;
	controlMsg->m_to = 0;
	
	m_robots[pos]->SendControlMsg(controlMsg);
}


// Add a new obstacles
void CSimulatorDoc::AddObstacle(CObstacle *globalObstacle)
{
	int pos, i;
	CObstacle *obst;

//	CSingleLock sLock(&(m_mutex));
//	sLock.Lock();

	pos = m_globalMap->m_obstacles.Add(globalObstacle);

	//Inserting obstacle in the local maps of the robot
	for(i=0; i<m_robots.GetSize(); i++){
		// Getting the "real" (runtime) of the obstacle class
		CRuntimeClass* pRuntimeClass = globalObstacle->GetRuntimeClass();
		obst = (CObstacle *) pRuntimeClass->CreateObject();
		obst->SetObstacle(globalObstacle);
		m_robots[i]->m_localMap->m_obstacles.Add(obst);
	}
	
	// Computing the Map and the Path
	m_globalMap->ComputeMap();
	m_globalMap->ComputePath();

//	sLock.Unlock();

	if (m_globalMap->m_isComputed)
		//( (CSimulatorView *)m_view )->m_mapView.Invalidate();
		m_mapView.Invalidate();
}

// Clean structures
void CSimulatorDoc::DeleteContents() 
{
//	CSingleLock sLock(&(m_mutex));
//	sLock.Lock();

	m_globalMap->DeleteContents();

	for(int i=0; i<m_robots.GetSize(); i++){
		m_robots[i]->DeleteContents();
	}

	m_robots.RemoveAll();	
	m_simTime = 0;
	m_box->m_exist = FALSE;