ROS：搜索某个点附近的可行点

ROS：搜索某个点附近的可行点

解决目标点或机器人子膨胀层里导致规划不出路径问题。
函数：

void planner::getNearFreePoint(const geometry_msgs::PoseStamped in,
                               geometry_msgs::PoseStamped& out,
                               double tolerance)；

tolerance：搜索半径,单位米。
in：输入的点
out：搜索到的可行点。

double planner::distance(double x1,double y1,double x2,double y2)
  {
    return sqrt( (x1-x2) * (x1-x2) + (y1-y2) * (y1-y2) );
  }

  bool planner::isAroundFree(unsigned int mx, unsigned int my)
  {
    if(mx <= 1 || my <= 1 || mx >= this->costmap_->getSizeInCellsX()-1 || my >= this->costmap_->getSizeInCellsY()-1)
      return false;
    int x,y;
    for(int i=-1;i<=1;i++)
    {
      for(int j=-1;j<=1;j++)
      {
        x = static_cast<int>(mx) + i;
        y = static_cast<int>(my) + j;
        if(this->costmap_->getCost(static_cast<unsigned int>(x),static_cast<unsigned int>(y)) != costmap_2d::FREE_SPACE)
          return false;
      }
    }
    return true;
  }

  void planner::getNearFreePoint(const geometry_msgs::PoseStamped in,
                                       geometry_msgs::PoseStamped& out,
                                       double tolerance)
  {
    out = in;
    unsigned int grid_size = static_cast<unsigned int>(tolerance/costmap_->getResolution() + 0.5);
    if(grid_size<1)
    {
      out = in;
      return;
    }

    unsigned int mx0,my0;
    if(costmap_->worldToMap(in.pose.position.x,in.pose.position.y,mx0,my0))
    {
      if(this->isAroundFree(mx0,my0))
        return;
      unsigned int minx,maxx,miny,maxy;
      double wx = 0.0,wy = 0.0;
      double min_move_cost = 10000000.0;
      minx = mx0-grid_size>0?mx0-grid_size:0;
      maxx = mx0+grid_size<costmap_->getSizeInCellsX()?mx0+grid_size:costmap_->getSizeInCellsX();
      miny = my0-grid_size>0?my0-grid_size:0;
      maxy = my0+grid_size<costmap_->getSizeInCellsY()?my0+grid_size:costmap_->getSizeInCellsY();
      for(unsigned int i=minx;i<=maxx;i++)
      {
        for(unsigned int j=miny;j<=maxy;j++)
        {
          costmap_->mapToWorld(i,j,wx,wy);
          double current_move_cost = this->distance(in.pose.position.x,in.pose.position.y,wx,wy);
          if(!this->isAroundFree(i,j) || current_move_cost > tolerance)
            continue;
          if(min_move_cost > current_move_cost)
          {
            min_move_cost = current_move_cost;
            out.pose.position.x = wx;
            out.pose.position.y = wy;
          }
        }
      }
    }
  }