double MyController::onAction(ActionEvent &evt)
{
	int count = 0;
	Vector3d pos;

	if (start == true){
		int step = 3;
		while (count<step){
			double dx = 0;
			double dz = -2.5;
			double addx = 0.0;
			double addz = 0.0;
			my->getPosition(pos);
			for(int i=0; i<motionNum; i++){
				addx += dx;
				addz += dz;
				if(motionNum)
					usleep(sleeptime);
				my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
				my->setJointAngle("LARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
				my->setJointAngle("LARM_JOINT3", DEG2RAD(LARM_JOINT3[i]));
				my->setJointAngle("RARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
				my->setJointAngle("RARM_JOINT3", DEG2RAD(RARM_JOINT3[i]));
				my->setJointAngle("LLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
				my->setJointAngle("LLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
				my->setJointAngle("LLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
				my->setJointAngle("RLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
				my->setJointAngle("RLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
				my->setJointAngle("RLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
			}
			for(int i=0; i<motionNum; i++){
				addx += dx;
				addz += dz;
				usleep(sleeptime);
				my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
				my->setJointAngle("RARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
				my->setJointAngle("RARM_JOINT3", DEG2RAD(-LARM_JOINT3[i]));
				my->setJointAngle("LARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
				my->setJointAngle("LARM_JOINT3", DEG2RAD(-RARM_JOINT3[i]));
				my->setJointAngle("RLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
				my->setJointAngle("RLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
				my->setJointAngle("RLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
				my->setJointAngle("LLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
				my->setJointAngle("LLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
				my->setJointAngle("LLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
			}
			count++;
			if(count==1){
				usleep(3000000);
			}

			start = false;
		}
		/*while (count<20){
			if (sw == false){
				my->getPosition(pos);
				my->setPosition(pos.x(), pos.y(), pos.z() - 10);
				my->setJointAngle("LARM_JOINT1", DEG2RAD(-30));
				my->setJointAngle("RLEG_JOINT2", DEG2RAD(-20));
				my->setJointAngle("RLEG_JOINT4", DEG2RAD(10));
				usleep(100000);
				my->setJointAngle("LARM_JOINT1", DEG2RAD(0));
				my->setJointAngle("RLEG_JOINT2", DEG2RAD(0));
				my->setJointAngle("RLEG_JOINT4", DEG2RAD(0));
				sw = true;
			}
			else{
				my->getPosition(pos);
				my->setPosition(pos.x(), pos.y(), pos.z() - 10);
				my->setJointAngle("RARM_JOINT1", DEG2RAD(-30));
				my->setJointAngle("LLEG_JOINT2", DEG2RAD(-20));
				my->setJointAngle("LLEG_JOINT4", DEG2RAD(10));
				usleep(100000);
				my->setJointAngle("RARM_JOINT1", DEG2RAD(0));
				my->setJointAngle("LLEG_JOINT2", DEG2RAD(0));
				my->setJointAngle("LLEG_JOINT4", DEG2RAD(0));
				sw = false;
			}
			count++;

			if(count==7){
				usleep(3000000);
			}

			start = false;
		}*/
	}

	return 0.1;
}
double MyController::onAction(ActionEvent &evt)
{
	Vector3d pos;

	if (start == true){

		my->getPosition(pos);

		double dx = 0;
		double dz = -2.5;
		double addx = 0.0;
		double addz = 0.0;

		if(count%2){
			for(int i=0; i<motionNum; i++){
				addx += dx;
				addz += dz;
				if(motionNum)
					usleep(sleeptime);
				my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
				my->setJointAngle("LARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
				my->setJointAngle("LARM_JOINT3", DEG2RAD(LARM_JOINT3[i]));
				my->setJointAngle("RARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
				my->setJointAngle("RARM_JOINT3", DEG2RAD(RARM_JOINT3[i]));
				my->setJointAngle("LLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
				my->setJointAngle("LLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
				my->setJointAngle("LLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
				my->setJointAngle("RLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
				my->setJointAngle("RLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
				my->setJointAngle("RLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
			}
		}
		else{
			for(int i=0; i<motionNum; i++){
				addx += dx;
				addz += dz;
				usleep(sleeptime);
				my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
				my->setJointAngle("RARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
				my->setJointAngle("RARM_JOINT3", DEG2RAD(-LARM_JOINT3[i]));
				my->setJointAngle("LARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
				my->setJointAngle("LARM_JOINT3", DEG2RAD(-RARM_JOINT3[i]));
				my->setJointAngle("RLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
				my->setJointAngle("RLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
				my->setJointAngle("RLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
				my->setJointAngle("LLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
				my->setJointAngle("LLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
				my->setJointAngle("LLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
			}
		}
		count++;

		if(count==2){
			sleep(3);
		}
		if(count==4){
			sendMsg("operator","Passed_through");
		}
		if(count>step){
			start = false;
		}
	}

	return 0.1;
}
示例#3
0
void SetAttrController::onRecvMessage(RecvMessageEvent &evt)
{
	double x, y, z;

	LOG_MSG(("message from : %s", evt.getSender()));
	int n = evt.getSize();
	for (int i=0; i<n; i++) {
		LOG_MSG(("[%d] (%s)", i, evt.getString(i)));
	}

	SimObj *o = getObj(myname());

	if (n>0 && o && !o->dynamics()) {
		const char *cmd = evt.getString(0);
		if (cmd) {
			if (strcmp(cmd, "p")==0) {
				// ----------------------------
				//	position set (x, y, z)
				// ----------------------------
				const char *param = evt.getString(1);
				if (param) {
					LOG_MSG(("set position (param=%s)", param));
					if (parseVector3(param, x, y, z)) {
						LOG_MSG(("(%f, %f, %f)", x, y, z));
						o->setPosition(x, y, z);
					}
				}
			} else if (strcmp(cmd, "vp")==0) {
				// ----------------------------
				//	view pos
				// ----------------------------
				const char *param = evt.getString(1);
				if (param) {
					LOG_MSG(("set view position (param=%s)", param));
					if (parseVector3(param, x, y, z)) {
						LOG_MSG(("(%f, %f, %f)", x, y, z));
						o->vpx(x);
						o->vpy(y);
						o->vpz(z);
					}
				}
			} else if (strcmp(cmd, "vv")==0) {
				// ----------------------------
				//	view vector
				// ----------------------------
				const char *param = evt.getString(1);
				if (param) {
					LOG_MSG(("set view vector (param=%s)", param));
					if (parseVector3(param, x, y, z)) {
						LOG_MSG(("(%f, %f, %f)", x, y, z));
						o->vvx(x);
						o->vvy(y);
						o->vvz(z);
					}
				}
			} else if (strcmp(cmd, "lep")==0) {
				// ----------------------------
				//	left eye pos
				// ----------------------------
				const char *param = evt.getString(1);
				if (param) {
					LOG_MSG(("set left eye position (param=%s)", param));
					if (parseVector3(param, x, y, z)) {
						LOG_MSG(("(%f, %f, %f)", x, y, z));
						o->lepx(x);
						o->lepy(y);
						o->lepz(z);
					}
				}
			} else if (strcmp(cmd, "lev")==0) {
				// ----------------------------
				//	left eye vector
				// ----------------------------
				const char *param = evt.getString(1);
				if (param) {
					LOG_MSG(("set left eye vector (param=%s)", param));
					if (parseVector3(param, x, y, z)) {
						LOG_MSG(("(%f, %f, %f)", x, y, z));
						o->levx(x);
						o->levy(y);
						o->levz(z);
					}
				}
			} else if (strcmp(cmd, "rep")==0) {
				// ----------------------------
				//	right eye pos
				// ----------------------------
				const char *param = evt.getString(1);
				if (param) {
					LOG_MSG(("set right eye position (param=%s)", param));
					if (parseVector3(param, x, y, z)) {
						LOG_MSG(("(%f, %f, %f)", x, y, z));
						o->repx(x);
						o->repy(y);
						o->repz(z);
					}
				}
			} else if (strcmp(cmd, "rev")==0) {
				// ----------------------------
				//	right eye vector
				// ----------------------------
				const char *param = evt.getString(1);
				if (param) {
					LOG_MSG(("set right eye vector (param=%s)", param));
					if (parseVector3(param, x, y, z)) {
						LOG_MSG(("(%f, %f, %f)", x, y, z));
						o->revx(x);
						o->revy(y);
						o->revz(z);
					}
				}
			}
		}
	}
}
double MyController::onAction(ActionEvent &evt) 
{ 
  // サービスが使用可能か定期的にチェックする  
  bool available = checkService("RobocupReferee");  

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

  // 使用可能  
  else if(available && m_ref == NULL){  
    // サービスに接続  
    m_ref = connectToService("RobocupReferee");  
  }  
 
  // 自分の位置取得
  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] == "recycle" ||
       m_entities[i] == "burnable" ||
       m_entities[i] == "room" ||
       m_entities[i] == "moderator_0" ||
       m_entities[i] == "Kinect_000" ||
       m_entities[i] == "unburnable"){
      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()){

	// ゴミを捨てる
	tpos.y(-100);
	tpos.x(myPos.x());
	tpos.z(myPos.z());
	ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
	ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
	ent->setPosition(tpos);
	ent->setPosition(tpos);
	ent->setPosition(tpos);
	usleep(500000);
	tpos.y(-50.0);
	ent->setPosition(tpos);
	ent->setPosition(tpos);
	ent->setPosition(tpos);

	std::string msg;
	// ゴミが所定のゴミ箱に捨てられているかチェック
	// リサイクル

  /*
	if(strcmp(myname(), "recycle") == 0){
	  // 空のペットボトルのみ点が入る
	  if(strcmp(ent->name(), "petbottle") == 0 ||
	     strcmp(ent->name(), "petbottle_2") == 0 ||
	     strcmp(ent->name(), "petbottle_4") == 0 ||
	     strcmp(ent->name(), "mayonaise_1") == 0 ) {
	    msg = "RobocupReferee/Clean up succeeded" "/1000";
	  }
	  else{
	    msg = "RobocupReferee/Clean up failed" "/-600";
	  }
	}
*/

	// 燃えるゴミ

/*	
	else if(strcmp(myname(), "burnable") == 0){
	  // 燃えるゴミに入れるべきものは無い
	  msg = "RobocupReferee/Clean up failed" "/-600";
	}
	// 缶瓶
	else if(strcmp(myname(), "unburnable") == 0){
	  if(strcmp(ent->name(), "can_0") == 0 ||
	     strcmp(ent->name(), "can_1") == 0 ||
	     strcmp(ent->name(), "can") == 0 ||
	     strcmp(ent->name(), "can_3") == 0) {
	    msg = "RobocupReferee/Clean up succeeded" "/1000";
	  }
	  else {
	    msg = "RobocupReferee/Clean up succeeded" "/-600";
	  }
	}
*/
	if(m_ref != NULL) {
	//  m_ref->sendMsgToSrv(msg.c_str());
	}
	//LOG_MSG((msg.c_str()));
      }
    }
  }

  return retValue;      
}  
//定期的に呼び出される関数
double RobotController::onAction(ActionEvent &evt)
{
	static int iImage = -1;
	static int ii = -20;


	if(ii < 21){


		SimObj *my = getObj(myname());

		//変数の宣言
		double x;
		double y;
		double z;
		double r;
		double thetaDEG;
		double thetaRAD;
		int depth_C[320*240];

		thetaDEG = (ii * 9) - 180;
		thetaRAD = DEG2RAD(thetaDEG);
		r = CAPTURE_DISTANCE + 15;


		//座標と角度を設定
		x = r*sin(thetaRAD);
		y = my->y();
		z = r*cos(thetaRAD);


		LOG_MSG(("x : %f", x));
		LOG_MSG(("y : %f", y));
		LOG_MSG(("z : %f", z));
		LOG_MSG(("thetaDEG : %f", thetaDEG));
		LOG_MSG(("thetaRAD : %f", thetaRAD));
		LOG_MSG(("r : %f", r));

			
		//デバッグ用		
		double xn;
		double yn;
		double zn;
		xn = my->x();
		yn = my->y();
		zn = my->z();
		LOG_MSG(("xn : %f", xn));
		LOG_MSG(("yn : %f", yn));
		LOG_MSG(("zn : %f", zn));
		

		my->setPosition(x,y,z);
		my->setAxisAndAngle(0.0, 1.0, 0.0, thetaRAD + PI);
		

		xn = my->x();
		yn = my->y();
		zn = my->z();
		LOG_MSG(("xn : %f", xn));
		LOG_MSG(("yn : %f", yn));
		LOG_MSG(("zn : %f", zn));


		



		if(m_view != NULL) {

			// ビット深度24,画像サイズ320X240の画像を取得します
			ViewImage *img = m_view->captureView(4, COLORBIT_24, IMAGE_320X240);
			if (img != NULL) {

				// 画像データを取得します
				char *buf = img->getBuffer();
				std::string cap_data;
				for(int i=0;i<320*240*3;i++){
					cap_data += buf[i];
				}

				char view_fname[256];
				sprintf(view_fname, "view%03d.txt", iImage);
				FILE *fwV = fopen(view_fname , "w");
				for(int i=0;i<320*240*3;i++){
					fprintf(fwV, "%03d \n", (unsigned char)buf[i]);
				}
				fclose(fwV);





				//Windows BMP 形式で保存します
				char fname[256];
				sprintf(fname, "s%03d.bmp", iImage);
				img->saveAsWindowsBMP(fname);

				//必要なくなったら削除します
				delete img;

				LOG_MSG(("captureIMG : %d", iImage));

			        //中央カメラの深度画像を取得します。
				ViewImage *img_C1 = m_view->distanceSensor2D(0.0,255.0,4);  
				ViewImage *img_C2 = m_view->distanceSensor2D(255.0,510.0,4);  
				ViewImage *img_C3 = m_view->distanceSensor2D(510.0,765.0,4); 
				ViewImage *img_C4 = m_view->distanceSensor2D(510.0,765.0,4);
			        //DepthImage *img = distanceSensor2D();
			        //取得した深度画像の高さと幅取得
			        int height_C = img_C1->getHeight();
			        int width_C = img_C1->getWidth();
				
	       	                //視野角取得(高さ方向)
			        double fov_C = 60.0; //my->FOV();
			        //焦点距離の計算(pixel単位)
	          		double fl_C = (img_C1->getHeight()/2)/tan(DEG2RAD(fov_C/2));	
				
				img_C1->saveAsWindowsBMP("2D_depth_C1.bmp");
				img_C2->saveAsWindowsBMP("2D_depth_C2.bmp");	
				img_C3->saveAsWindowsBMP("2D_depth_C3.bmp");
				img_C4->saveAsWindowsBMP("2D_depth_C4.bmp");				
				printf("saved img_C1,2,3,4 \n");		
				
				char *distance_C1 = img_C1->getBuffer();	
				char *distance_C2 = img_C2->getBuffer();
				char *distance_C3 = img_C3->getBuffer();
				char *distance_C4 = img_C4->getBuffer();
				for(int i=0; i<240; i++){
					for(int j=0; j<320; j++){
						depth_C[i*320+j] = (unsigned char)distance_C1[i*320+j] + (unsigned char)distance_C2[i*320+j] + (unsigned char)distance_C3[i*320+j] + (unsigned char)distance_C4[i*320+j];
					}
				}
				


				char depth_fname[256];
				sprintf(depth_fname, "depth%03d.txt", iImage++);
				
				FILE *fwC = fopen(depth_fname , "w");
				for(int i=0; i<240; i++){
					for(int j=0; j<320; j++){
						fprintf(fwC, "%04d \n", depth_C[i*320+j]);			
					}
				}
				fclose(fwC);

				delete img_C1;
				delete img_C2;
				delete img_C3;
				delete img_C4;

			}
		}

	}


	if(ii ==  21){
		LOG_MSG(("capture conpleted"));

	}



	ii++;

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

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

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

	// 自分の位置取得
	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(), "unknown_3") == 0 && tpos.y() != 54.04){ tpos.y(54.04); success = true;}
				else if(strcmp(ent->name(), "unknown_0") == 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(), "banana") == 0 && tpos.y() != 51.69){ tpos.y(51.69); 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_2") == 0 && tpos.y() != 67.45){ tpos.y(67.45); success = true;}
				else if(strcmp(ent->name(), "petbottle_4") == 0 && tpos.y() != 61.95){ tpos.y(61.95); success = true;}
				else if(strcmp(ent->name(), "unknown_4") == 0 && tpos.y() != 52.04){ tpos.y(52.04); success = true;}
				else if(strcmp(ent->name(), "unknown_1") == 0 && tpos.y() != 56.150){ tpos.y(56.150); success = true;}
				else if(strcmp(ent->name(), "unknown_2") == 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(), "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 = "RoboCupReferee/";
					msg += ent->name();
					msg += " succeeded/1000";
				}
				else{
					msg = "RoboCupReferee/";
					msg += ent->name();
					msg += " target failed/400";

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

	return retValue;
}