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 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 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;
}
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;
}