prevent `ProbRoadmapGraph::createGraph` from generating straight line from start to goal?

[VRichardJP]

I think there is some design issue with ProbRoadmapGraph::createGraph. The point of the function is to generate random paths to maximize the chances for teb to find a good path, yet it has a non negligible probability to create the straight line from start to goal. That means the function does eventually generate many time the same path, which causes teb to loose time optimizing paths that have already been optimized in the past.

The best way to avoid such useless computation is not to create the start->goal edge in the search graph. In other word, to force each generated path to go through at least 1 random point:

  // Insert Edges
  HcGraphVertexIterator it_i, end_i, it_j, end_j;
  for (boost::tie(it_i,end_i) = boost::vertices(graph_); it_i!=boost::prior(end_i); ++it_i) // ignore goal in this loop
  {
    for (boost::tie(it_j,end_j) = boost::vertices(graph_); it_j!=end_j; ++it_j) // check all forward connections
    {
      if (it_i==it_j) // same vertex found
        continue;

      // NEW
      if (*it_i == start_vtx && *it_j == goal_vtx)
        continue; // no straight line from start to goal
      // NEW

      Eigen::Vector2d distij = graph_[*it_j].pos-graph_[*it_i].pos;
      distij.normalize(); // normalize in place

      // Check if the direction is backwards:
      if (distij.dot(diff)<=obstacle_heading_threshold)
          continue; // diff is already normalized


      // Collision Check
      bool collision = false;
      for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst)
      {
        if ( (*it_obst)->checkLineIntersection(graph_[*it_i].pos,graph_[*it_j].pos, dist_to_obst) )
        {
          collision = true;
          break;
        }
      }
      if (collision)
        continue;

      // Create Edge
      boost::add_edge(*it_i,*it_j,graph_);
    }
  }