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 DemoRobotController::onAction(ActionEvent &evt) { switch(m_state) { case 0: { break; } case 1: { this->stopRobotMove(); break; } case 10: { // go straight a bit m_graspObjectName = m_trashName2; // at first, focusing to m_trashName2:can_0 m_robotObject->setWheelVelocity(m_angularVelocity, m_angularVelocity); m_time = 10.0/m_movingSpeed + evt.time(); // time to be elapsed m_state = 20; break; } case 20: { // direct to the trash if(evt.time() >= m_time && m_state==20) { stopRobotMove(); // at first, stop robot maneuver Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); // get position of trash double l_moveTime = rotateTowardObj(l_tpos); // rotate toward the position and calculate the time to be elapsed. m_time = l_moveTime+evt.time(); m_state = 30; } break; } case 30: { // proceed toward trash if(evt.time() >= m_time && m_state==30) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goToObj(l_tpos, 75.0); // go toward the position and calculate the time to be elapsed. m_time = l_moveTime+evt.time(); m_state = 40; } break; } case 40: { // get back a bit after colliding with the table if(evt.time() >= m_time && m_state==40) { this->stopRobotMove(); // at first, stop robot maneuver m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 20./m_movingSpeed + evt.time(); m_state = 50; } break; } case 50: { // detour: rotate toward relay point 1 if(evt.time() >= m_time && m_state==50) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_relayPoint1); m_time = l_moveTime+evt.time(); m_state = 60; } break; } case 60: { // detour: go toward relay point 1 if(evt.time() >= m_time && m_state==60) { this->stopRobotMove(); double l_moveTime = goToObj(m_relayPoint1, 0.0); m_time = l_moveTime+evt.time(); m_state = 70; } break; } case 70: { // rotate toward the position in front of trash if(evt.time() >= m_time && m_state==70) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_frontTrash1); m_time = l_moveTime+evt.time(); m_state = 80; } break; } case 80: { // go toward the position in front of trash if(evt.time() >= m_time && m_state==80) { this->stopRobotMove(); double l_moveTime = goToObj(m_frontTrash1, 0.0); m_time = l_moveTime+evt.time(); m_state = 90; } break; } case 90: { // rotate toward the trash if(evt.time() >= m_time && m_state==90) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 100; } break; } case 100: { // prepare the robot arm to grasping the trash if(evt.time() >= m_time && m_state==100) { this->stopRobotMove(); this->neutralizeArms(evt.time()); m_state = 105; } break; } case 105: { // fix robot direction for grasping if(evt.time() >= m_time1 && m_state==105) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==105) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==105) { Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 110; } break; } case 110: { // approach to the trash if(evt.time() >= m_time && m_state==110) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goToObj(l_tpos, 30.0); m_time = l_moveTime+evt.time(); m_state = 120; } break; } case 120: { // try to grasp trash if(evt.time() >= m_time && m_state==120) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goGraspingObject(l_tpos); m_time = l_moveTime+evt.time(); m_state = 125; } break; } case 125: { if(evt.time() >= m_time && m_state==125) { m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); this->neutralizeArms(evt.time()); m_state = 130; } break; } case 130: { if(evt.time() >= m_time1 && m_state==130) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==130) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==130) { m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 20./m_movingSpeed + evt.time(); m_state = 150; } break; } case 150: { if(evt.time() >= m_time && m_state==150) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_frontTrashBox2); m_time = l_moveTime + evt.time(); m_state = 160; } break; } case 160: { if(evt.time() >= m_time && m_state==160) { this->stopRobotMove(); double l_moveTime = goToObj(m_frontTrashBox2,0.0); m_time = l_moveTime + evt.time(); m_state = 161; } break; } case 161: { if(evt.time() >= m_time && m_state==161) { this->stopRobotMove(); this->prepareThrowing(evt.time()); m_state = 165; } break; } case 165: { if(evt.time() >= m_time1 && m_state==165) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==165) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==165) { Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 170; } break; } case 170: { if(evt.time() >= m_time && m_state==170) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = goToObj(l_tpos, 50.0); m_time = l_moveTime + evt.time(); m_state = 180; } break; } case 180: { if(evt.time() >= m_time && m_state==180) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 200; } break; } case 200: { // throw trash and get back a bit if(evt.time() >= m_time && m_state==200) { this->stopRobotMove(); this->throwTrash(); sleep(1); m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 50.0/m_movingSpeed + evt.time(); m_state = 225; } break; } case 225: { // recover robot arms if(evt.time() >= m_time && m_state==225) { this->stopRobotMove(); this->neutralizeArms(evt.time()); m_state = 240; } break; } //******************************************************************** case 240: { // go next if(evt.time() >= m_time1 && m_state==240) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==240) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==240) { this->stopRobotMove(); m_graspObjectName = m_trashName1; // set next target double l_moveTime = rotateTowardObj(m_frontTrash2); m_time = l_moveTime + evt.time(); m_state = 250; } break; } case 250: { // approach to neighbor of next target if(evt.time() >= m_time && m_state==250) { this->stopRobotMove(); double l_moveTime = goToObj(m_frontTrash2, 0.0); m_time = l_moveTime + evt.time(); m_state = 260; } break; } case 260: { if(evt.time() >= m_time && m_state==260) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName1); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 270; } break; } case 270: { // approach to next target if(evt.time() >= m_time && m_state==270) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName1); double l_moveTime = goToObj(l_tpos, 39.0); m_time = l_moveTime + evt.time(); m_state = 275; } break; } case 275: { if(evt.time() >= m_time && m_state==275) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName1); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 280; } break; } case 280: { if(evt.time() >= m_time && m_state==280) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName1); double l_moveTime = goGraspingObject(l_tpos); m_time = l_moveTime+evt.time(); m_state = 290; } break; } case 290: { if(evt.time() >= m_time && m_state==290) { m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); this->neutralizeArms(evt.time()); m_state = 300; } break; } case 300: { if(evt.time() >= m_time1 && m_state==300) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==300) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==300) { m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 20./m_movingSpeed + evt.time(); m_state = 310; } break; } case 310: { if(evt.time() >= m_time && m_state==310) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_frontTrashBox1); m_time = l_moveTime + evt.time(); m_state = 320; } break; } case 320: { if(evt.time() >= m_time && m_state==320) { this->stopRobotMove(); double l_moveTime = goToObj(m_frontTrashBox1,0.0); m_time = l_moveTime + evt.time(); m_state = 340; } break; } case 340: { if(evt.time() >= m_time && m_state==340) { this->stopRobotMove(); this->prepareThrowing(evt.time()); m_state = 350; } break; } case 350: { if(evt.time() >= m_time1 && m_state==350) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==350) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==350) { Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName1); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 360; } break; } case 360: { if(evt.time() >= m_time && m_state==360) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName1); double l_moveTime = goToObj(l_tpos, 50.0); m_time = l_moveTime + evt.time(); m_state = 370; } break; } case 370: { if(evt.time() >= m_time && m_state==370) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName1); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 380; } break; } case 380: { // throw trash and get back a bit if(evt.time() >= m_time && m_state==380) { this->stopRobotMove(); this->throwTrash(); sleep(1); m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 50.0/m_movingSpeed + evt.time(); m_state = 390; } break; } case 390: { // recover robot arms if(evt.time() >= m_time && m_state==390) { this->stopRobotMove(); m_state = 0; } break; } } return refreshRateOnAction; }
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 DemoRobotController::onAction(ActionEvent &evt) { switch(m_state) { case 0: { break; } case 1: { this->stopRobotMove(); break; } case 50: { // detour: rotate toward relay point 1 if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_relayPoint1); m_time = l_moveTime+evt.time(); m_state = 60; } break; } case 60: { // detour: go toward relay point 1 if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = goToObj(m_relayPoint1, 0.0); m_time = l_moveTime+evt.time(); m_state = 70; } break; } case 70: { // rotate toward the position in front of trash if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_frontDesk1); m_time = l_moveTime+evt.time(); m_state = 80; } break; } case 80: { // go toward the position in front of trash if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = goToObj(m_frontDesk1, 0.0); m_time = l_moveTime+evt.time(); m_state = 90; } break; } case 90: { // rotate toward the trash if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 100; } break; } case 100: { // prepare the robot arm to grasping the trash if(evt.time() >= m_time) { this->stopRobotMove(); this->neutralizeArms(evt.time()); m_state = 105; } break; } case 105: { // fix robot direction for grasping if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 110; } break; } case 110: { // approach to the trash if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goToObj(l_tpos, 30.0); m_time = l_moveTime+evt.time(); m_state = 120; } break; } case 120: { // try to grasp trash if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goGraspingObject(l_tpos); m_time = l_moveTime+evt.time(); m_state = 125; } break; } case 125: { if(evt.time() >= m_time) { m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); this->neutralizeArms(evt.time()); m_state = 130; } break; } case 130: { if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 20./m_movingSpeed + evt.time(); m_state = 150; } break; } case 150: { if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime; if(m_task == 1) l_moveTime = rotateTowardObj(m_frontTrashBox1); else l_moveTime = rotateTowardObj(m_frontTrashBox2); m_time = l_moveTime + evt.time(); m_state = 160; } break; } case 160: { if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime; if(m_task == 1) l_moveTime = goToObj(m_frontTrashBox1,0.0); else l_moveTime = goToObj(m_frontTrashBox2,0.0); m_time = l_moveTime + evt.time(); m_state = 161; } break; } case 161: { if(evt.time() >= m_time) { this->stopRobotMove(); this->prepareThrowing(evt.time()); m_state = 165; } break; } case 165: { if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashBoxName1); else this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 170; } break; } case 170: { if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashBoxName1); else this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = goToObj(l_tpos, 50.0); m_time = l_moveTime + evt.time(); m_state = 180; } break; } case 180: { if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashBoxName1); else this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 200; } break; } case 200: { // throw trash and get back a bit if(evt.time() >= m_time) { this->stopRobotMove(); this->throwTrash(); sleep(1); m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 50.0/m_movingSpeed + evt.time(); m_state = 225; } break; } case 225: { // recover robot arms if(evt.time() >= m_time) { this->stopRobotMove(); this->neutralizeArms(evt.time()); m_state = 240; } break; } case 240: { // go next if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { this->stopRobotMove(); broadcastMsg("Task_finished"); //broadcastMsg("Give_up"); m_state = 0; } break; } } return refreshRateOnAction; }