ROS：一种路径优化方法-拉直法

ROS：一种路径优化方法

此处提供一种路径优化方法

名字：拉直法，即将路径拉直，将路径上一些不在直线上的点删掉。取直线的原则为：不能与障碍物相撞。

参考代码：底部

算法说明：

如图：

1、设路径点为数组P[end],end为一个大于2的整数。
2、从路径一个点P[x]开始尝试与路径上其他的点P[x+2]～P[n]～P[end]拉直，若能拉直则删除Px与Pn之间所有的路径点（不包括Px和Pn）。
3、x取值范围[0,end-3]，依次增大，每次加1，x每加1需要与n为[x+2,end]上所有的点做拉直计算。
4、n的取值范围[x+2,end]。
5、当x >= end-2，算法结束。

效果：

参考代码：

bool path_optimization::collision(costmap_2d::Costmap2D* costmap, const double x, const double y)
{
  unsigned int mx,my;
  if(!costmap->worldToMap(x, y, mx, my))
    return true;
  if ((mx >= costmap->getSizeInCellsX()) || (my >= costmap->getSizeInCellsY()))
    return true;
  if (costmap->getCost(mx, my) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
    return true;
  return false;
}

bool path_optimization::isLineFree(costmap_2d::Costmap2D* costmap, const geometry_msgs::PoseStamped pose1, const geometry_msgs::PoseStamped pose2)
{
  std::pair<double, double> p1;
  p1.first = pose1.pose.position.x;
  p1.second = pose1.pose.position.y;

  std::pair<double, double> p2;
  p2.first = pose2.pose.position.x;
  p2.second = pose2.pose.position.y;

  double resolution = costmap->getResolution();

  std::pair<double, double> ptmp;
  ptmp.first = 0.0;
  ptmp.second = 0.0;

  double dist = sqrt( (p2.second-p1.second) * (p2.second-p1.second) +
                      (p2.first-p1.first) * (p2.first-p1.first) );
  if (dist < resolution)
  {
      return true;
  }
  else
  {
    int value = int(floor(dist/resolution));
    double theta = atan2( (p2.second - p1.second), (p2.first - p1.first) );
    int n = 1;
    for (int i = 0;i < value; i++)
    {
      ptmp.first = p1.first + resolution*cos(theta) * n;
      ptmp.second = p1.second + resolution*sin(theta) * n;
      if (collision(costmap, ptmp.first, ptmp.second))
        return false;
      n++;
    }
    return true;
  }
}

void path_optimization::cutPathPoint(costmap_2d::Costmap2D* costmap, std::vector<geometry_msgs::PoseStamped> &plan)
{
  size_t current_index = 0;
  size_t check_index = current_index+2;
  while(ros::ok())
  {
    if( current_index >= plan.size()-2 )
      return;
    if( this->isLineFree(costmap, plan[current_index], plan[check_index]) ) //点之间无障碍物
    {
      std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin() + static_cast<int>(current_index + 1) ;
      if(check_index-current_index-1 == 1)
      {
        plan.erase(it);
      }
      else
      {
        plan.erase(it,it+static_cast<int>( check_index-current_index-1) );
        check_index = current_index + 2;
      }
    }
    else
    {
      if(check_index < plan.size()-1 )
        check_index++;
      else
      {
        current_index++;
        check_index = current_index + 2;
      }
    }
  }
}