Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/BehaviorControl/GT2005BehaviorControl/GT2005Symbols/KickLoggerSymbols.cpp

Go to the documentation of this 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 }