// create a new experience for a given position
int Experience_Map::create_experience(double x, double y, double th, double delta_time_s)
{
experiences.resize(experiences.size()+1);
Experience * new_exp = &(*(experiences.end()-1));
new_exp->id = experiences.size()-1;
if (new_exp->id == 0)
{
new_exp->x_m = 0;
new_exp->y_m = 0;
new_exp->th_rad = 0;
} else {
new_exp->x_m = experiences[current_exp_id].x_m + accum_delta_x;
new_exp->y_m = experiences[current_exp_id].y_m + accum_delta_y;
new_exp->th_rad = gri::clip_rad_180(accum_delta_facing);
}
new_exp->x_pc = x;
new_exp->y_pc = y;
new_exp->th_pc = th;
new_exp->dock_visible = false;
new_exp->goal_to_current = -1;
new_exp->current_to_goal = -1;
if (experiences.size() != 1)
create_link(get_current_id(), experiences.size()-1, delta_time_s);
return experiences.size() - 1;
}
// create a new experience for a given position
// exp_id为action->dest_id为int32型的
int ExperienceMap::on_create_experience(unsigned int exp_id) //为每张经验地图给定坐标值角度参数
{
experiences.resize(experiences.size() + 1); //experiences为vector类型，长度变为了1,之前的experiences.reserve(10000);并没有给它分配10000的空间
Experience * new_exp = &(*(experiences.end() - 1)); //Experience为一个结构体类型，是经验地图的一个节点，new_exp指针为experiences容器的最后一个成员的地址
if (experiences.size() == 0) //struct Experience
{ //{
new_exp->x_m = 0; // int id; //它自己的标识
new_exp->y_m = 0; // double x_m, y_m, th_rad;
new_exp->th_rad = 0; // int vt_id;
} //
else // std::vector<unsigned int> links_from; //从这个经验地图链接
{ // std::vector<unsigned int> links_to; //链接到本次经验地图
new_exp->x_m = experiences[current_exp_id].x_m + accum_delta_x; //experiences[id].x_m+x //
new_exp->y_m = experiences[current_exp_id].y_m + accum_delta_y; //
new_exp->th_rad = clip_rad_180(accum_delta_facing); //第一次为π/2+里程计角度 // //目标导航
}// 之后和里程计角度、相对弧度累加共同构成经验地图th_rad // double time_from_current_s; //当前时间秒
new_exp->id = experiences.size() - 1; //id是从0开始计数 // unsigned int goal_to_current, current_to_goal; //目标到当前,当前到目标
// template<typename Archive>
new_exp->goal_to_current = -1; //目标到当前为-1 // void serialize(Archive& ar, const unsigned int version)
new_exp->current_to_goal = -1; //当前到目标为-1 // {
// ar & id;
// Link the current experience to the last. // ar & x_m & y_m & th_rad;
// FIXME: jumps back to last set pose with wheel odom? // ar & vt_id;
if (experiences.size() != 1) //每个经验地图对应一个链接,当action为CREATE_NODE,CREATE_EDGE进入 // ar & links_from & links_to;
on_create_link(get_current_id(), experiences.size() - 1, 0); // ar & time_from_current_s;
// ar & goal_to_current & current_to_goal;
return experiences.size() - 1; //可以认为返回值是当前经验地图的id号 // }
} //};