00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "sexpmonitor.h"
00023 #include <sstream>
00024 #include <zeitgeist/logserver/logserver.h>
00025 #include <zeitgeist/scriptserver/scriptserver.h>
00026 #include <oxygen/sceneserver/sceneserver.h>
00027 #include <oxygen/sceneserver/scene.h>
00028 #include <oxygen/sceneserver/transform.h>
00029 #include <oxygen/agentaspect/agentaspect.h>
00030 #include <soccer/soccertypes.h>
00031 #include <soccer/soccerbase/soccerbase.h>
00032 #include <soccer/soccerbase/soccerbase.h>
00033 #include <soccer/ball/ball.h>
00034 #include <soccer/gamestateaspect/gamestateaspect.h>
00035 #include <soccer/ballstateaspect/ballstateaspect.h>
00036 #include <soccer/agentstate/agentstate.h>
00037 #include <soccer/fieldflag/fieldflag.h>
00038 #include <netinet/in.h>
00039 #include <soccer/sayeffector/sayeffector.h>
00040
00041 using namespace oxygen;
00042 using namespace zeitgeist;
00043 using namespace std;
00044 using namespace boost;
00045
00046 SexpMonitor::SexpMonitor() : MonitorSystem(), mSendRotMatrix(false)
00047 {
00048 }
00049
00050 SexpMonitor::~SexpMonitor()
00051 {
00052 }
00053
00054 bool
00055 SexpMonitor::ConstructInternal()
00056 {
00057 return true;
00058 }
00059
00060 void
00061 SexpMonitor::ParseMonitorMessage(const string& data)
00062 {
00063 GetLog()->Debug() << "SexpMonitor received " << data << " from monitor\n";
00064
00065 if (mGameState.get() == 0)
00066 {
00067 return;
00068 }
00069
00070 if (mCommandParser.get() == 0)
00071 {
00072 GetLog()->Error() << "(SexpMonitor) ERROR: can't get TrainerCommandParser\n";
00073 return;
00074 }
00075
00076
00077 mCommandParser->ParseMonitorMessage(data);
00078 }
00079
00080 void
00081 SexpMonitor::AddPredicates(std::ostringstream& ss, const PredicateList& pList)
00082 {
00083 for (
00084 PredicateList::TList::const_iterator iter = pList.begin();
00085 iter != pList.end();
00086 ++iter
00087 )
00088 {
00089 const Predicate& pred = (*iter);
00090
00091 ss << "(";
00092 ss << pred.name;
00093
00094 const ParameterList& paramList = pred.parameter;
00095 ParameterList::TVector::const_iterator pIter = paramList.begin();
00096
00097 std::string param;
00098 while (
00099 (pIter != paramList.end()) &&
00100 (paramList.AdvanceValue(pIter, param))
00101 )
00102 {
00103 ss << " ";
00104 ss << param;
00105 }
00106
00107 ss << ")";
00108 }
00109 }
00110
00111 void
00112 SexpMonitor::AddAgents(shared_ptr<Scene> activeScene, std::ostringstream& ss) const
00113 {
00114 TLeafList nodes;
00115 activeScene->ListChildrenSupportingClass<AgentAspect>(nodes, true);
00116
00117 for (TLeafList::iterator i = nodes.begin(); i != nodes.end(); ++i)
00118 {
00119 shared_ptr<AgentAspect> aspect = shared_static_cast<AgentAspect>(*i);
00120 const salt::Vector3f& pos = aspect->GetWorldTransform().Pos();
00121
00122 ss << "(P ";
00123
00124 shared_ptr<AgentState> state = shared_static_cast<AgentState>
00125 (aspect->GetChildOfClass("AgentState"));
00126
00127 shared_ptr<SayEffector> sayEff = shared_static_cast<SayEffector>
00128 (aspect->GetChildOfClass("SayEffector"));
00129
00130 if (state.get() != 0)
00131 {
00132 ss << "(s " << state->GetTeamIndex() << ")";
00133 ss << "(id " << state->GetUniformNumber() << ")";
00134 }
00135
00136
00137 ss << "(pos " << pos << ")";
00138
00139 if (mSendRotMatrix)
00140 {
00141
00142 ss << "(rot ";
00143
00144 const salt::Matrix& trans = aspect->GetWorldTransform();
00145 for (int i=0;i<16;++i)
00146 {
00147 ss << trans.m[i] << " ";
00148 }
00149
00150 ss << ")";
00151 }
00152
00153
00154 shared_ptr<AgentAspect> agent;
00155 TTime time;
00156 mBallState->GetLastCollidingAgent(agent,time);
00157 if (agent == aspect)
00158 {
00159 ss << "(last)";
00160 }
00161 if (sayEff !=0 && sayEff->IfText())
00162 {
00163 ss <<"(say " <<sayEff->GetText()<<")";
00164 }
00165 ss << ")";
00166 }
00167 }
00168
00169 void
00170 SexpMonitor::AddFlags(shared_ptr<Scene> activeScene, std::ostringstream& ss)
00171 {
00172
00173 if (mSentFlags)
00174 {
00175 return;
00176 }
00177 mSentFlags = true;
00178
00179 TLeafList nodes;
00180 activeScene->ListChildrenSupportingClass<FieldFlag>(nodes, true);
00181
00182 for (TLeafList::iterator i = nodes.begin(); i != nodes.end(); ++i)
00183 {
00184 shared_ptr<FieldFlag> flag = shared_static_cast<FieldFlag>(*i);
00185 const salt::Vector3f& pos = flag->GetWorldTransform().Pos();
00186 shared_ptr<ObjectState> state = shared_dynamic_cast<ObjectState>
00187 (flag->GetChildOfClass("ObjectState"));
00188
00189 if (state.get() == 0) continue;
00190
00191 ss << "(" << state->GetPerceptName()[0] << " ";
00192
00193 ss << "(id " << state->GetID() << ")";
00194
00195 ss << "(pos " << pos[0] << " " << pos[1] << " " << pos[2] << ")";
00196
00197 ss << ")";
00198 }
00199 }
00200
00201 void
00202 SexpMonitor::AddBall(shared_ptr<Scene> activeScene, std::ostringstream& ss) const
00203 {
00204 shared_ptr<Ball> ball = shared_static_cast<Ball>(activeScene->GetChild("Ball"));
00205 const salt::Vector3f& pos = ball->GetWorldTransform().Pos();
00206 ss << "(B ";
00207
00208 ss << "(pos " << pos[0] << " " << pos[1] << " " << pos[2] << ")";
00209 ss << ")";
00210 }
00211
00212 string
00213 SexpMonitor::GetMonitorInfo(const oxygen::PredicateList& pList)
00214 {
00215 if (mGameState->IsFinished())
00216 {
00217 return "(Die)\n";
00218 }
00219
00220 shared_ptr<SceneServer> sceneServer =
00221 shared_dynamic_cast<SceneServer>(GetCore()->Get("/sys/server/scene"));
00222
00223 if (sceneServer.get() == 0)
00224 {
00225 GetLog()->Error() << "(SexpMonitor) cannot get SceneServer\n";
00226 return "";
00227 }
00228 shared_ptr<Scene> activeScene = sceneServer->GetActiveScene();
00229 ostringstream expression;
00230
00231 expression << "(Info ";
00232
00233 string reply;
00234 if (mCommandParser->SendAck(reply))
00235 {
00236 expression << "(ack " + reply + ")";
00237 }
00238
00239 AddPredicates(expression, pList);
00240 AddAgents(activeScene, expression);
00241 AddFlags(activeScene, expression);
00242 AddBall(activeScene, expression);
00243
00244 expression << ")\n";
00245
00246 return expression.str();
00247 }
00248
00249 string
00250 SexpMonitor::GetMonitorHeaderInfo(const oxygen::PredicateList& pList)
00251 {
00252
00253 ResetSentFlags();
00254
00255 ostringstream ss;
00256 ss << "(Init ";
00257 AddPredicates(ss, pList);
00258 ss << ")\n";
00259
00260 return ss.str();
00261 }
00262
00263 void
00264 SexpMonitor::OnLink()
00265 {
00266 SoccerBase::GetGameState(*this,mGameState);
00267
00268
00269
00270 mCommandParser = shared_dynamic_cast<TrainerCommandParser>
00271 (GetCore()->New("TrainerCommandParser"));
00272
00273 if (mCommandParser.get() == 0)
00274 {
00275 GetLog()->Error() << "ERROR: (SexpMonitor) failed to create parser TrainerCommandParser\n";
00276 return;
00277 }
00278
00279 mCommandParser->SetName("TrainerCommandParser");
00280 AddChildReference(mCommandParser);
00281
00282 UpdateCached();
00283 }
00284
00285 void
00286 SexpMonitor::UpdateCached()
00287 {
00288 mBallState = shared_dynamic_cast<BallStateAspect>
00289 (GetCore()->Get("/sys/server/gamecontrol/BallStateAspect"));
00290
00291 if (mBallState.get() == 0)
00292 {
00293 GetLog()->Error() << "(SexpMonitor) found no BallStateAspect\n";
00294 }
00295 }
00296
00297
00298 void
00299 SexpMonitor::OnUnlink()
00300 {
00301 mGameState.reset();
00302 mCommandParser.reset();
00303 mBallState.reset();
00304 }
00305
00306 void
00307 SexpMonitor::ResetSentFlags()
00308 {
00309 mSentFlags = false;
00310 }
00311
00312 void
00313 SexpMonitor::SendRotationMatrix(bool send)
00314 {
00315 mSendRotMatrix = send;
00316 }