void MyController::onRecvMsg(RecvMsgEvent &evt)
{
std::string msg = evt.getMsg();
if(msg == "Task_start"){
if(trials==0){
m_state = 100;
}
else{
m_state = 1100;
}
broadcastMsg("get message: Task_start");
}
else if(msg == "Task_end"){
m_my->setWheelVelocity(0.0, 0.0);
m_state = 0;
broadcastMsg("get message: Task_end");
trials++;
}
else if(msg == "Time_over"){
m_my->setWheelVelocity(0.0, 0.0);
m_state = 0;
}
/*else if(m_state == 207 && msg == "leave the elevator"){
broadcastMsg("get message: leave the elevator");
m_state = 208;
}*/
}
double DemoRobotController::rotateTowardObj(Vector3d pos)
{
// "pos" means target position
// get own position
Vector3d ownPosition;
m_robotObject->getPartsPosition(ownPosition,"RARM_LINK2");
// pointing vector for target
Vector3d l_pos = pos;
l_pos -= ownPosition;
// ignore variation on y-axis
l_pos.y(0);
// get own rotation matrix
Rotation ownRotation;
m_robotObject->getRotation(ownRotation);
// get angles arround y-axis
double qw = ownRotation.qw();
double qy = ownRotation.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0) theta = -1.0*theta;
// rotation angle from z-axis to x-axis
double tmp = l_pos.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// direction
if(l_pos.x() > 0) targetAngle = -1.0*targetAngle;
targetAngle += theta;
double angVelFac = 3.0;
double l_angvel = m_angularVelocity/angVelFac;
if(targetAngle == 0.0) {
return 0.0;
}
else {
// circumferential distance for the rotation
double l_distance = m_distance*M_PI*fabs(targetAngle)/(2.0*M_PI);
// Duration of rotation motion (micro second)
double l_time = l_distance / (m_movingSpeed/angVelFac);
// Start the rotation
if(targetAngle > 0.0) {
m_robotObject->setWheelVelocity(l_angvel, -l_angvel);
}
else{
m_robotObject->setWheelVelocity(-l_angvel, l_angvel);
}
return l_time;
}
}
// object まで移動
double MyController::goToObj(Vector3d pos, double velocity, double range, double now)
{
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
pos -= myPos;
// y方向は考えない
pos.y(0);
// 距離計算
double distance = pos.length() - range;
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
// 移動開始
m_my->setWheelVelocity(velocity, velocity);
// 到着時間取得
double time = distance / vel;
return now + time;
}
// move to a target point
double MyController::goToObj(Vector3d pos, double velocity, double range, double now)
{
// get own position
Vector3d myPos;
m_my->getPosition(myPos);
// vector from own position to a target position
pos -= myPos;
pos.y(0);
// distance to a target position
double distance = pos.length() - range;
// calcurate veloocity from radius of wheels
double vel = m_radius*velocity;
// start moving
m_my->setWheelVelocity(velocity, velocity);
// calcurate time of arrival
double time = distance / vel;
return now + time;
}
double DemoRobotController::goToObj(Vector3d pos, double range)
{
// get own position
Vector3d robotCurrentPosition;
//m_robotObject->getPosition(robotCurrentPosition);
m_robotObject->getPartsPosition(robotCurrentPosition,"RARM_LINK2");
// pointing vector for target
Vector3d l_pos = pos;
l_pos -= robotCurrentPosition;
// ignore y-direction
l_pos.y(0);
// measure actual distance
double distance = l_pos.length() - range;
// start moving
m_robotObject->setWheelVelocity(m_angularVelocity, m_angularVelocity);
// time to be elapsed
double l_time = distance / m_movingSpeed;
return l_time;
}
// object まで移動
double MyController::goToObj(Vector3d nextPos, double velocity, double range, double now)
{
printf("goToObj関数内 goToObj %lf %lf %lf \n", nextPos.x(), nextPos.y(), nextPos.z());
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
printf("goToObj関数内 ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z());
// 自分の位置からターゲットを結ぶベクトル
Vector3d pos = nextPos;
pos -= myPos;
// y方向は考えない
pos.y(0);
// 距離計算
double distance = pos.length() - range;
//printf("distance: %lf \n", distance);
//printf("range = %lf \n", range);
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
// 移動開始
m_my->setWheelVelocity(velocity, velocity);
// 到着時間取得
double time = distance / vel;
return now + time;
}
char* MyController::sendSceneInfo(std::string header, int camID) {
m_my->setWheelVelocity(0.0, 0.0);
Vector3d myPos;
m_my->getPosition(myPos);
double x = myPos.x();
double z = myPos.z();
double theta = calcHeadingAngle(); // y方向の回転は無しと考える
// カメラがついているリンク名取得
std::string link = ""; //"WAIST_LINK0"; //m_my->getCameraLinkName(3);
if (camID < 3) {
link = m_my->getCameraLinkName(camID);
} else {
link = "WAIST_LINK0";
}
//const dReal *lpos = m_my->getParts(link.c_str())->getPosition();
Vector3d lpos;
m_my->getParts(link.c_str())->getPosition(lpos);
// カメラの位置取得(リンク座標系)
Vector3d cpos;
m_my->getCamPos(cpos, camID);
// カメラの位置(絶対座標系, ロボットの回転はないものとする)
printf("linkpos: %lf %lf %lf \n", lpos.x(), lpos.y(), lpos.z());
printf("camerapos: %lf %lf %lf \n", cpos.x(), cpos.y(), cpos.z());
Vector3d campos(lpos.x() + cpos.z() * sin(DEG2RAD(theta)),
lpos.y() + cpos.y(),
lpos.z() + cpos.z() * cos(DEG2RAD(theta)));
//Vector3d campos(cpos.x(), cpos.y(), cpos.z());
// カメラの方向取得(ロボットの回転,関節の回転はないものとする)
Vector3d cdir;
m_my->getCamDir(cdir, camID);
char *replyMsg = new char[1024];
sprintf(replyMsg, "%s %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf",
header.c_str(), x, z, theta, campos.x(), campos.y(), campos.z(), cdir.x(), cdir.y(), cdir.z());
printf("%s \n", replyMsg);
m_srv->sendMsgToSrv(replyMsg);
m_my->setWheelVelocity(0.0, 0.0);
return replyMsg;
}
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::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2 * acos(fabs(qw));
if (qw * qy < 0) {
theta = -1 * theta;
}
printf("ロボットが向いている角度 current theta: %lf(deg) \n", theta * 180 / PI);
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
printf("ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z());
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpPos = pos;
tmpPos -= myPos;
// 近すぎるなら，回転なし
double dis = tmpPos.x() * tmpPos.x() + tmpPos.z() * tmpPos.z();
if (dis < 1.0) {
return 0.0;
}
// z方向からの角度
double rate = tmpPos.x() / tmpPos.z();
double targetAngle = atan(rate);
if (tmpPos.z() < 0) {
targetAngle += PI;
}
printf("回転する角度 targetAngle: %lf(deg) 結ぶベクトル tmpPos.x: %lf, tmpPos.z: %lf, rate: %lf \n", targetAngle*180.0/PI, tmpPos.x(), tmpPos.z(), rate);
targetAngle -= theta;
if (targetAngle > PI) {
targetAngle = targetAngle - 2 * PI;
}
printf("targetAngle: %lf(deg) currentAngle: %lf(deg) \n", targetAngle*180.0/PI, theta * 180.0 / PI);
if (targetAngle == 0.0) {
printf("donot need to 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;
printf("rotateTime: %lf \n", time);
// 車輪回転開始
if (targetAngle > 0.0) {
m_my->setWheelVelocity(-velocity, velocity);
} else {
m_my->setWheelVelocity(velocity, -velocity);
}
return now + time;
}
}
double MyController::onAction(ActionEvent &evt)
{
switch(m_state){
// wait
case 0:{
break;
}
// 1st section
case 100: {
m_time = rotateTowardObj(Vector3d(-50,60,500),m_vel_rot,evt.time());
m_state = 101;
break;
}
case 101: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-50,60,500), m_vel, 1.0, evt.time());
m_state = 102;
}
break;
}
case 102: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-50,60,575), m_vel, 1.0, evt.time());
m_state = 103;
}
break;
}
case 103: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-450,60,575),m_vel_rot,evt.time());
m_state = 104;
break;
}
}
case 104: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-450,60,575), m_vel, 1.0, evt.time());
m_state = 105;
}
break;
}
case 105: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
// wait for a walking person
usleep(3200000);
m_state = 106;
}
break;
}
case 106: {
m_time = rotateTowardObj(Vector3d(-525,60,575),m_vel_rot,evt.time());
m_state = 107;
break;
}
case 107: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-525,60,575), m_vel, 1.0, evt.time());
m_state = 200;
}
break;
}
// 2nd section
case 200: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-800,60,575),m_vel_rot,evt.time());
m_state = 201;
}
break;
}
case 201: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,575), m_vel, 1.0, evt.time());
m_state = 202;
}
break;
}
case 202: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-800,60,275),m_vel_rot,evt.time());
m_state = 203;
}
break;
}
case 203: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,275), m_vel, 1.0, evt.time());
m_state = 204;
}
break;
}
case 204: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-1100,60,275),m_vel_rot,evt.time());
m_state = 205;
}
break;
}
case 205: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-1100,60,275), m_vel, 1.0, evt.time());
m_state = 206;
}
break;
}
case 206: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
// tell robot entered in an elevator
broadcastMsg("Door_close");
m_state = 207;
}
break;
}
case 207: {
// wait for a while until a door opened
sleep(5);
m_state = 208;
break;
}
case 208: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-800,60,275),m_vel_rot,evt.time());
m_state = 209;
}
break;
}
case 209: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,275), m_vel, 1.0, evt.time());
m_state = 210;
}
break;
}
case 210: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
//
std::string msg = "Get_off";
broadcastMsg(msg);
//
sleep(14);
m_state = 300;
}
break;
}
// 3rd section
case 300: {
m_time = rotateTowardObj(Vector3d(-800,60,-200),m_vel_rot,evt.time());
m_state = 301;
break;
}
case 301: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,-200), m_vel, 1.0, evt.time());
m_state = 302;
}
break;
}
case 302: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-970,60,-200),m_vel_rot,evt.time());
m_state = 303;
}
break;
}
case 303: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-970,60,-200), m_vel, 1.0, evt.time());
m_state = 304;
}
break;
}
case 304: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-970,60,-400),m_vel_rot,evt.time());
m_state = 305;
}
break;
}
case 305: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-970,60,-400), m_vel, 1.0, evt.time());
m_state = 306;
}
break;
}
case 306: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-800,60,-550),m_vel_rot,evt.time());
m_state = 307;
}
break;
}
case 307: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,-550), m_vel, 1.0, evt.time());
m_state = 900;
}
break;
}
// finish line
case 900: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-800,60,-750),m_vel_rot,evt.time());
m_state = 901;
}
break;
}
case 901: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,-750), m_vel, 1.0, evt.time());
m_state = 999;
}
break;
}
// 1st section
case 1100: {
//m_time = rotateTowardObj(Vector3d(-50,60,500),m_vel,evt.time());
m_time = rotateTowardObj(Vector3d(-50,60,-300),m_vel_rot,evt.time());
m_state = 1101;
break;
}
case 1101: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
//m_time = goToObj(Vector3d(-50,60,500), m_vel*20, 1.0, evt.time());
m_time = goToObj(Vector3d(-50,60,-300), m_vel, 1.0, evt.time());
m_state = 1102;
}
break;
}
case 1102: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
//m_time = goToObj(Vector3d(-50,60,575), m_vel*20, 1.0, evt.time());
m_time = rotateTowardObj(Vector3d(-300,60,-300),m_vel_rot,evt.time());
m_state = 1103;
}
break;
}
case 1103: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
//m_time = rotateTowardObj(Vector3d(-450,60,575),m_vel,evt.time());
m_time = goToObj(Vector3d(-300,60,-300), m_vel, 1.0, evt.time());
m_state = 1104;
break;
}
}
case 1104: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-300,60,575),m_vel_rot,evt.time());
m_state = 1105;
}
break;
}
case 1105: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-300,60,575), m_vel, 1.0, evt.time());
m_state = 1106;
}
break;
}
case 1106: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-450,60,575),m_vel_rot,evt.time());
m_state = 1107;
}
break;
}
case 1107: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-450,60,575), m_vel, 1.0, evt.time());
m_state = 1108;
}
break;
}
case 1108: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
// wait for a walking person
usleep(3200000);
m_state = 1109;
}
break;
}
case 1109: {
m_time = rotateTowardObj(Vector3d(-525,60,575),m_vel_rot,evt.time());
m_state = 1110;
break;
}
case 1110: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-525,60,575), m_vel, 1.0, evt.time());
m_state = 1200;
}
break;
}
// 2nd section
case 1200: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-800,60,575),m_vel_rot,evt.time());
m_state = 1201;
}
break;
}
case 1201: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,575), m_vel, 1.0, evt.time());
m_state = 1202;
}
break;
}
case 1202: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-800,60,275),m_vel_rot,evt.time());
m_state = 1203;
}
break;
}
case 1203: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,275), m_vel, 1.0, evt.time());
m_state = 1204;
}
break;
}
case 1204: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-1100,60,275),m_vel_rot,evt.time());
m_state = 1205;
}
break;
}
case 1205: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-1100,60,275), m_vel, 1.0, evt.time());
m_state = 1206;
}
break;
}
case 1206: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
// tell robot entered in an elevator
broadcastMsg("Door_close");
m_state = 1207;
}
break;
}
case 1207: {
// wait for a while until a door opened
sleep(5);
m_state = 1208;
break;
}
case 1208: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-800,60,275),m_vel_rot,evt.time());
m_state = 1209;
}
break;
}
case 1209: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,275), m_vel, 1.0, evt.time());
m_state = 1210;
}
break;
}
case 1210: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
//
std::string msg = "Get_off";
broadcastMsg(msg);
//
sleep(14);
m_state = 1300;
}
break;
}
// 3rd section
case 1300: {
m_time = rotateTowardObj(Vector3d(-800,60,-200),m_vel_rot,evt.time());
m_state = 1301;
break;
}
case 1301: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,-200), m_vel, 1.0, evt.time());
m_state = 1302;
}
break;
}
case 1302: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-970,60,-200),m_vel_rot,evt.time());
m_state = 1303;
}
break;
}
case 1303: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-970,60,-200), m_vel, 1.0, evt.time());
m_state = 1304;
}
break;
}
case 1304: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-970,60,-400),m_vel_rot,evt.time());
m_state = 1305;
}
break;
}
case 1305: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-970,60,-400), m_vel, 1.0, evt.time());
m_state = 1306;
}
break;
}
case 1306: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-800,60,-550),m_vel_rot,evt.time());
m_state = 1307;
}
break;
}
case 1307: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,-550), m_vel, 1.0, evt.time());
m_state = 1900;
}
break;
}
// finish line
case 1900: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(Vector3d(-800,60,-750),m_vel_rot,evt.time());
m_state = 1901;
}
break;
}
case 1901: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(Vector3d(-800,60,-750), m_vel, 1.0, evt.time());
m_state = 999;
}
break;
}
case 999: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
broadcastMsg("Task_finished");
m_state = 0;
}
break;
}
}
return 0.05;
}
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 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::onAction(ActionEvent &evt)
{
if(first == false){
std::string msg = "start";
broadcastMsg(msg);
}
if(first==false){
FILE* fp;
x=0;
y=0;
z=0;
w=0; //チェックポイント
if((fp = fopen("node2.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;
}
switch(m_state){
// 初期状態
case 0: {
npos.x(temp.x[i]);
npos.y(temp.y[i]);
npos.z(temp.z[i]);
m_time= rotateTowardObj(npos,m_vel,evt.time());
m_state = 1;
break;
}
// 回転中
case 1: {
// 回転終了
if(evt.time() >= m_time){
// 回転を止める
m_my->setWheelVelocity(0.0, 0.0);
m_time = goToObj(npos, m_vel*20, 1.0, evt.time());
m_state = 2;
}
break;
}
case 2: {
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
if(temp.w[i] == 1.0){
usleep(3200000);
i++;
}else if(temp.w[i] == 2.0){
std::string msg = "elevator";
//"man_000"にメッセージを送信します
//broadcastMsgToSrv("Elevator");
sendMsg("man_000", msg);
m_state = 0;
i++;
}else if(temp.w[i] == 3.0){
std::string msg = "ok";
//"man_000"にメッセージを送信します
//broadcastMsgToSrv("Elevator");
sendMsg("man_000", msg);
sleep(10);
m_state = 0;
i++;
}else if(temp.w[i] == 4.0){
m_state =99;
}
else{
//std::string msg = "elevator";
//"man_000"にメッセージを送信します
//broadcastMsgToSrv("Elevator");
//sendMsg("man_000", msg);
m_state = 0;
i++;
}
}
break;
}
case 99: {
if(flg==false){
m_my->setWheelVelocity(0.0, 0.0);
//std::string msg = "Collision";
//broadcastMsg(msg);
flg=true;
}
}
}
return 0.1;
}
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
tmpp -= myPos;
// y方向は考えない
tmpp.y(0);
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
printf("ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z());
// エンティティの初期方向
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));
if(qw*qy < 0)
theta = -1*theta;
printf("ロボットが向いている角度 current theta: %lf(deg) \n", theta * 180 / PI);
// z方向からの角度
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// 方向
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
targetAngle += theta;
printf("targetAngle: %lf(deg) currentAngle: %lf(deg) \n", targetAngle*180.0/PI, theta * 180.0 / PI);
if(targetAngle == 0.0){
return 0.0;
}
else {
// 回転すべき円周距離
double distance = m_distance*PI*fabs(targetAngle)/(2*PI);
printf("distance: %lf \n", distance);
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
printf("radius: %lf, velocity: %lf, vel: %lf \n", m_radius, velocity, vel);
// 回転時間(u秒)
double time = distance / vel;
printf("rotateTime: %lf = dis: %lf / vel: %lf\n", time, distance, vel);
// 車輪回転開始
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else{
m_my->setWheelVelocity(-velocity, velocity);
}
return now + time;
}
}
double MyController::onAction(ActionEvent &evt)
{
/* if(!checkService("RecogTrash")){
m_srv == NULL;
m_state = 0;
return UPDATE_INTERVAL;
}
if(m_srv == NULL){
// ゴミ認識サービスが利用可能か調べる
if(checkService("RecogTrash")){
// ゴミ認識サービスに接続
m_srv = connectToService("RecogTrash");
return UPDATE_INTERVAL;
}
}*/
//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_JOINT4", 0.0, 0.0);
m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
sendSceneInfo("Start");
printf("Started! \n");
m_executed = true;
}
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());
if (m_lookObjFlg == 1.0) {
printf("looking to Obj \n");
m_state = 810;
} else {
printf("go to next node \n");
m_state = 815;
}
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 UPDATE_INTERVAL;
}
bool RobotController::goTo(Vector3d pos, double rangeToPoint)
{
double speed;
Vector3d ownPosition;
my->getPosition(ownPosition);
Rotation ownRotation;
my->getRotation(ownRotation);
double angle = getAngularXonVect(pos, ownPosition);
double dist = getDistoObj(pos,ownPosition);
double roll = getRoll(ownRotation);
if (angle > 3 || angle < -3) angle = M_PI;
// error on angle
if ((angle-roll)>-ERROR_ANGLE && (angle-roll)<ERROR_ANGLE)
{
// error on distance
if (dist-rangeToPoint < ERROR_DISTANCE && dist-rangeToPoint > -ERROR_DISTANCE)
{
stopRobotMove();
return true;
}
else
{
speed = dist-rangeToPoint;
if (dist-rangeToPoint < 5)
{
if ( dist-rangeToPoint > 0 )
my->setWheelVelocity(1, 1);
else
my->setWheelVelocity(-1, -1);
}
else if ( dist-rangeToPoint > 0 )
my->setWheelVelocity(Robot_speed , Robot_speed );
else
my->setWheelVelocity(-Robot_speed , -Robot_speed );
return false;
}
}
else
{
speed = fabs(angle-roll)*4;
if (speed/4 > 0.3)
if (angle < -M_PI_2 && roll > M_PI_2)
my->setWheelVelocity(-MAX_WHEEL_VELOCITY, MAX_WHEEL_VELOCITY);
else if (angle > M_PI_2 && roll < -M_PI_2)
my->setWheelVelocity(MAX_WHEEL_VELOCITY, -MAX_WHEEL_VELOCITY);
else if (angle < roll)
my->setWheelVelocity(MAX_WHEEL_VELOCITY, -MAX_WHEEL_VELOCITY);
else
my->setWheelVelocity(-MAX_WHEEL_VELOCITY, MAX_WHEEL_VELOCITY);
else if (angle < -M_PI_2 && roll > M_PI_2)
my->setWheelVelocity(-speed, speed);
else if (angle > M_PI_2 && roll < -M_PI_2)
my->setWheelVelocity(speed, -speed);
else if (angle < roll)
my->setWheelVelocity(speed, -speed);
else
my->setWheelVelocity(-speed, speed);
return false;
}
return false;
}
void DemoRobotController::stopRobotMove(void) {
m_robotObject->setWheelVelocity(0.0, 0.0);
}
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 MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
tmpp -= myPos;
// 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));
if(qw*qy < 0)
theta = -1*theta;
// z方向からの角度
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// 方向
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
targetAngle += theta;
if(targetAngle == 0.0){
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);
}
return now + time;
}
}
void MyController::onRecvMsg(RecvMsgEvent &evt)
{
// 送信者取得
std::string sender = evt.getSender();
std::cout << "sender: " << sender << std::endl;
char *all_msg = (char*)evt.getMsg();
printf("all_msg: %s \n", all_msg);
char *delim = (char *)(" ");
char *ctx;
char *header = strtok_r(all_msg, delim, &ctx);
if (strcmp(header, "RESET") == 0) {
printf("Received RESET \n");
setRobotPosition(0, -50);
setRobotHeadingAngle(0);
setCameraPosition(0, 3);
printf("Reseted RobotPosition \n");
//char* replyMsg = sendSceneInfo();
sendSceneInfo("Start");
m_executed = true;
return;
}
// 送信者がゴミ認識サービスの場合
if(sender == "RecogTrash") {
/*char *all_msg = (char*)evt.getMsg();
//printf("all_msg: %s \n", all_msg);
char *delim = (char *)(" ");
char *ctx;
char *header = strtok_r(all_msg, delim, &ctx);*/
double x = 0, y = 0, z = 0; // range = 0;
if(strcmp(header, "ObjDir") == 0) {
// 次に移動する座標を位置を取り出す
x = atof(strtok_r(NULL, delim, &ctx));
y = atof(strtok_r(NULL, delim, &ctx));
z = atof(strtok_r(NULL, delim, &ctx));
m_range = atof(strtok_r(NULL, delim, &ctx));
nextPos.set(x, y, z);
//printf("m_range = %lf \n", m_range);
printf("[ClientMess] ObjDir %lf %lf %lf range: %lf\n", nextPos.x(), nextPos.y(), nextPos.z(), m_range);
// ロボットのステートを更新
m_state = 20;
m_executed = false;
return;
}
// 物体のある場所に到着し、アームを伸ばし、物体を掴む
if(strcmp(header, "grab") == 0) {
printf("grab \n");
// 回転を止める
m_my->setWheelVelocity(0.0, 0.0);
m_state = 30;
m_executed = false;
return;
}
if(strcmp(header, "TrashBoxDir") == 0) {
printf("TrashBoxDir \n");
// 次に移動する座標を位置を取り出す
x = atof(strtok_r(NULL, delim, &ctx));
y = atof(strtok_r(NULL, delim, &ctx));
z = atof(strtok_r(NULL, delim, &ctx));
m_range = atof(strtok_r(NULL, delim, &ctx));
//printf("range = %lf \n", m_range);
nextPos.set(x, y, z);
printf("[ClientMess] TrashBoxDir %lf %lf %lf range: %lf\n", nextPos.x(), nextPos.y(), nextPos.z(), m_range);
m_state = 40;
m_executed = false;
return;
}
if(strcmp(header, "ThrowTrash") == 0) {
m_state = 50;
m_executed = false;
return;
}
if(strcmp(header, "Finish") == 0) {
m_state = 100;
m_executed = false;
return;
}
if(strcmp(header, "RandomRouteStart") == 0) {
m_state = 800;
m_executed = false;
return;
}
if(strcmp(header, "RandomRouteArrived") == 0) {
//printf("onRecv RandomRouteArrived \n");
m_state = 800;
m_executed = false;
return;
}
if(strcmp(header, "RandomRoute") == 0) {
x = atof(strtok_r(NULL, delim, &ctx));
z = atof(strtok_r(NULL, delim, &ctx));
m_range = atof(strtok_r(NULL, delim, &ctx));
nextPos.set(x, y, z);
double lookingX = atof(strtok_r(NULL, delim, &ctx));
double lookingZ = atof(strtok_r(NULL, delim, &ctx));
m_lookingPos.set(lookingX, 0, lookingZ);
m_state = 805;
m_executed = false;
return;
}
if(strcmp(header, "GoForwardVelocity") == 0) {
double wheelVel = atof(strtok_r(NULL, delim, &ctx));
printf("GoForwardVelocity coef: %lf \n", wheelVel);
wheelVel *= m_vel;
m_my->setWheelVelocity(wheelVel * 10., wheelVel * 10.);
//sendSceneInfo();
return;
}
if(strcmp(header, "RotateVelocity") == 0) {
double wheelVel = atof(strtok_r(NULL, delim, &ctx));
printf("RotateVelocity coef: %lf \n", wheelVel);
wheelVel *= m_vel;
m_my->setWheelVelocity(-wheelVel, wheelVel);
//sendSceneInfo();
return;
}
if(strcmp(header, "Stop") == 0) {
printf("Stop joyStick \n");
m_my->setWheelVelocity(0.0, 0.0);
sendSceneInfo();
return;
}
if (strcmp(header, "CamID") == 0) {
printf("Setting CameraID \n");
m_CamID = atoi(strtok_r(NULL, delim, &ctx));
return;
}
if (strcmp(header, "CaptureData") == 0) {
printf("SetRobotPosition \n");
double x = atof(strtok_r(NULL, delim, &ctx));
double z = atof(strtok_r(NULL, delim, &ctx));
setRobotPosition(x, z);
double angle = atof(strtok_r(NULL, delim, &ctx));
printf("setRobotHeadingAngle: %lf \n", angle);
setRobotHeadingAngle(angle);
char* replyMsg = sendSceneInfo();
//m_srv->sendMsgToSrv(replyMsg);
m_executed = true;
return;
}
} else if (sender == "SIGViewer") {
printf("mess from SIGViewer \n");
if(strcmp(header, "CameraAngle") == 0) {
double angle = atof(strtok_r(NULL, delim, &ctx));
printf("CameraAngle: %lf \n", angle);
setCameraPosition(angle, 3);
char* replyMsg = sendSceneInfo();
m_executed = true;
return;
} else if (strcmp(header, "RobotAngle") == 0) {
printf("rorate robor start \n");
double angle = atof(strtok_r(NULL, delim, &ctx));
printf("RobotHeadingAngle: %lf \n", angle);
setRobotHeadingAngle(angle);
char* replyMsg = sendSceneInfo();
//m_srv->sendMsgToSrv(replyMsg);
m_executed = true;
return;
} else if (strcmp(header, "RotateDir") == 0) {
// 次に移動する座標を位置を取り出す
double x = atof(strtok_r(NULL, delim, &ctx));
double z = atof(strtok_r(NULL, delim, &ctx));
nextPos.set(x, 0, z);
printf("RotateDir %lf %lf \n", nextPos.x(), nextPos.z());
// ロボットのステートを更新
m_state = 920;
m_executed = false;
return;
} else if(strcmp(header, "RobotPosition") == 0) {
double x = atof(strtok_r(NULL, delim, &ctx));
double z = atof(strtok_r(NULL, delim, &ctx));
setRobotPosition(x, z);
return;
}
}
}
void RobotController::stopRobotMove(void)
{
my->setWheelVelocity(0.0, 0.0);
}
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)
{
//std::cout << "m_state " << m_state << std::endl;
//std::cout << "the size of Vector " << Record_Postures.size() << std::endl;
switch(m_state)
{
case 10:
{
Robot_speed = Change_Robot_speed;
choose_task_arm(5, LEFT_ARM);
choose_task_arm(5, RIGHT_ARM);
// printf("got it in case!1 flag1 \n");
if (goTo(m_relayPoint1, 0) == true && moveArm(LEFT_ARM) == true && moveArm(RIGHT_ARM) == true)
{
m_state = 20;
// printf("got it in case!1 \n");
}
break;
}
case 20:
{
Robot_speed = Change_Robot_speed;
if (goTo(m_relayPoint2, 0) == true) m_state = 30;
break;
}
case 30:
{
Robot_speed = Change_Robot_speed;
if (goTo(m_relayFrontTable, 0) == true) m_state = 40;
break;
}
case 40: // Test if the cycle is finished or not
{
if (cycle > 0)
{
m_state = 41;
broadcastMsg("Show_me");
get_Kinect_Data();
m_time = evt.time() + 5;
break;
}
else
{
m_state = 49;
break;
}
}
case 41:
{
if(evt.time() > m_time) m_state = 42;
break;
}
case 42:
{
Kinect_Data_Off(); // finished analysing data
m_pointedObject = "petbottle";
m_pointedtrash = "recycle";
std::cout << "Task started Robot ........ " << std::endl;
//PrintPosture();
m_state = 50;
}
case 49:
{
break;
}
case 50: //Optional case !!!
{
Robot_speed = Change_Robot_speed;
if (m_pointedObject=="petbottle")
{
if (goTo(m_BottleFront, 0) == true) m_state = 60;
}
else if (m_pointedObject=="mugcup")
{
if (goTo(m_MuccupFront, 0) == true) m_state = 60;
}
else if (m_pointedObject=="can")
{
if (goTo(m_CanFront, 0) == true) m_state = 60;
}
break;
}
case 60: //preparation of the arm for grasp
{
Robot_speed = Change_Robot_speed;
recognizeObjectPosition(m_Object_togo, m_pointedObject);
if (goTo(m_Object_togo, 70) == true)
{
m_state = 70;
}
break;
}
case 70: //preparation of the arm for grasp
{
choose_task_arm(1, LEFT_ARM);
if (moveArm(LEFT_ARM) == true) m_state = 80;
break;
}
case 80: //move to the object
{
Robot_speed = 1;
if (goTo(m_Object_togo, 38) == true) m_state = 90;
break;
}
case 90: //move arm to grasp the object
{
choose_task_arm(2, LEFT_ARM);
grasp_left_hand();
if (moveArm(LEFT_ARM) == true) m_state = 100;
break;
}
case 100:
{
m_state = 110;
break;
}
case 110: //move arm to place in good position for moving
{
grasp_left_hand();
choose_task_arm(3, LEFT_ARM);
if (moveArm(LEFT_ARM) == true)
m_state = 120;
break;
}
case 120:
{
my->setWheelVelocity(-1.4,-1.4);
choose_task_arm(5, LEFT_ARM);
if (moveArm(LEFT_ARM) == true)
{
Robot_speed = Change_Robot_speed;
m_state = 130;
sleep(1);
}
break;
}
case 130: //move to the object
{
// Robot_speed = 1;
if (goTo(m_relayFrontTrash, 0) == true)
{
m_state = 140;
}
break;
}
case 140:
{
if (goTo(m_relayFrontTable_reset, 0) == true)
{
sleep(2);
m_state = 150;
}
break;
}
case 990: //move to the object
{
// Robot_speed = 1;
break;
}
case 150: //Optional case !!!
{
Robot_speed = Change_Robot_speed;
if (m_pointedtrash=="recycle")
{
if (goTo(m_RecycleFront, 0) == true) m_state = 160;
}
else if (m_pointedtrash=="burnable")
{
if (goTo(m_BurnableFront, 0) == true) m_state = 160;
}
else if (m_pointedtrash=="unburnable")
{
if (goTo(m_UnburnableFront, 0) == true) m_state = 160;
}
break;
}
case 160: //preparation of the arm for grasp
{
recognizeObjectPosition(m_Trash_togo, m_pointedtrash);
if (goTo(m_Trash_togo, 50) == true)
{
m_state = 170;
}
break;
}
case 170: //preparation of the arm for grasp
{
choose_task_arm(1, LEFT_ARM);
if (moveArm(LEFT_ARM) == true) m_state = 180;
break;
}
case 180:
{
Robot_speed = 1;
if (goTo(m_Trash_togo, 60) == true) m_state = 190;
break;
}
case 190: //move arm to grasp the object
{
choose_task_arm(3, LEFT_ARM);
if (moveArm(LEFT_ARM) == true)
{
m_state = 200;
release_left_hand();
}
break;
}
case 200:
{
my->setWheelVelocity(-2.5,-2.5);
choose_task_arm(2, LEFT_ARM);
if (moveArm(LEFT_ARM) == true)
{
Robot_speed = Change_Robot_speed;
m_state = 210;
}
break;
}
case 210: //move to the object
{
Robot_speed = Change_Robot_speed;
if (goTo(m_relayFrontTrash, 0) == true) m_state = 220;
break;
}
case 220: //preparation of the arm for grasp
{
choose_task_arm(5, LEFT_ARM);
if (moveArm(LEFT_ARM) == true) m_state = 230;
break;
}
case 230: //move to the object
{
Robot_speed = Change_Robot_speed;
if (goTo(m_relayFrontTable, 0) == true)
{
cycle = cycle-1;
m_state = 40;
broadcastMsg("Task_finished");
m_pointedtrash = "";
m_pointedObject = "";
m_state = 0;
}
break;
}
}
return m_onActionReturn;
}
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// get own position
Vector3d myPos;
m_my->getPosition(myPos);
// vector from own position to a target position
Vector3d tmpp = pos;
tmpp -= myPos;
// rotation about y-axis is always 0
tmpp.y(0);
// get own rotation
Rotation myRot;
m_my->getRotation(myRot);
// initial direction
Vector3d iniVec(0.0, 0.0, 1.0);
// get rotation angle about y-axis
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0){
theta = -theta;
}
// angle from z-axis
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// calcurate target angle
if(tmpp.x() > 0){
targetAngle = -1*targetAngle;
}
targetAngle += theta;
if(targetAngle<-M_PI){
targetAngle += 2*M_PI;
}
else if(targetAngle>M_PI){
targetAngle -= 2*M_PI;
}
if(targetAngle == 0.0){
return 0.0;
}
else {
// circumference length for rotation
double distance = m_distance*M_PI*fabs(targetAngle)/(2*M_PI);
// calcurate velocity from radius of wheels
double vel = m_radius*velocity;
// rotation time (micro second)
double time = distance / vel;
// start rotating
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else{
m_my->setWheelVelocity(-velocity, velocity);
}
return now + time;
}
}
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;
}
