Seems like a reasonable feature to expose. Applied the patch in r34304 in navigation trunk. Should go into D-Turtle.<div><br></div><div>Thanks for passing it on,</div><div><br></div><div>Eitan<br><br><div class="gmail_quote">
On Fri, Nov 26, 2010 at 3:35 AM, Christian Verbeek <span dir="ltr"><<a href="mailto:verbeek@servicerobotics.eu">verbeek@servicerobotics.eu</a>></span> wrote:<br><blockquote class="gmail_quote" style="margin:0 0 0 .8ex;border-left:1px #ccc solid;padding-left:1ex;">
Dear Eitan,<br>
<br>
I found it useful to enable navfn to be able to create a plan even if<br>
the goal position hits an obstacle. This is useful when you set goal<br>
positions through a user interface. If the user wants the robot to move<br>
near to walls or other obstacles then it is very likely that the goal<br>
position hits the obstacle and the robot does not move at all. Instead<br>
the intended behaviour is to get as close as possible and then stop if<br>
the robots sensors detect an obstacle.<br>
<br>
I modified nafvn_ros to clear a region in the costmap around the goal<br>
position before making the plan. This behaviour must be switched on by<br>
setting the clear_goal_position parameter to true (default is false<br>
keeping everything as before).<br>
<br>
What do you think? Could this be integrated into the ros mainstream?<br>
Would make things easier for me.<br>
<br>
The patch is generated from stacks/navigation/navfn<br>
<br>
Best wishes<br>
Christian<br>
<br>
Index: include/navfn/navfn_ros.h<br>
===================================================================<br>
--- include/navfn/navfn_ros.h    (revision 34269)<br>
+++ include/navfn/navfn_ros.h    (working copy)<br>
@@ -132,7 +132,7 @@<br>
       boost::shared_ptr<NavFn> planner_;<br>
       double inscribed_radius_, circumscribed_radius_, inflation_radius_;<br>
       ros::Publisher plan_pub_;<br>
-      bool initialized_, allow_unknown_;<br>
+      bool initialized_, allow_unknown_, clear_goal_position_;<br>
<br>
<br>
     private:<br>
Index: src/navfn_ros.cpp<br>
===================================================================<br>
--- src/navfn_ros.cpp    (revision 34269)<br>
+++ src/navfn_ros.cpp    (working copy)<br>
@@ -64,9 +64,10 @@<br>
       plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);<br>
<br>
       private_nh.param("allow_unknown", allow_unknown_, true);<br>
+      private_nh.param("clear_goal_position", clear_goal_position_, false);<br>
       private_nh.param("planner_window_x", planner_window_x_, 0.0);<br>
       private_nh.param("planner_window_y", planner_window_y_, 0.0);<br>
-<br>
+<br>
       double costmap_pub_freq;<br>
       private_nh.param("planner_costmap_publish_frequency",<br>
costmap_pub_freq, 0.0);<br>
<br>
@@ -252,17 +253,33 @@<br>
     tf::poseStampedMsgToTF(start, start_pose);<br>
     clearRobotCell(start_pose, mx, my);<br>
<br>
-    //make sure to resize the underlying array that Navfn uses<br>
-    planner_->setNavArr(costmap_.getSizeInCellsX(),<br>
costmap_.getSizeInCellsY());<br>
-    planner_->setCostmap(costmap_.getCharMap(), true, allow_unknown_);<br>
-<br>
     int map_start[2];<br>
     map_start[0] = mx;<br>
     map_start[1] = my;<br>
<br>
-    wx = goal.pose.position.x;<br>
+    wx = goal.pose.position.x;<br>
     wy = goal.pose.position.y;<br>
<br>
+    if( clear_goal_position_ )<br>
+    {<br>
+        //clear goal cell to be able to plan even if the goal is within<br>
an obstacle<br>
+        for( float x = wx - inscribed_radius_; x <= wx +<br>
inscribed_radius_; x += costmap_.getResolution() )<br>
+        {<br>
+            for( float y = wy - inscribed_radius_; y <= wy +<br>
inscribed_radius_; y += costmap_.getResolution() )<br>
+            {<br>
+                if(!costmap_.worldToMap(x, y, mx, my)){<br>
+                    ROS_WARN("The goal sent to the navfn planner is off<br>
the global costmap. Planning will always fail to this goal.");<br>
+                    return false;<br>
+                }<br>
+                costmap_.setCost(mx, my, costmap_2d::FREE_SPACE);<br>
+            }<br>
+        }<br>
+    }<br>
+<br>
+    //make sure to resize the underlying array that Navfn uses<br>
+    planner_->setNavArr(costmap_.getSizeInCellsX(),<br>
costmap_.getSizeInCellsY());<br>
+    planner_->setCostmap(costmap_.getCharMap(), true, allow_unknown_);<br>
+<br>
     if(!costmap_.worldToMap(wx, wy, mx, my)){<br>
       ROS_WARN("The goal sent to the navfn planner is off the global<br>
costmap. Planning will always fail to this goal.");<br>
       return false;<br>
<br>
--<br>
___________________________________________<br>
REC GmbH<br>
Dr. Christian Verbeek<br>
Robert-Koch-Str. 2, 82152 Planegg<br>
<br>
Tel:    +49 89 85689672<br>
Fax:    +49 89 85902327<br>
Mobile: +49 160 7056589<br>
e-mail: <a href="mailto:verbeek@servicerobotics.eu">verbeek@servicerobotics.eu</a><br>
<br>
Geschäftsführer: Dr. Christian Verbeek<br>
Registergericht: AG München (HRB 154463)<br>
____________________________________________<br>
<br>
<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>
</blockquote></div><br></div>