double MyController::onAction(ActionEvent &evt)
{
int count = 0;
Vector3d pos;
if (start == true){
int step = 3;
while (count<step){
double dx = 0;
double dz = -2.5;
double addx = 0.0;
double addz = 0.0;
my->getPosition(pos);
for(int i=0; i<motionNum; i++){
addx += dx;
addz += dz;
if(motionNum)
usleep(sleeptime);
my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
my->setJointAngle("LARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
my->setJointAngle("LARM_JOINT3", DEG2RAD(LARM_JOINT3[i]));
my->setJointAngle("RARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
my->setJointAngle("RARM_JOINT3", DEG2RAD(RARM_JOINT3[i]));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
my->setJointAngle("LLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
my->setJointAngle("RLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
}
for(int i=0; i<motionNum; i++){
addx += dx;
addz += dz;
usleep(sleeptime);
my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
my->setJointAngle("RARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
my->setJointAngle("RARM_JOINT3", DEG2RAD(-LARM_JOINT3[i]));
my->setJointAngle("LARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
my->setJointAngle("LARM_JOINT3", DEG2RAD(-RARM_JOINT3[i]));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
my->setJointAngle("RLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
my->setJointAngle("LLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
}
count++;
if(count==1){
usleep(3000000);
}
start = false;
}
/*while (count<20){
if (sw == false){
my->getPosition(pos);
my->setPosition(pos.x(), pos.y(), pos.z() - 10);
my->setJointAngle("LARM_JOINT1", DEG2RAD(-30));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(-20));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(10));
usleep(100000);
my->setJointAngle("LARM_JOINT1", DEG2RAD(0));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(0));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(0));
sw = true;
}
else{
my->getPosition(pos);
my->setPosition(pos.x(), pos.y(), pos.z() - 10);
my->setJointAngle("RARM_JOINT1", DEG2RAD(-30));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(-20));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(10));
usleep(100000);
my->setJointAngle("RARM_JOINT1", DEG2RAD(0));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(0));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(0));
sw = false;
}
count++;
if(count==7){
usleep(3000000);
}
start = false;
}*/
}
return 0.1;
}
double MyController::onAction(ActionEvent &evt)
{
Vector3d pos;
if (start == true){
my->getPosition(pos);
double dx = 0;
double dz = -2.5;
double addx = 0.0;
double addz = 0.0;
if(count%2){
for(int i=0; i<motionNum; i++){
addx += dx;
addz += dz;
if(motionNum)
usleep(sleeptime);
my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
my->setJointAngle("LARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
my->setJointAngle("LARM_JOINT3", DEG2RAD(LARM_JOINT3[i]));
my->setJointAngle("RARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
my->setJointAngle("RARM_JOINT3", DEG2RAD(RARM_JOINT3[i]));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
my->setJointAngle("LLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
my->setJointAngle("RLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
}
}
else{
for(int i=0; i<motionNum; i++){
addx += dx;
addz += dz;
usleep(sleeptime);
my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
my->setJointAngle("RARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
my->setJointAngle("RARM_JOINT3", DEG2RAD(-LARM_JOINT3[i]));
my->setJointAngle("LARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
my->setJointAngle("LARM_JOINT3", DEG2RAD(-RARM_JOINT3[i]));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
my->setJointAngle("RLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
my->setJointAngle("LLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
}
}
count++;
if(count==2){
sleep(3);
}
if(count==4){
sendMsg("operator","Passed_through");
}
if(count>step){
start = false;
}
}
return 0.1;
}
void SetAttrController::onRecvMessage(RecvMessageEvent &evt)
{
double x, y, z;
LOG_MSG(("message from : %s", evt.getSender()));
int n = evt.getSize();
for (int i=0; i<n; i++) {
LOG_MSG(("[%d] (%s)", i, evt.getString(i)));
}
SimObj *o = getObj(myname());
if (n>0 && o && !o->dynamics()) {
const char *cmd = evt.getString(0);
if (cmd) {
if (strcmp(cmd, "p")==0) {
// ----------------------------
// position set (x, y, z)
// ----------------------------
const char *param = evt.getString(1);
if (param) {
LOG_MSG(("set position (param=%s)", param));
if (parseVector3(param, x, y, z)) {
LOG_MSG(("(%f, %f, %f)", x, y, z));
o->setPosition(x, y, z);
}
}
} else if (strcmp(cmd, "vp")==0) {
// ----------------------------
// view pos
// ----------------------------
const char *param = evt.getString(1);
if (param) {
LOG_MSG(("set view position (param=%s)", param));
if (parseVector3(param, x, y, z)) {
LOG_MSG(("(%f, %f, %f)", x, y, z));
o->vpx(x);
o->vpy(y);
o->vpz(z);
}
}
} else if (strcmp(cmd, "vv")==0) {
// ----------------------------
// view vector
// ----------------------------
const char *param = evt.getString(1);
if (param) {
LOG_MSG(("set view vector (param=%s)", param));
if (parseVector3(param, x, y, z)) {
LOG_MSG(("(%f, %f, %f)", x, y, z));
o->vvx(x);
o->vvy(y);
o->vvz(z);
}
}
} else if (strcmp(cmd, "lep")==0) {
// ----------------------------
// left eye pos
// ----------------------------
const char *param = evt.getString(1);
if (param) {
LOG_MSG(("set left eye position (param=%s)", param));
if (parseVector3(param, x, y, z)) {
LOG_MSG(("(%f, %f, %f)", x, y, z));
o->lepx(x);
o->lepy(y);
o->lepz(z);
}
}
} else if (strcmp(cmd, "lev")==0) {
// ----------------------------
// left eye vector
// ----------------------------
const char *param = evt.getString(1);
if (param) {
LOG_MSG(("set left eye vector (param=%s)", param));
if (parseVector3(param, x, y, z)) {
LOG_MSG(("(%f, %f, %f)", x, y, z));
o->levx(x);
o->levy(y);
o->levz(z);
}
}
} else if (strcmp(cmd, "rep")==0) {
// ----------------------------
// right eye pos
// ----------------------------
const char *param = evt.getString(1);
if (param) {
LOG_MSG(("set right eye position (param=%s)", param));
if (parseVector3(param, x, y, z)) {
LOG_MSG(("(%f, %f, %f)", x, y, z));
o->repx(x);
o->repy(y);
o->repz(z);
}
}
} else if (strcmp(cmd, "rev")==0) {
// ----------------------------
// right eye vector
// ----------------------------
const char *param = evt.getString(1);
if (param) {
LOG_MSG(("set right eye vector (param=%s)", param));
if (parseVector3(param, x, y, z)) {
LOG_MSG(("(%f, %f, %f)", x, y, z));
o->revx(x);
o->revy(y);
o->revz(z);
}
}
}
}
}
}
double MyController::onAction(ActionEvent &evt)
{
// サービスが使用可能か定期的にチェックする
bool available = checkService("RobocupReferee");
if(!available && m_ref != NULL) m_ref = NULL;
// 使用可能
else if(available && m_ref == NULL){
// サービスに接続
m_ref = connectToService("RobocupReferee");
}
// 自分の位置取得
Vector3d myPos;
m_my->getPosition(myPos);
// 衝突中の場合,衝突が継続しているかチェック
if(colState){
CParts *parts = m_my->getMainParts();
bool state = parts->getCollisionState();
// 衝突していない状態に戻す
if(!state) colState = false;
}
int entSize = m_entities.size();
for(int i = 0; i < entSize; i++){
// ロボットまたはゴミ箱の場合は除く
if(m_entities[i] == "robot_000" ||
m_entities[i] == "recycle" ||
m_entities[i] == "burnable" ||
m_entities[i] == "room" ||
m_entities[i] == "moderator_0" ||
m_entities[i] == "Kinect_000" ||
m_entities[i] == "unburnable"){
continue;
}
// エンティティ取得
SimObj *ent = getObj(m_entities[i].c_str());
// 位置取得
Vector3d tpos;
ent->getPosition(tpos);
// ゴミ箱からゴミを結ぶベクトル
Vector3d vec(tpos.x()-myPos.x(), tpos.y()-myPos.y(), tpos.z()-myPos.z());
// ゴミがゴミ箱の中に入ったかどうか判定
if(abs(vec.x()) < tboxSize_x/2.0 &&
abs(vec.z()) < tboxSize_z/2.0 &&
tpos.y() < tboxMax_y &&
tpos.y() > tboxMin_y ){
// ゴミがリリースされているか確認
if(!ent->getIsGrasped()){
// ゴミを捨てる
tpos.y(-100);
tpos.x(myPos.x());
tpos.z(myPos.z());
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
ent->setPosition(tpos);
ent->setPosition(tpos);
usleep(500000);
tpos.y(-50.0);
ent->setPosition(tpos);
ent->setPosition(tpos);
ent->setPosition(tpos);
std::string msg;
// ゴミが所定のゴミ箱に捨てられているかチェック
// リサイクル
/*
if(strcmp(myname(), "recycle") == 0){
// 空のペットボトルのみ点が入る
if(strcmp(ent->name(), "petbottle") == 0 ||
strcmp(ent->name(), "petbottle_2") == 0 ||
strcmp(ent->name(), "petbottle_4") == 0 ||
strcmp(ent->name(), "mayonaise_1") == 0 ) {
msg = "RobocupReferee/Clean up succeeded" "/1000";
}
else{
msg = "RobocupReferee/Clean up failed" "/-600";
}
}
*/
// 燃えるゴミ
/*
else if(strcmp(myname(), "burnable") == 0){
// 燃えるゴミに入れるべきものは無い
msg = "RobocupReferee/Clean up failed" "/-600";
}
// 缶瓶
else if(strcmp(myname(), "unburnable") == 0){
if(strcmp(ent->name(), "can_0") == 0 ||
strcmp(ent->name(), "can_1") == 0 ||
strcmp(ent->name(), "can") == 0 ||
strcmp(ent->name(), "can_3") == 0) {
msg = "RobocupReferee/Clean up succeeded" "/1000";
}
else {
msg = "RobocupReferee/Clean up succeeded" "/-600";
}
}
*/
if(m_ref != NULL) {
// m_ref->sendMsgToSrv(msg.c_str());
}
//LOG_MSG((msg.c_str()));
}
}
}
return retValue;
}
//定期的に呼び出される関数
double RobotController::onAction(ActionEvent &evt)
{
static int iImage = -1;
static int ii = -20;
if(ii < 21){
SimObj *my = getObj(myname());
//変数の宣言
double x;
double y;
double z;
double r;
double thetaDEG;
double thetaRAD;
int depth_C[320*240];
thetaDEG = (ii * 9) - 180;
thetaRAD = DEG2RAD(thetaDEG);
r = CAPTURE_DISTANCE + 15;
//座標と角度を設定
x = r*sin(thetaRAD);
y = my->y();
z = r*cos(thetaRAD);
LOG_MSG(("x : %f", x));
LOG_MSG(("y : %f", y));
LOG_MSG(("z : %f", z));
LOG_MSG(("thetaDEG : %f", thetaDEG));
LOG_MSG(("thetaRAD : %f", thetaRAD));
LOG_MSG(("r : %f", r));
//デバッグ用
double xn;
double yn;
double zn;
xn = my->x();
yn = my->y();
zn = my->z();
LOG_MSG(("xn : %f", xn));
LOG_MSG(("yn : %f", yn));
LOG_MSG(("zn : %f", zn));
my->setPosition(x,y,z);
my->setAxisAndAngle(0.0, 1.0, 0.0, thetaRAD + PI);
xn = my->x();
yn = my->y();
zn = my->z();
LOG_MSG(("xn : %f", xn));
LOG_MSG(("yn : %f", yn));
LOG_MSG(("zn : %f", zn));
if(m_view != NULL) {
// ビット深度24,画像サイズ320X240の画像を取得します
ViewImage *img = m_view->captureView(4, COLORBIT_24, IMAGE_320X240);
if (img != NULL) {
// 画像データを取得します
char *buf = img->getBuffer();
std::string cap_data;
for(int i=0;i<320*240*3;i++){
cap_data += buf[i];
}
char view_fname[256];
sprintf(view_fname, "view%03d.txt", iImage);
FILE *fwV = fopen(view_fname , "w");
for(int i=0;i<320*240*3;i++){
fprintf(fwV, "%03d \n", (unsigned char)buf[i]);
}
fclose(fwV);
//Windows BMP 形式で保存します
char fname[256];
sprintf(fname, "s%03d.bmp", iImage);
img->saveAsWindowsBMP(fname);
//必要なくなったら削除します
delete img;
LOG_MSG(("captureIMG : %d", iImage));
//中央カメラの深度画像を取得します。
ViewImage *img_C1 = m_view->distanceSensor2D(0.0,255.0,4);
ViewImage *img_C2 = m_view->distanceSensor2D(255.0,510.0,4);
ViewImage *img_C3 = m_view->distanceSensor2D(510.0,765.0,4);
ViewImage *img_C4 = m_view->distanceSensor2D(510.0,765.0,4);
//DepthImage *img = distanceSensor2D();
//取得した深度画像の高さと幅取得
int height_C = img_C1->getHeight();
int width_C = img_C1->getWidth();
//視野角取得(高さ方向）
double fov_C = 60.0; //my->FOV();
//焦点距離の計算（pixel単位）
double fl_C = (img_C1->getHeight()/2)/tan(DEG2RAD(fov_C/2));
img_C1->saveAsWindowsBMP("2D_depth_C1.bmp");
img_C2->saveAsWindowsBMP("2D_depth_C2.bmp");
img_C3->saveAsWindowsBMP("2D_depth_C3.bmp");
img_C4->saveAsWindowsBMP("2D_depth_C4.bmp");
printf("saved img_C1,2,3,4 \n");
char *distance_C1 = img_C1->getBuffer();
char *distance_C2 = img_C2->getBuffer();
char *distance_C3 = img_C3->getBuffer();
char *distance_C4 = img_C4->getBuffer();
for(int i=0; i<240; i++){
for(int j=0; j<320; j++){
depth_C[i*320+j] = (unsigned char)distance_C1[i*320+j] + (unsigned char)distance_C2[i*320+j] + (unsigned char)distance_C3[i*320+j] + (unsigned char)distance_C4[i*320+j];
}
}
char depth_fname[256];
sprintf(depth_fname, "depth%03d.txt", iImage++);
FILE *fwC = fopen(depth_fname , "w");
for(int i=0; i<240; i++){
for(int j=0; j<320; j++){
fprintf(fwC, "%04d \n", depth_C[i*320+j]);
}
}
fclose(fwC);
delete img_C1;
delete img_C2;
delete img_C3;
delete img_C4;
}
}
}
if(ii == 21){
LOG_MSG(("capture conpleted"));
}
ii++;
return 1.0;
}
double MyController::onAction(ActionEvent &evt)
{
// サービスが使用可能か定期的にチェックする
bool available = checkService("RoboCupReferee");
if(!available && m_ref != NULL) m_ref = NULL;
// 使用可能
else if(available && m_ref == NULL){
// サービスに接続
m_ref = connectToService("RoboCupReferee");
}
// 自分の位置取得
Vector3d myPos;
m_my->getPosition(myPos);
// 衝突中の場合,衝突が継続しているかチェック
if(colState){
CParts *parts = m_my->getMainParts();
bool state = parts->getCollisionState();
// 衝突していない状態に戻す
if(!state) colState = false;
}
int entSize = m_entities.size();
for(int i = 0; i < entSize; i++){
// ロボットまたはゴミ箱の場合は除く
if(m_entities[i] == "robot_000" ||
m_entities[i] == "trashbox_0" ||
m_entities[i] == "trashbox_1" ||
m_entities[i] == "trashbox_2"){
continue;
}
// エンティティ取得
SimObj *ent = getObj(m_entities[i].c_str());
// 位置取得
Vector3d tpos;
ent->getPosition(tpos);
// ゴミ箱からゴミを結ぶベクトル
Vector3d vec(tpos.x()-myPos.x(), tpos.y()-myPos.y(), tpos.z()-myPos.z());
// ゴミがゴミ箱の中に入ったかどうか判定
if(abs(vec.x()) < tboxSize_x/2.0 &&
abs(vec.z()) < tboxSize_z/2.0 &&
tpos.y() < tboxMax_y &&
tpos.y() > tboxMin_y ){
// ゴミがリリースされているか確認
if(!ent->getIsGrasped()){
std::string msg;
bool success = false;
// 台の上に置く(成功)
if(strcmp(ent->name(), "mayonaise_0") == 0 && tpos.y() != 57.85) {tpos.y(57.85); success = true;}
else if(strcmp(ent->name(), "unknown_3") == 0 && tpos.y() != 54.04){ tpos.y(54.04); success = true;}
else if(strcmp(ent->name(), "unknown_0") == 0 && tpos.y() != 51.15){ tpos.y(51.15); success = true;}
else if(strcmp(ent->name(), "mugcup") == 0 && tpos.y() != 54.79){ tpos.y(54.79); success = true;}
else if(strcmp(ent->name(), "banana") == 0 && tpos.y() != 51.69){ tpos.y(51.69); success = true;}
else if(strcmp(ent->name(), "petbottle_0") == 0 && tpos.y() != 67.45){ tpos.y(67.45); success = true;}
else if(strcmp(ent->name(), "petbottle_2") == 0 && tpos.y() != 67.45){ tpos.y(67.45); success = true;}
else if(strcmp(ent->name(), "petbottle_4") == 0 && tpos.y() != 61.95){ tpos.y(61.95); success = true;}
else if(strcmp(ent->name(), "unknown_4") == 0 && tpos.y() != 52.04){ tpos.y(52.04); success = true;}
else if(strcmp(ent->name(), "unknown_1") == 0 && tpos.y() != 56.150){ tpos.y(56.150); success = true;}
else if(strcmp(ent->name(), "unknown_2") == 0 && tpos.y() != 60.650){ tpos.y(60.650); success = true;}
// 台の上に置く(失敗)
else if(strcmp(ent->name(), "petbottle_1") == 0 && tpos.y() != 67.45){ tpos.y(67.45);}
else if(strcmp(ent->name(), "petbottle_2") == 0 && tpos.y() != 67.45){ tpos.y(67.45);}
else if(strcmp(ent->name(), "petbottle_4") == 0 && tpos.y() != 61.95){ tpos.y(61.95);}
else if(strcmp(ent->name(), "mayonaise_1") == 0 && tpos.y() != 57.85){ tpos.y(57.85);}
else if(strcmp(ent->name(), "can_0") == 0 && tpos.y() != 55.335){ tpos.y(55.335);}
else if(strcmp(ent->name(), "can_1") == 0 && tpos.y() != 55.335){ tpos.y(55.335);}
else if(strcmp(ent->name(), "can_2") == 0 && tpos.y() != 57.050){ tpos.y(57.050);}
else if(strcmp(ent->name(), "can_3") == 0 && tpos.y() != 57.050){ tpos.y(57.050);}
else if(strcmp(ent->name(), "apple") == 0 && tpos.y() != 54.675){ tpos.y(54.675);}
else{continue;}
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
if(success){
msg = "RoboCupReferee/";
msg += ent->name();
msg += " succeeded/1000";
}
else{
msg = "RoboCupReferee/";
msg += ent->name();
msg += " target failed/400";
}
if(m_ref != NULL) {
m_ref->sendMsgToSrv(msg.c_str());
}
else{
LOG_MSG((msg.c_str()));
}
}
}
}
return retValue;
}