Beispiel #1
0
void ControllerImpl::disconnectToService(std::string name)
{
	BaseService *srv =  getService(name);
	if (srv != NULL) {
		// End service loop
		srv->endServiceLoop();
	}
}
char* MyController::sendSceneInfo(std::string header, int camID) {
		m_my->setWheelVelocity(0.0, 0.0);				

		Vector3d myPos;
		m_my->getPosition(myPos);
		double x = myPos.x();
		double z = myPos.z();
		double theta = calcHeadingAngle();			// y方向の回転は無しと考える	

		// カメラがついているリンク名取得
		std::string link = ""; //"WAIST_LINK0"; //m_my->getCameraLinkName(3);
		if (camID < 3) {
			link = m_my->getCameraLinkName(camID);
		} else {
			link = "WAIST_LINK0";
		}
		//const dReal *lpos = m_my->getParts(link.c_str())->getPosition();
		Vector3d lpos;
		m_my->getParts(link.c_str())->getPosition(lpos);

		// カメラの位置取得(リンク座標系)
		Vector3d cpos;
		m_my->getCamPos(cpos, camID);

		// カメラの位置(絶対座標系, ロボットの回転はないものとする)
		printf("linkpos: %lf %lf %lf \n", lpos.x(), lpos.y(), lpos.z());
		printf("camerapos: %lf %lf %lf \n", cpos.x(), cpos.y(), cpos.z());
		Vector3d campos(lpos.x() + cpos.z() * sin(DEG2RAD(theta)), 
										lpos.y() + cpos.y(), 
										lpos.z() + cpos.z() * cos(DEG2RAD(theta)));
		//Vector3d campos(cpos.x(), cpos.y(), cpos.z());
		
		// カメラの方向取得(ロボットの回転,関節の回転はないものとする)
		Vector3d cdir;
		m_my->getCamDir(cdir, camID);

		char *replyMsg = new char[1024];
		sprintf(replyMsg, "%s %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf", 
																			header.c_str(), x, z, theta, campos.x(), campos.y(), campos.z(), cdir.x(), cdir.y(), cdir.z());
		printf("%s \n", replyMsg);
		m_srv->sendMsgToSrv(replyMsg);

		m_my->setWheelVelocity(0.0, 0.0);				
	
	return replyMsg;
}
Beispiel #3
0
// static member function (callback)
void BaseService::ServiceMain(DWORD dwArgc, LPTSTR* lpszArgv) {

    // Get a pointer to the C++ object
    BaseService* pService = m_pThis;
    
    // Register the control request handler
    pService->m_hServiceStatus =
		RegisterServiceCtrlHandler(pService->m_szServiceName, Handler);

    if (pService->m_hServiceStatus == NULL)
        return;

	// Start the initialization
	pService->SetStatus(SERVICE_START_PENDING);

	if (pService->OnInitialize()) {
		pService->SetStatus(SERVICE_RUNNING);
        pService->Run();
	}
    // Tell the service manager we are stopped
    pService->SetStatus(SERVICE_STOPPED);

}
double MyController::onAction(ActionEvent &evt)
{
	if(!checkService("RecogTrash")){
		m_srv == NULL;
	}
	
	if(m_srv == NULL){
		// ゴミ認識サービスが利用可能か調べる
		if(checkService("RecogTrash")){
			// ゴミ認識サービスに接続
			m_srv = connectToService("RecogTrash");
		}
	}


	//if(evt.time() < m_time) printf("state: %d \n", m_state);
	switch(m_state) {
		// 初期状態
		case 0: {
			if(m_srv == NULL){
				// ゴミ認識サービスが利用可能か調べる
				if(checkService("RecogTrash")){
					// ゴミ認識サービスに接続
					m_srv = connectToService("RecogTrash");
				}
			} else if(m_srv != NULL && m_executed == false){  
				//rotate toward upper
				m_my->setJointVelocity("LARM_JOINT4", -m_jvel, 0.0);
				m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0);
				// 50°回転
				m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time();
				m_state = 5;
				m_executed = false;			
			}
			break;
		}


		case 5: {
			if(evt.time() > m_time && m_executed == false) {
				//m_my->setJointVelocity("LARM_JOINT1", 0.0, 0.0);
				m_my->setJointVelocity("LARM_JOINT4", 0.0, 0.0);
				m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
				sendSceneInfo("Start");				
				//m_srv->sendMsgToSrv("Start");
				printf("Started! \n");
				m_executed = true;
			}
			break;
		}

		// 物体の方向が帰ってきた
		case 20: {
			// 送られた座標に回転する
			m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
			//printf("debug %lf %lf \n", evt.time(), m_time);
			m_state = 21;
			m_executed = false;
			break;
		}
		
		case 21: {
			// ロボットが回転中
			//printf("debug %lf %lf \n", evt.time(), m_time);
			if(evt.time() > m_time && m_executed == false) {
				// 物体のある場所に到着したので、車輪と止め、関節を回転し始め、物体を拾う
				m_my->setWheelVelocity(0.0, 0.0);
				// 物体のある方向に回転した
				//printf("目的地の近くに移動します %lf %lf %lf \n", nextPos.x(), nextPos.y(), nextPos.z());	
				// 送られた座標に移動する
				m_time = goToObj(nextPos, m_vel*4, m_range, evt.time());
				//m_time = goToObj(nextPos, m_vel*4, 40, evt.time());
				m_state = 22;
				m_executed = false;
			}
			break;
		}

		case 22: {
			// 送られた座標に移動した
			if(evt.time() > m_time && m_executed == false) {
				m_my->setWheelVelocity(0.0, 0.0);
				printf("止める \n");
				Vector3d myPos;
				m_my->getPosition(myPos);
				double x = myPos.x();
				double z = myPos.z();
				double theta = 0;			// y方向の回転は無しと考える		
				char replyMsg[256];

				bool found = recognizeNearestTrash(m_tpos, m_tname);
				// ロボットのステートを更新
			
				if (found == true) {
					m_state = 500;				
					m_executed = false;		
					//printf("m_executed = false, state = 500 \n");		
				} else {
					//printf("Didnot found anything \n");		
					m_state = 10;
					m_executed = false;
				}

			}
			break;
		}


		case 30: {
			// 送られた座標に回転する
			m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
			//printf("case 30 time: %lf \n", m_time);
			m_state = 31;
			m_executed = false;
			break;
		}

		// 物体を掴むために、ロボットの向く角度をズラス
		case 31: {
			if(evt.time() > m_time && m_executed == false) {
				Vector3d grabPos;
				//printf("斜めにちょっとずれた以前の時間 time: %lf \n", evt.time());
				if(calcGrabPos(nextPos, 20, grabPos)) {
					m_time = rotateTowardObj(grabPos, m_vel / 5, evt.time());
					printf("斜め grabPos :%lf %lf %lf \n", grabPos.x(), grabPos.y(), grabPos.z());
					printf("time: %lf \n", m_time);
				}
				m_state = 32;
				m_executed = false;				
			}
			break;
		}

		// 物体の方向が帰ってきた
		case 32: {
			if(evt.time() > m_time && m_executed == false) {
				// 物体のある場所に到着したので、車輪と止め、関節を回転し始め、物体を拾う
				m_my->setWheelVelocity(0.0, 0.0);
				//printf("回転を止めた evt.time %lf \n", evt.time());
				// 関節の回転を始める
				m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0);
				// 50°回転
				m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time();
				m_state = 33;
				m_executed = false;
			}
			break;
		}

		case 33: {
			// 関節回転中
			if(evt.time() > m_time && m_executed == false) {
				// 関節の回転を止める
				m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
				// 自分の位置の取得
				Vector3d myPos;
				m_my->getPosition(myPos);
				double x = myPos.x();
				double z = myPos.z();
				double theta = 0;									// y方向の回転は無しと考える	
				//物体を掴めるか掴めないかによって処理を分岐させる
				if(m_grasp) {											// 物体を掴んだ

					// 捨てたゴミをゴミ候補
					std::vector<std::string>::iterator it;
					// ゴミ候補を得る
					it = std::find(m_trashes.begin(), m_trashes.end(), m_tname);
					// 候補から削除する
					m_trashes.erase(it);		
					printf("erased ... \n");	

					// ゴミ箱への行き方と問い合わせする
					char replyMsg[256];

					// もっとも近いゴミ箱を探す
					//bool found = recognizeNearestTrashBox(m_trashBoxPos, m_trashBoxName);

					// ゴミを置くべき座標を探す
					bool found = findPlace2PutObj(m_trashBoxPos, m_tname); 
					if(found) {
						// ゴミ箱が検出出来た
						std::cout << "trashboxName " << m_trashBoxName << std::endl;
						sprintf(replyMsg, "AskTrashBoxRoute %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf", 
																	x, z, theta, m_trashBoxPos.x(), m_trashBoxPos.y(), m_trashBoxPos.z());
					} else {
						sprintf(replyMsg, "AskTrashBoxPos %6.1lf %6.1lf %6.1lf", 
																	x, z, theta);
					}

					m_srv->sendMsgToSrv(replyMsg);	
					m_executed = true;	
							
				} else {					// 物体を掴めなかった、次に探す場所を問い合わせる
					// ゴミを掴めなかったもしくはゴミが無かった、次にゴミのある場所を問い合わせする
					// 逆方向に関節の回転を始める
					m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0);
					// 50°回転
					m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time();
					m_state = 34;				
					m_lastFailedTrash = m_tname;
					m_executed = false;
				}		
				
			}
			break;
		}
		
		case 34: {
			if(evt.time() > m_time && m_executed == false) {
				// 関節の回転を止める
				m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);

				// 自分の位置の取得
				Vector3d myPos;
				m_my->getPosition(myPos);
				double x = myPos.x();
				double z = myPos.z();
				double theta = 0;			
				char replyMsg[256];
				sprintf(replyMsg, "AskObjPos %6.1lf %6.1lf %6.1lf", x, z, theta);
				printf("case 34 debug %s \n", replyMsg);

				m_srv->sendMsgToSrv(replyMsg);			
				m_executed = true;
			}
			break;
		}

		case 40: {
			// 送られた座標に回転する
			m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
			m_state = 41;
			m_executed = false;
			break;
	  }

		case 41: {
			// 送られた座標に回転中
			if(evt.time() > m_time && m_executed == false) {
				// 送られた座標に移動する
				printf("目的地の近くに移動します %lf %lf %lf \n", nextPos.x(), nextPos.y(), nextPos.z());	
				m_time = goToObj(nextPos, m_vel*4, m_range, evt.time());
				m_state = 42;
				m_executed = false;
			}
			break;
	  }

		case 42: {
			// 送られた座標に移動中
			if(evt.time() > m_time && m_executed == false) {
				// 送られた座標に到着した、 自分の位置の取得
				Vector3d myPos;
				m_my->getPosition(myPos);
				double x = myPos.x();
				double z = myPos.z();
				double theta = 0;			// y方向の回転は無しと考える	
				char replyMsg[256];

				// もっとも近いゴミ箱を探す
				bool found = recognizeNearestTrashBox(m_trashBoxPos, m_trashBoxName);
				if(found) {
					// ゴミ箱が検出出来た
					std::cout << "trashboxName " << m_trashBoxName << std::endl;
					sprintf(replyMsg, "AskTrashBoxRoute %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf", 
																x, z, theta, m_trashBoxPos.x(), m_trashBoxPos.y(), m_trashBoxPos.z());
				} else {
					sprintf(replyMsg, "AskTrashBoxPos %6.1lf %6.1lf %6.1lf", 
																x, z, theta);
				}

				m_srv->sendMsgToSrv(replyMsg);
				m_executed = true;
			}
			break;
		}

		case 50: {
			if(evt.time() > m_time && m_executed == false) {
				Vector3d throwPos;
				//printf("斜めにちょっとずれた以前の時間 time: %lf \n", evt.time());

				// 送られた座標に到着した、 自分の位置の取得
				Vector3d myPos;
				m_my->getPosition(myPos);
				printf("robot pos %lf %lf \n", myPos.x(), myPos.z());


				// grasp中のパーツを取得します
				CParts *parts = m_my->getParts("RARM_LINK7");	
				// grasp中のパーツの座標を取得出来れば、回転する角度を逆算出来る。
				Vector3d partPos;
				if (parts->getPosition(partPos)) {
					printf("parts pos before rotate %lf %lf %lf \n", partPos.x(), partPos.y(), partPos.z());
				} 



				//if(calcGrabPos(nextPos, 20, throwPos)) {				
				if(calcGrabPos(nextPos, 20, throwPos)) {
					m_time = rotateTowardObj(throwPos, m_vel / 5, evt.time());
					printf("斜めに捨てる throwPos :%lf %lf %lf \n", throwPos.x(), throwPos.y(), throwPos.z());
					//printf("time: %lf \n", m_time);
				}
				m_state = 51;
				m_executed = false;				
			}
		
			break;
		}

		case 51: {
		  // ゴミ箱に到着したので、車輪を停止し、アームを下ろし、物体をゴミ箱に捨てる準備をする
			m_my->setWheelVelocity(0.0, 0.0);
			// grasp中のパーツを取得します
		  CParts *parts = m_my->getParts("RARM_LINK7");		
		  
			// grasp中のパーツの座標を取得出来れば、回転する角度を逆算出来る。
			Vector3d partPos;
			if (parts->getPosition(partPos)) {
				printf("parts pos after rotate %lf %lf %lf \n", partPos.x(), partPos.y(), partPos.z());
			} 

		  // releaseします
		  parts->releaseObj();		
			// ゴミが捨てられるまで少し待つ
		  sleep(1);
			// grasp終了
		  m_grasp = false;

			//confirmThrewTrashPos(m_threwPos, m_tname);
			//printf("捨てた座標: %lf %lf %lf \n", m_threwPos.x(), m_threwPos.y(), m_threwPos.z());	
			
			// 関節の回転を始める
		  m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0);
		  m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time() + 1.0;   
			m_state = 52;
			m_executed = false;
			break;
		}

		case 52: {
			// 関節が回転中
			if(evt.time() > m_time && m_executed == false) {
				// 関節が元に戻った、関節の回転を止める
				m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
				// 自分の位置の取得
				Vector3d myPos;
				m_my->getPosition(myPos);
				double x = myPos.x();
				double z = myPos.z();
				double theta = 0;										// y方向の回転は無しと考える	
				
				// ゴミを捨てたので、次にゴミのある場所を問い合わせする
				char replyMsg[256];
				
				if(recognizeNearestTrash(m_tpos, m_tname)) {
					m_executed = false;
					// 物体が発見された

					m_state = 500;
				} else {
					sprintf(replyMsg, "AskObjPos %6.1lf %6.1lf %6.1lf", x, z, theta);
					m_srv->sendMsgToSrv(replyMsg);
					m_executed = true;
				}
				
			}
			break;
		}
		
		case 100: {
			m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
			m_my->setWheelVelocity(0.0, 0.0);
			break;
		}


		case 800: {
			if(evt.time() > m_time && m_executed == false) {
				sendSceneInfo();
				m_executed = true;
			}
			break;
		}

		case 805: {
			if(evt.time() > m_time && m_executed == false) {
				// 送られた座標に移動する
				double range = 0;
				m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
				m_state = 807;
				m_executed = false;
			}
			break;
	  }

		case 807: {
			if(evt.time() > m_time && m_executed == false) {
				m_my->setWheelVelocity(0.0, 0.0);				
				printf("移動先 x: %lf, z: %lf \n", nextPos.x(), nextPos.z());				
				m_time = goToObj(nextPos, m_vel*4, m_range, evt.time());
				m_state = 810;
				m_executed = false;
			}
			break;
		}

		case 810: {
			// 送られた座標に移動中
			if(evt.time() > m_time && m_executed == false) {
				m_my->setWheelVelocity(0.0, 0.0);
				m_time = rotateTowardObj(m_lookingPos, m_rotateVel, evt.time());
				m_executed = false;
				m_state = 815;
			}
			break;
		}
		
		case 815: {
			// 送られた座標に移動中
			if(evt.time() > m_time && m_executed == false) {
				m_my->setWheelVelocity(0.0, 0.0);				
				sendSceneInfo();
				printf("sent data to SIGViewer \n");				
				m_executed = true;
			}
			break;
		}

		case 920: {
			// 送られた座標に回転する
			m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
			m_state = 921;
			m_executed = false;
			break;
		}

		case 921: {
			// ロボットが回転中
			if(evt.time() > m_time && m_executed == false) {
				m_my->setWheelVelocity(0.0, 0.0);
				m_executed = false;
			}
			break;
		}

		default: {
			break;
		}

	}

  return 0.05;      
}  
double MyController::onAction(ActionEvent &evt)
{
	// サービスが使用可能か定期的にチェックする
	bool available = checkService("CleanUpReferee");

	if(!available && m_ref != NULL) m_ref = NULL;

	// 使用可能  
	else if(available && m_ref == NULL){
		// サービスに接続  
		m_ref = connectToService("CleanUpReferee");
	}

	// 自分の位置取得
	Vector3d myPos;
	m_my->getPosition(myPos);

	// 衝突中の場合,衝突が継続しているかチェック
	if(colState){
		CParts *parts = m_my->getMainParts();
		bool state = parts->getCollisionState();

		// 衝突していない状態に戻す
		if(!state) colState = false;
	}

	int entSize = m_entities.size();
	for(int i = 0; i < entSize; i++){

		// ロボットまたはゴミ箱の場合は除く
		if(m_entities[i] == "robot_000"  ||
		   m_entities[i] == "trashbox_0" ||
		   m_entities[i] == "trashbox_1" ||
		   m_entities[i] == "trashbox_2"){
			continue;
		}
		// エンティティ取得
		SimObj *ent = getObj(m_entities[i].c_str());

		// 位置取得
		Vector3d tpos;
		ent->getPosition(tpos);

		// ゴミ箱からゴミを結ぶベクトル
		Vector3d vec(tpos.x()-myPos.x(), tpos.y()-myPos.y(), tpos.z()-myPos.z());

		// ゴミがゴミ箱の中に入ったかどうか判定
		if(abs(vec.x()) < tboxSize_x/2.0 &&
		   abs(vec.z()) < tboxSize_z/2.0 &&
		   tpos.y() < tboxMax_y     &&
		   tpos.y() > tboxMin_y     ){

			// ゴミがリリースされているか確認
			if(!ent->getIsGrasped()){

				std::string msg;	
				bool success = false;
				// 台の上に置く(成功)
				if(strcmp(ent->name(), "mayonaise_0") == 0 && tpos.y() != 57.85) {tpos.y(57.85); success = true;}
				else if(strcmp(ent->name(), "chigarette") == 0 && tpos.y() != 54.04){ tpos.y(54.04); success = true;}
				else if(strcmp(ent->name(), "chocolate") == 0 && tpos.y() != 51.15){ tpos.y(51.15); success = true;}
				else if(strcmp(ent->name(), "mugcup") == 0 && tpos.y() != 54.79){ tpos.y(54.79); success = true;}
				else if(strcmp(ent->name(), "petbottle_0") == 0 && tpos.y() != 67.45){ tpos.y(67.45); success = true;}
				else if(strcmp(ent->name(), "petbottle_3") == 0 && tpos.y() != 61.95){ tpos.y(61.95); success = true;}
				else if(strcmp(ent->name(), "clock") == 0 && tpos.y() != 56.150){ tpos.y(56.150); success = true;}
				else if(strcmp(ent->name(), "kettle") == 0 && tpos.y() != 60.650){ tpos.y(60.650); success = true;}
				// 台の上に置く(失敗)
				else if(strcmp(ent->name(), "petbottle_1") == 0 && tpos.y() != 67.45){ tpos.y(67.45);}
				else if(strcmp(ent->name(), "petbottle_2") == 0 && tpos.y() != 67.45){ tpos.y(67.45);}
				else if(strcmp(ent->name(), "petbottle_4") == 0 && tpos.y() != 61.95){ tpos.y(61.95);}
				else if(strcmp(ent->name(), "mayonaise_1") == 0 && tpos.y() != 57.85){ tpos.y(57.85);}
				else if(strcmp(ent->name(), "can_0") == 0 && tpos.y() != 55.335){ tpos.y(55.335);}
				else if(strcmp(ent->name(), "can_1") == 0 && tpos.y() != 55.335){ tpos.y(55.335);}
				else if(strcmp(ent->name(), "can_2") == 0 && tpos.y() != 57.050){ tpos.y(57.050);}
				else if(strcmp(ent->name(), "can_3") == 0 && tpos.y() != 57.050){ tpos.y(57.050);}
				else if(strcmp(ent->name(), "banana") == 0 && tpos.y() != 51.69){ tpos.y(51.69);}
				else if(strcmp(ent->name(), "apple") == 0 && tpos.y() != 54.675){ tpos.y(54.675);}
				else{continue;}

				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);
				ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
				ent->setPosition(tpos);
				usleep(100000);

				if(success){
					msg = "CleanUpReferee/";
					msg += ent->name();
					msg += " succeeded/1000";
				}
				else{
					msg = "CleanUpReferee/";
					msg += ent->name();
					msg += " failed/-600";

				}
	
				if(m_ref != NULL) {
					m_ref->sendMsgToSrv(msg.c_str());
				}
				else{
					LOG_MSG((msg.c_str()));
				}
			}
		}
	}

	return retValue;
}
Beispiel #6
0
// static member function (callback) to handle commands from the
// service control manager
void BaseService::Handler(DWORD dwControl) {

    // Get a pointer to the object
    BaseService* pService = m_pThis;
    DWORD currentState = pService->m_Status.dwCurrentState;

    switch (dwControl) {
    case SERVICE_CONTROL_STOP:
        pService->SetStatus(SERVICE_STOP_PENDING);
        pService->OnStop();
		currentState = SERVICE_STOPPED;
        break;

    case SERVICE_CONTROL_PAUSE:
		pService->SetStatus(SERVICE_PAUSE_PENDING);
        pService->OnPause();
		currentState = SERVICE_PAUSED;
        break;

    case SERVICE_CONTROL_CONTINUE:
		pService->SetStatus(SERVICE_CONTINUE_PENDING);
        pService->OnContinue();
		currentState = SERVICE_RUNNING;
        break;

    case SERVICE_CONTROL_INTERROGATE:
        pService->OnInterrogate();
        break;

    case SERVICE_CONTROL_SHUTDOWN:
        pService->OnShutdown();
        return;

    default:
        if (dwControl >= 128 && dwControl <= 255)
            pService->OnUserControl(dwControl);
        break;
    }

    // Report current status
	pService->SetStatus(currentState);
}
double MyController::onAction(ActionEvent &evt)
{
	/*gettimeofday(&t0, NULL);
	int sec, msec;
	if (t0.tv_usec < t1.tv_usec) {
		sec = t0.tv_sec - t1.tv_sec - 1;
		msec= 1000000 + t0.tv_usec - t1.tv_usec;
	}
	else {
		sec = t0.tv_sec - t1.tv_sec;
		msec = t0.tv_usec - t1.tv_usec;
	}
	LOG_MSG(("%d.%d",sec,msec));
	t1 = t0;*/

	// check whether Referee service is available or not
	bool available = checkService("RoboCupReferee");
	if(!available && m_ref != NULL) m_ref = NULL;
	else if(available && m_ref == NULL){
		m_ref = connectToService("RoboCupReferee");
	}

	if (check1_clear == true && a == false && b == false && d == false){
		//total = total + 300;
		//stringstream ss;
		//ss << total;
		//string result = ss.str();
		//sendMsg("SIGViewer", result);
		std::string msg = "RoboCupReferee/Check point1 clear/300";
		if(m_ref != NULL){
			m_ref->sendMsgToSrv(msg.c_str());
		}
		LOG_MSG((msg.c_str()));
		a = true;
	}

	if (elevator_clear == true && a == true && b == false && d == false){
		//total = total + 300;
		//stringstream ss2;
		//ss2 << total;
		//string result2 = ss2.str();
		//sendMsg("SIGViewer", result2);
		std::string msg = "RoboCupReferee/Elevator clear" "/300";
		if(m_ref != NULL){
			m_ref->sendMsgToSrv(msg.c_str());
		}
		LOG_MSG((msg.c_str()));
		b = true;
	}
	if (crowd_clear == true && a == true  && b == true && d == false){
		//total = total + 300;
		//stringstream ss5;
		//ss5 << total;
		//string result5 = ss5.str();
		//sendMsg("SIGViewer", result5);
		std::string msg = "RoboCupReferee/Crowded loacation clear" "/300";
		if(m_ref != NULL){
			m_ref->sendMsgToSrv(msg.c_str());
		}
		LOG_MSG((msg.c_str()));
		d = true;
	}
	if (all_clear == true && a == true  && b == true && d == true && e == false){
		//total = total + 100;
		//stringstream ss;
		//ss << total;
		//string result = ss.str();
		//sendMsg("SIGViewer", result);
		std::string msg = "RoboCupReferee/All check points clear" "/100";
		if(m_ref != NULL){
			m_ref->sendMsgToSrv(msg.c_str());
		}
		LOG_MSG((msg.c_str()));
		e = true;
	}
	return retValue;
}