inline bool operator()(unsigned x, unsigned y) const

  float aStar(Node2D& start,`
      Node2D& goal,
      Node2D* nodes2D,
      int width,
      int height,
      CollisionDetection& configurationSpace,
      Visualize& visualization)
  {
    const unsigned step = 1; // 0 compresses the path as much as possible and only records waypoints.
    // Set this to 1 if you want a detailed single-step path
    // (e.g. if you plan to further mangle the path yourself),
    // or any other higher value to output every Nth position.
    // (Waypoints are always output regardless of the step size.)
  
    JPS::PathVector path; // The resulting path will go here.
    // You may also use std::vector or whatever, as long as your vector type
    // has push_back(), begin(), end(), resize() methods (same semantics as std::vector).
    // Note that the path will NOT include the starting position!
    // --> If called with start == end it will report that a path has been found,
    //     but the resulting path vector will be empty!
  
  
    // Single-call interface:
    // (Further remarks about this function can be found near the bottom of this file.)
    // Note that the path vector is NOT cleared! New path points are appended at the end.
    bool found = JPS::findPath(path, configurationSpace, start.getX(), start.getY(), goal.getX(), goal.getY(), step);
    float score = 0;
    if(found){
      for(unsigned int i = 1; i < path.size(); i++){
        Node2D n2d(path[i].x, path[i].y, 0, 0, nullptr);
        n2d.discover();
        if (Constants::visualization2D) {
          visualization.publishNode2DPoses(n2d);
        }
        const int dx = (int(path[i].x - path[i-1].x));
        const int dy = (int(path[i].y - path[i-1].y));
        //Euclidean + Chebyshev can make the search faster. Why??
        score += sqrt((float)(dx*dx + dy*dy));
        //score += dx*dx + dy*dy;
        score += JPS::Heuristic::Chebyshev(path[i], path[i-1]);
      }
    }else{
      score = 1000000.0;
    }
    //ROS_INFO("%s score:%f", __FUNCTION__, score);
    return (float)score;
  
  }