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; }
// 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; }
// 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; }