Hi!

After getting some simple global path planning to run, I noticed some 
strange behavior of the resulting plan.  In a node, I create a new 
costmap, navfn and then call makePlan:

m_costmap2d = new costmap_2d::Costmap2DROS("costmap", m_tfListener);   
m_navFn = new navfn::NavfnROS("planner", m_costmap2d);
[...]

if(!m_navFn->makePlan(start, goal, plan) || plan.empty()){
  ROS_ERROR("Failed to find a  plan to point (%.2f, %.2f)", 
goal.pose.position.x, goal.pose.position.y);
  return false;
}

(start results from a static transform of the the base link at (0,0,0), 
and goal comes from "2D Nav goal" in RViz.

When visualizing the resulting plan in RViz, it's not always the 
shortest connection to the goal but a mixture of diagonal connections 
and a Manhattan distance path. To test, I loaded a completely empty map 
(no obstacles) as static map with res. 0.1. Depending on the goal 
location (a few cm up or down), I get "path1" or "path2" in the attached 
screenshot. Both seem to start slightly off the robot's pose (seems like 
1/2 map cell). Besides this, 1 looks right to me, while 2 doesn't look 
like a shortest path.

Is the path planner not taking diagonal movements properly into account, 
or is there a bug somewhere? In NavfnROS::makePlan, I notice that the 
plan coordinates returned from Navfn are floats, which get casted to 
unsigned int in order to compute the world coordinates in floats again, 
could this be a problem?

If helpful, I can create a simple test case as node, but it should be 
pretty straightforward to reproduce anyways...

Best regards,
Armin


-- 
Armin Hornung                              Albert-Ludwigs-Universität
www.informatik.uni-freiburg.de/~hornunga   Dept. of Computer Science
HornungA@informatik.uni-freiburg.de        Humanoid Robots Lab
Tel.: +49 (0)761-203-8010                  Georges-Köhler-Allee 79
Fax : +49 (0)761-203-8007                  D-79110 Freiburg, Germany