navfn and costmap_2d
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, reverting to previous footprint.
[ INFO] [1434622104.336959632]: Giving goal to path planner in the world
[ INFO] [1434622104.337030249]: GOAL: x = 1.000000, y = 0.600000
[ INFO] [1434622104.342911736]: Goal potential = 0.000000
[ INFO] [1434622104.343713882]: Size of path = 0
Can anyone please help me understand what is wrong with my code? Also, the navfn package does not seem to subscribe to any topics...how can I do this in code? I have done this using move_base
but now I need to use navfn as a separate entity in my own code.
Thanks.
Asked by RND on 2015-06-18 05:22:32 UTC
Comments
In order to increase the possibilities of getting an answer, I recommend you to write brief, more specific questions.
Asked by Javier V. Gómez on 2015-06-18 07:06:19 UTC
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.
Asked by RND on 2015-06-18 07:35:15 UTC
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.
Asked by svdeepak99 on 2020-04-14 10:42:13 UTC