Armin,<br><br>You are totally right. I don't know what I was thinking when I decided it would be a good idea to cast to an integer before converting from cell to world coordinates. I guess it was that the costmap's conversion function from cell to world coordinates takes integers values, but still.... pretty bad. Thanks so much for finding this, I've fixed the bug in both trunk (rev 28749) and the 1.0 branch (rev 28750) of navigation and the fixes will go out with the next patch release of both the 1.0 and the 1.1 series. I'll try to push out patch releases either later today or tomorrow and I believe it should fix the problem you're seeing.<br>
<br>Hope all is well and thanks again for catching this,<br><br>Eitan<br><br><div class="gmail_quote">On Wed, Apr 14, 2010 at 2:04 AM, Armin Hornung <span dir="ltr"><<a href="mailto:HornungA@informatik.uni-freiburg.de">HornungA@informatik.uni-freiburg.de</a>></span> wrote:<br>
<blockquote class="gmail_quote" style="border-left: 1px solid rgb(204, 204, 204); margin: 0pt 0pt 0pt 0.8ex; padding-left: 1ex;">Hi!<br>
<br>
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:<br>
<br>
m_costmap2d = new costmap_2d::Costmap2DROS("costmap", m_tfListener);   m_navFn = new navfn::NavfnROS("planner", m_costmap2d);<br>
[...]<br>
<br>
if(!m_navFn->makePlan(start, goal, plan) || plan.empty()){<br>
 ROS_ERROR("Failed to find a  plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y);<br>
 return false;<br>
}<br>
<br>
(start results from a static transform of the the base link at (0,0,0), and goal comes from "2D Nav goal" in RViz.<br>
<br>
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.<br>

<br>
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?<br>

<br>
If helpful, I can create a simple test case as node, but it should be pretty straightforward to reproduce anyways...<br>
<br>
Best regards,<br>
Armin<br><font color="#888888">
<br>
<br>
-- <br>
Armin Hornung                              Albert-Ludwigs-Universität<br>
<a href="http://www.informatik.uni-freiburg.de/%7Ehornunga" target="_blank">www.informatik.uni-freiburg.de/~hornunga</a>   Dept. of Computer Science<br>
<a href="mailto:HornungA@informatik.uni-freiburg.de" target="_blank">HornungA@informatik.uni-freiburg.de</a>        Humanoid Robots Lab<br>
Tel.: +49 (0)761-203-8010                  Georges-Köhler-Allee 79<br>
Fax : +49 (0)761-203-8007                  D-79110 Freiburg, Germany<br>
<br>
</font><br>_______________________________________________<br>
ros-users mailing list<br>
<a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
<a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
<br></blockquote></div><br>