Method createGraph

swarm_exploration/path_searching/src/topo_prm.cpp:99–187

}

list<GraphNode::Ptr> TopologyPRM::createGraph(Eigen::Vector3d start, Eigen::Vector3d end) {
  // std::cout << "[Topo]: searching----------------------" << std::endl;

  /* init the start, end and sample region */
  graph_.clear();
  // collis_.clear();

  GraphNode::Ptr start_node = GraphNode::Ptr(new GraphNode(start, GraphNode::Guard, 0));
  GraphNode::Ptr end_node = GraphNode::Ptr(new GraphNode(end, GraphNode::Guard, 1));

  graph_.push_back(start_node);
  graph_.push_back(end_node);

  // sample region
  sample_r_(0) = 0.5 * (end - start).norm() + sample_inflate_(0);
  sample_r_(1) = sample_inflate_(1);
  sample_r_(2) = sample_inflate_(2);

  // transformation
  translation_ = 0.5 * (start + end);

  Eigen::Vector3d xtf, ytf, ztf, downward(0, 0, -1);
  xtf = (end - translation_).normalized();
  ytf = xtf.cross(downward).normalized();
  ztf = xtf.cross(ytf);

  rotation_.col(0) = xtf;
  rotation_.col(1) = ytf;
  rotation_.col(2) = ztf;

  int node_id = 1;

  /* ---------- main loop ---------- */
  int sample_num = 0;
  double sample_time = 0.0;
  Eigen::Vector3d pt;
  ros::Time t1, t2;
  while (sample_time < max_sample_time_ && sample_num < max_sample_num_) {
    t1 = ros::Time::now();

    pt = getSample();
    ++sample_num;
    double dist;
    Eigen::Vector3d grad;
    // edt_environment_->evaluateEDTWithGrad(pt, -1.0, dist, grad);
    dist = edt_environment_->evaluateCoarseEDT(pt, -1.0);
    if (dist <= clearance_) {
      sample_time += (ros::Time::now() - t1).toSec();
      continue;
    }

    /* find visible guard */
    vector<GraphNode::Ptr> visib_guards = findVisibGuard(pt);
    if (visib_guards.size() == 0) {
      GraphNode::Ptr guard = GraphNode::Ptr(new GraphNode(pt, GraphNode::Guard, ++node_id));
      graph_.push_back(guard);
    } else if (visib_guards.size() == 2) {
      /* try adding new connection between two guard */