机器人自主探索-DSVP：代码解析

 写在前面

论文：DSVP: Dual-Stage Viewpoint Planner for Rapid Exploration by Dynamic Expansion

该文章对“Receding Horizon "Next-Best-View" Planner for 3D Exploration”进行改进

代码地址：GitHub - HongbiaoZ/dsv_planner: Dual-Stage Viewpoint Planner for Autonomous ExplorationDual-Stage Viewpoint Planner for Autonomous Exploration - GitHub - HongbiaoZ/dsv_planner: Dual-Stage Viewpoint Planner for Autonomous Explorationhttps://github.com/HongbiaoZ/dsv_planner

代码解析 ：

首先查看launch文件：

<launch>

<arg name="enable_bag_record" default="false"/>

<arg name="bag_name" default="dsvp_garage"/>

<arg name="simulation" default="false"/>

<arg name="use_boundary" default="false"/>

<arg name="planner_param_file" default="$(find dsvp_launch)/config/exploration.yaml" />

<arg name="octomap_param_file" default="$(find dsvp_launch)/config/octomap.yaml" />

<rosparam command="load" file="$(arg planner_param_file)" />

<rosparam command="load" file="$(arg octomap_param_file)" />

<node name="dsvplanner" pkg="dsvplanner" type="dsvplanner" output="screen" />

<group if="$(arg use_boundary)">

<node pkg="dsvp_launch" type="navigationBoundary" name="navigationBoundary" output="screen" required="true">

<param name="boundary_file_dir" type="string" value="$(find dsvp_launch)/data/boundary.ply" />

<param name="sendBoundary" type="bool" value="true" />

<param name="sendBoundaryInterval" type="int" value="2" />

</node>

</group>

<node name="exploration" pkg="dsvp_launch" type="exploration" output="screen" >

<param name="simulation" type="bool" value="$(arg simulation)" />

</node>

<include file="$(find graph_planner)/launch/graph_planner.launch"/>

<group if="$(arg enable_bag_record)">

<include file="$(find dsvp_launch)/launch/rosbag_record.launch">

<arg name="bag_name" value="$(arg bag_name)" />

</include>

</group>

<node pkg="rviz" type="rviz" name="dsvp_rviz" args="-d $(find dsvp_launch)/default.rviz"/>

</launch>

从launch文件可以看出，主要执行两个node文件，首先看第一个dsvplanner，其对应的cpp文件为drrtp_node.cpp

drrtp_node.cpp

在该代码中，定义一drrtPlanner对象planner，该对象的具体实现在drrtp.cpp中

#include <eigen3/Eigen/Dense>
#include <ros/ros.h>
#include <dsvplanner/drrtp.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "nbvPlanner");
  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");

  dsvplanner_ns::drrtPlanner planner(nh, nh_private);

  ros::spin();
  return 0;
}

drrtp.cpp

首先看该对象的构造函数

dsvplanner_ns::drrtPlanner::drrtPlanner(const ros::NodeHandle &nh,
                                        const ros::NodeHandle &nh_private)
    : nh_(nh), nh_private_(nh_private) {
  manager_ = new volumetric_mapping::OctomapManager(nh_, nh_private_);
  grid_ = new OccupancyGrid(nh_, nh_private_);
  dual_state_graph_ = new DualStateGraph(nh_, nh_private_, manager_, grid_);
  dual_state_frontier_ = new DualStateFrontier(nh_, nh_private_, manager_, grid_);
  drrt_ = new Drrt(manager_, dual_state_graph_, dual_state_frontier_, grid_);

  init();
  drrt_->setParams(params_);
  drrt_->init();

  ROS_INFO("Successfully launched DSVP node");
}

1. 构造函数中首先定义了4个对象：

1.1  manager_对象

manager_ = new volumetric_mapping::OctomapManager(nh_, nh_private_);

namespace volumetric_mapping {
OctomapManager::OctomapManager(const ros::NodeHandle &nh,
                               const ros::NodeHandle &nh_private)
    : nh_(nh), nh_private_(nh_private), world_frame_("map"),
      velodyne_cloud_topic_("/velodyne_cloud_topic"), robot_frame_("velodyne"),
      use_tf_transforms_(true), latch_topics_(true),
      timestamp_tolerance_ns_(10000000), Q_initialized_(false),
      Q_(Eigen::Matrix4d::Identity()), map_publish_frequency_(0.0) {
  setParametersFromROS();
  subscribe();
  advertiseServices();
  advertisePublishers();

  // After creating the manager, if the octomap_file parameter is set,
  // load the octomap at that path and publish it.
  std::string octomap_file;
  if (nh_private_.getParam("octomap_file", octomap_file)) {
    if (loadOctomapFromFile(octomap_file)) {
      ROS_INFO_STREAM(
          "Successfully loaded octomap from path: " << octomap_file);
      publishAll();
    } else {
      ROS_ERROR_STREAM("Could not load octomap from path: " << octomap_file);
    }
  }
}

subscribe():

void OctomapManager::subscribe() {
  pointcloud_sub_ = nh_.subscribe(velodyne_cloud_topic_, 1, &OctomapManager::insertPointcloudWithTf, this);
  octomap_sub_ = nh_.subscribe("input_octomap", 1, &OctomapManager::octomapCallback, this);
}

velodyne_cloud_topic_: /sensor_scan（lidar坐标系下的点云）

回调函数1：insertPointcloudWithTf（得到sensor坐标系到世界坐标系的变换，并将变换转换至相应格式）、光线追踪更新地图

void OctomapManager::insertPointcloudWithTf(
    const sensor_msgs::PointCloud2::ConstPtr &pointcloud) {
  // Look up transform from sensor frame to world frame.
  Transformation sensor_to_world;
  if (lookupTransform(pointcloud->header.frame_id, world_frame_,
                      pointcloud->header.stamp, &sensor_to_world)) {
    insertPointcloud(sensor_to_world, pointcloud);
  }
}

lookupTransformTf实现： 

1. pointcloud->header.frame_id为：/sensor_at_scan 在sensor_scan_generation功能包中定义;

2. world_frame_：/map

bool OctomapManager::lookupTransform(const std::string &from_frame,
                                     const std::string &to_frame,
                                     const ros::Time &timestamp,
                                     Transformation *transform) {
  if (use_tf_transforms_) {
    return lookupTransformTf(from_frame, to_frame, timestamp, transform);
  } else {
    return lookupTransformQueue(from_frame, to_frame, timestamp, transform);
  }
}

bool OctomapManager::lookupTransformTf(const std::string &from_frame,
                                       const std::string &to_frame,
                                       const ros::Time &timestamp,
                                       Transformation *transform) {
  tf::StampedTransform tf_transform;

  ros::Time time_to_lookup = timestamp;

  // If this transform isn't possible at the time, then try to just look up
  // the latest (this is to work with bag files and static transform publisher,
  // etc).
  if (!tf_listener_.canTransform(to_frame, from_frame, time_to_lookup)) {
    ros::Duration timestamp_age = ros::Time::now() - time_to_lookup;
    if (timestamp_age < tf_listener_.getCacheLength()) {
      time_to_lookup = ros::Time(0);
      // ROS_WARN("Using latest TF transform instead of timestamp match.");
    } else {
      // ROS_ERROR("Requested transform time older than cache limit.");
      return false;
    }
  }

  try {
    tf_listener_.lookupTransform(to_frame, from_frame, time_to_lookup,
                                 tf_transform);
  } catch (tf::TransformException &ex) {
    ROS_ERROR_STREAM(
        "Error getting TF transform from sensor data: " << ex.what());
    return false;
  }

  tf::transformTFToKindr(tf_transform, transform);
  return true;
}

void transformTFToKindr(const tf::Transform& tf_type,
                        kindr::minimal::QuatTransformation* kindr) {
  CHECK_NOTNULL(kindr);
  Eigen::Vector3d position;
  Eigen::Quaterniond rotation;

  quaternionTFToKindr(tf_type.getRotation(), &rotation);
  vectorTFToKindr(tf_type.getOrigin(), &position);

  // Enforce positive w.
  if (rotation.w() < 0) {
    rotation.coeffs() = -rotation.coeffs();
  }

  *kindr = kindr::minimal::QuatTransformation(rotation, position);
}

即进行类型转换（tf_transform--transform(得到rotation和position)）

insertPointcloud(sensor_to_world, pointcloud)：实现的是通过对当前帧点云中的每一个点光线追踪来更新free_cells和occupied_cells 

回调函数2： octomapCallback(这个暂时先不看它 )

1.2  grid_对象

grid_ = new OccupancyGrid(nh_, nh_private_);

 该对象的实现：

OccupancyGrid::OccupancyGrid(const ros::NodeHandle &nh,  const ros::NodeHandle &nh_private)
    : nh_(nh), nh_private_(nh_private) {
  initialize();
}

bool OccupancyGrid::initialize() {
  // Read in parameters
  if (!readParameters())
    return false;
  // Initialize subscriber
  odom_sub_.subscribe(nh_, sub_odom_topic_, 1);
  terrain_point_cloud_sub_.subscribe(nh_, sub_terrain_point_cloud_topic_, 1);
  sync_.reset(new Sync(syncPolicy(100), odom_sub_, terrain_point_cloud_sub_));
  sync_->registerCallback( boost::bind(&OccupancyGrid::terrainCloudAndOdomCallback, this, _1, _2));

  grid_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(pub_grid_points_topic_, 1);

  map_half_width_grid_num_ = int(kMapWidth / 2 / kGridSize); //  40/2/0.1
  map_width_grid_num_ = map_half_width_grid_num_ * 2 + 1;

  clearGrid();

  ROS_INFO("Successfully launched OccupancyGrid node");

  return true;
}

初始化一些参数以及订阅/state_estimation和/terrain_map_ext，执行回调函数terrainCloudAndOdomCallback

void OccupancyGrid::terrainCloudAndOdomCallback(
    const nav_msgs::Odometry::ConstPtr &odom_msg,
    const sensor_msgs::PointCloud2::ConstPtr &terrain_msg) {
  terrain_time_ = terrain_msg->header.stamp;
  robot_position_[0] = odom_msg->pose.pose.position.x;
  robot_position_[1] = odom_msg->pose.pose.position.y;
  robot_position_[2] = odom_msg->pose.pose.position.z;
  terrain_cloud_->clear();
  terrain_cloud_ds->clear();
  terrain_cloud_traversable_->clear();
  terrain_cloud_obstacle_->clear();
  pcl::fromROSMsg(*terrain_msg, *terrain_cloud_);

  pcl::VoxelGrid<pcl::PointXYZI> point_ds;
  point_ds.setLeafSize(kDownsampleSize, kDownsampleSize, kDownsampleSize);
  point_ds.setInputCloud(terrain_cloud_);
  // 地形消息下采样
  point_ds.filter(*terrain_cloud_ds);

  pcl::PointXYZI point;
  int terrainCloudSize = terrain_cloud_ds->points.size();
  for (int i = 0; i < terrainCloudSize; i++) {
    point.x = terrain_cloud_ds->points[i].x;
    point.y = terrain_cloud_ds->points[i].y;
    point.z = terrain_cloud_ds->points[i].z;
    point.intensity = terrain_cloud_ds->points[i].intensity;
    // crop all ground points
    // 0.2m到1m之间的障碍物认为是obstacle,(kObstacleHeightThre默认0.2)
    if (point.intensity > kObstacleHeightThre && point.intensity < kFlyingObstacleHeightThre) {
      terrain_cloud_obstacle_->push_back(point);
    } else if (point.intensity <= kObstacleHeightThre) {
      terrain_cloud_traversable_->push_back(point);
    }
  }
  
  clearGrid(); // grid全部填充为unknown状态
  updateGrid(); 
  publishGridMap();
}

void OccupancyGrid::updateGrid() {
  pcl::PointXYZI point;
  for (int i = 0; i < terrain_cloud_obstacle_->points.size(); i++) {
    point = terrain_cloud_obstacle_->points[i];
    // 将障碍物点换算成索引
    // kGridSize = 0.1
    int indX = int((point.x - robot_position_[0] + kGridSize / 2) / kGridSize) + map_half_width_grid_num_;
    int indY = int((point.y - robot_position_[1] + kGridSize / 2) / kGridSize) + map_half_width_grid_num_;
    // 应对负数四舍五入情况
    if (point.x - robot_position_[0] + kGridSize / 2 < 0)
      indX--;
    if (point.y - robot_position_[1] + kGridSize / 2 < 0)
      indY--;
    if (indX < 0)
      indX = 0;
    if (indY < 0)
      indY = 0;
    if (indX > map_width_grid_num_ - 1)
      indX = map_width_grid_num_ - 1;
    if (indY > map_width_grid_num_ - 1)
      indY = map_width_grid_num_ - 1;

    if (indX >= 0 && indX < map_width_grid_num_ && indY >= 0 && indY < map_width_grid_num_) {
      gridStatus grid_state = occupied;
      gridState_[indX][indY] = grid_state;
    }
  }
  for (int i = 0; i < terrain_cloud_traversable_->points.size(); i++) {
    point = terrain_cloud_traversable_->points[i];
    int indX = int((point.x - robot_position_[0] + kGridSize / 2) / kGridSize) + map_half_width_grid_num_;
    int indY = int((point.y - robot_position_[1] + kGridSize / 2) / kGridSize) + map_half_width_grid_num_;
    if (point.x - robot_position_[0] + kGridSize / 2 < 0)
      indX--;
    if (point.y - robot_position_[1] + kGridSize / 2 < 0)
      indY--;
    if (indX < 0)
      indX = 0;
    if (indY < 0)
      indY = 0;
    if (indX > map_width_grid_num_ - 1)
      indX = map_width_grid_num_ - 1;
    if (indY > map_width_grid_num_ - 1)
      indY = map_width_grid_num_ - 1;
    if (indX >= 0 && indX < map_width_grid_num_ && indY >= 0 && indY < map_width_grid_num_) {
      if (gridState_[indX][indY] == 2) {
        continue;
      }
      if (updateFreeGridWithSurroundingGrids(indX, indY) == false) {
        gridStatus grid_state = free;
        gridState_[indX][indY] = grid_state;
      } else {
        gridStatus grid_state = near_occupied;
        gridState_[indX][indY] = grid_state;
      }
    }
  }
}

point.x() - robot_position_[0]为当前点到机器人原点的向量

kGridSize默认为0.1，该向量加上kGridSize/2再除以kGridSize再转为int变量，表示四舍五入；

即假设当前点距离机器人2m，栅格的分辨率为0.1，则距离20个格子，再加上map_half_width_grid_num_（200）,即这个点再220处。

机器人是地图的中心，一个点在机器人右边20处，那么在整个地图里面，它应该是左边第220个格子。

void OccupancyGrid::publishGridMap() {
  grid_cloud_->clear();
  pcl::PointXYZI p1;
  geometry_msgs::Point p2;
  // typedef Vector2i GridIndex;
  GridIndex p3;
  for (int i = 0; i < map_width_grid_num_; i++) {
    for (int j = 0; j < map_width_grid_num_; j++) {
      p3[0] = i;
      p3[1] = j;
      // 返回点
      p2 = getPoint(p3);
      p1.x = p2.x;
      p1.y = p2.y;
      p1.z = p2.z;
      p1.intensity = gridState_[i][j];
      grid_cloud_->points.push_back(p1);
    }
  }
  sensor_msgs::PointCloud2 gridCloud2;
  pcl::toROSMsg(*grid_cloud_, gridCloud2);
  gridCloud2.header.stamp = terrain_time_;
  gridCloud2.header.frame_id = world_frame_id_;
  grid_cloud_pub_.publish(gridCloud2);
}

1.2 grid_对象总结 

grid_定义是仅执行初始化订阅者和发布者以及clearGrid()函数。

订阅/state_estimation以及/terrain_map_ext，执行回调函数，函数内执行以下：将点云中的点划分为四类：unknown , free , occupied , near_occupied ，并将每个点的该信息存储在intensity属性中，并将其以话题/occpancy_grid_map发布出去。

 1.3 定义全局图

dual_state_graph_ = new DualStateGraph(nh_, nh_private_, manager_, grid_);

 dual_state_graph_构造函数：

namespace dsvplanner_ns {
DualStateFrontier::DualStateFrontier(
    const ros::NodeHandle &nh, const ros::NodeHandle &nh_private,
    volumetric_mapping::OctomapManager *manager, OccupancyGrid *grid)
    : nh_(nh), nh_private_(nh_private) {
  manager_ = manager;
  grid_ = grid;
  initialize();
}

initialize()函数：

1. 初始化一些参数

2. 定义一些订阅者和发布者

// Initialize subscriber
  // sub_keypose_topic_: state_estimation_at_scan
  key_pose_sub_ = nh_.subscribe<nav_msgs::Odometry>(sub_keypose_topic_, 5, &DualStateGraph::keyposeCallback, this);
  // sub_path_topic_ : /graph_planner_path
  graph_planner_path_sub_ = nh_.subscribe<nav_msgs::Path>(sub_path_topic_, 1, &DualStateGraph::pathCallback, this);
  // sub_graph_planner_status_topic_ : /graph_planner_status
  graph_planner_status_sub_ = nh_.subscribe<graph_planner::GraphPlannerStatus>(sub_graph_planner_status_topic_, 1,
      &DualStateGraph::graphPlannerStatusCallback, this);

  // Initialize publishers
  // pub_local_graph_topic_: /local_graph
  local_graph_pub_ = nh_.advertise<graph_utils::TopologicalGraph>(pub_local_graph_topic_, 2);
  // pub_global_graph_topic_ : /global_graph
  global_graph_pub_ = nh_.advertise<graph_utils::TopologicalGraph>(pub_global_graph_topic_, 2);
  // pub_global_points_topic_ : /global_points
  graph_points_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(pub_global_points_topic_, 2);

发布的/global_graph仅用于显示 。

 一. 订阅话题/state_estimation_at_scan（等同于/state_estimation），执行回调函数keyposeCallback 

void DualStateGraph::keyposeCallback(const nav_msgs::Odometry::ConstPtr &msg) {
  geometry_msgs::Pose keypose;
  keypose.position.x = msg->pose.pose.position.x;
  keypose.position.y = msg->pose.pose.position.y;
  keypose.position.z = msg->pose.pose.position.z;
  keypose.orientation.y = 0;
  addNewGlobalVertexWithKeypose(keypose);
}

即每到一个机器人当前位姿，就执行addNewGlobalVertexWithKeypose()回调函数 

void DualStateGraph::addNewGlobalVertexWithKeypose(geometry_msgs::Pose &vertex_msg) {
  // Same as addNewLocalVertex but only adds vertex if a similar vertex doesn't already exist

  // Extract the point
  geometry_msgs::Point new_vertex_location;
  new_vertex_location = vertex_msg.position;

  // Check if a similar vertex already exists
  bool already_exists = false;
  bool too_long = false;
  double distance = -1;
  int closest_vertex_idx = -1;
  if (!global_graph_.vertices.empty()) {
    closest_vertex_idx = graph_utils_ns::GetClosestVertexIdxToPoint(global_graph_, new_vertex_location);
    auto &closest_vertex_location = global_graph_.vertices[closest_vertex_idx].location;
    distance = misc_utils_ns::PointXYZDist(new_vertex_location, closest_vertex_location);
    if (distance < kMinVertexDist / 2) {
      already_exists = true;
    }
    if (distance > kMaxLongEdgeDist || fabs(new_vertex_location.z - closest_vertex_location.z) > kMaxVertexDiffAlongZ) {
      too_long = true;
    }
  }

}

1. 该回调函数接受机器人当前位姿，提取出机器人当前位置信息， 并提取出global_graph_中距离当前点位置最近的点的索引closest_vertex_idx，以及其位置信息closest_vertex_location，

2. 判断当前点和最近的点的距离，如果距离太小或者太大，则忽略当前点。如果距离合适，则执行下面的函数:

  // If not, add a new one
  if (!already_exists && !too_long) {
    prev_track_vertex_idx_ = closest_vertex_idx;
    addNewGlobalVertex(vertex_msg);
    addGlobalEdgeWithoutCheck(prev_track_keypose_vertex_idx_, track_globalvertex_idx_, true);
    prev_track_keypose_vertex_idx_ = track_globalvertex_idx_;
  }

1. 将距离当前点最近的global_graph_中的点的索引closest_vertex_idx赋给prev_track_vertex_idx_，

2. 执行addNewGlobalVertex(vertex_msg)函数:

        2.1 将当前点的position、vertex_id(global_graph_.vertices.size())和information_gain添加到global_graph_中, 得到当前vertex的索引track_globalvertex_idx_，以及指针vertex_new

        2.2 当global_graph_中vertex的数量大于1时，定义当前vertex的parrent_idx为prev_track_vertex_idx_，执行函数addGlobalEdgeWithoutCheck(prev_track_vertex_idx_, track_globalvertex_idx_, false)。

        2.3 addGlobalEdgeWithoutCheck()函数功能：(在两个vertex之间添加edge)

                2.3.1 首先从global_graph_中根据传参的索引获取prev_vertex以及当前vertex并分别定义为start_vertex和end_vertex。

                2.3.2 接着检查两个vertex之间是否有edge，如果没有的话，进行添加

                2.3.3 首先计算两个vertex之间的距离dist_diff，然后定义两个edge对象：graph_utils::Edge edge_to_start以及graph_utils::Edge edge_to_end;

                2.3.4 添加两个edge : edge_to_start.vertex_id_end = start_vertex_idx，edge_to_end.vertex_id_end = end_vertex_idx;其中start_vertex_idx为prev_track_vertex_idx_，end_vertex_idx为track_globalvertex_idx_

                2.3.5 计算traversal cost：edge_to_start.traversal_costs = dist_diff，edge_to_end.traversal_costs = dist_diff;

                2.3.6 edge_to_start.keypose_edge = trajectory_edge，edge_to_end.keypose_edge = trajectory_edge;此时trajectory_edge传参为false.

                2.3.7 将edge添加到vertex中：start_vertex.edges.push_back(edge_to_end)，end_vertex.edges.push_back(edge_to_start).

        2.4 对当前vertex附近的vertex添加edge：计算global_graph_中每一个vertex距离当前vertex的路基，若小于一定阈值，则执行addGlobalEdge(graph_vertex.vertex_id, vertex_new.vertex_id)函数。

                2.4.1 函数声明：void DualStateGraph::addGlobalEdge(int start_vertex_idx, int end_vertex_idx)，即start_vertex_idx为满足阈值条件的nearby_vertex以及当前vertex。

                2.4.2 计算两个vertex之间的距离dist_edge

                2.4.3 接着调用ShortestPathBtwVertex(path, global_graph_, start_vertex_idx, end_vertex_idx)函数：Function for getting the shortest path on a graph between two vertexes；Output a sequence of vertex ids as the path-->path

                2.4.4 然后计算path的长度dist_path

                2.4.5 进行论文中公式6的判断，若满足条件，同时进行碰撞检测，满足条件者，调用addGlobalEdgeWithoutCheck()，添加边。

        2.5  执行addGlobalEdgeWithoutCheck(prev_track_keypose_vertex_idx_, track_globalvertex_idx_, true);

        紧接着prev_track_keypose_vertex_idx_ = track_globalvertex_idx_;

void DualStateGraph::addNewGlobalVertex(geometry_msgs::Pose &vertex_msg) {
  // Extract the point
  geometry_msgs::Point new_vertex_location;
  new_vertex_location = vertex_msg.position;

  // Create a new vertex
  graph_utils::Vertex vertex;
  vertex.location = new_vertex_location;
  vertex.vertex_id = (int)global_graph_.vertices.size();
  vertex.information_gain = vertex_msg.orientation.y;

  // Add this vertex to the graph
  global_graph_.vertices.push_back(vertex);
  track_globalvertex_idx_ = (int)global_graph_.vertices.size() - 1;
  auto &vertex_new = global_graph_.vertices[track_globalvertex_idx_];
  // If there are already other vertices
  if (global_graph_.vertices.size() > 1) {
    // Add a parent backpointer to previous vertex
    vertex_new.parent_idx = prev_track_vertex_idx_;

    // Add an edge to previous vertex 
    addGlobalEdgeWithoutCheck(prev_track_vertex_idx_, track_globalvertex_idx_, false);

    // Also add edges to nearby vertices
    for (auto &graph_vertex : global_graph_.vertices) {
      // If within a distance
      float dist_diff = misc_utils_ns::PointXYZDist(graph_vertex.location, vertex_new.location);
      if (dist_diff < kConnectVertexDistMax) {
        // Add edge from this nearby vertex to current vertex
        addGlobalEdge(graph_vertex.vertex_id, vertex_new.vertex_id);
      }
    }
  } else {
    // This is the first vertex -- no edges
    vertex.parent_idx = -1;
  }
}

void DualStateGraph::addGlobalEdgeWithoutCheck(int start_vertex_idx, int end_vertex_idx, bool trajectory_edge) {
  // Add an edge in the graph from vertex start_vertex_idx to vertex end_vertex_idx

  // Get the two vertices
  auto &start_vertex = global_graph_.vertices[start_vertex_idx];
  auto &end_vertex = global_graph_.vertices[end_vertex_idx];

  // Check if edge already exists -- don't duplicate
  for (auto &edge : start_vertex.edges) {
    if (edge.vertex_id_end == end_vertex_idx) {
      // don't add duplicate edge
      edge.keypose_edge = trajectory_edge;
      for (auto &edge : end_vertex.edges) {
        if (edge.vertex_id_end == start_vertex_idx) {
          // don't add duplicate edge
            edge.keypose_edge = trajectory_edge;
            break;
        }
      }
      return;
    }
  }

  // Compute the distance between the two points
  float dist_diff = misc_utils_ns::PointXYZDist(start_vertex.location, end_vertex.location);

  // Create two edge objects
  graph_utils::Edge edge_to_start;
  graph_utils::Edge edge_to_end;

  // Join the two edges
  edge_to_start.vertex_id_end = start_vertex_idx;
  edge_to_end.vertex_id_end = end_vertex_idx;

  // Compute the traversal cost
  // For now, this is just Euclidean distance
  edge_to_start.traversal_costs = dist_diff;
  edge_to_end.traversal_costs = dist_diff;

  edge_to_start.keypose_edge = trajectory_edge;
  edge_to_end.keypose_edge = trajectory_edge;

  // Add these two edges to the vertices
  start_vertex.edges.push_back(edge_to_end);
  end_vertex.edges.push_back(edge_to_start);

  globalEdgeSize_++;
}

void DualStateGraph::addGlobalEdge(int start_vertex_idx, int end_vertex_idx) {
  if (start_vertex_idx == end_vertex_idx) {
    return;
  }

  // Get the edge distance
  float dist_edge = misc_utils_ns::PointXYZDist(
      global_graph_.vertices[start_vertex_idx].location,
      global_graph_.vertices[end_vertex_idx].location);

  if (dist_edge > kMaxLongEdgeDist) {
    // VERY long edge. Don't add it
  } else {
    // Get the path distance
    std::vector<int> path;
    graph_utils_ns::ShortestPathBtwVertex(path, global_graph_, start_vertex_idx, end_vertex_idx);
    bool path_exists = true;
    float dist_path = 0;

    // Check if there is an existing path
    if (path.empty()) {
      // No path exists
      path_exists = false;
    } else {
      // Compute path length
      dist_path = graph_utils_ns::PathLength(path, global_graph_);
      // ROS_WARN("path exists of edge of length %f, where path exists of length
      // %f", dist_edge, dist_path);
    }

    // Check if ratio is beyond threshold
    // bool collision = false;
    // 论文中公式6定义的约束检查
    if ((!path_exists || (path_exists && ((dist_path / dist_edge) >= kExistingPathRatioThresholdGlobal))) &&
        (!zCollisionCheck(start_vertex_idx, end_vertex_idx, global_graph_))) {
      Eigen::Vector3d origin;
      Eigen::Vector3d end;
      origin.x() = global_graph_.vertices[start_vertex_idx].location.x;
      origin.y() = global_graph_.vertices[start_vertex_idx].location.y;
      origin.z() = global_graph_.vertices[start_vertex_idx].location.z;
      end.x() = global_graph_.vertices[end_vertex_idx].location.x;
      end.y() = global_graph_.vertices[end_vertex_idx].location.y;
      end.z() = global_graph_.vertices[end_vertex_idx].location.z;
      // 确保连线没有碰撞(光线追踪进行碰撞检测)
      if (volumetric_mapping::OctomapManager::CellStatus::kFree == manager_->getLineStatusBoundingBox(origin, end, robot_bounding)) {
        addGlobalEdgeWithoutCheck(start_vertex_idx, end_vertex_idx, false);
      }
    }
  }
}

 二. 订阅/graph_planner_path以及/graph_planner_status，并执行回调函数。这两个话题是由graph_planner节点发布的，暂时先不看。

1.4 dual_state_frontier对象 

// 定义一个全局边界求解器

dual_state_frontier_ = new DualStateFrontier(nh_, nh_private_, manager_, grid_);

DualStateFrontier::DualStateFrontier(
    const ros::NodeHandle &nh, const ros::NodeHandle &nh_private,
    volumetric_mapping::OctomapManager *manager, OccupancyGrid *grid)
    : nh_(nh), nh_private_(nh_private) {
  manager_ = manager;
  grid_ = grid;
  initialize();
}

 构造函数只执行了initialize()函数

bool DualStateFrontier::initialize() {
  // Read in parameters
  if (!readParameters())
    return false;

  // Initialize subscriber
  // /global_points
  graph_points_sub_ = nh_.subscribe(sub_graph_points_topic_, 1, &DualStateFrontier::graphPointsCallback, this);
  // /state_estimation
  odom_sub_.subscribe(nh_, sub_odom_topic_, 1);
  // /terrain_map_ext
  terrain_point_cloud_sub_.subscribe(nh_, sub_terrain_point_cloud_topic_, 1);
  sync_.reset(new Sync(syncPolicy(10), odom_sub_, terrain_point_cloud_sub_));
  sync_->registerCallback(boost::bind(&DualStateFrontier::terrainCloudAndOdomCallback, this, _1, _2));

  // /octomap_unknown
  unknown_points_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(pub_unknown_points_topic_, 1);
  // /global_frontier
  global_frontier_points_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(pub_global_frontier_points_topic_, 1);
  // /local_frontier
  local_frontier_points_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(pub_local_frontier_points_topic_, 1);
  terrain_elev_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/elevation_map", 1);

  if (kExecuteFrequency_ > 0.0) {
    executeTimer_ = nh_private_.createTimer(ros::Duration(1.0 / kExecuteFrequency_), &DualStateFrontier::execute, this);
  }
  for (int i = 0; i < kTerrainVoxelWidth * kTerrainVoxelWidth; i++) {
    terrain_voxel_elev_.push_back(robot_position_.z());
    terrain_voxel_points_num_.push_back(0);
    terrain_voxel_min_elev_.push_back(1000);
    terrain_voxel_max_elev_.push_back(-1000);
  }

  boundaryLoaded_ = false;

  ROS_INFO("Successfully launched DualStateFrontier node");

  return true;
}

 首先在一定频率下，不断执行execute()函数，而execute()函数则是调用getFrontiers()函数。

executeTimer_ = nh_private_.createTimer(ros::Duration(1.0 / kExecuteFrequency_), &DualStateFrontier::execute, this);

void DualStateFrontier::execute(const ros::TimerEvent &e) { getFrontiers(); }

void DualStateFrontier::getFrontiers() {
  // search_bounding[0] = 40
  // search_bounding[1] = 40
  // search_bounding[2] = 0.5
  // 给定一个范围，和位置中心，得到它周围的属于边界的点云，并将得到的边界点集合保存到local_frontier_中
  getUnknowPointcloudInBoundingBox(robot_position_, search_bounding);
  // 局部边界点的检查和更新
  localFrontierUpdate(robot_position_);
  // 全局边界点的检查和更新
  gloabalFrontierUpdate();
  // 对全局采样点的下采样
  globalFrontiersNeighbourCheck();
  // 对各个内容的发布，包括局部边界点，全局边界点，处于未知状态的点云
  publishFrontiers();
}

1. 首先调用getUnknowPointcloudInBoundingBox()函数：

        1.1 首先对中心位置进行调整，即调整center参数

        1.2 依次遍历给定范围内的x,y,z；调用octomap，根据点坐标得到索引值，并根据索引值得到具体的节点。

        1.3 如果octomap中没有这样的节点，就将该点加入到未知点集合unknown_points_中，如果该点在给定的范围之内，并且确实是一个边界点（如果该点确实是边界点，那么该点周围应该既有空闲也有未知），那么就加入到local_frontier_temp中

        1.4 对local_frontier_temp进行滤波得到local_frontier_

2. 接着调用localFrontierUpdate(robot_position_)函数：

        2.1  遍历local_frontier中的每一个点

        2.2 inSensorRangeofRobot()函数解析：只有其传参距离机器人当前位置在一定阈值之内，并且其向量的俯仰角小于15度，和机器人当前未知进行光线追踪的栅格单元都是free状态，则返回TRUE

        2.3 collisionCheckByTerrainWithVector(center, checkedPoint)函数：给定一个起点和终点，判断二者连线上有没有障碍物

        2.4 inSensorRangeofGraphPoints(checkedPoint)：输入一个点，利用kd树，在全局中找到最近得到点的集合，只要有一个可以被形参位置正常观测，并且没有障碍物，则认为是true

        2.5 满足一定条件的边界点被加入到local_frontier_pcl_以及global_frontier_中，并对local_frontier_pcl_进行降采样点得到local_frontier_

3. 调用gloabalFrontierUpdate()函数：提取 global_frontier_中的每一个点，如果该点属于被清空过的边界点，则跳出本次循环，得到该点在octomap中的索引值，如果该点属于边界点则添加到global_frontier_pcl_中，并对其进行滤波得到global_frontier_

4. globalFrontiersNeighbourCheck();： 遍历 global_frontier_中每一个点，通过KD树搜索其附近的边界点，如果存在，则只保存一个边界点。相当于对全局边界点的一个下采样，并将结果保存到global_frontier_pcl_和global_frontier_中

5. 发布/global_frontier以及/local_frontier ，/global_frontier比/local_frontier多了一次检查是否属于被清空的边界点，然后进行了两次滤波

 execute()函数总结：遍历当前机器人位置一定范围内的点，并找出边界点，后续通过降采样得到边界点并发布出去

2. 执行init()函数（读取参数以及确定订阅者和发布者）

bool dsvplanner_ns::drrtPlanner::init() {
  if (!setParams()) {
    ROS_ERROR("Set parameters fail. Cannot start planning!");
  }

  odomSub_ = nh_.subscribe(odomSubTopic, 10, &dsvplanner_ns::drrtPlanner::odomCallback, this);
  boundarySub_ = nh_.subscribe(boundarySubTopic, 10, &dsvplanner_ns::drrtPlanner::boundaryCallback, this);

  params_.newTreePathPub_ = nh_.advertise<visualization_msgs::Marker>(newTreePathPubTopic, 1000);
  params_.remainingTreePathPub_ = nh_.advertise<visualization_msgs::Marker>(remainingTreePathPubTopic, 1000);
  params_.boundaryPub_ = nh_.advertise<visualization_msgs::Marker>(boundaryPubTopic, 1000);
  params_.globalSelectedFrontierPub_ = nh_.advertise<sensor_msgs::PointCloud2>(globalSelectedFrontierPubTopic, 1000);
  params_.localSelectedFrontierPub_ = nh_.advertise<sensor_msgs::PointCloud2>(localSelectedFrontierPubTopic, 1000);
  params_.randomSampledPointsPub_ = nh_.advertise<sensor_msgs::PointCloud2>(randomSampledPointsPubTopic, 1000);
  params_.plantimePub_ = nh_.advertise<std_msgs::Float32>(plantimePubTopic, 1000);
  params_.nextGoalPub_ = nh_.advertise<geometry_msgs::PointStamped>(nextGoalPubTopic, 1000);
  params_.shutdownSignalPub = nh_.advertise<std_msgs::Bool>(shutDownTopic, 1000);

  plannerService_ = nh_.advertiseService(plannerServiceName, &dsvplanner_ns::drrtPlanner::plannerServiceCallback,this);
  cleanFrontierService_ = nh_.advertiseService(cleanFrontierServiceName, &dsvplanner_ns::drrtPlanner::cleanFrontierServiceCallback, this);

  return true;
}

 2.1 订阅/state_estimation，执行odomCallback()回调函数（机器人当前位姿传递给drrt求解器）

void dsvplanner_ns::drrtPlanner::odomCallback(const nav_msgs::Odometry &pose) {
  drrt_->setRootWithOdom(pose);
  // Planner is now ready to plan.
  drrt_->plannerReady_ = true;
}

void dsvplanner_ns::Drrt::setRootWithOdom(const nav_msgs::Odometry &pose) {
  root_[0] = pose.pose.pose.position.x;
  root_[1] = pose.pose.pose.position.y;
  root_[2] = pose.pose.pose.position.z;
}

 2.3 定义服务： 

服务类型：dsvplanner_srv

Header header
-------
geometry_msgs/Point[] goal
std_msgs/Int32 mode

plannerService_ = nh_.advertiseService(plannerServiceName, &dsvplanner_ns::drrtPlanner::plannerServiceCallback,this);
cleanFrontierService_ = nh_.advertiseService(cleanFrontierServiceName, &dsvplanner_ns::drrtPlanner::cleanFrontierServiceCallback, this);

graph_planner_node.cpp

1. 定义一个GraphPlanner对象

2. 执行execute()函数

graph_planner_ns::GraphPlanner graph_planner(node_handle);
graph_planner.execute();

graph_planner.cpp

此函数为graph_planner的实现函数，首先看其构造函数

GraphPlanner::GraphPlanner(const ros::NodeHandle &nh) {
  nh_ = nh;
  initialize();
}

 1. initialize()

bool GraphPlanner::initialize() {
  if (!readParameters())
    return false;

  // Initialize target waypoint
  waypoint_ = geometry_msgs::PointStamped();
  waypoint_.point.x = -1.0;
  waypoint_.point.y = -1.0;
  waypoint_.point.z = 0.0;
  // world_frame_id_ : map
  waypoint_.header.frame_id = world_frame_id_;

  // Initialize subscribers
  // /local_graph
  graph_sub_ = nh_.subscribe(sub_graph_topic_, 1, &GraphPlanner::graphCallback, this);
  // /graph_planner_command
  graph_planner_command_sub_ = nh_.subscribe(graph_planner_command_topic_, 1, &GraphPlanner::commandCallback, this);
  // /state_estimation
  odometry_sub_ = nh_.subscribe(sub_odometry_topic_, 1, &GraphPlanner::odometryCallback, this);
  // /terrain_map_ext
  terrain_sub_ = nh_.subscribe(sub_terrain_topic_, 1, &GraphPlanner::terrainCallback, this);

  // Initialize publishers
  // /way_point
  waypoint_pub_ = nh_.advertise<geometry_msgs::PointStamped>(pub_waypoint_topic_, 2);
  // /graph_planner_status
  graph_planner_status_pub_ = nh_.advertise<graph_planner::GraphPlannerStatus>(graph_planner_status_topic_, 2);
  // /graph_planner_path
  graph_planner_path_pub_ = nh_.advertise<nav_msgs::Path>(pub_path_topic_, 10);

  ROS_INFO("Successfully launched graph_planner node");

  return true;
}

 首先看订阅函数/state_estimation

1. 订阅/state_estimation，执行odometryCallback()回调函数：

        将机器人当前(x,y,z)位置传递给robot_pos_，以及yaw信息传递给robot_yaw_

void GraphPlanner::odometryCallback(
    const nav_msgs::Odometry::ConstPtr &odometry_msg) {
  double roll, pitch, yaw;
  geometry_msgs::Quaternion geo_quat = odometry_msg->pose.pose.orientation;
  tf::Matrix3x3(tf::Quaternion(geo_quat.x, geo_quat.y, geo_quat.z, geo_quat.w))
      .getRPY(roll, pitch, yaw);

  robot_yaw_ = yaw;
  robot_pos_ = odometry_msg->pose.pose.position;
}

2. 订阅/terrain_map_ext，执行回调函数terrainCallback：

        接受地型点云消息，大于0.2，小于1.2的点云被判断为会对机器人行动造成障碍，并将该点云保存到terrain_point_crop_中，并对其进行体素滤波得到terrain_point_crop_。

void GraphPlanner::terrainCallback(
    const sensor_msgs::PointCloud2::ConstPtr &terrain_msg) {
  terrain_point_->clear();
  terrain_point_crop_->clear();
  pcl::fromROSMsg(*terrain_msg, *terrain_point_);

  pcl::PointXYZI point;
  int terrainCloudSize = terrain_point_->points.size();
  for (int i = 0; i < terrainCloudSize; i++) {
    point = terrain_point_->points[i];

    float pointX = point.x;
    float pointY = point.y;
    float pointZ = point.z;

    // kObstacleHeightThres : 0.20 # the obstacle height that will be considered when filtering the terrain cloud
    // kOverheadObstacleHeightThres : 1.20 # the overhead obstacle height that will be considered when filtering the terrain cloud
    // 高于0.2 低于1.2的障碍物点云判断为会对机器人行动造成障碍
    if (point.intensity > kObstacleHeightThres && point.intensity < kOverheadObstacleHeightThres) {
      point.x = pointX;
      point.y = pointY;
      point.z = pointZ;
      terrain_point_crop_->push_back(point);
    }
  }
  pcl::VoxelGrid<pcl::PointXYZI> point_ds;
  point_ds.setLeafSize(kDownsampleSize, kDownsampleSize, kDownsampleSize);
  point_ds.setInputCloud(terrain_point_crop_);
  point_ds.filter(*terrain_point_crop_);
}

 3. 接着订阅/local_graph以及/graph_planner_command，并执行回调函数。这里暂且不看

 2. execute()

 此部分函数和exeploration.cpp()函数有关