navfn and costmap_2d

edit

Hello,

I am trying to use the classes navfn::NavfnROS and costmap_2d::Costmap2DROS in order to calculate a path from a start location to a goal location. I need to do this such that I can evaluate the paths to a set of goals in the world. I have implemented the following code:

using namespace std;
using geometry_msgs::Point;

int main(int argc, char** argv) 
{

    ros::init(argc, argv, "dijkstra");
    ros::NodeHandle n;

    tf::TransformListener tf(ros::Duration(10));
    costmap_2d::Costmap2DROS costmap("global_costmap", tf);

    costmap_2d::LayeredCostmap layers("map", false, true);

    std::vector<Point> polygon;
    geometry_msgs::Point vector_point;
    vector_point.x = -0.5;
    vector_point.y = -0.33;
    vector_point.z = 0.0;
    polygon.push_back(vector_point);
    vector_point.x = -0.5;
    vector_point.y = 0.33;
    vector_point.z = 0.0;
    polygon.push_back(vector_point);
    vector_point.x = 0.5;
    vector_point.y = 0.33;
    vector_point.z = 0.0;
    polygon.push_back(vector_point);
    vector_point.x = 0.5;
    vector_point.y = -0.33;
    vector_point.z = 0.0;
    polygon.push_back(vector_point);
    layers.setFootprint(polygon);

    //ROS_INFO("Inscribed Radius = %f, Circumscribed Radius = %f", polygon.at(0).x, polygon.at(1).x);

    costmap.getCostmap();
    costmap.start();

    navfn::NavfnROS navfn;
    navfn.initialize("my_navfn_planner", &costmap);

    std::vector<geometry_msgs::PoseStamped> plan;   // Initialize vector that stores the plan poses

    double goal_x = 1.0;
    double goal_y = 0.6;

    // Declare file name that will be used to store path planned poses
    ofstream myfile("plan_poses.txt");

    if(!myfile) {
        cout << "Cannot open file for writing!" << endl;
    }

    ROS_INFO("Giving goal to path planner in the world");
    ROS_INFO("GOAL: x = %f, y = %f", goal_x,goal_y);

    // construct a pose message for the start pose
        geometry_msgs::PoseStamped start_pose;
        start_pose.header.frame_id = "map";
        start_pose.header.stamp = ros::Time::now();

    start_pose.pose.position.x = 0.0;
        start_pose.pose.position.y = 0.0;
        start_pose.pose.position.z = 0.0;
        start_pose.pose.orientation.x = 0.0;
        start_pose.pose.orientation.y = 0.0;
        start_pose.pose.orientation.z = 0.0;
        start_pose.pose.orientation.w = 1;

    // construct a pose message for the goal
        geometry_msgs::PoseStamped goal_pose;
        goal_pose.header.frame_id = "map";
        goal_pose.header.stamp = ros::Time::now();

    goal_pose.pose.position.x = goal_x;
        goal_pose.pose.position.y = goal_y;
        goal_pose.pose.position.z = 0.0;
        goal_pose.pose.orientation.x = 0.0;
        goal_pose.pose.orientation.y = 0.0;
        goal_pose.pose.orientation.z = 0.0;
        goal_pose.pose.orientation.w = 1;


    geometry_msgs::Point goal;
    goal.x = 1.0;
    goal.y = 0.6;
    goal.z = 0.0;

    navfn.computePotential(goal);
    double potential = navfn.getPointPotential(goal);
    ROS_INFO("Goal potential = %f", potential);
    navfn.makePlan(start_pose, goal_pose, plan);
    ROS_INFO("Size of path = %d", plan.size());

    int plan_size = plan.size();
    for (int i=0; i < plan_size; i++)
    {
        myfile << plan.at(i).pose.position.x << "," << plan.at(i).pose.position.y << endl;
    }

    return 0;
}

In this code I am attempting to get the global costmap of the current map and use it to plan a path by navfn. However, the output on my console is this:

[ INFO] [1434622104.004652179]: Using plugin "obstacle_layer"
[ INFO] [1434622104.171565187]:     Subscribed to Topics: 
[ INFO] [1434622104.221140799]: Using plugin "inflation_layer"
[ERROR] [1434622104.254045545]: You must specify at least three points for the robot footprint ...