void MyController::onCollision(CollisionEvent &evt) { if (m_grasp == false){ typedef CollisionEvent::WithC C; //触れたエンティティの名前を得ます const std::vector<std::string> & with = evt.getWith(); // 衝突した自分のパーツを得ます const std::vector<std::string> & mparts = evt.getMyParts(); // 衝突したエンティティでループします for(int i = 0; i < with.size(); i++){ //右手に衝突した場合 if(mparts[i] == "RARM_LINK7"){ //自分を取得 SimObj *my = getObj(myname()); //自分の手のパーツを得ます CParts * parts = my->getParts("RARM_LINK7"); parts->graspObj(with[i]); m_grasp = true; } } } }
void RobotController::release_left_hand() { CParts * parts = my->getParts("LARM_LINK7"); if ( m_grasp_left == true){ printf("I'm releasing \n"); } parts->releaseObj(); m_grasp_left = false; }
void DemoRobotController::throwTrash(void) { // get the part info. CParts *parts = m_robotObject->getParts("RARM_LINK7"); // release grasping parts->releaseObj(); // wait a bit sleep(1); // set the grasping flag to neutral m_grasp = false; }
void RobotController::grasp_left_hand() { Vector3d hand, object; SimObj *obj = getObj(m_pointedObject.c_str()); obj->getPosition(object); my->getJointPosition(hand, "LARM_JOINT7"); double distance = getDist3D(hand,object); if (distance < GRASPABLE_DISTANCE && m_grasp_left == false) { CParts * parts = my->getParts("LARM_LINK7"); if (parts->graspObj(m_pointedObject)) { m_grasp_left = true; // broadcastMsg("Object_grasped"); } } }
void DemoRobotController::onCollision(CollisionEvent &evt) { if (m_grasp == false) { typedef CollisionEvent::WithC C; // Get name of entity which is touched by the robot const std::vector<std::string> & with = evt.getWith(); // Get parts of the robot which is touched by the entity const std::vector<std::string> & mparts = evt.getMyParts(); // loop for every collided entities for(int i = 0; i < with.size(); i++) { if(m_graspObjectName == with[i]) { // If the right hand touches the entity if(mparts[i] == "RARM_LINK7") { SimObj *my = getObj(myname()); CParts * parts = my->getParts("RARM_LINK7"); if(parts->graspObj(with[i])) m_grasp = true; } } } } }
void PartsTest::test_Cylinder() { Position pos(0.0, 1.0, 2.0); double rad = 0.1; double len = 1.5; CylinderParts cylinder("cylinder", pos, rad, len); Parts &parts = cylinder; int n; char *data= parts.toBinary(n); ASSERT(data != NULL); CParts * d = CParts::decode(data+2); ASSERT(d != NULL); ASSERT(strcmp(d->name(), "cylinder") == 0); ASSERT(cylinder.getType() == d->getType()); CylinderPartsCmpnt *ext = (CylinderPartsCmpnt*)( d->extdata()); ASSERT(ext != NULL); ASSERT_D_EQUAL(rad, ext->radius(), EPS); ASSERT_D_EQUAL(len, ext->length(), EPS); delete d; }
void PartsTest::test_Box() { Position pos(0.0, 1.0, 2.0); Size sz(3.0, 4.0, 5.0); BoxParts box("box", pos, sz); Parts &parts = box; int n; char *data= parts.toBinary(n); ASSERT(data != NULL); { CParts * d = CParts::decode(data+2); ASSERT(d != NULL); ASSERT(strcmp(d->name(), "box") == 0); ASSERT(box.getType() == d->getType()); BoxPartsCmpnt *ext = (BoxPartsCmpnt*)( d->extdata()); ASSERT(ext != NULL); Size &sz_ = ext->size(); ASSERT_D_EQUAL(sz_.x(), sz.x(), EPS); ASSERT_D_EQUAL(sz_.y(), sz.y(), EPS); ASSERT_D_EQUAL(sz_.z(), sz.z(), EPS); delete d; } { // clone test CParts *clone = ( (CParts&)box ).clone(); ASSERT(clone != NULL); ASSERT(strcmp(clone->name(), "box") == 0); ASSERT(box.getType() == clone->getType()); BoxPartsCmpnt *ext = (BoxPartsCmpnt*)( clone->extdata()); ASSERT(ext != NULL); Size &sz_ = ext->size(); ASSERT_D_EQUAL(sz_.x(), sz.x(), EPS); ASSERT_D_EQUAL(sz_.y(), sz.y(), EPS); ASSERT_D_EQUAL(sz_.z(), sz.z(), EPS); delete clone; } }
void PartsTest::test_Sphere() { Position pos(0.0, 1.0, 2.0); const double RADIUS = 5.0; SphereParts sphere("sphere", pos, RADIUS); Parts &parts = sphere; int n; char *data = parts.toBinary(n); ASSERT(data != NULL); { CParts * d = CParts::decode(data+2); ASSERT(d != NULL); ASSERT(strcmp(d->name(), "sphere") == 0); ASSERT(sphere.getType() == d->getType()); SpherePartsCmpnt *ext = (SpherePartsCmpnt*)d->extdata(); ASSERT(ext != NULL); ASSERT_D_EQUAL(RADIUS, ext->radius(), EPS); delete d; } { CParts *clone = ( (CParts&)parts ).clone(); ASSERT(clone != NULL); ASSERT(strcmp(clone->name(), "sphere") == 0); ASSERT(sphere.getType() == clone->getType()); SpherePartsCmpnt *ext = (SpherePartsCmpnt*)clone->extdata(); ASSERT(ext != NULL); ASSERT_D_EQUAL(RADIUS, ext->radius(), EPS); delete clone; } }
double MyController::onAction(ActionEvent &evt) { switch(m_state){ // 初期状態 case 0: { // ゴミがある場所と名前を取得します if(!this->recognizeTrash(m_tpos,m_tname)){ // ゴミが見つからないので終了 broadcastMsgToSrv("I cannot find trash"); m_state = 8; } // ゴミが見つかった else{ // ゴミの方向に回転をはじめる m_time = rotateTowardObj(m_tpos, m_vel, evt.time()); m_state = 1; } break; } // ゴミの方向に回転中 case 1: { // 回転終了 if(evt.time() >= m_time){ // 回転を止める m_my->setWheelVelocity(0.0, 0.0); // 関節の回転を始める // orig m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0); // 50°回転 m_time = DEG2RAD(50) / m_jvel + evt.time(); // ゴミを取りに関節を曲げる状態に移行します m_state = 2; } break; } // 関節を回転中 case 2: { // 関節回転終了 if(evt.time() >= m_time){ m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); // graspしたいパーツを取得します CParts *parts = m_my->getParts("RARM_LINK7"); // graspします parts->graspObj(m_tname); // ゴミ箱の位置を取得します SimObj *trashbox = getObj("trashbox_1"); Vector3d pos; trashbox->getPosition(pos); // ゴミ箱の方向に移動を開始します m_time = rotateTowardObj(pos, m_vel, evt.time()); m_state = 3; } break; } // ゴミ箱の方向に回転中 case 3: { // ゴミ箱到着 if(evt.time() >= m_time){ // ここではゴミ箱の名前 位置は知っているものとします SimObj *trashbox = getObj("trashbox_1"); Vector3d pos; trashbox->getPosition(pos); // ゴミ箱の近くに移動します m_time = goToObj(pos, m_vel*4, 40.0, evt.time()); m_state = 4; } break; } // ゴミを持ってゴミ箱に向かっている状態 case 4: { // ゴミ箱に到着 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // grasp中のパーツを取得します CParts *parts = m_my->getParts("RARM_LINK7"); // releaseします parts->releaseObj(); // ゴミが捨てられるまで少し待つ sleep(1); // 捨てたゴミをゴミ候補から削除 std::vector<std::string>::iterator it; it = std::find(m_trashes.begin(), m_trashes.end(), m_tname); m_trashes.erase(it); // 関節の回転を始める m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0); m_time = DEG2RAD(50) / m_jvel + evt.time() + 1.0; m_state = 5; } break; } // ゴミを捨てて関節を戻している状態 case 5: { // 関節が元に戻った if(evt.time() >= m_time){ // 関節の回転を止める m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); // 最初にいた方向に体を回転させます m_time = rotateTowardObj(m_inipos, m_vel, evt.time()); m_state = 6; } break; } // 元に場所に戻る方向に回転している状態 case 6: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 最初にいた場所に移動します m_time = goToObj(m_inipos, m_vel*4, 5.0, evt.time()); m_state = 7; } break; } // 元の場所に向かっている状態 case 7: { // 元の場所に到着 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 最初の方向に回転(z軸と仮定) m_time = rotateTowardObj(Vector3d(0.0, 0.0, 10000.0), m_vel, evt.time()); m_state = 8; } break; } // 元の向きに回転している状態 case 8: { if(evt.time() >= m_time){ // 回転を止める m_my->setWheelVelocity(0.0, 0.0); // 最初の状態に戻る m_state = 0; } } } return 0.1; }
double RobotController::onAction(ActionEvent &evt) { switch(m_state){ // 初期姿勢を設定 seting initial pose case 0: { //broadcastMsgToSrv("Let's start the clean up task\n"); sendMsg("VoiceReco_Service","Let's start the clean up task\n"); double angL1 =m_my->getJointAngle("LARM_JOINT1")*180.0/(PI); double angL4 =m_my->getJointAngle("LARM_JOINT4")*180.0/(PI); double angR1 =m_my->getJointAngle("RARM_JOINT1")*180.0/(PI); double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI); double thetaL1 = -20-angL1; double thetaL4 = -160-angL4; double thetaR1 = -20-angR1; double thetaR4 = -160-angR4; if(thetaL1<0) m_my->setJointVelocity("LARM_JOINT1", -m_jvel, 0.0); else m_my->setJointVelocity("LARM_JOINT1", m_jvel, 0.0); if(thetaL4<0) m_my->setJointVelocity("LARM_JOINT4", -m_jvel, 0.0); else m_my->setJointVelocity("LARM_JOINT4", m_jvel, 0.0); if(thetaR1<0) m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0); else m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0); if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0); else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0); m_time_LA1 = DEG2RAD(abs(thetaL1))/ m_jvel + evt.time(); m_time_LA4 = DEG2RAD(abs(thetaL4))/ m_jvel + evt.time(); m_time_RA1 = DEG2RAD(abs(thetaR1))/ m_jvel + evt.time(); m_time_RA4 = DEG2RAD(abs(thetaR4))/ m_jvel + evt.time(); m_state = 1; break; } // 初期姿勢に移動 moving initial pose case 1: { if(evt.time() >= m_time_LA1) m_my->setJointVelocity("LARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time_LA4) m_my->setJointVelocity("LARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time_RA1) m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time_RA4) m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time_LA1 && evt.time() >= m_time_LA4 && evt.time() >= m_time_RA1 && evt.time() >= m_time_RA4){ // 位置Aの方向に回転を開始します setting position a for rotating //broadcastMsgToSrv("Moving to the table"); m_time = rotateTowardObj(pos_a, m_vel, evt.time()); m_state = 2; } break; } // 位置Aの方向に回転 rotating to position a case 2: { // 回転終了 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 位置Aに移動します setting position a for moving m_time = goToObj(pos_a, m_vel*4, 0.0, evt.time()); m_state = 3; } break; } // 位置Aに移動 moving to position a case 3: { // 位置Aに到着 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 位置Bの方向に回転を開始します setting position b for rotating m_time = rotateTowardObj(pos_b, m_vel, evt.time()); m_state = 4; } break; } // 位置Bの方向に回転 rotating to position b case 4: { // 回転終了 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 位置Bに移動します setting position b for moving m_time = goToObj(pos_b, m_vel*4, 0.0, evt.time()); m_state = 5; } break; } // 位置Bに移動 moving to position b case 5: { // 位置Bに到着 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // テーブルの方向に回転を開始します setting table position for rotating SimObj *table = getObj("table_0"); Vector3d pos; table->getPosition(pos); m_time = rotateTowardObj(pos, m_vel, evt.time()); m_state = 6; } break; } // テーブルの方向に回転 rotating to table case 6: { // 回転終了 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // ゴミがある場所と名前を取得します // ゴミが見つからなかった if(!this->recognizeTrash(m_tpos,m_tname)){ //broadcastMsgToSrv("No trash detected"); //broadcastMsgToSrv("Task finished"); sleep(10); } // ゴミが見つかった trash detected else{ //broadcastMsgToSrv("Please show which trash to take\n"); sendMsg("VoiceReco_Service","Please show me which object to take"); m_state = 7; } } break; } // wating to point case 7: { break; } // 物体認識開始 starting object recognition case 8: { // m_tpos object direction on object SimObj *target = this->getObj(m_pointedObject.c_str()); target->getPosition(m_tpos); //broadcastMsgToSrv("Ok I will take it\n"); msg_ob = "I will take " + m_pointedObject ; sendMsg("VoiceReco_Service",msg_ob); // ゴミの方向に回転をはじめる m_time = rotateTowardObj(m_tpos, m_vel, evt.time()); m_state = 9; break; } // ゴミの方向に回転をはじめる setting trash position for rotating case 9: { m_time = rotateTowardObj(m_tpos, m_vel, evt.time()); m_state = 10; break; } // ゴミの方向に回転中 rotating to trash case 10: { // 回転終了 if(evt.time() >= m_time){ // 回転を止める m_my->setWheelVelocity(0.0, 0.0); //ゴミの位置まで移動をはじめる setting trash position for moving m_time = goToObj(m_tpos, m_vel*4, 25.0, evt.time()); m_state = 11; } break; } // ゴミの位置まで移動中 moving to trash case 11: { // 移動終了 if(evt.time() >= m_time){ // 移動を止める m_my->setWheelVelocity(0.0, 0.0); // 関節の回転を始める setting arm for grasping double angR1 =m_my->getJointAngle("RARM_JOINT1")*180.0/(PI); double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI); double thetaR1 = -30.0-angR1; double thetaR4 = 0.0-angR4; if(thetaR1<0) m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0); else m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0); if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0); else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0); m_time_RA1 = DEG2RAD(abs(thetaR1) )/ m_jvel + evt.time(); m_time_RA4 = DEG2RAD(abs(thetaR4) )/ m_jvel + evt.time(); // ゴミを取りに関節を曲げる状態に移行します m_state = 12; } break; } // 関節を回転中 rotating arm for grasping case 12: { // 関節回転終了 if(evt.time() >= m_time_RA1) m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time_RA4) m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time_RA1 && evt.time() >= m_time_RA4){ if(m_grasp) { //broadcastMsgToSrv("grasping the trash"); // 関節の回転を始める setting arm for taking double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI); double thetaR4 = -90.0-angR4; if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0); else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0); m_time_RA4 = DEG2RAD(abs(thetaR4) )/ m_jvel + evt.time(); // 関節を戻す状態に移行します m_state = 13; } else{ // graspできない broadcastMsgToSrv("Unreachable"); } } break; } // 関節を回転中 rotating arm for taking case 13: { // 関節回転終了 if(evt.time() >= m_time_RA4){ m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0); // 位置Aの方向に回転を開始します setting position a for rotating //broadcastMsgToSrv("Moving to the trashbox"); sendMsg("VoiceReco_Service","Now I will go to the trash boxes"); m_time = rotateTowardObj(pos_a, m_vel, evt.time()); m_state = 14; } break; } // 位置Aの方向に回転 rotating to position a case 14: { // 回転終了 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 位置Aに移動します setting position a for moving m_time = goToObj(pos_a, m_vel*4, 0.0, evt.time()); m_state = 15; } break; } // 位置Aの位置まで移動中 movig to position a case 15: { // 移動終了 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); //broadcastMsgToSrv("Please tell me which trash box \n"); sendMsg("VoiceReco_Service","Please show me which trash box to use"); m_state = 16; } break; } // watig to point4 case 16: { break; } // ゴミ箱認識 starting trash box recognitiong-g-0 case 17: { // m_tpos object direction on object SimObj *target_trash = this->getObj(m_pointedtrash.c_str()); target_trash->getPosition(m_tpos); //broadcastMsgToSrv("Ok I will throw the trash in trash box \n"); msg_trash = "Ok I will put "+ m_pointedObject+"in"+ m_pointedtrash + "\n"; sendMsg("VoiceReco_Service",msg_trash); // ゴミの方向に回転をはじめる setting position trash box for rotating m_time = rotateTowardObj(m_tpos, m_vel, evt.time()); m_state = 18; break; } // ゴミ箱の方向に回転中 rotating to trash box case 18: { if(evt.time() >= m_time){ // 回転を止める m_my->setWheelVelocity(0.0, 0.0); //ゴミの位置まで移動をはじめる setting trash position for moving m_time = goToObj(m_tpos, m_vel*4, 30.0, evt.time()); m_state = 19; } break; } // ゴミを持ってゴミ箱に向かっている状態 moving to trash box case 19: { // ゴミ箱に到着 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // grasp中のパーツを取得します getting grasped tarts CParts *parts = m_my->getParts("RARM_LINK7"); // releaseします parts->releaseObj(); // ゴミが捨てられるまで少し待つ sleep(1); // 捨てたゴミをゴミ候補から削除 deleting grasped object from list std::vector<std::string>::iterator it; it = std::find(m_trashes.begin(), m_trashes.end(), m_pointedObject); m_trashes.erase(it); // grasp終了 m_grasp = false; m_state = 1; } break; } } return 0.01; }
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::rotateTowardGrabPos(Vector3d pos, double velocity, double now) { printf("start rotate %lf \n", now); //自分を取得 SimObj *my = getObj(myname()); //printf("向く座標 %lf %lf %lf \n", pos.x(), pos.y(), pos.z()); // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); //自分の手のパーツを得ます CParts * parts = my->getParts("RARM_LINK7"); Vector3d partPos; parts->getPosition(partPos); // 自分の位置からターゲットを結ぶベクトル Vector3d tmpp = pos; //tmpp -= myPos; tmpp -= partPos; // y方向は考えない tmpp.y(0); // 自分の回転を得る Rotation myRot; m_my->getRotation(myRot); // エンティティの初期方向 Vector3d iniVec(0.0, 0.0, 1.0); // y軸の回転角度を得る(x,z方向の回転は無いと仮定) double qw = myRot.qw(); double qy = myRot.qy(); double theta = 2*acos(fabs(qw)); //printf("qw: %lf theta: %lf \n", qw, theta); if(qw*qy < 0) { //printf("qw * qy < 0 \n"); theta = -1*theta; } // z方向からの角度 //printf("結ぶベクトル 座標 %lf %lf %lf \n", tmpp.x(), tmpp.y(), tmpp.z()); double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0)); //printf("tmp: %lf \n", tmp); double targetAngle = acos(tmp); //printf("targetAngle: %lf ---> %lf \n", targetAngle, targetAngle*180.0/PI); // 方向 //printf("tmpp.x() %lf \n", tmpp.x()); if(tmpp.x() > 0) { targetAngle = -1*targetAngle; //printf("targetAngle: %lf deg \n", targetAngle*180.0/PI); } targetAngle += theta; //printf("targetAngle: %lf <--- %lf \n", targetAngle, theta); //printf("qw: %lf qy: %lf theta: %lf tmp: %lf targetAngle: %lf \n", qw, qy, theta, tmp, targetAngle); if(targetAngle == 0.0){ //printf("donot need rotate \n"); return 0.0; } else { // 回転すべき円周距離 double distance = m_distance*PI*fabs(targetAngle)/(2*PI); // 車輪の半径から移動速度を得る double vel = m_radius*velocity; // 回転時間(u秒) double time = distance / vel; // 車輪回転開始 if(targetAngle > 0.0){ m_my->setWheelVelocity(velocity, -velocity); } else { m_my->setWheelVelocity(-velocity, velocity); } //printf("distance: %lf vel: %lf time: %lf \n", distance, vel, time); printf("rotate time: %lf, time to stop: %lf \n", time, now + time); return now + time; } }
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) { dlopen("libpython2.7.so", RTLD_LAZY | RTLD_GLOBAL); Py_Initialize(); //initialization of the python interpreter //return 1.0; SimObj *stick = getObj("robot_test"); SimObj *box = getObj("box_001"); SimObj *goal_001 = getObj("checkpoint_001"); double torque; // The target position Vector3d targetPos; box->getPosition(targetPos); // The Goal Position: The sphere is placed at Goal position. Vector3d goalPos; goal_001->getPosition(goalPos); // calculating the displacement vector Vector3d displacementVect; displacementVect.x(goalPos.x()-targetPos.x()); displacementVect.y(goalPos.y()-targetPos.y()); displacementVect.z(goalPos.z()-targetPos.z()); // stick->addForce(-500,0,500); // This adds force to on the stick tool. // stick->addForce(0,0,5000); // Vector3d angularVel; // stick->getAngularVelocity(angularVel); // LOG_MSG((" Current angular Velocity is : %f %f %f ", angularVel.x(), angularVel.y(), angularVel.z() )); // if ( abs(angularVel.y()) > 0.1 ) // { // LOG_MSG((" Current angular Velocity is : %f %f %f ", angularVel.x(), angularVel.y(), angularVel.z() )); // double* ptr1 = NULL; // ptr1 = controlAngularVelocity(angularVel, 3.0, 0, 1.0); // torque = ptr1[0] * 500; // // torque = ptr1[0] * 12000 ; // cout << "The torque applied for controlling angular velocity is " << torque << " N. m" << endl; // stick->addTorque( 0 , torque, 0); // } // to control the rotation of the tool // Rotation currentToolRot; // stick->getRotation(currentToolRot); // double *ptr = NULL; // ptr = controlRotation(initialToolRot, currentToolRot, 20.0, 0.0, 20.0); // torque = ptr[1] * 200 ; // cout << "The torque applied for controlling the rotation = " << torque << endl; // stick->addTorque(0, torque,0); // double massOfTool; // massOfTool = stick->getMass(); // cout << "The mass is " << massOfTool <<endl; // Vector3d velocityOfTarget; // box->getLinearVelocity(velocityOfTarget); // double netVelocityTarget; // netVelocityTarget=( pow(velocityOfTarget.x(),2) + pow(velocityOfTarget.y(), 2) + pow(velocityOfTarget.z(), 2 ) ); // netVelocityTarget = sqrt(netVelocityTarget); // stick->getPosition(pos); // stick->getPartsPosition(pos, "LINK1"); // LOG_MSG((" LINK1 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() )); // Vector3d targetPos; // box->getPosition(targetPos); // Vector3d goalPos; // goal_001->getPosition(goalPos); // if (abs( goalPos.z() - targetPos.z()) < 1.4 ) // { // cout << "The distance to goal is " << abs( goalPos.z() - targetPos.z()) << endl; // cout << "The goal has been reached " << endl; // exit(0); // } // if (netVelocityTarget > 0.1 ) // { // double angle = atan ( (targetPos.z() - startPosition.z()) / (targetPos.x() - startPosition.x() ) ) * 180 / PI; // cout << "The angle is" << angle; // storePosition(targetPos); // } std::vector<std::string> s; for(std::map<std::string, CParts *>::iterator it = stick->getPartsCollection().begin(); it != stick->getPartsCollection().end(); ++it){ if (it->first != "body") s.push_back(it->first); } std::string linkName; Size si; cout << "The total links are " << s.size() << endl; for (int i = 0; i < s.size(); i++){ const char* linkName = s[i].c_str(); CParts *link = stick->getParts(linkName); link->getPosition(pos); link->getRotation(linkRotation); if (link->getType() == 0){ BoxParts* box = (BoxParts*) link; si = box->getSize(); // cout << linkName << endl; cout << linkName << " position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl; // cout << linkName << " size : x = " << si.x() << " y = " << si.y() << " z = " << si.z() << endl; // cout << linkName << " Rotation: qw " << linkRotation.qw() << " qx = "<< linkRotation.qx() << " qy = "<< linkRotation.qy() // << " qz = "<< linkRotation.qz() << endl; try{ py::object main_module = py::import("__main__"); py::object main_namespace = main_module.attr("__dict__"); main_module.attr("linkName") = linkName; main_module.attr("length") = si.x(); main_module.attr("height") = si.y(); main_module.attr("breadth") = si.z(); main_module.attr("linkPos") = "[" + tostr(pos.x())+" , "+ tostr(pos.y())+ " , " + tostr(pos.z()) + "]"; main_module.attr("rotation") = "[" + tostr(linkRotation.qw())+" , "+ tostr(linkRotation.qx())+ " , " + tostr(linkRotation.qy()) + " , " + tostr(linkRotation.qz()) +"]"; // calculating the rotation of the tool. py::exec("import ast", main_namespace); py::exec("import transformations as T", main_namespace); py::exec("rotation = ast.literal_eval(rotation)", main_namespace); // py::exec("angles = T.euler_from_quaternion(rotation) ", main_namespace); // py::exec("print angles", main_namespace); py::exec("linkPos = ast.literal_eval(linkPos)", main_namespace); py::exec_file("vertices.py", main_namespace, main_namespace ); py::exec("getNormals(linkName, linkPos, rotation, length, height, breadth)", main_namespace); main_module.attr("targetPos") = "[" + tostr(targetPos.x())+" , "+ tostr(targetPos.y())+ " , " + tostr(targetPos.z()) + "]"; main_module.attr("displacementVect") = "[" + tostr(displacementVect.x())+" , "+ tostr(displacementVect.y())+ " , " + tostr(displacementVect.z()) + "]"; py::exec_file("displacementVector.py", main_namespace, main_namespace ); } catch(boost::python::error_already_set const &){ // Parse and output the exception std::string perror_str = parse_python_exception(); std::cout << "Error in Python: " << perror_str << std::endl; } } else if(link->getType() == 1){ CylinderParts* cyl = (CylinderParts*) link; cout << "Cylinder Position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl; cout << "Cylinder Length : length = " << cyl->getLength() << endl; cout << "Cylinder Radius : rad = " << cyl->getRad() << endl; } else if(link->getType() == 2){ SphereParts* sph = (SphereParts*) link; cout << "Sphere Position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl; cout << "Sphere Radius : rad = " << sph->getRad() << endl; } } // stick->getPartsPosition(pos, s[1]); // stick->getPartsPosition(pos, "LINK1"); // LOG_MSG((" LINK1 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() )); // stick->getPartsPosition(pos, "LINK2"); // LOG_MSG((" LINK2 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() )); // stick->getPartsPosition(pos, "LINK3"); // LOG_MSG((" LINK3 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() )); return 0.00001; }
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; }