Far planne初始化中的Callback函数（1）
reset_graph_sub_ = nh.subscribe("/reset_visibility_graph", 5, &FARMaster::ResetGraphCallBack, this);ResetGraphCallBack
inline void ResetGraphCallBack(const std_msgs::EmptyConstPtr& msg) {
is_reset_env_ = true;
}很简单，就把msg消息接收然后设置is_reset_env_为true
odom_sub_ = nh.subscribe("/odom_world", 5, &FARMaster::OdomCallBack, this);OdomCallBack
void FARMaster::OdomCallBack(const nav_msgs::OdometryConstPtr& msg) {
// transform from odom frame to mapping frame
std::string odom_frame = msg->header.frame_id;
tf::Pose tf_odom_pose;
tf::poseMsgToTF(msg->pose.pose, tf_odom_pose);// 把里程计信息转化成tf_odom里程计信息
// odom frame 要和 world frame一致
if (!FARUtil::IsSameFrameID(odom_frame, master_params_.world_frame)) {
if (FARUtil::IsDebug) ROS_WARN_ONCE("FARMaster: odom frame does NOT match with world frame!");
tf::StampedTransform odom_to_world_tf_stamp;
try
{
tf_listener_->waitForTransform(master_params_.world_frame, odom_frame, ros::Time(0), ros::Duration(1.0));
tf_listener_->lookupTransform(master_params_.world_frame, odom_frame, ros::Time(0), odom_to_world_tf_stamp);
tf_odom_pose = odom_to_world_tf_stamp * tf_odom_pose;
}
catch (tf::TransformException ex){
ROS_ERROR("Tracking odom TF lookup: %s",ex.what());
return;
}
}
//从tf中更新机器人坐标
robot_pos_.x = tf_odom_pose.getOrigin().getX();
robot_pos_.y = tf_odom_pose.getOrigin().getY();
robot_pos_.z = tf_odom_pose.getOrigin().getZ();
//初始化robot_pos是（0，0，0.75）
// extract robot heading
FARUtil::robot_pos = robot_pos_; //把far_master里的 robot_pos_ 更新到FARUtil里的robot_pos下
//std::cout<<"robot_pos_:"<<robot_pos_<<std::endl;
double roll, pitch, yaw;
tf_odom_pose.getBasis().getRPY(roll, pitch, yaw); //解算roll pitch yaw
robot_heading_ = Point3D(cos(yaw), sin(yaw), 0);
// 第一次is_odom_init肯定是false ，就要执行下面的程序
if (!is_odom_init_) {
// system start time
FARUtil::systemStartTime = ros::Time::now().toSec();
FARUtil::map_origin = robot_pos_;
map_handler_.UpdateRobotPosition(robot_pos_); //这里面就获得了nav_graph_的globalGraphNodes
}
is_odom_init_ = true;
}这部分代码就是获取小车位姿信息，然后初始化map_handler里的三个grid的。
第一步，获取odom frame下的位置信息，然后转为tf的pose信息。
第二步，对比odom_frame的帧id是否和world_frame一致。
第三步，用FARMASTER下的robot_pos_去接收机器人坐标
第四步，把机器人坐标从FARMASTER下的robot_pos_传递到FARUtil下的robot_pos
FATRUtil库是作者自己编写的一些工具，基本上每个cpp都包含了，所以迁移到这里面也方便别的cpp调用机器人位置信息
第五步，判断是否是第一次odom初始化，默认为false 所以执行下面的设置。
void MapHandler::UpdateRobotPosition(const Point3D& odom_pos) {
// is_init_在maphandlelr初始化的时候默认是false
if (!is_init_) this->SetMapOrigin(odom_pos); // world_obs_cloud_grid_ 和 world_free_cloud_grid_ 都->SetOrigin 这样里面的origin都更新 而且这个只更新一次
// world_obs_cloud_grid_ 的resolution是（5，5，0.8）
//虽然world_obs_cloud_grid_和world_free_cloud_grid_的origin坐标不变，但是robot_cell_sub_是根据odom和origin算出来的，这个是会变的
robot_cell_sub_ = world_obs_cloud_grid_->Pos2Sub(Eigen::Vector3d(odom_pos.x, odom_pos.y, odom_pos.z));
//初始odom_pos(0,0,0.75) origin(-500,-500,-50) robot_cell_sub_(500/5,500/5,(0.75+50)/0.8)->因为是vector3i的，所以是(100,100,63)
// Get neighbor indices
// indices是和neighbor_sub有关的换算，所以indices通过换算可以还原得到sub点
//! 每次执行UpdateRobotPosition的时候都会更新obs_indices和free_indices
neighbor_free_indices_.clear(), neighbor_obs_indices_.clear();
const int N = neighbor_Lnum_ / 2; //N=6 L*L*H
const int H = neighbor_Hnum_ / 2; //H=2
Eigen::Vector3i neighbor_sub;
for (int i = -N; i <= N; i++) {
neighbor_sub.x() = robot_cell_sub_.x() + i;
for (int j = -N; j <= N; j++) {
neighbor_sub.y() = robot_cell_sub_.y() + j;
// additional terrain points -1
neighbor_sub.z() = robot_cell_sub_.z() - H - 1; //这个是固定的z 63-2-1=60
//判断这个neighbor_sub点有没有超出范围
if (world_obs_cloud_grid_->InRange(neighbor_sub)) {
int ind = world_obs_cloud_grid_->Sub2Ind(neighbor_sub); //获得一个范围的粒子的ind序列
neighbor_free_indices_.insert(ind); //为了防止重复扫描包含，所以用了无序集合来保存
}
//这个存进的是neighbor_obs_indices_的集合 因为z的范围更大 所以neighbor_obs_indices比neighbor_free_indices要大
for (int k =-H; k <= H; k++) {
// z从61到65
neighbor_sub.z() = robot_cell_sub_.z() + k;
if (world_obs_cloud_grid_->InRange(neighbor_sub)) {
int ind = world_obs_cloud_grid_->Sub2Ind(neighbor_sub);
neighbor_obs_indices_.insert(ind), neighbor_free_indices_.insert(ind);
}
}
}
}
this->SetTerrainHeightGridOrigin(odom_pos);
}这个UpdateRobotPosition主要就是更新world_obs_cloud_grid_& world_free_cloud_grid_ & terrain_height_grid_这三个grid 。次要的就是更新了neighbor_free_indices_ 和neighbor_obs_indices_
terrain_sub_ = nh.subscribe("/terrain_cloud", 1, &FARMaster::TerrainCallBack, this);TerrainCallBack
void FARMaster::TerrainCallBack(const sensor_msgs::PointCloud2ConstPtr& pc) {
if (!is_odom_init_) return;
// update map grid robot center
map_handler_.UpdateRobotPosition(FARUtil::robot_pos); //! FARUtil::robot_pos是从OdomCallback里更新来的
//如果保持更新
if (!is_stop_update_) {
//处理点云，并进行下采样
this->PrcocessCloud(pc, temp_cloud_ptr_);
//原始点云pc通过滤波、删除无效点、对齐world_frame后，输出temp_cloud_ptr_
//! terrain_range 15
//保留以robot_pos为中心，边长为2倍terrain_range，高为2倍kTolerZ的立方体内的点云，过滤掉这个立方体以外的点云
FARUtil::CropBoxCloud(temp_cloud_ptr_, robot_pos_, Point3D(master_params_.terrain_range,
master_params_.terrain_range,
FARUtil::kTolerZ));
//! 读入temp_cloud_ptr_，循环每一个点，根据点的intensity来划分free和obs
FARUtil::ExtractFreeAndObsCloud(temp_cloud_ptr_, temp_free_ptr_, temp_obs_ptr_);//返回输出 temp_free_ptr_ 和 temp_obs_ptr_
//! 动态环境下执行去除overlapcloud
if (!master_params_.is_static_env) {
FARUtil::RemoveOverlapCloud(temp_obs_ptr_, FARUtil::stack_dyobs_cloud_, true);
}
//! 进行obscloud更新，主要就是填充global_visited_induces_(访问过的点)和util_obs_modified_list_索引，并下采样一波temp_obs_ptr_
map_handler_.UpdateObsCloudGrid(temp_obs_ptr_);
//! 进行freecloud更新，主要就是填充global_visited_induces_(访问过的点)和util_free_modified_list_索引，并下采样一波temp_free_ptr_
map_handler_.UpdateFreeCloudGrid(temp_free_ptr_);
// extract new points
// 把现有的temp_obs_ptr和surround_obs_cloud_拼接，然后下采样滤波，根据intensity判断一轮
// 最后得到cur_new_cloud_
FARUtil::ExtractNewObsPointCloud(temp_obs_ptr_,
FARUtil::surround_obs_cloud_,
FARUtil::cur_new_cloud_);
} else { // stop env update
temp_cloud_ptr_->clear();
FARUtil::cur_new_cloud_->clear();
}
// extract surround free cloud & update terrain height
map_handler_.GetSurroundFreeCloud(FARUtil::surround_free_cloud_); //surround_free_cloud_最开始被clear掉了，进函数也要被clear掉一次
map_handler_.UpdateTerrainHeightGrid(FARUtil::surround_free_cloud_, terrain_height_ptr_); //通过surround_free_cloud_去更新terrain_height_ptr_
// update surround obs cloud
map_handler_.GetSurroundObsCloud(FARUtil::surround_obs_cloud_);
// extract dynamic obstacles
FARUtil::cur_dyobs_cloud_->clear();
if (!master_params_.is_static_env && !is_stop_update_) {
this->ExtractDynamicObsFromScan(FARUtil::cur_scan_cloud_, FARUtil::surround_obs_cloud_, FARUtil::cur_dyobs_cloud_);
if (FARUtil::cur_dyobs_cloud_->size() > FARUtil::kDyObsThred) {
if (FARUtil::IsDebug) ROS_WARN("FARMaster: dynamic obstacle detected, size: %ld", FARUtil::cur_dyobs_cloud_->size());
FARUtil::InflateCloud(FARUtil::cur_dyobs_cloud_, master_params_.voxel_dim, 1, true);
map_handler_.RemoveObsCloudFromGrid(FARUtil::cur_dyobs_cloud_);
FARUtil::RemoveOverlapCloud(FARUtil::surround_obs_cloud_, FARUtil::cur_dyobs_cloud_);
FARUtil::FilterCloud(FARUtil::cur_dyobs_cloud_, master_params_.voxel_dim);
// update new cloud
*FARUtil::cur_new_cloud_ += *FARUtil::cur_dyobs_cloud_;
FARUtil::FilterCloud(FARUtil::cur_new_cloud_, master_params_.voxel_dim);
}
// update world dynamic obstacles
FARUtil::StackCloudByTime(FARUtil::cur_dyobs_cloud_, FARUtil::stack_dyobs_cloud_, FARUtil::kObsDecayTime);
}
// create and update kdtrees
FARUtil::StackCloudByTime(FARUtil::cur_new_cloud_, FARUtil::stack_new_cloud_, FARUtil::kNewDecayTime); // kNewDecayTime 1.0
FARUtil::UpdateKdTrees(FARUtil::stack_new_cloud_);
if (!FARUtil::surround_obs_cloud_->empty()) is_cloud_init_ = true;
/* visualize clouds */
planner_viz_.VizPointCloud(new_PCL_pub_, FARUtil::stack_new_cloud_);
planner_viz_.VizPointCloud(dynamic_obs_pub_, FARUtil::cur_dyobs_cloud_);
planner_viz_.VizPointCloud(surround_free_debug_, FARUtil::surround_free_cloud_);
planner_viz_.VizPointCloud(surround_obs_debug_, FARUtil::surround_obs_cloud_);
planner_viz_.VizPointCloud(terrain_height_pub_, terrain_height_ptr_);
// visualize map grid
PointStack neighbor_centers, occupancy_centers;
map_handler_.GetNeighborCeilsCenters(neighbor_centers);
map_handler_.GetOccupancyCeilsCenters(occupancy_centers);
planner_viz_.VizMapGrids(neighbor_centers, occupancy_centers, map_params_.cell_length, map_params_.cell_height);
// DBBUG visual raycast grids
if (!master_params_.is_static_env) {
scan_handler_.GridVisualCloud(scan_grid_ptr_, GridStatus::RAY);
planner_viz_.VizPointCloud(scan_grid_debug_, scan_grid_ptr_);
}
}这个就复杂的多了首先更新robot_pos信息，因为odom已经初始化过了，所以不用再次SetMapOrigin。会更新neighbor_obs_indices和neighbor_free_indices
点云查找和改变主要的思想就是 原生point点 -> 跟grid有关的缩放的sub点 -> index序列，然后用序列来搜索，序列可以反变换成点云输出 部分函数参考 Pos2Sub、 Sub2Ind、 Ind2Pos、Ind2Sub、Sub2Pos
如果我们的环境是保持更新的，那么接下去：
第一步，处理点云信息 通过过滤、删除无效点、对齐world_frame 输出temp_cloud_ptr
第二步，对temp_cloud_ptr进行CropBox过滤，保存立方体内部的点云。（以robot_pos为中心，边长为2倍terrain_range，高为2倍kTolerZ的立方体内的点云，过滤掉这个立方体以外的点云）terrain_range为15
比如，上图的(15.0581,-1.49592,-0.0430446 - 0.00112512) 就被过滤掉了，因为超过了15，就被过滤掉了。
第三步，传入temp_cloud_ptr_，根据点云里每个点的intensity不同来划分free和obs。（intensity就是代表激光反射的强度，如果反射到物体上，intensity就大）
第四步，判断是不是动态环境，如果是动态环境就执行RemoveOverlapCloud
第五步，根据第三步的free和obs来更新对应的ObsCloudGrid和FreeCloudGrid
第六步，根据ObsCloudGrid更新完的temp_obs_ptr_和surround_obs_cloud_拼接，然后下采样滤波，根据intensity判断一轮，最后得到cur_new_cloud_ 。我理解为cur_new_cloud_就是新的obs点云
第七步，抽取surround free cloud （把world_free_cloud_grid_里面的加进去，输出更新以后的surround free cloud ）
第八步，通过surround_free_cloud_去更新terrain_height_ptr_（主要更新了terrain_grid_occupy_list_，得到了可以通行的点组terrain_height_ptr_，更新了flat_terrain_cloud_和kdtree_terrain_clould_。最后通过一系列的过滤，把UpdateRobotPosition来的neighbor_obs_indices进行过滤得到新的neighbor_obs_indices，然后再根据膨胀系数，得到extend_obs_indices_ ）
第九步，更新surround obs cloud
第十步，动态障碍物部分
第十一步，根据时间来更新stack_new_cloud_ （原本有个stack_new_cloud_，存的是之前的点，把它和cur_new_cloud_合并，并剔除掉与cur_new_cloud_差距在1秒外的所有点云）
第十二步，根据stack_new_cloud_更新kdtree_new_cloud_
第十三步，如果surround_obs_cloud_ 不为空，那么cloud就初始化成功is_cloud_init_ = true
/* visualize clouds */
planner_viz_.VizPointCloud(new_PCL_pub_, FARUtil::stack_new_cloud_);
planner_viz_.VizPointCloud(dynamic_obs_pub_, FARUtil::cur_dyobs_cloud_);
planner_viz_.VizPointCloud(surround_free_debug_, FARUtil::surround_free_cloud_);
planner_viz_.VizPointCloud(surround_obs_debug_, FARUtil::surround_obs_cloud_);
planner_viz_.VizPointCloud(terrain_height_pub_, terrain_height_ptr_);
// visualize map grid
PointStack neighbor_centers, occupancy_centers;
map_handler_.GetNeighborCeilsCenters(neighbor_centers);
map_handler_.GetOccupancyCeilsCenters(occupancy_centers);
planner_viz_.VizMapGrids(neighbor_centers, occupancy_centers, map_params_.cell_length, map_params_.cell_height);剩下的这些都是可视化操作了，我就不说了。