robotlabmate.cpp

多机器人合作中的动态角色分配仿真算法是多机器人合作领域的一个比较著名的仿真软件

		arm[4] = CPoint( round( m_armW[0] ),
			             round( m_armW[1] ) );

	}
	pDC->SelectStockObject(NULL_BRUSH);
	pDC->SelectObject(&RED_PEN);
	pDC->Polygon(arm,5);

	if (m_status == LEAD)
		pDC->SelectStockObject(BLACK_BRUSH);
	else if (m_status == FOLLOW)
		pDC->SelectObject(&RED);
	else if ( (m_status == PUSH)) 
		pDC->SelectObject(&YELLOW);
	else if (m_status == DOCK)
		pDC->SelectStockObject(GRAY_BRUSH);
	else if (m_status == WAIT)
		pDC->SelectObject(&MAGENTA);

	m_polygon.Draw(pDC);
}


void CRobotLabmate::Output(double simTime)
{
}

void CRobotLabmate::Serialize(CArchive& ar)
{
	short i;

	CRobotNonHolonomic::Serialize(ar);

	m_polygon.Serialize(ar);
	if (ar.IsStoring()) {
		ar << m_sizex;
		ar << m_sizey;
		ar << m_backupAngle;
		ar << m_backupAngleFinal;
		ar << m_beginLead;
		
		for(i=0; i<3; i++) {
			ar << m_armB[i];
			ar << m_armW[i];
			ar << m_armR[i];
			ar << m_armA[i];
			ar << m_tA2R[i];
			ar << m_tR2A[i];
			ar << m_force[i];
			ar << m_force_w[i];
		}
	}
	else {
		ar >> m_sizex;
		ar >> m_sizey;
		ar >> m_backupAngle;
		ar >> m_backupAngleFinal;
		ar >> m_beginLead;

		for(i=0; i<3; i++) {
			ar >> m_armB[i];
			ar >> m_armW[i];
			ar >> m_armR[i];
			ar >> m_armA[i];
			ar >> m_tA2R[i];
			ar >> m_tR2A[i];
			ar >> m_force[i];
			ar >> m_force_w[i];
		}
	}
}

// Initliaze the transformation matrix between box position and arm position
void CRobotLabmate::InitializeArmBox(CPoint p, CBox* box, double theta)
{
	m_armB[0] = p.x - box->m_x;
	m_armB[1] = p.y - box->m_y;
	m_armB[2] = theta- box->m_theta;
}

void CRobotLabmate::DetectBox(CBox *box)
{
	// For now, working for rectangular boxes
	DistanceCircleRect(m_armW[0], m_armW[1], 0, box->m_rect, m_distBox, m_angleBox, m_inContactBox, m_contactPoint);
}

// change robot orientation recomputing shape and arm position
void CRobotLabmate::SetOrientation(double angle)
{
	CRobot::SetOrientation(angle);
	ComputeArmPos();

	m_polygon.Recompute(m_x, m_y, m_theta);
}

// handler for control messages
LRESULT CRobotLabmate::OnControlMsg(WPARAM wParam, LPARAM lParam)
{
	CControlMsg *controlMsg, *newControlMsg;
	
	controlMsg = (CControlMsg *)wParam;

	switch (controlMsg->m_code){

	// somebody requested a leadership change. The robots will poll to see who will be the new leader
	case LCHANGE:
		m_v = 0;
		m_w = 0;

		newControlMsg = new CControlMsg;
		newControlMsg->m_code = POOL;
		newControlMsg->m_from = m_id;
		newControlMsg->m_x = m_x;
		newControlMsg->m_y = m_y;
		SendControlMsg(newControlMsg);

		break;

	// deciding who will be the next leader
	case POOL:
	case PUSHOK:
		double distGoal, distGoalLeader;
		m_controlMsgPoll[controlMsg->m_from] = controlMsg;
		m_controlMsgCount++;
		if (m_controlMsgCount == m_numRobots){
			// Deciding Leader
			distGoalLeader = 10000;
			m_leader = 0;
			for (short i=0; i<m_numRobots; i++) {
				distGoal = sqrt( (m_localMap->m_goalReal.x - m_controlMsgPoll[i]->m_x) * (m_localMap->m_goalReal.x - m_controlMsgPoll[i]->m_x) + (m_localMap->m_goalReal.y - m_controlMsgPoll[i]->m_y) * (m_localMap->m_goalReal.y - m_controlMsgPoll[i]->m_y) );
				if (distGoal < distGoalLeader) {
					m_leader = (short) m_controlMsgPoll[i]->m_from;
					distGoalLeader = distGoal;
				}
			}
			// Changing Leader
			ChangeLeader(m_leader, m_controlMsgPoll[m_leader]->m_x, m_controlMsgPoll[m_leader]->m_y);
			m_controlMsgCount = 0;
		}
		break;
	
	// dock phase is finished
	case DOCKOK:
		m_controlMsgCount++;
		if (m_controlMsgCount == m_numRobots) {
			m_status = PUSH;
			m_controlMsgCount = 0;
		}
		break;
		
	// the robot received the leadership from another robot
	case LPASS:
		m_v = 0;
		m_w = 0;
		if (controlMsg->m_to == m_id){
			m_status = LEAD;
			
			m_localMap->m_originReal = CPoint(round(m_x), round(m_y));
			m_localMap->ComputePath();
			
			newControlMsg = new CControlMsg;
			newControlMsg->m_code = NEWLEADER;
			newControlMsg->m_from = m_id;
			newControlMsg->m_x = m_x;
			newControlMsg->m_y = m_y;
			SendControlMsg(newControlMsg);
		}
		break;
		
	// the robot will do a backup
	case BACKUPMSG:
		m_v = 0;
		m_w = 0;
		if (controlMsg->m_to == m_id){
			m_status = BACKUP;
			m_beginLead = controlMsg->m_tstamp;

			m_backupAngleFinal = controlMsg->m_extra;
			m_backupAngle = 0;

			newControlMsg = new CControlMsg;
			newControlMsg->m_code = NEWLEADER;
			newControlMsg->m_from = m_id;
			newControlMsg->m_x = m_x;
			newControlMsg->m_y = m_y;
			SendControlMsg(newControlMsg);
		}
		break;
	
	// adapting controllers to the new leader
	case NEWLEADER:
		if (controlMsg->m_from != m_id)
			ChangeLeader((short) controlMsg->m_from, controlMsg->m_x, controlMsg->m_y);
		break;

	}

	CRobot::OnControlMsg(wParam, lParam);

	return 0L;
}

// forces applied on the box
void CRobotLabmate::ForceOnBoxes(CBox *box,double &fx, double &fy, double &ftheta)
{
	fx += m_force_w[0];
	fy += m_force_w[1];
	ftheta += m_force_w[2];
}

// backup maneuver to avoid obstacles
void CRobotLabmate::BackupManeuver(double simTime, double dt)
{
	simTime -= m_beginLead;

	if (simTime <= 2) {
		m_v = -simTime * 20;
		m_w = 0;
	}
	else if (simTime <= 4) {
		m_v = -40;
		m_w = 0;
	}
//	else if( fabs(m_backupAngle) < fabs(m_backupAngleFinal)){
//		m_w = (m_backupAngleFinal > 0) ? PI/20 : -PI/20;
//		m_v = -40;
//		m_backupAngle += m_w * dt;
//	}
	else {
		CControlMsg *controlMsg = new CControlMsg;
		controlMsg->m_code = LPASS;
		controlMsg->m_tstamp = simTime;
		controlMsg->m_extra = 0;
		controlMsg->m_from = m_id;
//		controlMsg->m_to = (m_id < 3) ? m_id + 2 : m_id - 2; // passing to the opposite robot
		controlMsg->m_to = 0;
		SendControlMsg(controlMsg);
		m_status = FOLLOW;
		m_v = 0;
		m_w = 0;
		return;
	}
}

// deciding who will be the next leader. Each robot runs the same algorithm, with data received
// from all robots. For now, the leader will be the robot that is closer to the goal.
void CRobotLabmate::ChangeLeader(short leader, double posX, double posY)
{
	if (leader != m_id){
		double aux_x = posX - m_x;
		double aux_y = posY - m_y;
		m_beta = m_theta - atan2(aux_y, aux_x);;
		m_status = FOLLOW;
		m_r = sqrt(aux_x * aux_x + aux_y * aux_y);

		// *** review
		if (fabs( aux_x * cos(m_theta) + aux_y * sin(m_theta) ) < 5){  // matrix is singular. Controller 2 must be used
			m_controlType = 2;
			if (sin(m_beta) > 0)
				m_r = -m_r;
		}
		else{
			m_controlType = 1;
		}
	}
	else
		m_status = LEAD;

	m_leader = leader;
}

// Data message handler. The followers use a byciclie controller, as described in
// Chaimowicz, Sugar, Kumar and Campos, ICRA2001
LRESULT CRobotLabmate::OnDataMsg(WPARAM wParam, LPARAM lParam)
{
	CDataMsg *dataMsg;
	
	dataMsg = (CDataMsg *)wParam;

	if (dataMsg->m_from != m_id)
		if ( m_controlType == 1){   
			m_v = dataMsg->m_v * cos(dataMsg->m_theta - m_theta + m_beta) / cos(m_beta);
			m_w = (dataMsg->m_v / m_r)* sin(dataMsg->m_theta - m_theta) / cos(m_beta); 

			// lookahead controller
			//m_v += m_armR[0] - (MAX_ARM_DISP - 10);
			//m_w += (m_armR[1]/(MAX_ARM_DISP - 10) );

		}
		else {
			m_v = dataMsg->m_v + dataMsg->m_w * m_r;
			m_w = dataMsg->m_w;
		}
	
	return 0L;
}