void MapLoader::load_level() {
Logger::info("Cargando nivel");
xmlpp::Node *visible_layer = (*level_)->get_first_child("visible-object-layer");
const xmlpp::Node::NodeList &rectangles = visible_layer->get_children("rectangle");
for (std::list<xmlpp::Node *>::const_iterator it = rectangles.begin();
it != rectangles.end(); ++it) {
// Lo agrega al engine
add_floor(*it);
}
xmlpp::Node *control_layer = (*level_)->get_first_child("control-object-layer");
const xmlpp::Node::NodeList &startpoints = control_layer->get_children("startpoint");
clean_start_points();
for (std::list<xmlpp::Node *>::const_iterator it = startpoints.begin();
it != startpoints.end(); ++it) {
// LO AGREGA EN EL MAP LOADER!!! SE NECESITA LIMPIAR ESTO!!!
add_startpoint(*it);
}
const xmlpp::Node::NodeList &spawnpoints = control_layer->get_children("spawnpoint");
for (std::list<xmlpp::Node *>::const_iterator it = spawnpoints.begin();
it != spawnpoints.end(); ++it) {
// Lo agrega al engine
add_spawnpoint(*it);
}
const xmlpp::Node::NodeList &goals = control_layer->get_children("goal");
for (std::list<xmlpp::Node *>::const_iterator it = goals.begin();
it != goals.end(); ++it) {
// Lo agrega al engine
add_goal(*it);
}
}
void BD_Factory::add_gaol(double f_RES) {
Array<const ExprNode> new_args(sip.n_arg+sip.p_arg);
int I=0;
int J=0;
for (int K=0; K<sip.n_arg+sip.p_arg; K++) {
if (!sip.is_param[K]) new_args.set_ref(K,new_vars[I++]);
else
// necessary for applying goal function but ignored at the end
// (there is no parameter in the goal function):
new_args.set_ref(K,ExprConstant::new_(sip.p_domain[J++],true));
}
const ExprNode* goal_node=&((*sip.sys.goal)(new_args));
if (problem==ORA) {
const ExprConstant& f_RES_node=ExprConstant::new_scalar(f_RES);
goal_node = &((*goal_node) - f_RES);
add_ctr(ExprCtr(*goal_node,LEQ));
const ExprNode& minus_eta=-new_vars[sip.n_arg];
add_goal(minus_eta);
delete &minus_eta;
} else {
add_goal(*goal_node);
}
// cleanup
for (int K=0; K<sip.n_arg+sip.p_arg; K++) {
if (sip.is_param[K]) {
if (!new_args[K].fathers.is_empty())
ibex_error("parameters in the objective");
delete &new_args[K];
}
}
cleanup(*goal_node,false);
f_ctrs_copy.clone.clean();
}
void Experience_Map::goto_dock()
{
std::vector <int> docking_experiences;
if (goal_list.size() == 0 || experiences[goal_list[0]].dock_visible == false)
{
goal_timeout_s = 0;
waypoint_exp_id = -1;
goal_list.clear();
for (unsigned int id = 0; id < experiences.size(); id++)
{
if (experiences[id].dock_visible)
{
docking_experiences.push_back(id);
}
}
if (docking_experiences.size())
{
add_goal(docking_experiences[rand()%docking_experiences.size()]);
}
}
}
// add_goal(pose->pose.position.x, pose->pose.position.y);
void ExperienceMap::add_goal(double x_m, double y_m) //未发布目标点就不会进入,记录下目标点和经验地图的某个点距离小于0.1米的目标点
{
int min_id = -1;
double min_dist = DBL_MAX;
double dist;
//goal_list为一个整型的双端容器(deque),未指定过长度
if (MAX_GOALS != 0 && goal_list.size() >= MAX_GOALS) //MAX_GOALS等于10,goal_list.size()大于10就会return
return;
for (unsigned int i = 0; i < experiences.size(); i++) //找到经验地图中距离目标点最小的位移距离的点，记录该距离和该点id号
{
dist = sqrt((experiences[i].x_m - x_m) * (experiences[i].x_m - x_m) + (experiences[i].y_m - y_m) * (experiences[i].y_m - y_m));
if (dist < min_dist)
{
min_id = i;
min_dist = dist;
}
}
if (min_dist < 0.1)
add_goal(min_id); //另一个重载函数，等效于goal_list.push_back(id);若目标点和经验地图的某个点小于0.1米，就记录下这个地图id,最多记录10个
}
Level* LevelLoader::load_level(const std::string& level_file_name) {
parser_.parse_file(level_file_name);
xmlpp::Node* level_node = parser_.get_document()->get_root_node();
level_ = new Level(
level_node->eval_to_string("@title"),
static_cast<int>(level_node->eval_to_number("@width")),
static_cast<int>(level_node->eval_to_number("@height")),
static_cast<int>(level_node->eval_to_number("@players_size")));
xmlpp::Node *visible_layer = (level_node)->get_first_child("visible-object-layer");
const xmlpp::Node::NodeList &rectangles = visible_layer->get_children("rectangle");
for (std::list<xmlpp::Node *>::const_iterator it = rectangles.begin();
it != rectangles.end(); ++it) {
add_floor(*it);
}
xmlpp::Node *control_layer = (level_node)->get_first_child("control-object-layer");
const xmlpp::Node::NodeList &startpoints = control_layer->get_children("startpoint");
for (std::list<xmlpp::Node *>::const_iterator it = startpoints.begin();
it != startpoints.end(); ++it) {
add_startpoint(*it);
}
const xmlpp::Node::NodeList &spawnpoints = control_layer->get_children("spawnpoint");
for (std::list<xmlpp::Node *>::const_iterator it = spawnpoints.begin();
it != spawnpoints.end(); ++it) {
add_spawnpoint(*it);
}
const xmlpp::Node::NodeList &goals = control_layer->get_children("goal");
for (std::list<xmlpp::Node *>::const_iterator it = goals.begin();
it != goals.end(); ++it) {
add_goal(*it);
}
return level_;
}
void Experience_Map::add_goal(double x_m, double y_m)
{
int min_id = -1;
double min_dist = DBL_MAX;
double dist;
if (MAX_GOALS != 0 && goal_list.size() >= MAX_GOALS)
return;
for (unsigned int i=0; i < experiences.size(); i++)
{
dist = sqrt((experiences[i].x_m-x_m)*(experiences[i].x_m-x_m) + (experiences[i].y_m-y_m)*(experiences[i].y_m-y_m));
if (dist < min_dist)
{
min_id = i;
min_dist = dist;
}
}
if (min_dist < 0.1)
add_goal(min_id);
// cout << "add_goal " << x_m << " " << y_m << " experiences[min_id] " << experiences[min_id].x_m << " " << experiences[min_id].y_m << endl;
}
char *call_lp(int next_expr(int))
{int i,constraints; expr_type_t goal_type;
char *retval, *retval2;
/* initially the expression to be checked is in entropy_expr.
determine first the variables */
init_var_assignment(); /* start collecting variables */
add_expr_variables(); /* variables in the expression to be checked */
constraints=0;
for(i=0;next_expr(i)==0;i++){ /* go over all constraints */
constraints++;
// Markov constraints give several cols
if(entropy_expr.type==ent_Markov){
constraints+=entropy_expr.n-3;
}
add_expr_variables();
}
/* figure out final variables, rows, cols, number of Shannon */
if(do_variable_assignment()){ // number of variables is less than 2
return "number of final random variables is less than 2";
}
/* get memory for row and column permutation */
cols += constraints;
rowperm=malloc((rows+1)*sizeof(int));
colperm=malloc((cols+1)*sizeof(int));
if(!rowperm || !colperm){
if(rowperm){ free(rowperm); rowperm=NULL; }
if(colperm){ free(colperm); colperm=NULL; }
return "the problem is too large, not enough memory";
}
for(i=0;i<=rows;i++) rowperm[i]=i; perm_array(rows+1,rowperm);
for(i=0;i<=cols;i++) colperm[i]= i-1; perm_array(cols+1,colperm);
/* the expression to be checked, this will be the goal */
if(constraints) next_expr(-1);
goal_type=entropy_expr.type; // ent_eq, ent_ge
create_glp(); // create a new glp instance
for(i=0;i<entropy_expr.n;i++){
row_idx[i+1]=varidx(entropy_expr.item[i].var);
row_val[i+1]=entropy_expr.item[i].coeff;
}
add_goal(entropy_expr.n); // right hand side value
// go over the columns add them to the lp instance
for(i=1;i<=cols;i++){
int colct=colperm[i];
if(add_shannon(i,colct)){ // this is a constraint
add_constraint(i,colct-(shannon+var_no),next_expr);
}
}
/* call the lp */
init_glp_parameters();
if(parm.presolve!=GLP_ON) // generate the first basis
glp_adv_basis(P,0);
retval=invoke_lp();
// call again with -1.0 when checking for ent_eq
if(goal_type==ent_eq && (retval==EXPR_TRUE || retval==EXPR_FALSE)){
next_expr(-1); // reload the problem
for(i=0;i<entropy_expr.n;i++){
row_idx[i+1]=varidx(entropy_expr.item[i].var);
row_val[i+1]=-entropy_expr.item[i].coeff;
}
add_goal(entropy_expr.n); // right hand side value
retval2=invoke_lp();
if(retval2==EXPR_TRUE){
if(retval==EXPR_FALSE) retval=EQ_LE_ONLY;
} else if(retval2==EXPR_FALSE){
if(retval==EXPR_TRUE) retval=EQ_GE_ONLY;
} else {
retval=retval2;
}
}
/* release allocated memory */
release_glp();
if(rowperm){ free(rowperm); rowperm=NULL; }
if(colperm){ free(colperm); colperm=NULL; }
return retval;
}
