GT2005: Modules/BehaviorControl/GT2005BehaviorControl/GT2005Symbols/KickLoggerSymbols.cpp Source File

00001 /** 
00002 * @file KickLoggerSymbols.cpp
00003 *
00004 * Implementation of class KickLoggerSymbols.
00005 *
00006 * @author Carsten Schumann
00007 */
00008 
00009 #include "KickLoggerSymbols.h"
00010 
00011 KickLoggerSymbols::KickLoggerSymbols(BehaviorControlInterfaces& interfaces)
00012 : BehaviorControlInterfaces(interfaces), kickLogger()
00013 {
00014 }
00015 
00016 
00017 void KickLoggerSymbols::registerSymbols(Xabsl2Engine& engine)
00018 {
00019   engine.registerBooleanInputSymbol("kicklogger.save",this,
00020     (bool (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::kickLoggerSave);
00021   engine.registerBooleanInputSymbol("kicklogger.set-before-state",this,
00022     (bool (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::kickLoggerSetBeforeState);
00023   engine.registerBooleanInputSymbol("kicklogger.setExecuteState",this,
00024     (bool (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::kickLoggerSetExecuteState);
00025   engine.registerBooleanInputSymbol("kicklogger.setAfterStateAndCommit",this,
00026     (bool (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::kickLoggerSetAfterStateAndCommit);
00027   engine.registerBooleanInputSymbol("kicklogger.execute",this,
00028     (bool (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::doExecute);
00029   engine.registerBooleanInputSymbol("kicklogger.near",this,
00030     (bool (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::isNear);
00031   engine.registerBooleanInputSymbol("kicklogger.ceilcamActive",this,
00032     (bool (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::ceilCamActive);
00033   engine.registerBooleanInputSymbol("kicklogger.ball-has-stopped",this,
00034     (bool (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::ballHasStopped);
00035   engine.registerBooleanInputSymbol("kicklogger.ball-outside-area",this,
00036     (bool (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::ballOutsideArea);
00037   engine.registerDecimalInputSymbol("kicklogger.walktoTargetX",this,
00038     (double (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::getWalktoTargetX);
00039   engine.registerDecimalInputSymbol("kicklogger.walktoTargetY",this,
00040     (double (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::getWalktoTargetY);
00041   engine.registerDecimalInputSymbol("kicklogger.walktoTargetR",this,
00042     (double (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::getWalktoTargetR);
00043   engine.registerDecimalInputSymbol("kicklogger.kick",this,
00044     (double (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::getKick);
00045   engine.registerDecimalInputSymbol("kicklogger.ball-relative.x",this,
00046     (double (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::getPropagatedRelativeX);
00047   engine.registerDecimalInputSymbol("kicklogger.trigger-relative.x",this,
00048     (double (Xabsl2FunctionProvider::*)())&KickLoggerSymbols::getTriggerX);
00049 
00050 
00051 
00052 }
00053 
00054 void KickLoggerSymbols::update()
00055 {
00056   /** USE CEILING CAM
00057     if ( (this->gtCamWorldState.getBallModel().seen.positionField.x != 0)
00058     || (this->gtCamWorldState.getBallModel().seen.positionField.y != 0)){
00059     this->lastCeilCamBallPosition.x=this->gtCamWorldState.getBallModel().seen.positionField.x;
00060     this->lastCeilCamBallPosition.y=this->gtCamWorldState.getBallModel().seen.positionField.y;
00061     this->lastCeilCamBallPositionTime=SystemCall::getCurrentSystemTime();
00062 
00063     if ((this->gtCamWorldState.getBallModel().seen.positionField.x == this->lastCeilCamData.x) &&
00064         (this->gtCamWorldState.getBallModel().seen.positionField.y == this->lastCeilCamData.y))
00065     {
00066       this->lastCeilCamDataCount++;
00067     } else {
00068       this->lastCeilCamDataCount=0;
00069       this->lastCeilCamData.x=this->gtCamWorldState.getBallModel().seen.positionField.x;
00070       this->lastCeilCamData.y=this->gtCamWorldState.getBallModel().seen.positionField.y;
00071     }
00072   }*/
00073 
00074   if (ballModel.ballWasSeen){
00075     this->lastCeilCamBallPosition.x=this->ballModel.seen.positionField.x;
00076     this->lastCeilCamBallPosition.y=this->ballModel.seen.positionField.y;
00077     this->lastCeilCamBallPositionTime=SystemCall::getCurrentSystemTime();
00078 
00079     if ((this->ballModel.seen.positionField.x == this->lastCeilCamData.x) &&
00080         (this->ballModel.seen.positionField.y == this->lastCeilCamData.y))
00081     {
00082       this->lastCeilCamDataCount++;
00083     } else {
00084       this->lastCeilCamDataCount=0;
00085       this->lastCeilCamData.x=this->ballModel.seen.positionField.x;
00086       this->lastCeilCamData.y=this->ballModel.seen.positionField.y;
00087     }
00088   }
00089  
00090 
00091 //  int randx=((rand() % 20)+2)*10; // ==> 20...210 20-69, 70-119, 120-169, 170..210
00092 //  int randy=((rand() % 33) - 16)*10; // ==> -160 .. 160 -32..32, 32..96, 96..
00093 /*  if (this->executeTarget.x < 70){
00094   } else if (this->executeTarget < 120){
00095     if (this->executeTarget.y < -96){
00096     } else if (this->executeTarget.y < -32){
00097     } else if (this->executeTarget.y < 32){
00098     } else if (this->executeTarget.y < 96){
00099     } else 
00100   } else if (this->executeTarget < 170){
00101   } else {
00102   }
00103 */
00104 /*
00105  
00106 estimateTimeToReachBall();
00107   computeRole();
00108   
00109   outgoingBehaviorTeamMessage.estimatedTimeToReachBall = estimatedTimeToReachBall;
00110   outgoingBehaviorTeamMessage.dynamicRole = role;
00111   */
00112 }
00113 
00114 
00115 bool KickLoggerSymbols::kickLoggerSave(){
00116   OUTPUT(idText,text,"MSH2004StrategySymbols::kickEngineSave");
00117   this->kickLogger.save(); 
00118 //  (**kickEngine).save();
00119   return true;
00120 }
00121 
00122 double KickLoggerSymbols::getWalktoTargetX(){
00123   if (!this->walktoTargetGenerated){
00124     this->generateWalktoTarget();
00125   }
00126   return this->walktoTarget.x;
00127 }
00128 
00129 double KickLoggerSymbols::getWalktoTargetY(){
00130   if (!this->walktoTargetGenerated){
00131     this->generateWalktoTarget();
00132   }
00133   return this->walktoTarget.y;
00134 }
00135 
00136 double KickLoggerSymbols::getWalktoTargetR(){
00137   if (!this->walktoTargetGenerated){
00138     this->generateWalktoTarget();
00139   }
00140   return this->walktoRotation;
00141 }
00142 
00143 bool KickLoggerSymbols::doExecute(){
00144   Vector2<double> relative = Geometry::fieldCoord2Relative(this->robotPose,ballModel.propagated.positionField);
00145   OUTPUT(idText,text,"Execute?: Trigger x="<<executeTarget.x<<", y="<<executeTarget.y<<" | Current x="<<relative.x<<" y="<<relative.y);
00146   if (relative.x < executeTarget.x){
00147     if (executeCounter<2){
00148       executeCounter++;
00149       return false;
00150     } else {
00151       return true;
00152     }
00153   }
00154   executeCounter=0;
00155   return false;
00156 }
00157 
00158 bool KickLoggerSymbols::isNear(){
00159   Vector2<double> relative = Geometry::fieldCoord2Relative(this->robotPose,ballModel.propagated.positionField);
00160   if (relative.x < 250){
00161     return true;
00162   }
00163   return false;
00164 }
00165 
00166 void KickLoggerSymbols::generateWalktoTarget(){
00167   int randx=((rand() % 20)+2)*10; // ==> 20...210
00168   int randy=((rand() % 33) - 16)*10; // ==> -160 .. 160
00169 
00170   this->executeTarget=Vector2<double>(randx,randy); //Geometry::relative2FieldCoord(robotPose,ballModel.seen.positionRobot.x+randx,ballModel.seen.positionRobot.y+randy);
00171   this->walktoTarget=Geometry::relative2FieldCoord(robotPose,(ballModel.seen.positionRobot.x+randx)*2,(ballModel.seen.positionRobot.y+randy)*2);
00172 
00173   this->walktoRotation=(robotPose.translation - walktoTarget).angle();
00174 
00175   this->currentRecord.trigger_relative.x=(int) randx;
00176   this->currentRecord.trigger_relative.y=(int) randy;
00177 
00178   OUTPUT(idText,text,"Walkto-Target: Trigger x="<<randx<<", y="<<randy<<" | Walktarget x="<<walktoTarget.x<<" y="<<walktoTarget.x);
00179 
00180   this->walktoTargetGenerated=true;
00181 
00182 }
00183 
00184 bool KickLoggerSymbols::kickLoggerSetBeforeState(){
00185 //  this->ballBefore=this->getBallPosition();
00186   this->currentRecord.before_robot_translation.x=(int) this->robotPose.translation.x;
00187   this->currentRecord.before_robot_translation.y=(int) this->robotPose.translation.y;
00188   this->currentRecord.before_robot_rotation=(int) toDegrees(this->robotPose.rotation);
00189   this->currentRecord.before_ball_translation.x=(int) this->getBallPosition().x;
00190   this->currentRecord.before_ball_translation.y=(int) this->getBallPosition().y;
00191 
00192   OUTPUT(idText,text,"MSH2004StrategySymbols::kickLoggerSetBeforeState: "
00193     <<" Ballx="<<this->currentRecord.before_ball_translation.x
00194     <<" Bally="<<this->currentRecord.before_ball_translation.y
00195     <<" Robotx="<<this->currentRecord.before_robot_translation.x
00196     <<" Roboty="<<this->currentRecord.before_robot_translation.y
00197     <<" Robotr="<<this->currentRecord.before_robot_rotation
00198   );
00199 
00200 
00201 //  this->robotBefore=this->robotPose;
00202   this->walktoTargetGenerated=false;
00203   return true;
00204 }
00205  
00206 bool KickLoggerSymbols::kickLoggerSetExecuteState(){
00207   this->currentRecord.while_ball_relative_translation.x=(int) Geometry::fieldCoord2Relative(this->robotPose,ballModel.propagated.positionField).x;
00208   this->currentRecord.while_ball_relative_translation.y=(int) Geometry::fieldCoord2Relative(this->robotPose,ballModel.propagated.positionField).y;
00209   this->currentRecord.while_robot_translation.x=(int) this->robotPose.translation.x;
00210   this->currentRecord.while_robot_translation.y=(int) this->robotPose.translation.y;
00211   this->currentRecord.while_robot_rotation=(int) toDegrees(this->robotPose.rotation);
00212   this->currentRecord.while_ball_translation.x=(int) this->getBallPosition().x;
00213   this->currentRecord.while_ball_translation.y=(int) this->getBallPosition().y;
00214   this->currentRecord.while_robot_speed.x=(int) motionInfo.executedMotionRequest.walkRequest.walkParams.translation.x;
00215   this->currentRecord.while_robot_speed.y=(int) motionInfo.executedMotionRequest.walkRequest.walkParams.translation.x;
00216   this->currentRecord.while_robot_speed.z=(int) toDegrees(motionInfo.executedMotionRequest.walkRequest.walkParams.rotation);
00217 
00218   strcpy(this->currentRecord.kick,SpecialActionRequest::getSpecialActionIDName((SpecialActionRequest::SpecialActionID) (int)this->getKick()));
00219 //  this->relativePositionWhenExecuted.x=(int) ballModel.seen.positionRobot.x;
00220 //  this->relativePositionWhenExecuted.y=(int) ballModel.seen.positionRobot.y;
00221 //  this->ballBefore=this->getBallPosition();
00222 
00223 //  this->rotationWhenExceuted=toDegrees(this->robotPose.rotation);
00224 
00225 //  this->speedWhenExecuted.x=(int) motionInfo.executedMotionRequest.walkRequest.walkParams.translation.x;
00226 //  this->speedWhenExecuted.y=(int) motionInfo.executedMotionRequest.walkRequest.walkParams.translation.y;
00227 //  this->speedWhenExecuted.z=(int) toDegrees(motionInfo.executedMotionRequest.walkRequest.walkParams.rotation);
00228 
00229   OUTPUT(idText,text,"MSH2004StrategySymbols::kickLoggerSetExecuteState "
00230     <<" rpx="<<this->currentRecord.while_ball_relative_translation.x
00231     <<" rpy="<<this->currentRecord.while_ball_relative_translation.y
00232     <<" rot="<<this->currentRecord.while_robot_rotation
00233     <<" sx="<<this->currentRecord.while_robot_speed.x
00234     <<" sy="<<this->currentRecord.while_robot_speed.y
00235     <<" sz="<<this->currentRecord.while_robot_speed.z);
00236 
00237   return true;
00238 }
00239 
00240 bool KickLoggerSymbols::kickLoggerSetAfterStateAndCommit(){
00241   this->currentRecord.after_ball_translation=this->getBallPosition();
00242   this->currentRecord.after_robot_translation.x=(int) this->robotPose.translation.x;
00243   this->currentRecord.after_robot_translation.y=(int) this->robotPose.translation.y;
00244   this->currentRecord.after_robot_rotation=(int) toDegrees(this->robotPose.rotation);
00245 
00246   Vector2<int> relativeBallMovement=this->currentRecord.after_ball_translation - this->currentRecord.before_ball_translation;
00247   this->currentRecord.direction=(int) toDegrees(relativeBallMovement.angle()) - (int) this->currentRecord.while_robot_rotation;
00248   this->currentRecord.distance=relativeBallMovement.abs();
00249 
00250   kickLogger.addResult(this->currentRecord);
00251   OUTPUT(idText,text,"MSH2004StrategySymbols::kickLoggerSetAfterState:"
00252     <<" Ballx="<<this->getBallPosition().x
00253     <<" Bally="<<this->getBallPosition().y
00254     <<" Dir="<<this->currentRecord.direction);
00255   return true;
00256 }
00257 
00258 bool KickLoggerSymbols::ballHasStopped(){
00259   /** USE CEILING CAM
00260     OUTPUT(idText,text,"MSH2004StrategySymbols::ballHasStopped: "
00261     <<" Speed="<<this->gtCamWorldState.getBallModel().seen.speedField.abs());
00262     return this->gtCamWorldState.getBallModel().seen.speedField.abs() < 50; //5cm/s
00263    */
00264   return this->ballModel.seen.speedField.abs() < 75; //5cm/s
00265 }
00266 
00267 double KickLoggerSymbols::getKick(){
00268   return (double) SpecialActionRequest::armLeft;
00269 }
00270 
00271 double KickLoggerSymbols::getPropagatedRelativeX(){
00272   Vector2<double> relative = Geometry::fieldCoord2Relative(this->robotPose,ballModel.getKnownPosition(BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted));
00273   return relative.x;
00274 }
00275 
00276 double KickLoggerSymbols::getTriggerX(){
00277   return executeTarget.x;
00278 }
00279 
00280 Vector2<int> KickLoggerSymbols::getBallPosition(){
00281   if (SystemCall::getCurrentSystemTime() - this->lastCeilCamBallPositionTime < 3000){
00282      return Vector2<int>((int)lastCeilCamBallPosition.x,(int)lastCeilCamBallPosition.y);
00283   } else {
00284      return Vector2<int>(-100000,-100000);
00285   }
00286 }
00287 
00288 bool KickLoggerSymbols::ceilCamActive(){
00289   return this->lastCeilCamDataCount<100;
00290 }
00291 
00292 bool KickLoggerSymbols::ballOutsideArea(){
00293   /** USE CEILING CAM
00294    return (this->getBallPosition().x > -700) || (this->getBallPosition().x < - 2300) || (this->getBallPosition().y < -1300) || (this->getBallPosition().y > 1300);
00295    */
00296   return false;
00297 }