[ros-users] Oddness in global plan from navfn / costmap_2d

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