void RobotController::onRecvMsg(RecvMsgEvent &evt)
{
	std::string sender = evt.getSender();

	// 送信者がゴミ認識サービスの場合
	char *all_msg = (char*)evt.getMsg();
	std::string msg;
	msg= evt.getMsg();

	if(msg == "start" && m_state ==0)
	{
		m_state = 1; // initial settings
	}
	else if(msg == "kitchen" && m_state ==0)
	{
		m_state = 10; // move to the kitchen
	}
	else if(msg == "lobby" && m_state ==0)
	{
		m_state = 20; // move to the lobby
	}
	else if(msg == "bed room" && m_state ==0)
	{
		m_state = 30; // move to the bed room
	}
	else
	{
		broadcastMsgToSrv("Task finished\n");
	}
}
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;      
}  
예제 #3
0
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(first == false){
    std::string msg = "start";  
    broadcastMsg(msg);
    first = true; 
  }

    int count=0;
    double r=0.0; //2点間の直線距離
    double angle;

    if(end==false){
        if(start==true){

            Vector3d pos;
            Vector3d npos;

            if(first==false){
                FILE* fp;           
                x=0;
                y=0;
                z=0;
                w=0; //チェックポイント
      
                dx=0;
                dy=0;
                dz=0;

                if((fp = fopen("node.txt", "r")) == NULL) {
	             printf("File do not exist.\n");
	             exit(0);
                }
                while(fscanf(fp, "%lf,%lf,%lf,%lf", &x, &y, &z,&w) != EOF) {
                    temp.x[i]=x;
                    temp.y[i]=y;
                    temp.z[i]=z;
                    temp.w[i]=w;
                    i++;
                }
                fclose(fp);
                first = true;
                i=0;
            }

            if(stop==false){
                my->getPosition(pos);

                npos.x(temp.x[i]); 
                npos.y(temp.y[i]); 
                npos.z(temp.z[i]); 

                angle = rotateTowardObj(npos);   

                if(angle < 0.0){
                    angle = -1.0 * angle;
                }
 		/*ここに相当する部分を書く*/
                //my->setAxisAndAngle(0, 1.0, 0, -angle);
   	        // 回転すべき円周距離
                double distance = m_distance*PI*fabs(targetAngle)/(2*PI);
                if(targetAngle > 0.0){
                    m_my->setWheelVelocity(velocity, -velocity);
                }
                else{
                    m_my->setWheelVelocity(-velocity, velocity);
                }
                // 車輪の半径から移動速度を得る
                double vel = m_radius*velocity;
    
                // 回転時間(u秒)
                double time = (distance / vel) + evt.time();
    
                if(evt.time>=time){
		    m_my->setWheelVelocity(0, 0);
                }

                dx=(temp.x[i]-pos.x());
                dy=(temp.y[i]-pos.y());
                dz=(temp.z[i]-pos.z());

                r=sqrt(pow(dx,2)+pow(dz,2));
                //ここまでが現在地から次の座標までの距離と角度の計算

                double vel = m_radius*velocity;

                // 移動開始
                m_my->setWheelVelocity(velocity, velocity);

                // 到着時間取得
                double time2 = r / vel;

                count = 0;
                count2= 0;
                i++;
            }


            if(temp.w[i-1] == 1.0){
                std::string msg = "point1";  
                //"robot_000"にメッセージを送信します  
                sendMsg("man_001", msg); 

            }else if(temp.w[i-1] == 2.0){

                stop=true;
                broadcastMsgToSrv("elevator");

                if(elevator==true){
                   stop=false;
                }
            }
        }
    }

/***************************************************************************/
  
/*
      if(strstr(myname() , "robot_004")!= NULL){    
        m_my->setWheelVelocity(m_vel*2, m_vel*2);
      }
*/
  return 0.1;      
}  
void RobotController::onInit(InitEvent &evt)
{
  m_robotState = 0;
  //エージェントの正面の定義はz軸の正の向きを向いていると仮定する
  m_defaultNormalVector = Vector3d(0,0,1);
  m_onActionReturn = 0.01;
  m_speedDelta = 2.0;

  m_my = getRobotObj(myname());

  // 初期位置取得
  m_my->getPartsPosition(m_inipos,"RARM_LINK2");

	m_relayPoint0 = Vector3d(0, 30, 0);
	m_relayPoint1 = Vector3d(0, 30, 350);
	m_relayPoint2 = Vector3d(-400, 30, 0);
	
	m_kitchenPoint = Vector3d(200, 30, 350);;
	m_bedroomPoint = Vector3d(-400, 30, 200);;
	m_lobbyPoint = Vector3d(-400, 30, -200);;

  // 車輪間距離
  m_distance = 10.0;

  // 車輪半径
  m_radius  = 10.0;

  // 移動終了時間初期化
  m_time = 0.0;
  m_time_LA1 = 0.0;
  m_time_LA4 = 0.0;
  m_time_RA1 = 0.0;
  m_time_RA4 = 0.0;

  // 車輪の半径と車輪間距離設定
  m_my->setWheel(m_radius, m_distance);
  m_state = 0;

  srand((unsigned)time( NULL ));

  // 車輪の回転速度
  m_vel = 1.0;

  // 関節の回転速度
  m_jvel = 1.0;

  // grasp初期化
  m_grasp = false;
  m_recogSrv = NULL;
  m_sended = false;

  // ここではゴミの名前が分かっているとします
  m_trashes.push_back("petbottle_1");
  m_trashes.push_back("can_0");
  m_trashes.push_back("apple");

  // ゴミ箱登録
  m_trashboxs.push_back("trashbox_0");
  m_trashboxs.push_back("trashbox_1");
  m_trashboxs.push_back("trashbox_2");

	broadcastMsgToSrv("Waiting 'start' message\n");

}