navfn and costmap_2d

asked 2015-06-18 05:22:32 -0600

RND gravatar image

updated 2015-06-18 07:34:29 -0600

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 ...
(more)
edit retag flag offensive close merge delete

Comments

In order to increase the possibilities of getting an answer, I recommend you to write brief, more specific questions.

Javier V. Gómez gravatar image Javier V. Gómez  ( 2015-06-18 07:06:19 -0600 )edit

Shall I place the code in a separate file? I would need others to see this code since I do not know what I'm doing wrong.

RND gravatar image RND  ( 2015-06-18 07:35:15 -0600 )edit

Were you able to get this work finally? I am also trying to do the same & would be helpful if you could tell me how you did it or share the code. This question has got 414 views but no answers.

svdeepak99 gravatar image svdeepak99  ( 2020-04-14 10:42:13 -0600 )edit