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

[Eitan Marder-Eppstein]

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

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
>
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20101129/cdcdfa6b/attachment-0003.html>