[ros-users] navfn create plan even if goal hits obstacle

Dear Eitan,

I found it useful to enable navfn to be able to create a plan even if
the goal position hits an obstacle. This is useful when you set goal
positions through a user interface. If the user wants the robot to move
near to walls or other obstacles then it is very likely that the goal
position hits the obstacle and the robot does not move at all. Instead
the intended behaviour is to get as close as possible and then stop if
the robots sensors detect an obstacle.

I modified nafvn_ros to clear a region in the costmap around the goal
position before making the plan. This behaviour must be switched on by
setting the clear_goal_position parameter to true (default is false
keeping everything as before).

What do you think? Could this be integrated into the ros mainstream?
Would make things easier for me.

The patch is generated from stacks/navigation/navfn

Best wishes
Christian

Index: include/navfn/navfn_ros.h
===================================================================
--- include/navfn/navfn_ros.h    (revision 34269)
+++ include/navfn/navfn_ros.h    (working copy)
@@ -132,7 +132,7 @@
       boost::shared_ptr<NavFn> planner_;
       double inscribed_radius_, circumscribed_radius_, inflation_radius_;
       ros::Publisher plan_pub_;
-      bool initialized_, allow_unknown_;
+      bool initialized_, allow_unknown_, clear_goal_position_;

     private:
Index: src/navfn_ros.cpp
===================================================================
--- src/navfn_ros.cpp    (revision 34269)
+++ src/navfn_ros.cpp    (working copy)
@@ -64,9 +64,10 @@
       plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);

       private_nh.param("allow_unknown", allow_unknown_, true);
+      private_nh.param("clear_goal_position", clear_goal_position_, false);
       private_nh.param("planner_window_x", planner_window_x_, 0.0);
       private_nh.param("planner_window_y", planner_window_y_, 0.0);
-       
+
       double costmap_pub_freq;
       private_nh.param("planner_costmap_publish_frequency",
costmap_pub_freq, 0.0);

@@ -252,17 +253,33 @@
     tf::poseStampedMsgToTF(start, start_pose);
     clearRobotCell(start_pose, mx, my);

-    //make sure to resize the underlying array that Navfn uses
-    planner_->setNavArr(costmap_.getSizeInCellsX(),
costmap_.getSizeInCellsY());
-    planner_->setCostmap(costmap_.getCharMap(), true, allow_unknown_);
-
     int map_start[2];
     map_start[0] = mx;
     map_start[1] = my;

-    wx = goal.pose.position.x;
+    wx = goal.pose.position.x;
     wy = goal.pose.position.y;

+    if( clear_goal_position_ )
+    {
+        //clear goal cell to be able to plan even if the goal is within
an obstacle
+        for( float x = wx - inscribed_radius_; x <= wx +
inscribed_radius_; x += costmap_.getResolution() )
+        {
+            for( float y = wy - inscribed_radius_; y <= wy +
inscribed_radius_; y += costmap_.getResolution() )
+            {
+                if(!costmap_.worldToMap(x, y, mx, my)){
+                    ROS_WARN("The goal sent to the navfn planner is off
the global costmap. Planning will always fail to this goal.");
+                    return false;
+                }
+                costmap_.setCost(mx, my, costmap_2d::FREE_SPACE);
+            }
+        }
+    }
+
+    //make sure to resize the underlying array that Navfn uses
+    planner_->setNavArr(costmap_.getSizeInCellsX(),
costmap_.getSizeInCellsY());
+    planner_->setCostmap(costmap_.getCharMap(), true, allow_unknown_);
+
     if(!costmap_.worldToMap(wx, wy, mx, my)){
       ROS_WARN("The goal sent to the navfn planner is off the global
costmap. Planning will always fail to this goal.");
       return false;

--
___________________________________________
REC GmbH
Dr. Christian Verbeek
Robert-Koch-Str. 2, 82152 Planegg

Tel:    +49 89 85689672
Fax:    +49 89 85902327
Mobile: +49 160 7056589
e-mail: verbeek@servicerobotics.eu

Geschäftsführer: Dr. Christian Verbeek
Registergericht: AG München (HRB 154463)
____________________________________________