[ros-users] navfn create plan even if goal hits obstacle
Eitan Marder-Eppstein
eitan at willowgarage.com
Fri Dec 3 17:01:34 UTC 2010
Christian,
Ah yes, I forgot, that parameter only exists in the unstable tag of
navigation because it was a fairly recent change to navfn that extends the
api and can't go into cturtle. If you check out the unstable tag, you should
be good to go. Unstable should still run on top of a cturtle system if you
want to test it out, most of the changes involve extending APIs.
Sorry about the confusion and hope all is well,
Eitan
On Fri, Dec 3, 2010 at 1:51 AM, Christian Verbeek <
verbeek at servicerobotics.eu> wrote:
> Eitan,
>
> What parameter are you talking about? There is no such parameter for navfn
> in cturtle.
>
> Regards
> Christian
>
> Am 01.12.2010 20:18, schrieb Eitan Marder-Eppstein:
>
> 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
>>
>>
>
> _______________________________________________
> ros-users mailing listros-users at code.ros.orghttps://code.ros.org/mailman/listinfo/ros-users
>
>
>
> --
> ___________________________________________
> 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
>
>
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20101203/8fff324b/attachment-0003.html>
More information about the ros-users
mailing list