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

Eitan Marder-Eppstein eitan at willowgarage.com
Wed Dec 1 19:18:09 UTC 2010


Bhaskara,

This is different in that it will actually create plans to space that's
completely occupied where as the default_tolerance parameter guarantees that
the endpoint of the plan is in free space. It just searches in a window
around the user-specified goal

After thinking about it a little though, maybe clearing obstacles out in
this situation isn't such a good thing, and it does seem like the use case
Christian has would have been solved by setting the default_tolerance
parameter.

Christian, is there any reason that you want to actually clear the obstacles
from the goal region as opposed to attempting to find a valid plan within a
wider region without clearing obstacles?

If there's no compelling reason for it, perhaps I'll back out the change.

Hope all is well,

Eitan

On Wed, Dec 1, 2010 at 10:33 AM, Bhaskara Marthi
<bhaskara at willowgarage.com>wrote:

>
>
> On Mon, Nov 29, 2010 at 9:43 AM, Eitan Marder-Eppstein <
> eitan at willowgarage.com> wrote:
>
>> Seems like a reasonable feature to expose. Applied the patch in r34304 in
>> navigation trunk. Should go into D-Turtle.
>>
>> Thanks for passing it on,
>>
>> Eitan
>>
>>
>> Couldn't we get this behavior by just setting the default_tolerance
> parameter of navfn?
> - Bhaskara
>
>
>> On Fri, Nov 26, 2010 at 3:35 AM, Christian Verbeek <
>> verbeek at servicerobotics.eu> wrote:
>>
>>> 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 at servicerobotics.eu
>>>
>>> Geschäftsführer: Dr. Christian Verbeek
>>> Registergericht: AG München (HRB 154463)
>>> ____________________________________________
>>>
>>>
>>> _______________________________________________
>>> ros-users mailing list
>>> ros-users at code.ros.org
>>> https://code.ros.org/mailman/listinfo/ros-users
>>>
>>
>>
>> _______________________________________________
>> ros-users mailing list
>> ros-users at code.ros.org
>> https://code.ros.org/mailman/listinfo/ros-users
>>
>>
>
>
> --
> Bhaskara Marthi
> Research Scientist
> Willow Garage Inc.
> 650-475-2856
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
>
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20101201/51408e41/attachment-0003.html>


More information about the ros-users mailing list