void CameraController::rotateTowardRobot(Vector3d r_pos) { m_my->setPosition(m_rotatePos); // カメラの位置を得る m_my->getPosition(m_pos); // カメラの位置からロボットを結ぶベクトル Vector3d tmpp = r_pos; tmpp -= m_pos; // y方向は考えない tmpp.y(0); // カメラの回転角度を得る Rotation myRot; m_my->getRotation(myRot); // カメラのy軸の回転角度を得る(x,z方向の回転は無いと仮定) double qw = myRot.qw(); double qy = myRot.qy(); double theta = 2*acos(fabs(qw)); if(qw*qy < 0) theta = -1*theta; // ロボットまでの回転角度を得る double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0)); double targetAngle = acos(tmp); if(tmpp.x() > 0) targetAngle = -1*targetAngle; // 角度差から回転量を得る targetAngle += theta; m_my->setAxisAndAngle(0, 1, 0, -targetAngle, 0); }
double RobotController::onAction(ActionEvent &evt) { LOG_MSG(("\ncurrent time : %f", evt.time())); static int cnt = 0; try { const char *name = myname(); SimObj *obj = getObj(name); obj->dump(); if (!obj->dynamics()) { double angle = 2*PI*cnt*0.01; double xx = 5*sin(angle); double yy = 0.5; double zz = 5*cos(angle); LOG_MSG(("pos (%f, %f, %f)", xx, yy, zz)); obj->setPosition(xx, yy, zz); obj->setAxisAndAngle(0.0, 1.0, 0.0, angle); } obj->dump(); } catch(SimObj::NoAttributeException &) { } catch(SimObj::AttributeReadOnlyException &) { } catch(SimObj::Exception &) { } cnt++; return 0.1; }
double DetectEntitiesController::onAction(ActionEvent &evt) { std::vector<std::string> agents; std::vector<std::string>::iterator i; static int count = 0; // LOG_MSG(("bear onAction(count=%d)\n", count)); bool b = detectEntities(agents); if (b) { int n = agents.size(); if (n>0) { LOG_MSG(("%d entities detected", n)); for (int i=0; i<n; i++) { std::string name = agents[i]; LOG_MSG(("[%d] (%s)", i, name.c_str())); } } } else { LOG_MSG(("detectEntities failed")); } try { SimObj *o = getObj(myname()); if (o && !o->dynamics()) { double deg = count * 10.0; double rad = -3.141592/180*deg; o->setAxisAndAngle(0.0, 1.0, 0.0, rad); } } catch(SimObj::NoAttributeException &) { } catch(SimObj::AttributeReadOnlyException &) { } catch(SimObj::Exception &) { } count++; return 2.0; }
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; }
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; }