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

[Bhaskara Marthi]

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
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.osuosl.org/pipermail/ros-users/attachments/20101201/66afd23b/attachment-0002.html>