int dummyEffect(const ParameterList & parameterList,
predicateCallbackType predicateCallback,
numericalFluentCallbackType numericalFluentCallback,
int relaxed, vector<double>& writtenVars)
{
ROS_DEBUG("Calling %s module", __func__);
if(predicateCallback == NULL)
return false;
PredicateList pl;
ParameterList paramList;
paramList.push_back(Parameter("","","pos-1"));
pl.push_back(Predicate("free", paramList));
PredicateList* plp = &pl;
bool ret = predicateCallback(plp);
if(!ret)
return false;
if(pl.empty()) // callback removed our predicate!
return false;
// now we could / should react on the value of the predicate. skip
// this for the moment....
std::cout << "(free pos-1) is currently " << pl.front().value <<
std::endl;
// write arbitrary value back
writtenVars[0] = 17.0;
return 0;
}
void
SexpMonitor::AddPredicates(std::ostringstream& ss, const PredicateList& pList)
{
for (
PredicateList::TList::const_iterator iter = pList.begin();
iter != pList.end();
++iter
)
{
const Predicate& pred = (*iter);
ss << "(";
ss << pred.name;
const ParameterList& paramList = pred.parameter;
ParameterList::TVector::const_iterator pIter = paramList.begin();
std::string param;
while (
(pIter != paramList.end()) &&
(paramList.AdvanceValue(pIter, param))
)
{
ss << " ";
ss << param;
}
ss << ")";
}
}
void GameStateItem::PutFloatParam(const string& name, PredicateList& pList)
{
float value;
if (! SoccerBase::GetSoccerVar(*this,name,value))
{
return;
}
Predicate& pred = pList.AddPredicate();
pred.name = name;
pred.parameter.AddValue(value);
}
void GameStateItem::GetInitialPredicates(PredicateList& pList)
{
ResetSentFlags();
// field geometry parameter
PutFloatParam("FieldLength",pList);
PutFloatParam("FieldWidth",pList);
PutFloatParam("FieldHeight",pList);
PutFloatParam("GoalWidth",pList);
PutFloatParam("GoalDepth",pList);
PutFloatParam("GoalHeight",pList);
PutFloatParam("BorderSize",pList);
PutFloatParam("FreeKickDistance",pList);
PutFloatParam("WaitBeforeKickOff",pList);
// agent parameter
PutFloatParam("AgentMass",pList);
PutFloatParam("AgentRadius",pList);
PutFloatParam("AgentMaxSpeed",pList);
// ball parameter
PutFloatParam("BallRadius",pList);
PutFloatParam("BallMass",pList);
// soccer rule parameters
PutFloatParam("RuleGoalPauseTime",pList);
PutFloatParam("RuleKickInPauseTime",pList);
PutFloatParam("RuleHalfTime",pList);
// play modes
Predicate& pred = pList.AddPredicate();
pred.name = "play_modes";
for (int i=0; i<PM_NONE; ++i)
{
pred.parameter.AddValue
(SoccerBase::PlayMode2Str(static_cast<TPlayMode>(i)));
}
}
void SparkMonitorClient::ParseCustomPredicates(sexp_t* sexp, PredicateList& pList)
{
if (
(sexp == 0) ||
(sexp->ty != SEXP_VALUE)
)
{
return;
}
Predicate& pred = pList.AddPredicate();
pred.name = sexp->val;
sexp = sexp->next;
while (sexp != 0)
{
if (sexp->ty == SEXP_VALUE)
{
pred.parameter.AddValue(sexp->val);
}
sexp = sexp->next;
}
}
void GameStateItem::GetPredicates(PredicateList& pList)
{
if (mGameState.get() == 0)
{
return;
}
Predicate& timePred = pList.AddPredicate();
timePred.name = "time";
timePred.parameter.AddValue(mGameState->GetTime());
if (! mSentLeftTeamname)
{
// team names
string name = mGameState->GetTeamName(TI_LEFT);
if (! name.empty())
{
Predicate& teamPredLeft = pList.AddPredicate();
teamPredLeft.name = "team_left";
teamPredLeft.parameter.AddValue(name);
mSentLeftTeamname = true;
}
}
if (! mSentRightTeamname)
{
// team names
string name = mGameState->GetTeamName(TI_RIGHT);
if (! name.empty())
{
Predicate& teamPredRight = pList.AddPredicate();
teamPredRight.name = "team_right";
teamPredRight.parameter.AddValue(name);
mSentRightTeamname = true;
}
}
// game half
TGameHalf half = mGameState->GetGameHalf();
if (half != mLastHalf)
{
mLastHalf = half;
Predicate& halfPred = pList.AddPredicate();
halfPred.name = "half";
halfPred.parameter.AddValue(static_cast<int>(half));
}
// scores
int left_score = mGameState->GetScore(TI_LEFT);
if (left_score != mLastLeftScore)
{
mLastLeftScore = left_score;
Predicate& scoreLeftPred = pList.AddPredicate();
scoreLeftPred.name = "score_left";
scoreLeftPred.parameter.AddValue(left_score);
}
int right_score = mGameState->GetScore(TI_RIGHT);
if (right_score != mLastRightScore)
{
mLastRightScore = right_score;
Predicate& scoreRightPred = pList.AddPredicate();
scoreRightPred.name = "score_right";
scoreRightPred.parameter.AddValue(right_score);
}
// gamestate
TPlayMode play_mode = mGameState->GetPlayMode();
if (play_mode != mLastPlayMode)
{
mLastPlayMode = play_mode;
Predicate& modePred = pList.AddPredicate();
modePred.name = "play_mode";
modePred.parameter.AddValue(static_cast<int>(play_mode));
}
}
double can_load(const ParameterList & parameterList,
predicateCallbackType predicateCallback, numericalFluentCallbackType numericalFluentCallback, int )
{
ROS_DEBUG("Calling module %s", __func__);
static int count = 0;
count++;
//cout << "Count: " << count << endl;
if(DEBUG_MOD)
cout << parameterList << endl;
PredicateList* list = NULL;
predicateCallback(list);
if(list == NULL) {
exit(1);
return false;
}
if(DEBUG_MOD) {
cout << "Predicates are: " << endl;
cout << *list << endl;
}
string me = parameterList.front().value;
string pack = parameterList.back().value;
deque<string> packs_in_truck;
for(PredicateList::iterator it = list->begin(); it != list->end(); it++) {
Predicate p = *it;
if(!p.value)
continue;
if(p.name != "in")
continue;
if(p.parameters.back().value == me)
packs_in_truck.push_back(p.parameters.front().value);
}
if(DEBUG_MOD) {
for(deque<string>::iterator it = packs_in_truck.begin(); it != packs_in_truck.end(); it++) {
cout << *it << endl;
}
if(packs_in_truck.empty()) {
cout << "No packs in " << me << endl;
} else {
cout << packs_in_truck.size() << " packs i truck " << me << endl;
}
}
packs_in_truck.push_back(pack);
NumericalFluentList* nfl = new NumericalFluentList();
for(deque<string>::iterator it = packs_in_truck.begin(); it != packs_in_truck.end(); it++) {
ParameterList pl;
pl.push_back(Parameter("p", "package", *it));
nfl->push_back(NumericalFluent("package-size", pl));
}
ParameterList plt;
plt.push_back(Parameter("v", "vehicle", me));
nfl->push_back(NumericalFluent("capacity", plt));
if(!numericalFluentCallback(nfl)) {
exit(1);
return false;
}
if(DEBUG_MOD)
cout << *nfl << endl;
// ideally: get sizes 1 - n-2, size n-1 == package, size n == capcity
// 1..n-2 + n == full_load
deque<double> packs;
for(NumericalFluentList::iterator it = nfl->begin(); it != nfl->end(); it++) {
packs.push_back((*it).value);
}
double cap = packs.back();
packs.erase(packs.end() - 1);
double pSize = packs.back();
packs.erase(packs.end() - 1);
if(DEBUG_MOD)
cout << "Packs" << packs.size() <<endl;
double sumPacks = 0;
for(deque<double>::iterator it = packs.begin(); it != packs.end(); it++) {
if(DEBUG_MOD)
cout << *it << endl;
sumPacks += *it;
}
if(DEBUG_MOD) {
cout << "newpack " << pSize << endl;
cout << "cap " << cap << endl;
}
double loadCap = sumPacks + cap;
if(DEBUG_MOD) {
cout << "loadCap " << loadCap;
cout << endl;
}
packs.push_back(pSize);
bool loadPoss = checkLoading(packs, loadCap);
return loadPoss;
}
Conjunct& operator|| (const Conjunct& c) {
for (auto& p : c.mPredicates) {
mPredicates.emplace_back(p);
}
return *this;
}
Conjunct& operator|| (const Predicate& predicate) {
mPredicates.push_back(predicate);
return *this;
}
Conjunct(const Predicate& predicate) {
mPredicates.push_back(predicate);
}
Conjunct(Predicate&& predicate) {
mPredicates.emplace_back(std::move(predicate));
}